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