Cleanup

Assembly-CSharp.csproj

@@ -144,7 +144,6 @@
     <Compile Include="Assets\ZED\Tools\Mixed Reality Calibration\Scripts\TranslateControl.cs" />
     <Compile Include="Assets\ZED\Examples\Drone Shooter\Scripts\Utilities\ZEDProjectile.cs" />
     <Compile Include="Assets\ZED\Examples\Drone Shooter\Scripts\Simple\DroneSpawner.cs" />
-    <Compile Include="Assets\StreetLight\Interfaces\IPositionCalculator.cs" />
     <Compile Include="Assets\ZED\Tools\Mixed Reality Calibration\Scripts\TransformControl.cs" />
     <Compile Include="Assets\StreetLight\Serialization\GameObjectConverter.cs" />
     <Compile Include="Assets\ZED\Tools\Mixed Reality Calibration\Scripts\ToggleButton.cs" />
@@ -211,6 +210,7 @@
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\Utilities\ZEDLogMessage.cs" />
     <Compile Include="Assets\CalibrationMarkerBehavior.cs" />
     <Compile Include="Assets\CalibrationRecorderBehavior.cs" />
+    <Compile Include="Assets\StreetLight\Poco\CalibrationPoint.cs" />
   </ItemGroup>
   <ItemGroup>
     <None Include="Assets\ZED\Examples\GreenScreen\Shaders\Mask_Quad.shader" />

Assets/CalibrationRecorderBehavior.cs

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

@@ -422,3 +422,68 @@ Rigidbody:
   m_Interpolate: 0
   m_Constraints: 0
   m_CollisionDetection: 0
+--- !u!1001 &2139725572
+PrefabInstance:
+  m_ObjectHideFlags: 0
+  serializedVersion: 2
+  m_Modification:
+    m_TransformParent: {fileID: 0}
+    m_Modifications:
+    - target: {fileID: 1180261165772202, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_Name
+      value: ZED_Rig_Mono
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_RootOrder
+      value: 4
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalPosition.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalPosition.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalPosition.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalRotation.w
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalRotation.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalRotation.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalRotation.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.x
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.y
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 4329734432191940, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: m_LocalEulerAnglesHint.z
+      value: 0
+      objectReference: {fileID: 0}
+    - target: {fileID: 114491592745282986, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: inputType
+      value: 1
+      objectReference: {fileID: 0}
+    - target: {fileID: 114491592745282986, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}
+      propertyPath: svoInputFileName
+      value: C:/Users/nick.steyer/Git/Files/HD1080_SN39598725_17-45-47.svo
+      objectReference: {fileID: 0}
+    m_RemovedComponents: []
+  m_SourcePrefab: {fileID: 100100000, guid: 76db3eb81fd21ae45bab5204e324ae42, type: 3}

Assets/StreetLight/Adapters/ZedPersonDetector.cs

@@ -16,14 +16,12 @@ namespace Assets.StreetLight.Adapters
     internal class ZedPersonDetector : IPersonDetector
     {
         private readonly ZEDManager zedManager;
-        private readonly IPositionCalculator positionCalculator;
 
         public event EventHandler<IEnumerable<Person>> PersonsDetected;
 
-        public ZedPersonDetector(ZEDManager zedManager, IPositionCalculator positionCalculator)
+        public ZedPersonDetector(ZEDManager zedManager)
         {
             this.zedManager = zedManager;
-            this.positionCalculator = positionCalculator;
             zedManager.OnObjectDetection += ZedManager_OnObjectDetection;
             zedManager.OnZEDReady += ZedManager_OnZEDReady;
 
@@ -44,7 +42,7 @@ namespace Assets.StreetLight.Adapters
 
             var detectedPersons = from p in persons
                                   let position = p.Get3DWorldPosition()
-                                  select new Person(positionCalculator, p.id, new Vector3(position.x, position.y, position.z));
+                                  select new Person(p.id, new Vector3(position.x, position.y, position.z));
 
             PersonsDetected?.Invoke(this, detectedPersons.ToList());
         }

Assets/StreetLight/Interfaces/IPositionCalculator.cs

@@ -1,9 +0,0 @@
-using UnityEngine;
-
-namespace Assets.StreetLight.Interfaces
-{
-    public interface IPositionCalculator
-    {
-        Vector3 WorldPositionToUnityPosition(Vector3 worldPosition);
-    }
-}

Assets/StreetLight/MyZED3DObjectVisualizer.cs

@@ -4,6 +4,7 @@ using System.Collections.Generic;
 using System.Linq;
 using Assets.Logging;
 using Assets.StreetLight.Poco;
+using Assets.StreetLight.Scripts;
 using Assets.ZED.SDK.Helpers.Scripts;
 using UnityEngine;
 

Assets/StreetLight/PersonVisualizer.cs

@@ -1,5 +1,6 @@
 using Assets.Logging;
 using Assets.StreetLight.Poco;
+using Assets.StreetLight.Scripts;
 using Assets.ZED.SDK.Helpers.Scripts;
 using System;
 using System.Collections;
@@ -7,125 +8,128 @@ using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
 
-public class PersonVisualizer : MonoBehaviour
+namespace Assets.StreetLight
 {
-    PersonManager PersonManager => personManagerLazy.Value;
-    Lazy<PersonManager> personManagerLazy;
-
-    public GameObject boundingBoxPrefab;
-
-    IDictionary<int, GameObject> markers = new Dictionary<int, GameObject>();
-
-    private void Awake()
+    public class PersonVisualizer : MonoBehaviour
     {
-        personManagerLazy = new Lazy<PersonManager>(FindObjectOfType<PersonManager>);
-    }
+        PersonManager PersonManager => personManagerLazy.Value;
+        Lazy<PersonManager> personManagerLazy;
 
-    GameObject[] spheres;
+        public GameObject boundingBoxPrefab;
 
-    void Start()
-    {
-        spheres = new GameObject[] { GameObject.CreatePrimitive(PrimitiveType.Sphere), GameObject.CreatePrimitive(PrimitiveType.Sphere) };
-    }
+        IDictionary<int, GameObject> markers = new Dictionary<int, GameObject>();
 
-    void Update()
-    {
-        int count = 0;
-        foreach (var person in PersonManager.Persons.OrderBy(p => p.Id))
+        private void Awake()
         {
-            GameObject sphere = spheres[count];
-            sphere.transform.position = new Vector3(person.UnityPosition.x, person.UnityPosition.y, person.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++;
+            personManagerLazy = new Lazy<PersonManager>(FindObjectOfType<PersonManager>);
         }
-    }
-
-    float GroundPositionToSceneCoordinate(float minValue, float actualValue)
-    {
-        return 0;
-    }
 
-    private void ReturnBoxToPool(int id, RectTransform bbox)
-    {
-        bbox.gameObject.SetActive(false);
-        bbox.name = "Unused";
-
-        bboxPool.Push(bbox.gameObject);
+        GameObject[] spheres;
 
-        if (liveBBoxes.ContainsKey(id))
+        void Start()
         {
-            liveBBoxes.Remove(id);
+            spheres = new GameObject[] { GameObject.CreatePrimitive(PrimitiveType.Sphere), GameObject.CreatePrimitive(PrimitiveType.Sphere) };
         }
-        else
+
+        void Update()
         {
-            Debug.LogError("Tried to remove object ID " + id + " from active bboxes, but it wasn't in the dictionary.");
+            int count = 0;
+            foreach (var person in PersonManager.Persons.OrderBy(p => p.Id))
+            {
+                GameObject sphere = spheres[count];
+                var unityPosition = PersonManager.PositionCalculator.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++;
+            }
         }
-    }
 
-    private Dictionary<int, RectTransform> liveBBoxes = new Dictionary<int, RectTransform>();
-    private Dictionary<int, Color> idColorDict = new Dictionary<int, Color>();
-
-    private Stack<GameObject> bboxPool = new Stack<GameObject>();
+        float GroundPositionToSceneCoordinate(float minValue, float actualValue)
+        {
+            return 0;
+        }
 
-    /// <summary>
-    /// Returs the RectTransform within the GameObject (instantiated from boundingBoxPrefab) that represents the provided DetectedObject.
-    /// If none exists, it retrieves one from the pool (or instantiates a new one if none is available) and
-    /// sets it up with the proper ID and colors.
-    /// </summary>
-    private RectTransform GetBBoxForObject(Person dobj)
-    {
-        if (!liveBBoxes.ContainsKey(dobj.Id))
+        private void ReturnBoxToPool(int id, RectTransform bbox)
         {
-            GameObject newbox = GetAvailableBBox();
-            //newbox.transform.SetParent(canvas.transform, false);
-            newbox.name = "Object #" + dobj.Id;
+            bbox.gameObject.SetActive(false);
+            bbox.name = "Unused";
+
+            bboxPool.Push(bbox.gameObject);
 
-            Color col;
-            if (idColorDict.ContainsKey(dobj.Id))
+            if (liveBBoxes.ContainsKey(id))
             {
-                col = idColorDict[dobj.Id];
+                liveBBoxes.Remove(id);
             }
             else
             {
-                col = GetNextColor();
-                idColorDict.Add(dobj.Id, col);
+                Debug.LogError("Tried to remove object ID " + id + " from active bboxes, but it wasn't in the dictionary.");
             }
+        }
 
-            BBox2DHandler boxhandler = newbox.GetComponent<BBox2DHandler>();
-            if (boxhandler)
-            {
-                boxhandler.SetColor(col);
-                boxhandler.SetID(dobj.Id);
-            }
+        private Dictionary<int, RectTransform> liveBBoxes = new Dictionary<int, RectTransform>();
+        private Dictionary<int, Color> idColorDict = new Dictionary<int, Color>();
 
+        private Stack<GameObject> bboxPool = new Stack<GameObject>();
 
-            RectTransform newrecttrans = newbox.GetComponent<RectTransform>();
-            if (!newrecttrans)
+        /// <summary>
+        /// Returs the RectTransform within the GameObject (instantiated from boundingBoxPrefab) that represents the provided DetectedObject.
+        /// If none exists, it retrieves one from the pool (or instantiates a new one if none is available) and
+        /// sets it up with the proper ID and colors.
+        /// </summary>
+        private RectTransform GetBBoxForObject(Person dobj)
+        {
+            if (!liveBBoxes.ContainsKey(dobj.Id))
             {
-                Debug.LogError("BBox prefab needs a RectTransform in the root object.");
-                return null;
+                GameObject newbox = GetAvailableBBox();
+                //newbox.transform.SetParent(canvas.transform, false);
+                newbox.name = "Object #" + dobj.Id;
+
+                Color col;
+                if (idColorDict.ContainsKey(dobj.Id))
+                {
+                    col = idColorDict[dobj.Id];
+                }
+                else
+                {
+                    col = GetNextColor();
+                    idColorDict.Add(dobj.Id, col);
+                }
+
+                BBox2DHandler boxhandler = newbox.GetComponent<BBox2DHandler>();
+                if (boxhandler)
+                {
+                    boxhandler.SetColor(col);
+                    boxhandler.SetID(dobj.Id);
+                }
+
+
+                RectTransform newrecttrans = newbox.GetComponent<RectTransform>();
+                if (!newrecttrans)
+                {
+                    Debug.LogError("BBox prefab needs a RectTransform in the root object.");
+                    return null;
+                }
+
+                liveBBoxes[dobj.Id] = newrecttrans;
+                return newrecttrans;
             }
+            else return liveBBoxes[dobj.Id];
 
-            liveBBoxes[dobj.Id] = newrecttrans;
-            return newrecttrans;
         }
-        else return liveBBoxes[dobj.Id];
-
-    }
 
-    public List<Color> boxColors = new List<Color>()
+        public List<Color> boxColors = new List<Color>()
     {
         new Color(.231f, .909f, .69f, 1),
         new Color(.098f, .686f, .816f, 1),
@@ -134,45 +138,46 @@ public class PersonVisualizer : MonoBehaviour
         new Color(.989f, .388f, .419f, 1)
     };
 
-    private int nextColorIndex = 0;
+        private int nextColorIndex = 0;
 
-    private Color GetNextColor()
-    {
-        if (boxColors.Count == 0)
+        private Color GetNextColor()
         {
-            return new Color(.043f, .808f, .435f, 1);
-        }
+            if (boxColors.Count == 0)
+            {
+                return new Color(.043f, .808f, .435f, 1);
+            }
 
-        if (nextColorIndex >= boxColors.Count)
-        {
-            nextColorIndex = 0;
-        }
+            if (nextColorIndex >= boxColors.Count)
+            {
+                nextColorIndex = 0;
+            }
 
-        Color returncol = boxColors[nextColorIndex];
+            Color returncol = boxColors[nextColorIndex];
 
-        nextColorIndex++;
+            nextColorIndex++;
 
 
-        return returncol;
-    }
+            return returncol;
+        }
 
-    /// <summary>
-    /// Gets an available GameObject (instantiated from boundingBoxPrefab) from the pool,
-    /// or instantiates a new one if none are available.
-    /// </summary>
-    /// <returns></returns>
-    private GameObject GetAvailableBBox()
-    {
-        if (bboxPool.Count == 0)
+        /// <summary>
+        /// Gets an available GameObject (instantiated from boundingBoxPrefab) from the pool,
+        /// or instantiates a new one if none are available.
+        /// </summary>
+        /// <returns></returns>
+        private GameObject GetAvailableBBox()
         {
-            GameObject newbbox = Instantiate(boundingBoxPrefab);
-            newbbox.transform.SetParent(transform, false);
-            bboxPool.Push(newbbox);
-        }
+            if (bboxPool.Count == 0)
+            {
+                GameObject newbbox = Instantiate(boundingBoxPrefab);
+                newbbox.transform.SetParent(transform, false);
+                bboxPool.Push(newbbox);
+            }
 
-        GameObject bbox = bboxPool.Pop();
-        bbox.SetActive(true);
+            GameObject bbox = bboxPool.Pop();
+            bbox.SetActive(true);
 
-        return bbox;
+            return bbox;
+        }
     }
-}
+}

Assets/StreetLight/Poco/CalibrationPoint.cs

@@ -0,0 +1,16 @@
+using UnityEngine;
+
+namespace Assets.StreetLight.Poco
+{
+    public class CalibrationPoint
+    {
+        public Vector3 WorldPosition { get; }
+        public Vector3 UnityPosition { get; }
+
+        public CalibrationPoint(Vector3 worldPosition, Vector3 unityPosition)
+        {
+            WorldPosition = worldPosition;
+            UnityPosition = unityPosition;
+        }
+    }
+}

Assets/StreetLight/Interfaces/IPositionCalculator.cs.meta → Assets/StreetLight/Poco/CalibrationPoint.cs.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: 5485cbfc38a2d014d82ee326d801e2df
+guid: 841a7d3018fe28d4fa21f3c6a81825b3
 MonoImporter:
   externalObjects: {}
   serializedVersion: 2

Assets/StreetLight/Poco/Person.cs

@@ -1,19 +1,14 @@
-using Assets.StreetLight.Interfaces;
-using UnityEngine;
+using UnityEngine;
 
 namespace Assets.StreetLight.Poco
 {
     public class Person
     {
-        private readonly IPositionCalculator positionCalculator;
-
         public int Id { get; set; }
         public Vector3 WorldPosition { get; set; }
-        public Vector3 UnityPosition => positionCalculator.WorldPositionToUnityPosition(WorldPosition);
 
-        public Person(IPositionCalculator positionCalculator, int id, Vector3 worldPosition)
+        public Person(int id, Vector3 worldPosition)
         {
-            this.positionCalculator = positionCalculator;
             Id = id;
             WorldPosition = worldPosition;
         }

Assets/StreetLight/Scripts/PersonManager.cs

@@ -8,53 +8,57 @@ using System.Collections.ObjectModel;
 using System.Runtime.CompilerServices;
 using UnityEngine;
 
-public class PersonManager : MonoBehaviour
+namespace Assets.StreetLight.Scripts
 {
-    public ObservableCollection<Person> Persons { get; set; }
+    public class PersonManager : MonoBehaviour
+    {
+        public ObservableCollection<Person> Persons { get; private set; }
+        public PositionCalculator PositionCalculator { get; private set; }
 
-    private IPersonDetector personDetector;
+        private IPersonDetector personDetector;
 
-    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) ),
-
-        ////    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(new List<Tuple<Vector3, Vector3>>());
-
-        Persons = new ObservableCollection<Person>();
-        personDetector = new ZedPersonDetector(FindObjectOfType<ZEDManager>(), positionCalculator);
-        personDetector.PersonsDetected += PersonDetector_PersonsDetected;
-    }
+        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) ),
 
-    private void PersonDetector_PersonsDetected(object sender, IEnumerable<Person> e)
-    {
-        Persons.Clear();
-        foreach (var person in e)
+            ////    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))
+        };
+
+            PositionCalculator = new PositionCalculator(calibrationVectors);
+
+            Persons = new ObservableCollection<Person>();
+            personDetector = new ZedPersonDetector(FindObjectOfType<ZEDManager>());
+            personDetector.PersonsDetected += PersonDetector_PersonsDetected;
+        }
+
+        private void PersonDetector_PersonsDetected(object sender, IEnumerable<Person> e)
         {
-            Persons.Add(person);
+            Persons.Clear();
+            foreach (var person in e)
+            {
+                Persons.Add(person);
+            }
         }
-    }
 
-    void Update()
-    {
+        void Update()
+        {
+        }
     }
-}
+}

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -1,85 +1,98 @@
 using Assets.StreetLight.Interfaces;
-using System.Collections.Generic;
-using UnityEngine;
-using System;
-using Unity.VisualScripting;
-using Assets.StreetLight.Serialization;
-using MathNet;
+using Assets.StreetLight.Poco;
 using MathNet.Numerics.LinearAlgebra;
 using MathNet.Numerics.LinearAlgebra.Double;
-using System.Linq.Expressions;
+using System;
+using System.Collections.Generic;
+using UnityEngine;
 
-internal class PositionCalculator : IPositionCalculator
+namespace Assets.StreetLight.Scripts
 {
-    // World to Unity Position
-    private List<Tuple<Vector3, Vector3>> calibrationVectors;
+    public class PositionCalculator
+    {
+        private List<CalibrationPoint> calibrationVectors;
+        private Matrix<double> homography;
 
-    private Matrix<double> lambda;
+        public PositionCalculator(List<CalibrationPoint> calibrationVectors)
+        {
+            this.calibrationVectors = calibrationVectors;
 
-    public PositionCalculator(List<Tuple<Vector3, Vector3>> calibrationVectors)
-    {
-        this.calibrationVectors = calibrationVectors;
+            CalculateHomographySimple();
+        }
 
-        if (calibrationVectors?.Count != 0)
+        private void CalculateHomographySimple()
         {
-            var cv = calibrationVectors;
+            if (calibrationVectors?.Count >= 4)
+            {
+                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},
-           {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},
-           {cv[2].Item1.x, cv[2].Item1.z, 1, 0, 0, 0, -cv[2].Item2.x*cv[2].Item1.x, -cv[2].Item2.x*cv[2].Item1.z},
-           {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> 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},
+                {0, 0, 0, cv[1].WorldPosition.x, cv[1].WorldPosition.z, 1, -cv[1].UnityPosition.z*cv[1].WorldPosition.x, -cv[1].UnityPosition.z*cv[1].WorldPosition.z},
+                {cv[2].WorldPosition.x, cv[2].WorldPosition.z, 1, 0, 0, 0, -cv[2].UnityPosition.x*cv[2].WorldPosition.x, -cv[2].UnityPosition.x*cv[2].WorldPosition.z},
+                {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].Item2.x},
-              {cv[0].Item2.z},
-              {cv[1].Item2.x},
-              {cv[1].Item2.z},
-              {cv[2].Item2.x},
-              {cv[2].Item2.z},
-              {cv[3].Item2.x},
-              {cv[3].Item2.z}
-                }
-                );
+                {cv[0].UnityPosition.x},
+                {cv[0].UnityPosition.z},
+                {cv[1].UnityPosition.x},
+                {cv[1].UnityPosition.z},
+                {cv[2].UnityPosition.x},
+                {cv[2].UnityPosition.z},
+                {cv[3].UnityPosition.x},
+                {cv[3].UnityPosition.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;
+                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
             {
-            { 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}
-            });
+                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
+            }
+        }
 
-            testCalibration();
+        public Vector3 CalculateUnityPosition(Person person)
+        {
+            return WorldPositionToUnityPosition(person.WorldPosition);
         }
-    }
 
-    public Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
-    {
-        var vector = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });
+        private Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
+        {
+            var vector = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });
 
-        var output = lambda * vector;
-        var scaledOutput = output / output[2];
+            var output = homography * vector;
+            var scaledOutput = output / output[2];
+            var resultVector = new Vector3((float)scaledOutput[0], 0, (float)scaledOutput[1]);
 
-        return new Vector3((float)scaledOutput[0], 0, (float)scaledOutput[1]);
-        throw new System.NotImplementedException();
-    }
+            return resultVector;
+        }
 
-    private void testCalibration()
-    {
-        foreach (var c in calibrationVectors)
+        /// <summary>
+        /// For internal debugging only.
+        /// </summary>
+        private void testCalibration()
         {
-            var test = DenseVector.OfArray(new double[] { c.Item1.x, c.Item1.z, 1 });
-            var testOutput = lambda * test;
-            var scaledTestOutput = testOutput / testOutput[2];
+            foreach (var c in calibrationVectors)
+            {
+                var test = DenseVector.OfArray(new double[] { c.WorldPosition.x, c.WorldPosition.z, 1 });
+                var testOutput = homography * test;
+                var scaledTestOutput = testOutput / testOutput[2];
+            }
         }
     }
 }