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