Protoype for calibration

Assembly-CSharp.csproj

@@ -210,6 +210,7 @@
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\PlaneDetection\ZEDPlaneDetectionManager.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\Utilities\ZEDLogMessage.cs" />
     <Compile Include="Assets\CalibrationMarkerBehavior.cs" />
+    <Compile Include="Assets\CalibrationRecorderBehavior.cs" />
   </ItemGroup>
   <ItemGroup>
     <None Include="Assets\ZED\Examples\GreenScreen\Shaders\Mask_Quad.shader" />

Assets/CalibrationMarkerBehavior.cs

@@ -11,10 +11,6 @@ public class CalibrationMarkerBehavior : MonoBehaviour
 {
     Vector3[] frustumCorners;
     List<Vector3> targetPositions;
-    float startTime;
-    float duration = 2.0f;
-    Vector3 startPoint;
-    Vector3 endPoint;
 
     // Start is called before the first frame update
     void Start()
@@ -43,11 +39,11 @@ public class CalibrationMarkerBehavior : MonoBehaviour
     {
         var marker = GameObject.Find("CalibrationMarker");
 
-        var speed = 2.5f;
+        const float speed = 2.5f;
 
         if (Vector3.Distance(marker.transform.position, currentTarget) >= 0.05f)
         {
-            marker.transform.position = marker.transform.position + Vector3.ClampMagnitude(normalizedTranslationVector * Time.deltaTime * speed, (currentTarget - marker.transform.position).magnitude);
+            marker.transform.position += Vector3.ClampMagnitude(speed * Time.deltaTime * normalizedTranslationVector, (currentTarget - marker.transform.position).magnitude);
         }
         else if (targetIndex < targetPositions.Count - 1)
         {
@@ -55,7 +51,5 @@ public class CalibrationMarkerBehavior : MonoBehaviour
             currentTarget = targetPositions[targetIndex];
             normalizedTranslationVector = (currentTarget - marker.transform.position).normalized;
         }
-
-
     }
 }

Assets/CalibrationRecorderBehavior.cs

@@ -0,0 +1,50 @@
+using System;
+using System.Collections;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
+using System.IO;
+using System.Linq;
+using System.Threading.Tasks;
+using UnityEngine;
+
+public class CalibrationRecorderBehavior : MonoBehaviour
+{
+    PersonManager PersonManager => personManagerLazy.Value;
+    Lazy<PersonManager> personManagerLazy;
+
+    private void Awake()
+    {
+        personManagerLazy = new Lazy<PersonManager>(FindObjectOfType<PersonManager>);
+    }
+
+    BlockingCollection<Action> taskQueue;
+
+    string fileName = @$"C:\Users\nick.steyer\Desktop\{DateTime.Now:yyyy-dd-M_HH-mm-ss}.csv";
+
+    void Start()
+    {
+        taskQueue = new BlockingCollection<Action>();
+        if (!File.Exists(fileName))
+        {
+            File.WriteAllLines(fileName, new string[] { "WorldX,WorldY,WorldZ,UnityX,UnityY,UnityZ" });
+        }
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        var persons = PersonManager.Persons;
+
+        if (PersonManager.Persons.Count == 1)
+        {
+            var person = PersonManager.Persons.Single();
+            var marker = GameObject.Find("CalibrationMarker");
+
+            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}") }));
+            Task.Run(() => taskQueue.Take().Invoke());
+        }
+    }
+}

Assets/CalibrationRecorderBehavior.cs.meta

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

Assets/Scenes/Calibration.unity

@@ -207,6 +207,50 @@ Transform:
   m_Father: {fileID: 0}
   m_RootOrder: 0
   m_LocalEulerAnglesHint: {x: 90, y: 0, z: 0}
+--- !u!1 &919955599
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 919955601}
+  - component: {fileID: 919955600}
+  m_Layer: 0
+  m_Name: CalibrationRecorder
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!114 &919955600
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 919955599}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 2ff7a32f4db31ec4885acbcd99389e5e, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+--- !u!4 &919955601
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 919955599}
+  m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
+  m_LocalPosition: {x: -4.6961923, y: -4.180311, z: 7.6556225}
+  m_LocalScale: {x: 1, y: 1, z: 1}
+  m_ConstrainProportionsScale: 0
+  m_Children: []
+  m_Father: {fileID: 0}
+  m_RootOrder: 3
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
 --- !u!1 &1915020868
 GameObject:
   m_ObjectHideFlags: 0

Assets/StreetLight/PersonVisualizer.cs

@@ -28,11 +28,6 @@ public class PersonVisualizer : MonoBehaviour
         spheres = new GameObject[] { GameObject.CreatePrimitive(PrimitiveType.Sphere), GameObject.CreatePrimitive(PrimitiveType.Sphere) };
     }
 
-    float minX = int.MaxValue;
-    float minY = int.MaxValue;
-    float maxX = int.MinValue;
-    float maxY = int.MinValue;
-
     void Update()
     {
         int count = 0;

Assets/StreetLight/Scripts/PersonManager.cs

@@ -16,29 +16,29 @@ public class PersonManager : MonoBehaviour
 
     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 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) ),
+
+        ////    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<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) ),
-
         //    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<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) ),
-            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 positionCalculator = new PositionCalculator(calibrationVectors);
+
+        var positionCalculator = new PositionCalculator(new List<Tuple<Vector3, Vector3>>());
 
         Persons = new ObservableCollection<Person>();
         personDetector = new ZedPersonDetector(FindObjectOfType<ZEDManager>(), positionCalculator);

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -20,10 +20,12 @@ internal class PositionCalculator : IPositionCalculator
     {
         this.calibrationVectors = calibrationVectors;
 
-        var cv = calibrationVectors;
+        if (calibrationVectors?.Count != 0)
+        {
+            var cv = calibrationVectors;
 
-        Matrix<double> matrixA = DenseMatrix.OfArray(new double[,]
-        { {cv[0].Item1.x, cv[0].Item1.z, 1, 0, 0, 0, -cv[0].Item2.x*cv[0].Item1.x, -cv[0].Item2.x*cv[0].Item1.z},
+            Matrix<double> matrixA = DenseMatrix.OfArray(new double[,]
+            { {cv[0].Item1.x, cv[0].Item1.z, 1, 0, 0, 0, -cv[0].Item2.x*cv[0].Item1.x, -cv[0].Item2.x*cv[0].Item1.z},
            {0, 0, 0, cv[0].Item1.x, cv[0].Item1.z, 1, -cv[0].Item2.z*cv[0].Item1.x, -cv[0].Item2.z*cv[0].Item1.z},
            {cv[1].Item1.x, cv[1].Item1.z, 1, 0, 0, 0, -cv[1].Item2.x*cv[1].Item1.x, -cv[1].Item2.x*cv[1].Item1.z},
            {0, 0, 0, cv[1].Item1.x, cv[1].Item1.z, 1, -cv[1].Item2.z*cv[1].Item1.x, -cv[1].Item2.z*cv[1].Item1.z},
@@ -31,10 +33,10 @@ internal class PositionCalculator : IPositionCalculator
            {0, 0, 0, cv[2].Item1.x, cv[2].Item1.z, 1, -cv[2].Item2.z*cv[2].Item1.x, -cv[2].Item2.z*cv[2].Item1.z},
            {cv[3].Item1.x, cv[3].Item1.z, 1, 0, 0, 0, -cv[3].Item2.x*cv[3].Item1.x, -cv[3].Item2.x*cv[3].Item1.z},
            {0, 0, 0, cv[3].Item1.x, cv[3].Item1.z, 1, -cv[3].Item2.z*cv[3].Item1.x, -cv[3].Item2.z*cv[3].Item1.z}
-        });
+            });
 
-        Matrix<double> matrixB = DenseMatrix.OfArray(new double[,]
-            {
+            Matrix<double> matrixB = DenseMatrix.OfArray(new double[,]
+                {
               {cv[0].Item2.x},
               {cv[0].Item2.z},
               {cv[1].Item2.x},
@@ -43,20 +45,21 @@ internal class PositionCalculator : IPositionCalculator
               {cv[2].Item2.z},
               {cv[3].Item2.x},
               {cv[3].Item2.z}
-            }
-            );
+                }
+                );
 
-        var lambda = (matrixA.Transpose() * matrixA).Inverse() * matrixA.Transpose() * matrixB;
-        //lambda = lambda.InsertRow(lambda.RowCount, DenseVector.OfArray(new double[] { 1 }));
-        //lambda = lambda.Resize(3, 3);
-        this.lambda = DenseMatrix.OfArray(new double[,]
-        {
+            var lambda = (matrixA.Transpose() * matrixA).Inverse() * matrixA.Transpose() * matrixB;
+            //lambda = lambda.InsertRow(lambda.RowCount, DenseVector.OfArray(new double[] { 1 }));
+            //lambda = lambda.Resize(3, 3);
+            this.lambda = 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();
+            testCalibration();
+        }
     }
 
     public Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
@@ -72,7 +75,7 @@ internal class PositionCalculator : IPositionCalculator
 
     private void testCalibration()
     {
-        foreach(var c in calibrationVectors)
+        foreach (var c in calibrationVectors)
         {
             var test = DenseVector.OfArray(new double[] { c.Item1.x, c.Item1.z, 1 });
             var testOutput = lambda * test;