WIP: RANSAC

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -17,31 +17,27 @@ namespace Assets.StreetLight.Scripts
         {
             this.calibrationVectors = calibrationVectors;
 
-            CalculateHomographySimple();
+            CalculateHomographyRansac();
         }
 
-        private void CalculateHomographySVD()
+        private void CalculateHomographyRansac()
         {
             if (!(calibrationVectors?.Count >= 4))
             {
                 throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
             }
-        }
 
-        private void CalculateHomographyRansac()
-        {
-            if (!(calibrationVectors?.Count >= 4))
+            var iterations = RansacIterations(0.35f, 4, 0.99f);
+
+            for (int i = 0; i < iterations; i++)
             {
-                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
-            }
 
-            // TODO: RANSAC with conditioning
-            var inlierThreshold = 5.0;
-            var inlierProbability = 0.25;
-            var samplesPerInteration = 4;
-            var confidence = 0.99;
+            }
+        }
 
-            throw new NotImplementedException();
+        private int RansacIterations(float inlierProbability, int samplesPerIteration, float successProbability)
+        {
+            return (int)Math.Ceiling(Math.Log(1 - successProbability, 1 - Math.Pow(inlierProbability, samplesPerIteration)));
         }
 
         private void CalculateHomographySimple()