MSAC

[imago.git] / src / gridf3.py

diff --git a/src/gridf3.py b/src/gridf3.py
index 34e2315..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
@@ -103,9 +112,14 @@ 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]
+        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:
@@ -150,6 +164,29 @@ def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
     diag2 = Line(line2)
     diag2.points = ransac.filter_near(data, diag2, 2)
 
+    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))