Make calibration recordings work

Assets/CalibrationMarkerBehavior.cs

@@ -1,15 +1,7 @@
 using Assets.StreetLight.Scripts;
 using System;
-using System.Collections;
 using System.Collections.Generic;
-using System.Collections.Specialized;
-using System.Drawing;
-using System.Linq;
-using System.Net;
-using TMPro;
-using Unity.VisualScripting;
 using UnityEngine;
-using UnityEngine.Timeline;
 
 public class CalibrationMarkerBehavior : MonoBehaviour
 {
@@ -73,6 +65,7 @@ public class CalibrationMarkerBehavior : MonoBehaviour
     {
         var marker = GameObject.Find("CalibrationMarker");
 
+        // TODO: make variable
         const float speed = 0.5f;
 
         if (Vector3.Distance(marker.transform.position, currentTarget) >= 0.05f)
@@ -90,7 +83,7 @@ public class CalibrationMarkerBehavior : MonoBehaviour
             PathFinished?.Invoke(this, EventArgs.Empty);
         }
 
-        UnityEngine.Color color = UnityEngine.Color.Lerp(UnityEngine.Color.green, UnityEngine.Color.red, Time.deltaTime / 0.2f);
+        Color color = Color.Lerp(Color.green, Color.red, Time.deltaTime / 0.2f);
         Debug.Log(string.Format("<color=#{0:X2}{1:X2}{2:X2}>{3}</color>", (byte)(color.r * 255f), (byte)(color.g * 255f), (byte)(color.b * 255f), $"deltaTime: {Time.deltaTime}"));
     }
 }

Assets/CalibrationRecorderBehavior.cs

@@ -1,14 +1,10 @@
-using Assets.StreetLight;
 using Assets.StreetLight.Scripts;
-using Newtonsoft.Json;
 using System;
-using System.Collections;
 using System.Collections.Concurrent;
-using System.Collections.Generic;
+using System.Globalization;
 using System.IO;
 using System.Linq;
 using System.Threading.Tasks;
-using Unity.VisualScripting;
 using UnityEngine;
 
 namespace Assets
@@ -30,20 +26,43 @@ namespace Assets
         void Start()
         {
             taskQueue = new BlockingCollection<Action>();
-            calibrationFileName = Path.Combine(Configuration.Instance.OutputCalibrationFilesDirectory, $"{DateTime.Now:yyyy-MM-dd_HH-mm-ss}.csv");
-            if (!File.Exists(calibrationFileName))
-            {
-                File.WriteAllLines(calibrationFileName, new string[] { "Time,WorldX,WorldY,WorldZ,UnityX,UnityY,UnityZ" });
-            }
+
+            calibrationFileName = Path.Combine(@".\CalibrationRecordings", $"CalibrationRecording{DateTime.Now:yyyy-MM-dd_HH-mm-ss}.csv");
+
+            InitializeCalibrationFile();
 
             enabled = false;
             PersonManager.DetectionReady += PersonManager_DetectionReady;
-
             marker = GameObject.Find("CalibrationMarker");
+
             var markerBehavior = FindObjectOfType<CalibrationMarkerBehavior>();
             markerBehavior.PathFinished += MarkerBehavior_PathFinished;
         }
 
+        private void InitializeCalibrationFile()
+        {
+            var directoryPath = Path.GetDirectoryName(calibrationFileName);
+            if (!Directory.Exists(directoryPath))
+            {
+                Directory.CreateDirectory(directoryPath);
+            }
+
+            if (!File.Exists(calibrationFileName))
+            {
+                File.WriteAllText(calibrationFileName, string.Empty);
+            }
+        }
+
+        private void AppendNewCalibrationPoint(float worldX, float worldY, float unityX, float unityY)
+        {
+            var valuesToWrite = new float[] { worldX, worldY, unityX, unityY }.Select(f => f.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture));
+            var line = string.Join(" ", valuesToWrite);
+
+            taskQueue.Add(() => File.AppendAllLines(calibrationFileName, new string[] { line }));
+            Task.Run(() => taskQueue.Take().Invoke());
+
+        }
+
         private void MarkerBehavior_PathFinished(object sender, EventArgs e)
         {
             enabled = false;
@@ -56,8 +75,6 @@ namespace Assets
 
         void Update()
         {
-            var persons = PersonManager.Persons;
-
             if (PersonManager.Persons.Count == 1)
             {
                 var person = PersonManager.Persons.Single();
@@ -65,8 +82,7 @@ namespace Assets
                 var personPosition = person.WorldPosition;
                 var markerPosition = marker.transform.position;
 
-                taskQueue.Add(() => File.AppendAllLines(calibrationFileName, 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());
+                AppendNewCalibrationPoint(personPosition.x, personPosition.z, markerPosition.x, markerPosition.z);
             }
         }
     }

Assets/Logging/DetectionFrameLogger.cs

@@ -57,7 +57,7 @@ namespace Assets.Logging
 
             string json = JsonConvert.SerializeObject(pocoFrame, new Vector2Converter(), new Vector3Converter(), new QuaternionConverter());
 
-            File.AppendAllText(@"C:\Users\nick.steyer\SmartStreetLight\log.txt", json);
+            File.AppendAllText(@".\log.txt", json);
         }
     }
 

Assets/StreamingAssets/Configuration.json

@@ -1,4 +1,5 @@
 {
 	"OutputCalibrationFilesDirectory": "C:\\Git\\git.tk.informatik.tu-darmstadt.de\\StreetLight\\Calibration",
-	"InputCalibrationFilePath": "C:\\Git\\git.tk.informatik.tu-darmstadt.de\\StreetLight\\CalibrationBright2022-10-01_19-13-20.csv"
+	"InputCalibrationFilePath": "C:\\Git\\git.tk.informatik.tu-darmstadt.de\\StreetLight\\CalibrationBright2022-10-01_19-13-20.csv",
+	"HomographyFilePath" : "C:\\Users\\nick.steyer\\SmartStreetLight\\StreetLight\\Assets\\StreamingAssets\\homography.csv"
 }

Assets/StreetLight/Configuration.cs

@@ -7,40 +7,58 @@ namespace Assets.StreetLight
 {
     public sealed class Configuration
     {
-        private string outputCalibrationFilesDirectory;
+        //private string outputCalibrationFilesDirectory;
 
-        /// <summary>
-        /// The directory path where new calibration files are saved when the calibration is run.
-        /// </summary>
-        [JsonRequired]
-        public string OutputCalibrationFilesDirectory
-        {
-            get => outputCalibrationFilesDirectory;
-            set
-            {
-                if (!Directory.Exists(value))
-                {
-                    throw new InvalidOperationException($"Directory \"{value}\" does not exist.");
-                }
-                outputCalibrationFilesDirectory = value;
-            }
-        }
+        ///// <summary>
+        ///// The directory path where new calibration files are saved when the calibration is run.
+        ///// </summary>
+        //[JsonRequired]
+        //public string OutputCalibrationFilesDirectory
+        //{
+        //    get => outputCalibrationFilesDirectory;
+        //    set
+        //    {
+        //        if (!Directory.Exists(value))
+        //        {
+        //            throw new InvalidOperationException($"Directory \"{value}\" does not exist.");
+        //        }
+        //        outputCalibrationFilesDirectory = value;
+        //    }
+        //}
+
+        //private string inputCalibrationFilePath;
+        ///// <summary>
+        ///// The file path to the calibration file that is used as input for the actual calibration.
+        ///// </summary>
+        //[JsonRequired]
+        //public string InputCalibrationFilePath
+        //{
+        //    get => inputCalibrationFilePath;
+        //    set
+        //    {
+        //        if (!File.Exists(value))
+        //        {
+        //            throw new InvalidOperationException($"File \"{value}\" does not exist.");
+        //        }
+        //        inputCalibrationFilePath = value;
+        //    }
+        //}
 
-        private string inputCalibrationFilePath;
+        private string homographyFilePath;
         /// <summary>
-        /// The file path to the calibration file that is used as input for the actual calibration.
+        /// The file path to the csv file containing the homography for mapping world coodrinates to Unity coordinates.
         /// </summary>
         [JsonRequired]
-        public string InputCalibrationFilePath
+        public string HomographyFilePath
         {
-            get => inputCalibrationFilePath;
+            get => homographyFilePath;
             set
             {
                 if (!File.Exists(value))
                 {
                     throw new InvalidOperationException($"File \"{value}\" does not exist.");
                 }
-                inputCalibrationFilePath = value;
+                homographyFilePath = value;
             }
         }
 

Assets/StreetLight/PersonVisualizer.cs

@@ -1,9 +1,6 @@
-using Assets.Logging;
 using Assets.StreetLight.Poco;
 using Assets.StreetLight.Scripts;
-using Assets.ZED.SDK.Helpers.Scripts;
 using System;
-using System.Collections;
 using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
@@ -37,21 +34,8 @@ namespace Assets.StreetLight
             foreach (var person in PersonManager.Persons.OrderBy(p => p.Id))
             {
                 GameObject sphere = spheres[count];
-                var unityPosition = PersonManager.PositionCalculator.CalculateUnityPosition(person);
+                var unityPosition = PersonManager.CalculateUnityPosition(person);
                 sphere.transform.position = new Vector3(unityPosition.x, unityPosition.y, unityPosition.z);
-
-                //var camera = Camera.main;
-                //Vector3[] frustumCorners = new Vector3[4];
-                //camera.CalculateFrustumCorners(new Rect(0, 0, 1, 1), camera.transform.position.y, Camera.MonoOrStereoscopicEye.Mono, frustumCorners);
-
-                //GameObject sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
-                //sphere.transform.position = new Vector3(0, 0, 0);
-                //sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
-                //sphere.transform.position = new Vector3(0, 0, Camera.main.rect.width);
-                //sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
-                //sphere.transform.position = new Vector3(Camera.main.rect.height, 0, 0);
-                //sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
-                //sphere.transform.position = new Vector3(Camera.main.rect.height, 0, Camera.main.rect.width);
                 count++;
             }
         }

Assets/StreetLight/Poco/Person.cs

@@ -5,6 +5,9 @@ namespace Assets.StreetLight.Poco
     public class Person
     {
         public int Id { get; set; }
+
+        // todo: add notify property changed
+        // todo: make world position private and add get position method for unity position
         public Vector3 WorldPosition { get; set; }
 
         public Person(int id, Vector3 worldPosition)

Assets/StreetLight/Scripts/PersonManager.cs

@@ -1,7 +1,6 @@
 using Assets.StreetLight.Adapters;
 using Assets.StreetLight.Interfaces;
 using Assets.StreetLight.Poco;
-using Newtonsoft.Json;
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
@@ -15,7 +14,7 @@ namespace Assets.StreetLight.Scripts
     public class PersonManager : MonoBehaviour
     {
         public ObservableCollection<Person> Persons { get; private set; }
-        public PositionCalculator PositionCalculator { get; private set; }
+        private PositionCalculator PositionCalculator;
 
         private IPersonDetector personDetector;
 
@@ -23,40 +22,56 @@ namespace Assets.StreetLight.Scripts
 
         void Start()
         {
-            var lines = File.ReadAllLines(Configuration.Instance.InputCalibrationFilePath);
-
-            var calibrationPoints = new List<CalibrationPoint>();
-
-            foreach (var line in lines.Skip(1))
+            if (File.Exists(Configuration.Instance.HomographyFilePath))
             {
-                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))));
+                var homographyArray = GetHomographyArrayFromFile();
+                PositionCalculator = new PositionCalculator(homographyArray);
             }
 
-            PositionCalculator = new PositionCalculator(calibrationPoints);
-
             Persons = new ObservableCollection<Person>();
             personDetector = new ZedPersonDetector(FindObjectOfType<ZEDManager>());
             personDetector.PersonsDetected += InvokeDetectionReady;
             personDetector.PersonsDetected += PersonDetector_PersonsDetected;
         }
 
+        private double[,] GetHomographyArrayFromFile()
+        {
+            var homographyString = File.ReadAllLines(Configuration.Instance.HomographyFilePath);
+
+            var values = homographyString.Select(l => l.Split(' ').Select(s => double.Parse(s, CultureInfo.InvariantCulture)).ToArray()).ToArray();
+
+            double[,] array = new double[3, 3];
+
+            for (int i = 0; i < 3; i++)
+            {
+                for (int j = 0; j < 3; j++)
+                {
+                    array[i, j] = values[i][j];
+                }
+            }
+
+            return array;
+        }
+
         private void InvokeDetectionReady(object sender, IEnumerable<Person> e)
         {
             DetectionReady?.Invoke(this, EventArgs.Empty);
             personDetector.PersonsDetected -= InvokeDetectionReady;
         }
 
+        public Vector3 CalculateUnityPosition(Person person)
+        {
+            if (PositionCalculator == null)
+            {
+                throw new InvalidOperationException("Cannot calculate Unity position because the PositionCalculator is not initialized. This might occur when the homography file is not found or not in the right format.");
+            }
+
+            return PositionCalculator.WorldPositionToUnityPosition(person.WorldPosition);
+        }
+
         private void PersonDetector_PersonsDetected(object sender, IEnumerable<Person> e)
         {
+            // TODO: update carefully instead of clear and add all
             Persons.Clear();
             foreach (var person in e)
             {

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -1,106 +1,20 @@
-using Assets.StreetLight.Interfaces;
-using Assets.StreetLight.Poco;
+using Assets.StreetLight.Poco;
 using MathNet.Numerics.LinearAlgebra;
 using MathNet.Numerics.LinearAlgebra.Double;
-using MathNet.Numerics.Statistics;
-using Newtonsoft.Json;
-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;
 
 namespace Assets.StreetLight.Scripts
 {
     public class PositionCalculator
     {
-        private readonly List<CalibrationPoint> calibrationVectors;
-        private Matrix<double> homography;
+        private readonly Matrix<double> homography;
 
-        public PositionCalculator(List<CalibrationPoint> calibrationVectors)
+        public PositionCalculator(double[,] homographyArray)
         {
-            this.calibrationVectors = calibrationVectors;
-
-            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))
-            {
-                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
-            }
-
-            var lines = calibrationVectors.Select(v => $"{v.WorldPosition.x.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)} {v.WorldPosition.z.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)} {v.UnityPosition.x.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)} {v.UnityPosition.z.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)}");
-
-            var pairsString = string.Join(Environment.NewLine, lines);
-
-            File.WriteAllText(@"C:\Git\git.tk.informatik.tu-darmstadt.de\StreetLight\Assets\StreamingAssets\pairs.csv", pairsString);
-            var homographyString = File.ReadAllLines(@"C:\Git\git.tk.informatik.tu-darmstadt.de\StreetLight\Assets\StreamingAssets\homography.csv");
-
-            var values = homographyString.Select(l => l.Split(' ').Select(s => double.Parse(s, CultureInfo.InvariantCulture)).ToArray()).ToArray();
-
-            double[,] array = new double[3, 3];
-
-            for (int i = 0; i < 3; i++)
-            {
-                for (int j = 0; j < 3; j++)
-                {
-                    array[i, j] = values[i][j];
-                }
-            }
-
-            homography = DenseMatrix.OfArray(array);
-        }
-
-        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);
-        }
-
-        public Vector3 CalculateUnityPosition(Person person)
-        {
-            return WorldPositionToUnityPosition(person.WorldPosition);
+            homography = DenseMatrix.OfArray(homographyArray);
         }
 
-        private Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
+        public Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
         {
             var homogeneousWorldPosition = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });