Homography with 4 calibration points

Assembly-CSharp-Editor.csproj

@@ -26,7 +26,7 @@
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <NoWarn>0169</NoWarn>
-    <AllowUnsafeBlocks>False</AllowUnsafeBlocks>
+    <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
     <DebugType>pdbonly</DebugType>
@@ -35,7 +35,7 @@
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <NoWarn>0169</NoWarn>
-    <AllowUnsafeBlocks>False</AllowUnsafeBlocks>
+    <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
   <PropertyGroup>
     <NoConfig>true</NoConfig>
@@ -331,6 +331,9 @@
     <Reference Include="log4netPlastic">
       <HintPath>Library\PackageCache\com.unity.collab-proxy@1.15.16\Lib\Editor\PlasticSCM\log4netPlastic.dll</HintPath>
     </Reference>
+    <Reference Include="MathNet.Numerics">
+      <HintPath>Assets\MathNet.Numerics.dll</HintPath>
+    </Reference>
     <Reference Include="Unity.VisualScripting.Antlr3.Runtime">
       <HintPath>Library\PackageCache\com.unity.visualscripting@1.7.6\Runtime\VisualScripting.Flow\Dependencies\NCalc\Unity.VisualScripting.Antlr3.Runtime.dll</HintPath>
     </Reference>

Assembly-CSharp.csproj

@@ -8,7 +8,8 @@
     <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
     <ProductVersion>10.0.20506</ProductVersion>
     <SchemaVersion>2.0</SchemaVersion>
-    <RootNamespace></RootNamespace>
+    <RootNamespace>
+    </RootNamespace>
     <ProjectGuid>{4322513C-6AC2-9058-38C0-BCFE306751F6}</ProjectGuid>
     <OutputType>Library</OutputType>
     <AppDesignerFolder>Properties</AppDesignerFolder>
@@ -26,7 +27,7 @@
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <NoWarn>0169</NoWarn>
-    <AllowUnsafeBlocks>False</AllowUnsafeBlocks>
+    <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
     <DebugType>pdbonly</DebugType>
@@ -35,7 +36,7 @@
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
     <NoWarn>0169</NoWarn>
-    <AllowUnsafeBlocks>False</AllowUnsafeBlocks>
+    <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
   </PropertyGroup>
   <PropertyGroup>
     <NoConfig>true</NoConfig>
@@ -58,6 +59,7 @@
   <ItemGroup>
     <Compile Include="Assets\StreetLight\Serialization\Matrix4x4Converter.cs" />
     <Compile Include="Assets\ZED\Tools\Mixed Reality Calibration\Scripts\ChooseTrackedObjectMenu.cs" />
+    <Compile Include="Assets\StreetLight\Scripts\PositionCalculator.cs" />
     <Compile Include="Assets\Ball.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\ObjectDetection\DetectedObject.cs" />
     <Compile Include="Assets\ZED\Examples\SpatialMapping\Scripts\EnemyBehavior.cs" />
@@ -143,6 +145,7 @@
     <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" />
@@ -207,8 +210,6 @@
     <Compile Include="Assets\ZED\Tools\Mixed Reality Calibration\Scripts\LookAtCameraPartialAxis.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\PlaneDetection\ZEDPlaneDetectionManager.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\Utilities\ZEDLogMessage.cs" />
-    <Compile Include="Assets\StreetLight\Interfaces\IPositionCalculator.cs" />
-    <Compile Include="Assets\StreetLight\Scripts\PositionCalculator.cs" />
   </ItemGroup>
   <ItemGroup>
     <None Include="Assets\ZED\Examples\GreenScreen\Shaders\Mask_Quad.shader" />
@@ -239,6 +240,7 @@
     <None Include="Assets\ZED\SDK\Helpers\Shaders\ZED_Utils.cginc" />
     <None Include="Assets\ZED\Examples\OpenCV ArUco Detection\Shaders\UnlitTrans.shader" />
     <None Include="Assets\ZED\Examples\Drone Shooter\Resources\Muzzleflash_FX\WFX_S Particle Add A8.shader" />
+    <None Include="Assets\smcs.rsp" />
     <None Include="Assets\ZED\SDK\Plugins\win64\sl_unitywrapper.dll" />
     <None Include="Assets\ZED\Examples\GreenScreen\Shaders\Outlined.shader" />
     <None Include="Assets\ZED\Examples\Planetarium\Resources\Self-Illumin.shader" />
@@ -247,6 +249,7 @@
     <None Include="Assets\ZED\Examples\GreenScreen\Shaders\GreenScreen.shader" />
     <None Include="Assets\ZED\SDK\Helpers\Shaders\GUI\ZED_Default_OverlayNoZTest.shader" />
     <None Include="Assets\ZED\Examples\GreenScreen\Shaders\YUV.shader" />
+    <None Include="Assets\MathNet.Numerics.dll" />
     <None Include="Assets\ZED\SDK\Helpers\Shaders\PlaneDetection\GeometryWirePlane.shader" />
     <None Include="Assets\ZED\Examples\Planetarium\Resources\Sun\Shader\Sun.shader" />
     <None Include="Assets\ZED\SDK\Helpers\Shaders\PostProcessing\ZED_Post-Processing.shader" />
@@ -526,6 +529,9 @@
     <Reference Include="UnityEditor.UnityConnectModule">
       <HintPath>C:\Program Files\Unity\Hub\Editor\2021.3.2f1\Editor\Data\Managed\UnityEngine\UnityEditor.UnityConnectModule.dll</HintPath>
     </Reference>
+    <Reference Include="MathNet.Numerics">
+      <HintPath>Assets\MathNet.Numerics.dll</HintPath>
+    </Reference>
     <Reference Include="Unity.VisualScripting.Antlr3.Runtime">
       <HintPath>Library\PackageCache\com.unity.visualscripting@1.7.6\Runtime\VisualScripting.Flow\Dependencies\NCalc\Unity.VisualScripting.Antlr3.Runtime.dll</HintPath>
     </Reference>
@@ -991,4 +997,4 @@
   <Target Name="AfterBuild">
   </Target>
   -->
-</Project>
+</Project>

Assets/MathNet.Numerics.dll.meta

@@ -0,0 +1,33 @@
+fileFormatVersion: 2
+guid: 0af45205cc8dcb946a4e86a41bf43880
+PluginImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  iconMap: {}
+  executionOrder: {}
+  defineConstraints: []
+  isPreloaded: 0
+  isOverridable: 0
+  isExplicitlyReferenced: 0
+  validateReferences: 1
+  platformData:
+  - first:
+      Any: 
+    second:
+      enabled: 1
+      settings: {}
+  - first:
+      Editor: Editor
+    second:
+      enabled: 0
+      settings:
+        DefaultValueInitialized: true
+  - first:
+      Windows Store Apps: WindowsStoreApps
+    second:
+      enabled: 0
+      settings:
+        CPU: AnyCPU
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/StreetLight/PersonVisualizer.cs

@@ -21,9 +21,11 @@ public class PersonVisualizer : MonoBehaviour
         personManagerLazy = new Lazy<PersonManager>(FindObjectOfType<PersonManager>);
     }
 
+    GameObject[] spheres;
+
     void Start()
     {
-
+        spheres = new GameObject[] { GameObject.CreatePrimitive(PrimitiveType.Sphere), GameObject.CreatePrimitive(PrimitiveType.Sphere) };
     }
 
     float minX = int.MaxValue;
@@ -33,9 +35,10 @@ public class PersonVisualizer : MonoBehaviour
 
     void Update()
     {
+        int count = 0;
         foreach (var person in PersonManager.Persons)
         {
-            GameObject sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
+            GameObject sphere = spheres[count];
             sphere.transform.position = new Vector3(person.UnityPosition.x, person.UnityPosition.y, person.UnityPosition.z);
 
             //var camera = Camera.main;
@@ -50,6 +53,7 @@ public class PersonVisualizer : MonoBehaviour
             //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/Scripts/PersonManager.cs

@@ -1,6 +1,7 @@
 using Assets.StreetLight.Adapters;
 using Assets.StreetLight.Interfaces;
 using Assets.StreetLight.Poco;
+using System;
 using System.Collections;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
@@ -21,12 +22,20 @@ public class PersonManager : MonoBehaviour
         var cylinderDL = GameObject.Find("CylinderDL");
         var sphere = GameObject.Find("Sphere");
         // World to Unity Position
-        var calibrationVectors = new Dictionary<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 Vector3(2.538264f, -2.015675f, 2.318164f), new Vector3(cylinderUR.transform.position.x, cylinderUR.transform.position.y, cylinderUR.transform.position.z) },
-            { new Vector3(-1.551626f, -1.86093f, 2.275376f), new Vector3(cylinderDR.transform.position.x, cylinderDR.transform.position.y, cylinderDR.transform.position.z) },
-            { new Vector3(-1.095514f, -1.981828f, 2.849167f), new Vector3(cylinderDL.transform.position.x, cylinderDL.transform.position.y, cylinderDL.transform.position.z) },
-            { new Vector3(0.6326299f, -1.908295f, 2.349819f), new Vector3(cylinderDL.transform.position.x, cylinderDL.transform.position.y, cylinderDL.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);
@@ -38,6 +47,7 @@ public class PersonManager : MonoBehaviour
 
     private void PersonDetector_PersonsDetected(object sender, IEnumerable<Person> e)
     {
+        Persons.Clear();
         foreach (var person in e)
         {
             Persons.Add(person);

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -1,18 +1,83 @@
 using Assets.StreetLight.Interfaces;
 using System.Collections.Generic;
 using UnityEngine;
+using OpenCvSharp;
+using System;
+using Unity.VisualScripting;
+using Assets.StreetLight.Serialization;
+using MathNet;
+using MathNet.Numerics.LinearAlgebra;
+using MathNet.Numerics.LinearAlgebra.Double;
+using System.Linq.Expressions;
 
 internal class PositionCalculator : IPositionCalculator
 {
-    private Dictionary<Vector3, Vector3> calibrationVectors;
+    // World to Unity Position
+    private List<Tuple<Vector3, Vector3>> calibrationVectors;
 
-    public PositionCalculator(Dictionary<Vector3, Vector3> calibrationVectors)
+    private Matrix<double> lambda;
+
+    public PositionCalculator(List<Tuple<Vector3, Vector3>> calibrationVectors)
     {
         this.calibrationVectors = calibrationVectors;
+
+        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> 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}
+            }
+            );
+
+        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();
     }
 
     public Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
     {
+        var vector = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });
+
+        var output = lambda * vector;
+        var scaledOutput = output / output[2];
+
+        return new Vector3((float)scaledOutput[0], 0, (float)scaledOutput[1]);
         throw new System.NotImplementedException();
     }
+
+    private void testCalibration()
+    {
+        foreach(var c in calibrationVectors)
+        {
+            var test = DenseVector.OfArray(new double[] { c.Item1.x, c.Item1.z, 1 });
+            var testOutput = lambda * test;
+            var scaledTestOutput = testOutput / testOutput[2];
+        }
+    }
 }

ProjectSettings/ProjectSettings.asset

@@ -153,7 +153,8 @@ PlayerSettings:
   resolutionScalingMode: 0
   androidSupportedAspectRatio: 1
   androidMaxAspectRatio: 2.1
-  applicationIdentifier: {}
+  applicationIdentifier:
+    Standalone: com.DefaultCompany.StreetLight
   buildNumber:
     Standalone: 0
     iPhone: 0
@@ -617,7 +618,7 @@ PlayerSettings:
   managedStrippingLevel: {}
   incrementalIl2cppBuild: {}
   suppressCommonWarnings: 1
-  allowUnsafeCode: 0
+  allowUnsafeCode: 1
   useDeterministicCompilation: 1
   enableRoslynAnalyzers: 1
   additionalIl2CppArgs: 

packages.config

@@ -1,4 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <packages>
+  <package id="Emgu.TF" version="2.8.0.1418" targetFramework="net471" />
   <package id="EntityFramework" version="6.4.4" targetFramework="net471" />
+  <package id="MathNet.Numerics" version="5.0.0" targetFramework="net471" />
+  <package id="System.ValueTuple" version="4.4.0" targetFramework="net471" />
 </packages>