X-Git-Url: http://git.tomasm.cz/imago.git/blobdiff_plain/6043532d944c572b124e0363c416929d74ea8f23..0c2752278f9a18ba11cd2e92d7a7e573eda2e730:/src/gridf3.py

diff --git a/src/gridf3.py b/src/gridf3.py
index 92f07c4..ac7fa2a 100644
--- a/src/gridf3.py
+++ b/src/gridf3.py
@@ -9,6 +9,9 @@ from geometry import l2ad
 
 # TODO comments, refactoring, move methods to appropriate modules
 
+class GridFittingFailedError(Exception):
+    pass
+
 def plot_line(line, c, size):
     points = linef.line_from_angl_dist(line, size)
     pyplot.plot(*zip(*points), color=c)
@@ -42,6 +45,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
@@ -105,7 +120,7 @@ def gen_corners(d1, d2):
             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:
-            pass
+            continue
             # there is not a corresponding intersection
             # TODO create an intersection?
         try:
@@ -142,42 +157,50 @@ def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
 
     points = [l.points for l in new_lines1]
 
-    line1, cons = ransac.estimate(points, 2, 800, Diagonal_model)
-    points2 = map(lambda l: [(p if not p in cons else None) for p in l], points)
-    line2, cons2 = ransac.estimate(points2, 2, 800, Diagonal_model)
-    center = intersection(line1, line2)
-    data = sum(points, [])
-    diag1 = Line(line1)
-    diag1.points = ransac.filter_near(data, diag1, 2)
-    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])
-        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))
-    
+    for trial in xrange(3):
+        line1, cons = ransac.estimate(points, 2, 800, Diagonal_model)
+        points2 = map(lambda l: [(p if not p in cons else None) for p in l], points)
+        line2, cons2 = ransac.estimate(points2, 2, 800, Diagonal_model)
+        center = intersection(line1, line2)
+        data = sum(points, [])
+        diag1 = Line(line1)
+        diag1.points = ransac.filter_near(data, diag1, 2)
+        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))
+        
+        try:
+            sc, grid = min(map(lambda g: (score(sum(g, []), data), g), grids))
+            break
+        except ValueError:
+            pass
+    else:
+        raise GridFittingFailedError
+        
     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])