From 33c8ac60f68f6ab030d894b208774505cab20ea8 Mon Sep 17 00:00:00 2001
From: Tomas Musil <tomik.musil@gmail.com>
Date: Sat, 5 Jul 2014 02:04:34 +0200
Subject: [PATCH 1/1] MSAC

---
 src/gridf3.py | 13 +++++++++++++
 src/ransac.py | 30 ++++++++++++++++++++++--------
 2 files changed, 35 insertions(+), 8 deletions(-)

diff --git a/src/gridf3.py b/src/gridf3.py
index 4f2a1e3..0ea26d9 100644
--- a/src/gridf3.py
+++ b/src/gridf3.py
@@ -42,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
@@ -167,6 +179,7 @@ def find(lines, size, l1, l2, bounds, hough, show_all, do_something, logger):
         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()
diff --git a/src/ransac.py b/src/ransac.py
index 4f0d9a0..5d5d218 100644
--- a/src/ransac.py
+++ b/src/ransac.py
@@ -7,7 +7,6 @@ import numpy as NP
 # TODO comments
 # TODO threshold
 
-
 def points_to_line((x1, y1), (x2, y2)):
     return (y2 - y1, x1 - x2, x2 * y1 - x1 * y2)
 
@@ -37,26 +36,41 @@ class Linear_model:
     def initial(self):
         return random.sample(self.data, 2)
 
+    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 iterate(model, distance):
-    consensus = 0
+    score = float("inf")
     consensual = model.initial()
-    while (len(consensual) > consensus):
-        consensus = len(consensual)
+    estimate = model.get(consensual)
+    new_score, consensual = model.score(estimate, distance)
+    while (new_score < score):
+        score = new_score
         try:
             estimate = model.get(consensual)
         except NP.linalg.LinAlgError:
             pass
-        consensual = filter_near(model.data, estimate, distance)
-    return consensus, estimate, consensual
+        estimate = model.get(consensual)
+        new_score, consensual = model.score(estimate, distance)
+    return score, estimate, consensual
         
 def estimate(data, dist, k, modelClass=Linear_model):
     model = modelClass(data)
-    best = 0
+    best = float("inf")
     estimate = None
     consensual = None
     for i in xrange(0, k):
         new, new_estimate, new_consensual = iterate(model, dist)
-        if new > best:
+        if new < best:
             best = new
             estimate = new_estimate
             consensual = new_consensual
-- 
2.4.2