gridf3.py

   1 import pickle
   2 import matplotlib.pyplot as pyplot
   3 import random
   4
   5 from src.intrsc import intersections_from_angl_dist
   6 import src.linef as linef
   7 import src.ransac as ransac
   8
   9 def plot_line(line, c):
  10     points = linef.line_from_angl_dist(line, (520, 390))
  11     pyplot.plot(*zip(*points), color=c)
  12
  13 def plot_line_g((a, b, c), max_x):
  14     find_y = lambda x: - (c + a * x) / b
  15     pyplot.plot([0, max_x], [find_y(0), find_y(max_x)], color='b')
  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 intersection((a1, b1, c1), (a2, b2, c2)):
  46     delim = float(a1 * b2 - b1 * a2)
  47     x = (b1 * c2 - c1 * b2) / delim
  48     y = (c1 * a2 - a1 * c2) / delim
  49     return x, y
  50
  51 class Point:
  52     def __init__(self, (x, y)):
  53         self.x = x
  54         self.y = y
  55
  56     def __getitem__(self, key):
  57         if key == 0:
  58             return self.x
  59         elif key == 1:
  60             return self.y
  61
  62     def __iter__(self):
  63         yield self.x
  64         yield self.y
  65
  66     def __len__(self):
  67         return 2
  68
  69 class Line:
  70     def __init__(self, (a, b, c)):
  71         self.a, self.b, self.c = (a, b, c)
  72         self.points = []
  73
  74     @classmethod
  75     def from_ad(cls, (a, d), size):
  76         p = linef.line_from_angl_dist((a, d), size)
  77         return cls(ransac.points_to_line(*p))
  78
  79     def __iter__(self):
  80         yield self.a
  81         yield self.b
  82         yield self.c
  83
  84     def __len__(self):
  85         return 3
  86
  87 lines = pickle.load(open('lines.pickle'))
  88
  89 size = (520, 390)
  90 new_lines1 = map(lambda l: Line.from_ad(l, size), lines[0])
  91 new_lines2 = map(lambda l: Line.from_ad(l, size), lines[1])
  92 for l1 in new_lines1:
  93     for l2 in new_lines2:
  94         p = Point(intersection(l1, l2))
  95         p.l1 = l1
  96         p.l2 = l2
  97         l1.points.append(p)
  98         l2.points.append(p)
  99
 100 points = [l.points for l in new_lines1]
 101
 102 def gen_corners(d1, d2):
 103     for c1 in d1.points:
 104         if c1 in d2.points:
 105             continue
 106         pass
 107     # TODO TODO TODO
 108
 109
 110 while True:
 111     line1, cons = ransac.estimate(points, 2, 800, Diagonal_model)
 112     points2 = map(lambda l: [(p if not p in cons else None) for p in l], points)
 113     line2, cons2 = ransac.estimate(points2, 2, 800, Diagonal_model)
 114     center = intersection(line1, line2)
 115     data = sum(points, [])
 116     diag1 = Line(line1)
 117     diag1.points = ransac.filter_near(data, diag1, 2)
 118     diag2 = Line(line2)
 119     diag2.points = ransac.filter_near(data, diag2, 2)
 120
 121
 122     plot_line_g(diag1, 520)
 123     plot_line_g(diag2, 520)
 124     pyplot.scatter(*zip(*sum(points, [])))
 125     pyplot.scatter([center[0]], [center[1]], color='r')
 126     pyplot.xlim(0, 520)
 127     pyplot.ylim(0, 390)
 128     pyplot.show()
 129
 130 #map(lambda l: plot_line(l, 'g'), sum(lines, []))
 131
 132 pyplot.show()
 133
 134