src/gridf3.py

   1 import random
   2 from math import sqrt
   3
   4 from intrsc import intersections_from_angl_dist
   5 import linef as linef
   6 import ransac as ransac
   7 import manual as manual
   8 from geometry import l2ad
   9
  10 # TODO comments, refactoring, move methods to appropriate modules
  11
  12 class GridFittingFailedError(Exception):
  13     pass
  14
  15 def plot_line(line, c, size):
  16     points = linef.line_from_angl_dist(line, size)
  17     pyplot.plot(*zip(*points), color=c)
  18
  19
  20 class Diagonal_model:
  21     def __init__(self, data):
  22         self.data = [p for p in sum(data, []) if p]
  23         self.lines = data
  24         self.gen = self.initial_g()
  25
  26     def initial_g(self):
  27         l1, l2 = random.sample(self.lines, 2)
  28         for i in xrange(len(l1)):
  29             for j in xrange(len(l2)):
  30                 if i == j:
  31                     continue
  32                 if l1[i] and l2[j]:
  33                     yield (l1[i], l2[j])
  34
  35     def initial(self):
  36         try:
  37             return self.gen.next()
  38         except StopIteration:
  39             self.gen = self.initial_g()
  40             return self.gen.next()
  41
  42     def get(self, sample):
  43         if len(sample) == 2:
  44             return ransac.points_to_line(*sample)
  45         else:
  46             return ransac.least_squares(sample)
  47
  48     def score(self, est, dist):
  49         cons = []
  50         score = 0
  51         a, b, c = est
  52         dst = lambda (x, y): abs(a * x + b * y + c) / sqrt(a*a+b*b)
  53         for p in self.data:
  54             d = dst(p)
  55             if d <= dist:
  56                 cons.append(p)
  57             score += min(d, dist)
  58         return score, cons
  59
  60 def intersection((a1, b1, c1), (a2, b2, c2)):
  61     delim = float(a1 * b2 - b1 * a2)
  62     x = (b1 * c2 - c1 * b2) / delim
  63     y = (c1 * a2 - a1 * c2) / delim
  64     return x, y
  65
  66 class Point:
  67     def __init__(self, (x, y)):
  68         self.x = x
  69         self.y = y
  70
  71     def __getitem__(self, key):
  72         if key == 0:
  73             return self.x
  74         elif key == 1:
  75             return self.y
  76
  77     def __iter__(self):
  78         yield self.x
  79         yield self.y
  80
  81     def __len__(self):
  82         return 2
  83
  84     def to_tuple(self):
  85         return (self.x, self.y)
  86
  87 class Line:
  88     def __init__(self, (a, b, c)):
  89         self.a, self.b, self.c = (a, b, c)
  90         self.points = []
  91
  92     @classmethod
  93     def from_ad(cls, (a, d), size):
  94         p = linef.line_from_angl_dist((a, d), size)
  95         return cls(ransac.points_to_line(*p))
  96
  97     def __iter__(self):
  98         yield self.a
  99         yield self.b
 100         yield self.c
 101
 102     def __len__(self):
 103         return 3
 104
 105     def __getitem__(self, key):
 106         if key == 0:
 107             return self.a
 108         elif key == 1:
 109             return self.b
 110         elif key == 2:
 111             return self.c
 112
 113 def gen_corners(d1, d2):
 114     for c1 in d1.points:
 115         if c1 in d2.points:
 116             continue
 117         pass
 118         try:
 119             c2 = [p for p in d2.points if p in c1.l1.points][0]
 120             c3 = [p for p in d1.points if p in c2.l2.points][0]
 121             c4 = [p for p in d2.points if p in c3.l1.points][0]
 122         except IndexError:
 123             continue
 124             # there is not a corresponding intersection
 125             # TODO create an intersection?
 126         try:
 127             yield manual.lines(map(lambda p: p.to_tuple(), [c2, c1, c3, c4]))
 128         except (TypeError, ZeroDivisionError):
 129             pass
 130             # the square was too small to fit 17 lines inside
 131             # TODO define SquareTooSmallError or something
 132
 133 def dst(p, l):
 134     (x, y), (a, b, c) = p, ransac.points_to_line(*l)
 135     return abs(a * x + b * y + c) / sqrt(a*a+b*b)
 136
 137 def score(lines, points):
 138     score = 0
 139     for p in points:
 140         s = min(map(lambda l: dst(p, l), lines))
 141         s = min(s, 2)
 142         score += s
 143     return score
 144
 145
 146 def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
 147     logger("finding the grid")
 148     new_lines1 = map(lambda l: Line.from_ad(l, size), lines[0])
 149     new_lines2 = map(lambda l: Line.from_ad(l, size), lines[1])
 150     for l1 in new_lines1:
 151         for l2 in new_lines2:
 152             p = Point(intersection(l1, l2))
 153             p.l1 = l1
 154             p.l2 = l2
 155             l1.points.append(p)
 156             l2.points.append(p)
 157
 158     points = [l.points for l in new_lines1]
 159
 160     for trial in xrange(3):
 161         line1, cons = ransac.estimate(points, 2, 800, Diagonal_model)
 162         points2 = map(lambda l: [(p if not p in cons else None) for p in l], points)
 163         line2, cons2 = ransac.estimate(points2, 2, 800, Diagonal_model)
 164         center = intersection(line1, line2)
 165         data = sum(points, [])
 166         diag1 = Line(line1)
 167         diag1.points = ransac.filter_near(data, diag1, 2)
 168         diag2 = Line(line2)
 169         diag2.points = ransac.filter_near(data, diag2, 2)
 170
 171         if show_all:
 172             import matplotlib.pyplot as pyplot
 173             import Image
 174
 175             def plot_line_g((a, b, c), max_x):
 176                 find_y = lambda x: - (c + a * x) / b
 177                 pyplot.plot([0, max_x], [find_y(0), find_y(max_x)], color='b')
 178
 179             fig = pyplot.figure(figsize=(8, 6))
 180             plot_line_g(diag1, size[0])
 181             plot_line_g(diag2, size[0])
 182             pyplot.scatter(*zip(*sum(points, [])))
 183             pyplot.scatter([center[0]], [center[1]], color='r')
 184             pyplot.xlim(0, size[0])
 185             pyplot.ylim(0, size[1])
 186             pyplot.gca().invert_yaxis()
 187             fig.canvas.draw()
 188             size_f = fig.canvas.get_width_height()
 189             buff = fig.canvas.tostring_rgb()
 190             image_p = Image.fromstring('RGB', size_f, buff, 'raw')
 191             do_something(image_p, "finding diagonal")
 192
 193
 194         grids = list(gen_corners(diag1, diag2))
 195
 196         try:
 197             sc, grid = min(map(lambda g: (score(sum(g, []), data), g), grids))
 198             break
 199         except ValueError:
 200             pass
 201     else:
 202         raise GridFittingFailedError
 203
 204     grid_lines = [[l2ad(l, size) for l in grid[0]],
 205                   [l2ad(l, size) for l in grid[1]]]
 206     grid_lines[0].sort(key=lambda l: l[1])
 207     grid_lines[1].sort(key=lambda l: l[1])
 208     if grid_lines[0][0][0] > grid_lines[1][0][0]:
 209         grid_lines = grid_lines[1], grid_lines[0]
 210
 211     return grid, grid_lines
 212