X-Git-Url: http://git.tomasm.cz/imago.git/blobdiff_plain/ed2f10b265485040197402bbd5dbe0dd6058cb11..be8e99bf1ebe4006580e5c4e775afcc8d7c003b5:/src/gridf3.py

diff --git a/src/gridf3.py b/src/gridf3.py
index 693b8ee..e1b8c9f 100644
--- a/src/gridf3.py
+++ b/src/gridf3.py
@@ -73,6 +73,8 @@ class Diagonal_model:
 
 def intersection((a1, b1, c1), (a2, b2, c2)):
     delim = float(a1 * b2 - b1 * a2)
+    if delim == 0:
+        return None
     x = (b1 * c2 - c1 * b2) / delim
     y = (c1 * a2 - a1 * c2) / delim
     return x, y
@@ -139,7 +141,7 @@ def gen_corners(d1, d2):
             # TODO create an intersection?
         try:
             yield manual.lines(map(lambda p: p.to_tuple(), [c2, c1, c3, c4]))
-        except (TypeError, ZeroDivisionError):
+        except (TypeError):
             pass
             # the square was too small to fit 17 lines inside
             # TODO define SquareTooSmallError or something
@@ -170,49 +172,66 @@ def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
             l2.points.append(p)
 
     points = [l.points for l in new_lines1]
-
-    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, [])
+    def dst_p(x, y):
+        x = x - size[0] / 2
+        y = y - size[1] / 2
+        return sqrt(x * x + y * y)
+
+    model = Diagonal_model(points)
+    diag_lines = ransac.ransac_multi(6, points, 2, 800, model=model)
+    diag_lines = [l[0] for l in diag_lines]
+    centers = []
+    cen_lin = []
+    for i in xrange(len(diag_lines)):
+        line1 = diag_lines[i]
+        for line2 in diag_lines[i+1:]:
+            c = intersection(line1, line2)
+            if c and dst_p(*c) < min(size) / 2:
+                cen_lin.append((line1, line2, c))
+                centers.append(c)
+
+    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))
+        for l in diag_lines:
+            plot_line_g(l, size[0])
+        pyplot.scatter(*zip(*sum(points, [])))
+        pyplot.scatter(*zip(*centers), 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 diagonals")
+
+    data = sum(points, [])
+    # TODO what if lines are missing?
+    sc = float("inf")
+    grid = None
+    for (line1, line2, c) in cen_lin:
         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
+            new_sc, new_grid = min(map(lambda g: (score(sum(g, []), data), g), grids))
+            if new_sc < sc:
+                sc, grid = new_sc, new_grid
         except ValueError:
             pass
-    else:
+    if not grid:
         raise GridFittingFailedError
         
     grid_lines = [[l2ad(l, size) for l in grid[0]],