src/gridf3.py

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