WIP: first version of ransac homography

Assembly-CSharp.csproj

@@ -86,6 +86,7 @@
     <Compile Include="Assets\ZED\Examples\MultiCam\Scripts\CopyToSurface.cs" />
     <Compile Include="Assets\StreetLight\PersonVisualizer.cs" />
     <Compile Include="Assets\ZED\Examples\OpenCV ArUco Detection\Scripts\Core\ZEDToOpenCVRetriever.cs" />
+    <Compile Include="Assets\StreetLight\Poco\HomographyCorrespondence.cs" />
     <Compile Include="Assets\Bumper.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\Utilities\GUIMessage.cs" />
     <Compile Include="Assets\Logging\DetectionFrameLogger.cs" />

Assets/StreamingAssets/Configuration.json

@@ -1,4 +1,4 @@
 {
-	"OutputCalibrationFilesDirectory" : "C:\\Users\\nick.steyer\\SmartStreetLight\\Calibration",
-	"InputCalibrationFilePath" : "C:\\Users\\nick.steyer\\SmartStreetLight\\StreetLight\\Calibration.csv"
+	"OutputCalibrationFilesDirectory": "C:\\Users\\nick.steyer\\SmartStreetLight\\Calibration",
+	"InputCalibrationFilePath": "C:\\Users\\nick.steyer\\SmartStreetLight\\StreetLight\\Calibration2022-10-01_18-14-47.csv"
 }

Assets/StreetLight/Poco/HomographyCorrespondence.cs

@@ -0,0 +1,23 @@
+using MathNet.Numerics.LinearAlgebra;
+using System.Diagnostics;
+
+namespace Assets.StreetLight.Poco
+{
+    [DebuggerDisplay("{" + nameof(GetDebuggerDisplay) + "(),nq}")]
+    public class HomographyCorrespondence
+    {
+        public Vector<double> WorldPosition { get; }
+        public Vector<double> UnityPosition { get; }
+
+        public HomographyCorrespondence(Vector<double> worldPosition, Vector<double> unityPosition)
+        {
+            WorldPosition = worldPosition;
+            UnityPosition = unityPosition;
+        }
+
+        private string GetDebuggerDisplay()
+        {
+            return $"World: {WorldPosition}, Unity: {UnityPosition}";
+        }
+    }
+}

Assets/StreetLight/Poco/HomographyCorrespondence.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e9a79cc4f86779d46bd1eb5404337682
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -2,9 +2,11 @@
 using Assets.StreetLight.Poco;
 using MathNet.Numerics.LinearAlgebra;
 using MathNet.Numerics.LinearAlgebra.Double;
+using MathNet.Numerics.Statistics;
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using System.Security.Cryptography;
 using UnityEngine;
 using Random = System.Random;
 
@@ -22,6 +24,11 @@ namespace Assets.StreetLight.Scripts
             CalculateHomographyRansac();
         }
 
+        /// <summary>
+        /// Uses RANSAC and an SVD to calculate the homography from many calibration points.
+        /// </summary>
+        /// <exception cref="InvalidOperationException"></exception>
+        /// <remarks>Heavily based on the formulas and algorithms discussed in Computer Vision I by Stefan Roth</remarks>
         private void CalculateHomographyRansac()
         {
             if (!(calibrationVectors?.Count >= 4))
@@ -29,13 +36,174 @@ namespace Assets.StreetLight.Scripts
                 throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
             }
 
+            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();
+
             var iterations = RansacIterations(0.35f, 4, 0.99f);
+            var threshold = 5.0f;
+
+            var maxInliers = 0;
+            Matrix<double> H = Matrix<double>.Build.Random(0, 0);
+            List<HomographyCorrespondence> inliers = new List<HomographyCorrespondence>();
 
             var random = new Random();
             for (int i = 0; i < iterations; i++)
             {
-                var sample = calibrationVectors.OrderBy(_ => random.Next()).Take(4);
+                var sample = correspondences.OrderBy(_ => random.Next()).Take(4).ToArray();
+
+                var (conditionedCorrespondences, T1, T2) = ConditionPoints(sample);
+
+                var (currentH, HC) = ComputeHomography(conditionedCorrespondences, T1, T2);
+
+                var homographyDistances = ComputeHomographyDistances(currentH, correspondences);
+
+                var currentInliers = FindInliers(correspondences, homographyDistances, threshold);
+                var numberOfCurrentInliers = currentInliers.Count;
+
+                if (currentInliers.Count > maxInliers)
+                {
+                    H = currentH;
+                    maxInliers = currentInliers.Count;
+                    inliers = currentInliers;
+                }
+            }
+
+            var recomputedHomography = RecomputeHomography(inliers);
+
+            homography = recomputedHomography;
+        }
+
+        private Matrix<double> RecomputeHomography(List<HomographyCorrespondence> inliers)
+        {
+            var (conditionedPoints, T1, T2) = ConditionPoints(inliers);
+            var (H, HC) = ComputeHomography(conditionedPoints, T1, T2);
+            return H;
+        }
+
+        private List<HomographyCorrespondence> FindInliers(IList<HomographyCorrespondence> correspondences, IList<double> homographyDistances, float threshold)
+        {
+            var inliers = new List<HomographyCorrespondence>();
+
+            for (int i = 0; i < correspondences.Count; i++)
+            {
+                var distance = homographyDistances[i];
+                if (distance < threshold)
+                {
+                    inliers.Add(correspondences[i]);
+                }
+            }
+
+            return inliers;
+        }
+
+        private IList<double> ComputeHomographyDistances(Matrix<double> currentH, IEnumerable<HomographyCorrespondence> correspondences)
+        {
+            var distances = new List<double>();
+            var inverseH = currentH.Inverse();
+
+            foreach (var correspondence in correspondences)
+            {
+                var x1 = correspondence.WorldPosition;
+                var x2 = correspondence.UnityPosition;
+                var x1Transformed = TransformPoint(x1, homography);
+                var x2Transformed = TransformPoint(x2, inverseH);
+                distances.Add(Math.Pow((x1Transformed - x2).L1Norm(), 2) + Math.Pow((x1 - x2Transformed).L1Norm(), 2));
+            }
+
+            return distances;
+        }
+
+        private Vector<double> TransformPoint(Vector<double> point, Matrix<double> homography)
+        {
+            if (point.Count != 3 || homography.RowCount != 3 || homography.ColumnCount != 3)
+            {
+                throw new ArgumentException();
             }
+
+            var homogeneousPoint = DenseVector.OfArray(new double[] { point[0], point[1], 1 });
+            var transformedHomogeneousPoint = homography * homogeneousPoint;
+            var normalizedTransformedHomogeneousPoint = transformedHomogeneousPoint / transformedHomogeneousPoint[2];
+            var transformedPoint = DenseVector.OfArray(new double[] { normalizedTransformedHomogeneousPoint[0], normalizedTransformedHomogeneousPoint[1] });
+            return transformedPoint;
+        }
+
+        private (Matrix<double>, Matrix<double>) ComputeHomography(ICollection<(Vector<double>, Vector<double>)> conditionedCorrespondences, Matrix<double> t1, Matrix<double> t2)
+        {
+            var numberOfCorrespondences = conditionedCorrespondences.Count;
+
+            var A = new List<double[]>();
+
+            foreach (var correspondence in conditionedCorrespondences)
+            {
+                var point1 = correspondence.Item1 / correspondence.Item1[2];
+                var point2 = correspondence.Item2 / correspondence.Item2[2];
+                A.Add(new double[] { 0, 0, 0, point1[0], point1[1], point1[2], -point2[1] * point1[0], -point2[1] * point1[1], -point2[1] * point1[2] });
+                A.Add(new double[] { -point1[0], -point1[1], -point1[2], 0, 0, 0, point2[0] * point1[0], point2[0] * point1[1], point2[0] * point1[2] });
+            }
+
+            var matrix = DenseMatrix.OfRowArrays(A);
+            var svd = matrix.Svd(true);
+            var h = svd.VT.EnumerateRows().Last();
+            var HC = DenseMatrix.OfArray(new[,]
+            {
+                { h[0], h[1], h[2] },
+                { h[3], h[4], h[5] },
+                { h[6], h[7], h[8] }
+            });
+
+            var normalizedHC = HC / HC[2, 2];
+
+            var H = t2.Inverse() * (normalizedHC * t1);
+
+            var normalizedH = H / H[2, 2];
+
+            return (normalizedH, normalizedHC);
+        }
+
+        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 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},
+                {0, 0, 1}
+            });
+
+            var unityPoints = correspondences.Select(i => i.UnityPosition);
+            var s_prime = unityPoints.Select(i => i.Count).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},
+                {0, 0, 1}
+            });
+
+            var result = new List<(Vector<double>, Vector<double>)>();
+            foreach (var correspondence in correspondences)
+            {
+                var homogeneousPointWorld = DenseVector.OfArray(new double[] { correspondence.WorldPosition[0], correspondence.WorldPosition[1], 1 });
+                var homogeneousTransformedPointWorld = T * homogeneousPointWorld;
+
+                var homogeneousPointUnity = DenseVector.OfArray(new double[] { correspondence.UnityPosition[0], correspondence.UnityPosition[1], 1 });
+                var homogeneousTransformedPointUnity = T_prime * homogeneousPointUnity;
+
+                //if (new[] { homogeneousTransformedPointUnity[1], homogeneousTransformedPointUnity[0], homogeneousTransformedPointWorld[1], homogeneousTransformedPointWorld[0] }.Any(i => i < -1 || i > 1))
+                //{
+
+                //}
+
+                result.Add((homogeneousTransformedPointWorld, homogeneousTransformedPointUnity));
+            }
+
+            return (result, T, T_prime);
         }
 
         private int RansacIterations(float inlierProbability, int samplesPerIteration, float successProbability)