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):
  13     points = linef.line_from_angl_dist(line, (520, 390))
  14     pyplot.plot(*zip(*points), color=c)
  15
  16 def plot_line_g((a, b, c), max_x):
  17     find_y = lambda x: - (c + a * x) / b
  18     pyplot.plot([0, max_x], [find_y(0), find_y(max_x)], color='b')
  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 intersection((a1, b1, c1), (a2, b2, c2)):
  49     delim = float(a1 * b2 - b1 * a2)
  50     x = (b1 * c2 - c1 * b2) / delim
  51     y = (c1 * a2 - a1 * c2) / delim
  52     return x, y
  53
  54 class Point:
  55     def __init__(self, (x, y)):
  56         self.x = x
  57         self.y = y
  58
  59     def __getitem__(self, key):
  60         if key == 0:
  61             return self.x
  62         elif key == 1:
  63             return self.y
  64
  65     def __iter__(self):
  66         yield self.x
  67         yield self.y
  68
  69     def __len__(self):
  70         return 2
  71
  72     def to_tuple(self):
  73         return (self.x, self.y)
  74
  75 class Line:
  76     def __init__(self, (a, b, c)):
  77         self.a, self.b, self.c = (a, b, c)
  78         self.points = []
  79
  80     @classmethod
  81     def from_ad(cls, (a, d), size):
  82         p = linef.line_from_angl_dist((a, d), size)
  83         return cls(ransac.points_to_line(*p))
  84
  85     def __iter__(self):
  86         yield self.a
  87         yield self.b
  88         yield self.c
  89
  90     def __len__(self):
  91         return 3
  92
  93     def __getitem__(self, key):
  94         if key == 0:
  95             return self.a
  96         elif key == 1:
  97             return self.b
  98         elif key == 2:
  99             return self.c
 100
 101 def gen_corners(d1, d2):
 102     for c1 in d1.points:
 103         if c1 in d2.points:
 104             continue
 105         pass
 106         c2 = [p for p in d2.points if p in c1.l1.points][0]
 107         c3 = [p for p in d1.points if p in c2.l2.points][0]
 108         c4 = [p for p in d2.points if p in c3.l1.points][0]
 109         try:
 110             yield manual.lines(map(lambda p: p.to_tuple(), [c2, c1, c3, c4]))
 111         except TypeError:
 112             pass
 113             # the square was too small to fit 17 lines inside
 114             # TODO define SquareTooSmallError or something
 115
 116 def dst(p, l):
 117     (x, y), (a, b, c) = p, ransac.points_to_line(*l)
 118     return abs(a * x + b * y + c) / sqrt(a*a+b*b)
 119
 120 def score(lines, points):
 121     score = 0
 122     for p in points:
 123         s = min(map(lambda l: dst(p, l), lines))
 124         s = min(s, 2)
 125         score += s
 126     return score
 127
 128
 129 def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
 130     logger("finding the grid")
 131     new_lines1 = map(lambda l: Line.from_ad(l, size), lines[0])
 132     new_lines2 = map(lambda l: Line.from_ad(l, size), lines[1])
 133     for l1 in new_lines1:
 134         for l2 in new_lines2:
 135             p = Point(intersection(l1, l2))
 136             p.l1 = l1
 137             p.l2 = l2
 138             l1.points.append(p)
 139             l2.points.append(p)
 140
 141     points = [l.points for l in new_lines1]
 142
 143     line1, cons = ransac.estimate(points, 2, 800, Diagonal_model)
 144     points2 = map(lambda l: [(p if not p in cons else None) for p in l], points)
 145     line2, cons2 = ransac.estimate(points2, 2, 800, Diagonal_model)
 146     center = intersection(line1, line2)
 147     data = sum(points, [])
 148     diag1 = Line(line1)
 149     diag1.points = ransac.filter_near(data, diag1, 2)
 150     diag2 = Line(line2)
 151     diag2.points = ransac.filter_near(data, diag2, 2)
 152
 153     grids = list(gen_corners(diag1, diag2))
 154
 155     sc, grid = min(map(lambda g: (score(sum(g, []), data), g), grids))
 156
 157     grid_lines = [[l2ad(l, size) for l in grid[0]],
 158                   [l2ad(l, size) for l in grid[1]]]
 159     grid_lines[0].sort(key=lambda l: l[1])
 160     grid_lines[1].sort(key=lambda l: l[1])
 161     if grid_lines[0][0][0] > grid_lines[1][0][0]:
 162         grid_lines = grid_lines[1], grid_lines[0]
 163
 164     return grid, grid_lines
 165
 166 def test():
 167     import pickle
 168     import matplotlib.pyplot as pyplot
 169
 170     lines = pickle.load(open('lines.pickle'))
 171
 172     size = (520, 390)
 173     new_lines1 = map(lambda l: Line.from_ad(l, size), lines[0])
 174     new_lines2 = map(lambda l: Line.from_ad(l, size), lines[1])
 175     for l1 in new_lines1:
 176         for l2 in new_lines2:
 177             p = Point(intersection(l1, l2))
 178             p.l1 = l1
 179             p.l2 = l2
 180             l1.points.append(p)
 181             l2.points.append(p)
 182
 183     points = [l.points for l in new_lines1]
 184
 185     line1, cons = ransac.estimate(points, 2, 800, Diagonal_model)
 186     points2 = map(lambda l: [(p if not p in cons else None) for p in l], points)
 187     line2, cons2 = ransac.estimate(points2, 2, 800, Diagonal_model)
 188     center = intersection(line1, line2)
 189     data = sum(points, [])
 190     diag1 = Line(line1)
 191     diag1.points = ransac.filter_near(data, diag1, 2)
 192     diag2 = Line(line2)
 193     diag2.points = ransac.filter_near(data, diag2, 2)
 194
 195     plot_line_g(diag1, 520)
 196     plot_line_g(diag2, 520)
 197     pyplot.scatter(*zip(*sum(points, [])))
 198     pyplot.scatter([center[0]], [center[1]], color='r')
 199     pyplot.xlim(0, 520)
 200     pyplot.ylim(0, 390)
 201     pyplot.show()
 202
 203     grids = map(manual.lines, list(gen_corners(diag1, diag2)))
 204     plot_grid = lambda g: map(lambda l: pyplot.plot(*zip(*l), color='g'), sum(g, []))
 205     map(plot_grid, grids)
 206     pyplot.show()
 207
 208     sc, grid = min(map(lambda g: (score(sum(g, []), data), g), grids))
 209
 210     map(lambda l: pyplot.plot(*zip(*l), color='g'), sum(grid, []))
 211     pyplot.scatter(*zip(*sum(points, [])))
 212     pyplot.xlim(0, 520)
 213     pyplot.ylim(0, 390)
 214     pyplot.show()