Read calibration from CSV file

Assets/CalibrationRecorderBehavior.cs

@@ -29,7 +29,7 @@ namespace Assets
             taskQueue = new BlockingCollection<Action>();
             if (!File.Exists(fileName))
             {
-                File.WriteAllLines(fileName, new string[] { "WorldX,WorldY,WorldZ,UnityX,UnityY,UnityZ" });
+                File.WriteAllLines(fileName, new string[] { "Time,WorldX,WorldY,WorldZ,UnityX,UnityY,UnityZ" });
             }
         }
 
@@ -46,7 +46,7 @@ namespace Assets
                 var personPosition = person.WorldPosition;
                 var markerPosition = marker.transform.position;
 
-                taskQueue.Add(() => File.AppendAllLines(fileName, new string[] { FormattableString.Invariant($"{personPosition.x},{personPosition.y},{personPosition.z},{markerPosition.x},{markerPosition.y},{markerPosition.z}") }));
+                taskQueue.Add(() => File.AppendAllLines(fileName, new string[] { FormattableString.Invariant($"{DateTime.Now:yyyy-MM-dd-HH-mm-ss-ff},{personPosition.x},{personPosition.y},{personPosition.z},{markerPosition.x},{markerPosition.y},{markerPosition.z}") }));
                 Task.Run(() => taskQueue.Take().Invoke());
             }
         }

Assets/Scenes/Calibration.unity

@@ -479,7 +479,7 @@ PrefabInstance:
       objectReference: {fileID: 0}
     - target: {fileID: 114491592745282986, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
       propertyPath: inputType
-      value: 1
+      value: 0
       objectReference: {fileID: 0}
     - target: {fileID: 114491592745282986, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
       propertyPath: svoInputFileName

Assets/StreetLight/Scripts/PersonManager.cs

@@ -5,7 +5,12 @@ using System;
 using System.Collections;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using System.Globalization;
+using System.IO;
+using System.Linq;
 using System.Runtime.CompilerServices;
+using System.Text;
+using System.Threading;
 using UnityEngine;
 
 namespace Assets.StreetLight.Scripts
@@ -19,29 +24,26 @@ namespace Assets.StreetLight.Scripts
 
         void Start()
         {
-            var cylinderUL = GameObject.Find("CylinderUL");
-            var cylinderUR = GameObject.Find("CylinderUR");
-            var cylinderDR = GameObject.Find("CylinderDR");
-            var cylinderDL = GameObject.Find("CylinderDL");
-            var sphere = GameObject.Find("Sphere");
-            //// World to Unity Position
-            ////var calibrationVectors = new List<Tuple<Vector3, Vector3>> {
-            ////    new Tuple<Vector3, Vector3>( new Vector3(-0.5225359f, -2.012292f, 3.934204f), new Vector3(cylinderUL.transform.position.x, cylinderUL.transform.position.y, cylinderUL.transform.position.z) ),
+            var filePath = @"C:\Users\nick.steyer\SmartStreetLight\StreetLight\Calibration.csv";
 
-            ////    new Tuple<Vector3, Vector3>(new Vector3(2.538264f, -2.015675f, 2.318164f), new Vector3(cylinderUR.transform.position.x, cylinderUR.transform.position.y, cylinderUR.transform.position.z)),
-            ////    new Tuple<Vector3, Vector3>(new Vector3(-1.551626f, -1.86093f, 2.275376f), new Vector3(cylinderDR.transform.position.x, cylinderDR.transform.position.y, cylinderDR.transform.position.z)),
-            ////    new Tuple<Vector3, Vector3>(new Vector3(-1.095514f, -1.981828f, 2.849167f), new Vector3(cylinderDL.transform.position.x, cylinderDL.transform.position.y, cylinderDL.transform.position.z)),
-            ////    new Tuple<Vector3, Vector3>(new Vector3(0.6326299f, -1.908295f, 2.349819f), new Vector3(sphere.transform.position.x, sphere.transform.position.y, sphere.transform.position.z))
-            ////};
-            var calibrationVectors = new List<CalibrationPoint> {
-            new CalibrationPoint(new Vector3(-0.5225359f, -2.012292f, 3.934204f), new Vector3(cylinderUL.transform.position.x, cylinderUL.transform.position.y, cylinderUL.transform.position.z)),
-            new CalibrationPoint(new Vector3(2.538264f, -2.015675f, 2.318164f), new Vector3(cylinderUR.transform.position.x, cylinderUR.transform.position.y, cylinderUR.transform.position.z)),
-            new CalibrationPoint(new Vector3(-1.551626f, -1.86093f, 2.275376f), new Vector3(cylinderDR.transform.position.x, cylinderDR.transform.position.y, cylinderDR.transform.position.z)),
-            new CalibrationPoint(new Vector3(-1.095514f, -1.981828f, 2.849167f), new Vector3(cylinderDL.transform.position.x, cylinderDL.transform.position.y, cylinderDL.transform.position.z)),
-            new CalibrationPoint(new Vector3(0.6326299f, -1.908295f, 2.349819f), new Vector3(sphere.transform.position.x, sphere.transform.position.y, sphere.transform.position.z))
-        };
+            var calibrationPoints = new List<CalibrationPoint>();
 
-            PositionCalculator = new PositionCalculator(calibrationVectors);
+            var lines = File.ReadAllLines(filePath);
+            foreach (var line in lines.Skip(1))
+            {
+                var coordinates = line.Split(',').Skip(1).ToArray();
+                calibrationPoints.Add(new CalibrationPoint(
+                    new Vector3(
+                        float.Parse(coordinates[0], CultureInfo.InvariantCulture),
+                        float.Parse(coordinates[1], CultureInfo.InvariantCulture),
+                        float.Parse(coordinates[2], CultureInfo.InvariantCulture)),
+                    new Vector3(
+                        float.Parse(coordinates[3], CultureInfo.InvariantCulture),
+                        float.Parse(coordinates[4], CultureInfo.InvariantCulture),
+                        float.Parse(coordinates[5], CultureInfo.InvariantCulture))));
+            }
+
+            PositionCalculator = new PositionCalculator(calibrationPoints);
 
             Persons = new ObservableCollection<Person>();
             personDetector = new ZedPersonDetector(FindObjectOfType<ZEDManager>());

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -20,14 +20,41 @@ namespace Assets.StreetLight.Scripts
             CalculateHomographySimple();
         }
 
+        private void CalculateHomographySVD()
+        {
+            if (!(calibrationVectors?.Count >= 4))
+            {
+                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
+            }
+        }
+
+        private void CalculateHomographyRansac()
+        {
+            if (!(calibrationVectors?.Count >= 4))
+            {
+                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 void CalculateHomographySimple()
         {
-            if (calibrationVectors?.Count >= 4)
+            if (!(calibrationVectors?.Count >= 4))
             {
-                var cv = calibrationVectors;
+                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
+            }
+
+            var cv = calibrationVectors;
 
-                Matrix<double> matrixA = DenseMatrix.OfArray(new double[,]
-                {
+            Matrix<double> matrixA = DenseMatrix.OfArray(new double[,]
+            {
                 {cv[0].WorldPosition.x, cv[0].WorldPosition.z, 1, 0, 0, 0, -cv[0].UnityPosition.x*cv[0].WorldPosition.x, -cv[0].UnityPosition.x*cv[0].WorldPosition.z},
                 {0, 0, 0, cv[0].WorldPosition.x, cv[0].WorldPosition.z, 1, -cv[0].UnityPosition.z*cv[0].WorldPosition.x, -cv[0].UnityPosition.z*cv[0].WorldPosition.z},
                 {cv[1].WorldPosition.x, cv[1].WorldPosition.z, 1, 0, 0, 0, -cv[1].UnityPosition.x*cv[1].WorldPosition.x, -cv[1].UnityPosition.x*cv[1].WorldPosition.z},
@@ -36,10 +63,10 @@ namespace Assets.StreetLight.Scripts
                 {0, 0, 0, cv[2].WorldPosition.x, cv[2].WorldPosition.z, 1, -cv[2].UnityPosition.z*cv[2].WorldPosition.x, -cv[2].UnityPosition.z*cv[2].WorldPosition.z},
                 {cv[3].WorldPosition.x, cv[3].WorldPosition.z, 1, 0, 0, 0, -cv[3].UnityPosition.x*cv[3].WorldPosition.x, -cv[3].UnityPosition.x*cv[3].WorldPosition.z},
                 {0, 0, 0, cv[3].WorldPosition.x, cv[3].WorldPosition.z, 1, -cv[3].UnityPosition.z*cv[3].WorldPosition.x, -cv[3].UnityPosition.z*cv[3].WorldPosition.z}
-                });
+            });
 
-                Matrix<double> matrixB = DenseMatrix.OfArray(new double[,]
-                {
+            Matrix<double> matrixB = DenseMatrix.OfArray(new double[,]
+            {
                 {cv[0].UnityPosition.x},
                 {cv[0].UnityPosition.z},
                 {cv[1].UnityPosition.x},
@@ -48,22 +75,17 @@ namespace Assets.StreetLight.Scripts
                 {cv[2].UnityPosition.z},
                 {cv[3].UnityPosition.x},
                 {cv[3].UnityPosition.z}
-                });
+            });
 
-                var lambda = (matrixA.Transpose() * matrixA).Inverse() * matrixA.Transpose() * matrixB;
-                homography = DenseMatrix.OfArray(new double[,]
-                {
+            var lambda = (matrixA.Transpose() * matrixA).Inverse() * matrixA.Transpose() * matrixB;
+            homography = DenseMatrix.OfArray(new double[,]
+            {
                 { lambda[0,0], lambda[1,0], lambda[2,0]},
                 { lambda[3,0], lambda[4,0], lambda[5,0]},
                 { lambda[6,0], lambda[7,0], 1}
-                });
+            });
 
-                testCalibration();
-            }
-            else
-            {
-                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
-            }
+            testCalibration();
         }
 
         public Vector3 CalculateUnityPosition(Person person)

Calibration.csv

@@ -0,0 +1,6 @@
+WorldX,WorldY,WorldZ,UnityX,UnityY,UnityZ
+2022-09-27-16-53-24-95,-0.5225359,-2.012292,3.934204,-11.2,0,4.8
+2022-09-27-16-53-25-06,2.538264,-2.015675,2.318164,11.2,0,4.8
+2022-09-27-16-53-25-16,-1.551626,-1.86093,2.275376,10,0,-1.9
+2022-09-27-16-53-25-26,-1.095514,-1.981828,2.849167,-9.6,0,-1.5
+2022-09-27-16-53-25-36,0.6326299,-1.908295,2.349819,0,0,0