MSAC

[imago.git] / src / gridf3.py

diff --git a/src/gridf3.py b/src/gridf3.py
index bdf3a82..0ea26d9 100644 (file)
--- a/src/gridf3.py
+++ b/src/gridf3.py
@@ -9,13 +9,10 @@ from geometry import l2ad
 
 # TODO comments, refactoring, move methods to appropriate modules
 
-def plot_line(line, c):
-    points = linef.line_from_angl_dist(line, (520, 390))
+def plot_line(line, c, size):
+    points = linef.line_from_angl_dist(line, size)
     pyplot.plot(*zip(*points), color=c)
 
-def plot_line_g((a, b, c), max_x):
-    find_y = lambda x: - (c + a * x) / b
-    pyplot.plot([0, max_x], [find_y(0), find_y(max_x)], color='b')
 
 class Diagonal_model:
     def __init__(self, data):
@@ -45,6 +42,18 @@ class Diagonal_model:
         else:
             return ransac.least_squares(sample)
 
+    def score(self, est, dist):
+        cons = []
+        score = 0
+        a, b, c = est
+        dst = lambda (x, y): abs(a * x + b * y + c) / sqrt(a*a+b*b)
+        for p in self.data:
+            d = dst(p)
+            if d <= dist:
+                cons.append(p)
+            score += min(d, dist)
+        return score, cons
+
 def intersection((a1, b1, c1), (a2, b2, c2)):
     delim = float(a1 * b2 - b1 * a2)
     x = (b1 * c2 - c1 * b2) / delim
@@ -69,6 +78,9 @@ class Point:
     def __len__(self):
         return 2
 
+    def to_tuple(self):
+        return (self.x, self.y)
+
 class Line:
     def __init__(self, (a, b, c)):
         self.a, self.b, self.c = (a, b, c)
@@ -100,10 +112,20 @@ def gen_corners(d1, d2):
         if c1 in d2.points:
             continue
         pass
-        c2 = [p for p in d2.points if p in c1.l1.points][0]
-        c3 = [p for p in d1.points if p in c2.l2.points][0]
-        c4 = [p for p in d2.points if p in c3.l1.points][0]
-        yield [c1, c2, c3, c4]
+        try:
+            c2 = [p for p in d2.points if p in c1.l1.points][0]
+            c3 = [p for p in d1.points if p in c2.l2.points][0]
+            c4 = [p for p in d2.points if p in c3.l1.points][0]
+        except IndexError:
+            continue
+            # there is not a corresponding intersection
+            # TODO create an intersection?
+        try:
+            yield manual.lines(map(lambda p: p.to_tuple(), [c2, c1, c3, c4]))
+        except TypeError:
+            pass
+            # the square was too small to fit 17 lines inside
+            # TODO define SquareTooSmallError or something
 
 def dst(p, l):
     (x, y), (a, b, c) = p, ransac.points_to_line(*l)
@@ -115,7 +137,6 @@ def score(lines, points):
         s = min(map(lambda l: dst(p, l), lines))
         s = min(s, 2)
         score += s
-    print score
     return score
 
 
@@ -143,12 +164,39 @@ def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
     diag2 = Line(line2)
     diag2.points = ransac.filter_near(data, diag2, 2)
 
-    grids = map(manual.lines, list(gen_corners(diag1, diag2)))
+    if show_all:
+        import matplotlib.pyplot as pyplot
+        import Image
+
+        def plot_line_g((a, b, c), max_x):
+            find_y = lambda x: - (c + a * x) / b
+            pyplot.plot([0, max_x], [find_y(0), find_y(max_x)], color='b')
+
+        fig = pyplot.figure(figsize=(8, 6))
+        plot_line_g(diag1, size[0])
+        plot_line_g(diag2, size[0])
+        pyplot.scatter(*zip(*sum(points, [])))
+        pyplot.scatter([center[0]], [center[1]], color='r')
+        pyplot.xlim(0, size[0])
+        pyplot.ylim(0, size[1])
+        pyplot.gca().invert_yaxis()
+        fig.canvas.draw()
+        size_f = fig.canvas.get_width_height()
+        buff = fig.canvas.tostring_rgb()
+        image_p = Image.fromstring('RGB', size_f, buff, 'raw')
+        do_something(image_p, "finding diagonal")
+
+
+    grids = list(gen_corners(diag1, diag2))
 
     sc, grid = min(map(lambda g: (score(sum(g, []), data), g), grids))
     
     grid_lines = [[l2ad(l, size) for l in grid[0]], 
                   [l2ad(l, size) for l in grid[1]]]
+    grid_lines[0].sort(key=lambda l: l[1])
+    grid_lines[1].sort(key=lambda l: l[1])
+    if grid_lines[0][0][0] > grid_lines[1][0][0]:
+        grid_lines = grid_lines[1], grid_lines[0]
 
     return grid, grid_lines