Beginn debugging homography calculation, change conditioning

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -5,8 +5,11 @@ using MathNet.Numerics.LinearAlgebra.Double;
 using MathNet.Numerics.Statistics;
 using System;
 using System.Collections.Generic;
+using System.Globalization;
+using System.IO;
 using System.Linq;
 using System.Security.Cryptography;
+using System.Threading;
 using UnityEngine;
 using Random = System.Random;
 
@@ -24,6 +27,27 @@ namespace Assets.StreetLight.Scripts
             CalculateHomographyRansac();
         }
 
+        private List<HomographyCorrespondence> LoadSampleCorrespondences()
+        {
+            var p1 = File.ReadAllLines(@"C:\Users\nick.steyer\Desktop\p1.csv");
+            var p2 = File.ReadAllLines(@"C:\Users\nick.steyer\Desktop\p2.csv");
+
+            var correspondences = new List<HomographyCorrespondence>();
+
+            int length = p1.Length;
+
+            for (int i = 0; i < length; i++)
+            {
+                var p1Coordinates = p1[i].Split(',').Select(i => double.Parse(i, CultureInfo.InvariantCulture)).ToArray();
+                var p2Coordinates = p2[i].Split(',').Select(i => double.Parse(i, CultureInfo.InvariantCulture)).ToArray();
+                correspondences.Add(new HomographyCorrespondence(
+                    DenseVector.OfArray(p1Coordinates),
+                    DenseVector.OfArray(p2Coordinates)));
+            }
+
+            return correspondences;
+        }
+
         /// <summary>
         /// Uses RANSAC and an SVD to calculate the homography from many calibration points.
         /// </summary>
@@ -39,6 +63,8 @@ namespace Assets.StreetLight.Scripts
             var correspondences = calibrationVectors.Select(v =>
             new HomographyCorrespondence(DenseVector.OfArray(new double[] { v.WorldPosition.x, v.WorldPosition.z }), DenseVector.OfArray(new double[] { v.UnityPosition.x, v.UnityPosition.z }))).ToList();
 
+            correspondences = LoadSampleCorrespondences();
+
             var iterations = RansacIterations(0.35f, 4, 0.99f);
             var threshold = 5.0f;
 
@@ -49,7 +75,9 @@ namespace Assets.StreetLight.Scripts
             var random = new Random();
             for (int i = 0; i < iterations; i++)
             {
-                var sample = correspondences.OrderBy(_ => random.Next()).Take(4).ToArray();
+                //var sample = correspondences.OrderBy(_ => random.Next()).Take(4).ToArray();
+                var indices = new[] { 96, 93, 63, 118 };
+                var sample = correspondences.Where((_, index) => indices.Contains(index)).ToArray();
 
                 var (conditionedCorrespondences, T1, T2) = ConditionPoints(sample);
 
@@ -99,6 +127,7 @@ namespace Assets.StreetLight.Scripts
         private IList<double> ComputeHomographyDistances(Matrix<double> currentH, IEnumerable<HomographyCorrespondence> correspondences)
         {
             var distances = new List<double>();
+            currentH.MapInplace(q => Math.Round(q, 15));
             var inverseH = currentH.Inverse();
 
             foreach (var correspondence in correspondences)
@@ -163,26 +192,28 @@ namespace Assets.StreetLight.Scripts
         private (ICollection<(Vector<double>, Vector<double>)>, Matrix<double>, Matrix<double>) ConditionPoints(IEnumerable<HomographyCorrespondence> correspondences)
         {
             var worldPoints = correspondences.Select(i => i.WorldPosition);
-            var s = worldPoints.Select(i => i.Count).Max(i => i) / 2;
+            var sx = worldPoints.Select(i => i[0]).Max(i => i) / 2;
+            var sy = worldPoints.Select(i => i[1]).Max(i => i) / 2;
             var meanX = worldPoints.Select(i => i[0]).Mean();
             var meanY = worldPoints.Select(i => i[1]).Mean();
 
             var T = DenseMatrix.OfArray(new double[,]
             {
-                {1 / s, 0, - meanX / s},
-                {0, 1 / s, - meanY / s},
+                {1 / sx, 0, - meanX / sx},
+                {0, 1 / sy, - meanY / sy},
                 {0, 0, 1}
             });
 
             var unityPoints = correspondences.Select(i => i.UnityPosition);
-            var s_prime = unityPoints.Select(i => i.Count).Max(i => i) / 2;
+            var sx_prime = unityPoints.Select(i => i[0]).Max(i => i) / 2;
+            var sy_prime = unityPoints.Select(i => i[1]).Max(i => i) / 2;
             var meanX_prime = unityPoints.Select(i => i[0]).Mean();
             var meanY_prime = unityPoints.Select(i => i[1]).Mean();
 
             var T_prime = DenseMatrix.OfArray(new double[,]
             {
-                {1 / s_prime, 0, - meanX_prime / s_prime},
-                {0, 1 / s_prime, - meanY_prime / s_prime},
+                {1 / sx_prime, 0, - meanX_prime / sx_prime},
+                {0, 1 / sy_prime, - meanY_prime / sy_prime},
                 {0, 0, 1}
             });