Cleanup code

Assembly-CSharp.csproj

@@ -120,7 +120,6 @@
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\MR\ZEDMirror.cs" />
     <Compile Include="Assets\ZED\Examples\Object Detection\Scripts\BBox2DHandler.cs" />
     <Compile Include="Assets\ZED\SDK\NativeInterface\ZEDCommon.cs" />
-    <Compile Include="Assets\StreetLight\HomographyCalculator.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\Utilities\ZEDSVOManager.cs" />
     <Compile Include="Assets\ZED\Tools\Mixed Reality Calibration\Scripts\ToggleGroup3D.cs" />
     <Compile Include="Assets\ZED\SDK\Helpers\Scripts\SkeletonTracking\RigBone.cs" />

Assets/StreetLight/HomographyCalculator.cs

@@ -1,44 +0,0 @@
-using MathNet.Numerics.LinearAlgebra;
-using MathNet.Numerics.LinearAlgebra.Complex;
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.IO;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
-
-namespace Assets.StreetLight
-{
-    internal class HomographyCalculator
-    {
-        internal Matrix<double> InverseMatrix(Matrix<double> matrix)
-        {
-            ProcessStartInfo start = new ProcessStartInfo
-            {
-                FileName = "cmd.exe",
-                //Arguments = "conda run -n HomographyCalculator python C:\\Users\\nick.steyer\\SmartStreetLight\\HomographyCalculator\\main.py",
-                UseShellExecute = false,// Do not use OS shell
-                CreateNoWindow = true, // We don't need new window
-                RedirectStandardInput = true,
-                RedirectStandardOutput = true,// Any output, generated by application will be redirected back
-                RedirectStandardError = true // Any error in standard output will be redirected back (for example exceptions)
-            };
-
-            using (Process process = Process.Start(start))
-            {
-                using (var input = process.StandardInput)
-                {
-                    input.WriteLine("C:\\Users\\nick.steyer\\Anaconda3\\Scripts\\activate.bat");
-                    input.WriteLine("conda run -n HomographyCalculator python C:\\Users\\nick.steyer\\SmartStreetLight\\HomographyCalculator\\main.py");
-                }
-
-                using StreamReader reader = process.StandardOutput;
-                string stderr = process.StandardError.ReadToEnd(); // Here are the exceptions from our Python script
-                string result = reader.ReadToEnd(); // Here is the result of StdOut(for example: print "test")
-            }
-
-            return null;
-        }
-    }
-}

Assets/StreetLight/HomographyCalculator.cs.meta

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

Assets/StreetLight/Scripts/PositionCalculator.cs

@@ -18,7 +18,7 @@ namespace Assets.StreetLight.Scripts
 {
     public class PositionCalculator
     {
-        private List<CalibrationPoint> calibrationVectors;
+        private readonly List<CalibrationPoint> calibrationVectors;
         private Matrix<double> homography;
 
         public PositionCalculator(List<CalibrationPoint> calibrationVectors)
@@ -28,27 +28,6 @@ namespace Assets.StreetLight.Scripts
             CalculateHomographyRansac();
         }
 
-        private List<HomographyCorrespondence> LoadSampleCorrespondences()
-        {
-            var p1 = File.ReadAllLines(Path.Combine(Application.streamingAssetsPath, "p1.csv"));
-            var p2 = File.ReadAllLines(Path.Combine(Application.streamingAssetsPath, "p2.csv"));
-
-            var correspondences = new List<HomographyCorrespondence>();
-
-            int length = p1.Length;
-
-            for (int i = 0; i < length; i++)
-            {
-                var p1Coordinates = p1[i].Split(',').Select(i => double.Parse(i, CultureInfo.InvariantCulture)).ToArray();
-                var p2Coordinates = p2[i].Split(',').Select(i => double.Parse(i, CultureInfo.InvariantCulture)).ToArray();
-                correspondences.Add(new HomographyCorrespondence(
-                    DenseVector.OfArray(p1Coordinates),
-                    DenseVector.OfArray(p2Coordinates)));
-            }
-
-            return correspondences;
-        }
-
         /// <summary>
         /// Uses RANSAC and an SVD to calculate the homography from many calibration points.
         /// </summary>
@@ -81,108 +60,6 @@ namespace Assets.StreetLight.Scripts
             }
 
             homography = DenseMatrix.OfArray(array);
-
-            return;
-            var correspondences = calibrationVectors.Select(v =>
-            new HomographyCorrespondence(DenseVector.OfArray(new double[] { v.WorldPosition.x, v.WorldPosition.z }), DenseVector.OfArray(new double[] { v.UnityPosition.x, v.UnityPosition.z }))).ToList();
-
-            correspondences = LoadSampleCorrespondences();
-
-            var iterations = RansacIterations(0.35f, 4, 0.99f);
-            var threshold = 5.0f;
-
-            var maxInliers = 0;
-            Matrix<double> H = Matrix<double>.Build.Random(0, 0);
-            List<HomographyCorrespondence> inliers = new List<HomographyCorrespondence>();
-
-            var random = new Random();
-            for (int i = 0; i < iterations; i++)
-            {
-                //var sample = correspondences.OrderBy(_ => random.Next()).Take(4).ToArray();
-                var indices = new[] { 96, 93, 63, 118 };
-                var sample = correspondences.Where((_, index) => indices.Contains(index)).ToArray();
-
-                var (conditionedCorrespondences, T1, T2) = ConditionPoints(sample);
-
-                var (currentH, HC) = ComputeHomography(conditionedCorrespondences, T1, T2);
-
-                var homographyDistances = ComputeHomographyDistances(currentH, correspondences);
-
-                var currentInliers = FindInliers(correspondences, homographyDistances, threshold);
-                var numberOfCurrentInliers = currentInliers.Count;
-
-                if (currentInliers.Count > maxInliers)
-                {
-                    H = currentH;
-                    maxInliers = currentInliers.Count;
-                    inliers = currentInliers;
-                }
-            }
-
-            var recomputedHomography = RecomputeHomography(inliers);
-
-            homography = recomputedHomography;
-        }
-
-        private Matrix<double> RecomputeHomography(List<HomographyCorrespondence> inliers)
-        {
-            var (conditionedPoints, T1, T2) = ConditionPoints(inliers);
-            var (H, HC) = ComputeHomography(conditionedPoints, T1, T2);
-            return H;
-        }
-
-        private List<HomographyCorrespondence> FindInliers(IList<HomographyCorrespondence> correspondences, IList<double> homographyDistances, float threshold)
-        {
-            var inliers = new List<HomographyCorrespondence>();
-
-            for (int i = 0; i < correspondences.Count; i++)
-            {
-                var distance = homographyDistances[i];
-                if (distance < threshold)
-                {
-                    inliers.Add(correspondences[i]);
-                }
-            }
-
-            return inliers;
-        }
-
-        private IList<double> ComputeHomographyDistances(Matrix<double> currentH, IEnumerable<HomographyCorrespondence> correspondences)
-        {
-            var distances = new List<double>();
-            currentH.MapInplace(q => Math.Round(q, 15));
-            var inverseH = currentH.Inverse();
-
-            var matrixString = string.Join(Environment.NewLine, currentH.EnumerateRows().Select(row => string.Join(" ", row.Enumerate().Select(number => number.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)))));
-            File.WriteAllText(@"C:\Users\Nick\Desktop\currentH.csv", matrixString);
-
-            var calc = new HomographyCalculator();
-            //var foo = calc.InverseMatrix(currentH);
-
-            foreach (var correspondence in correspondences)
-            {
-                var x1 = correspondence.WorldPosition;
-                var x2 = correspondence.UnityPosition;
-                var x1Transformed = TransformPoint(x1, currentH);
-                var x2Transformed = TransformPoint(x2, inverseH);
-                distances.Add(Math.Pow((x1Transformed - x2).L1Norm(), 2) + Math.Pow((x1 - x2Transformed).L1Norm(), 2));
-            }
-
-            return distances;
-        }
-
-        private Vector<double> TransformPoint(Vector<double> point, Matrix<double> homography)
-        {
-            if (point.Count != 2 || homography.RowCount != 3 || homography.ColumnCount != 3)
-            {
-                throw new ArgumentException();
-            }
-
-            var homogeneousPoint = DenseVector.OfArray(new double[] { point[0], point[1], 1 });
-            var transformedHomogeneousPoint = homography * homogeneousPoint;
-            var normalizedTransformedHomogeneousPoint = transformedHomogeneousPoint / transformedHomogeneousPoint[2];
-            var transformedPoint = DenseVector.OfArray(new double[] { normalizedTransformedHomogeneousPoint[0], normalizedTransformedHomogeneousPoint[1] });
-            return transformedPoint;
         }
 
         private (Matrix<double>, Matrix<double>) ComputeHomography(ICollection<(Vector<double>, Vector<double>)> conditionedCorrespondences, Matrix<double> t1, Matrix<double> t2)
@@ -218,103 +95,6 @@ namespace Assets.StreetLight.Scripts
             return (normalizedH, normalizedHC);
         }
 
-        private (ICollection<(Vector<double>, Vector<double>)>, Matrix<double>, Matrix<double>) ConditionPoints(IEnumerable<HomographyCorrespondence> correspondences)
-        {
-            var worldPoints = correspondences.Select(i => i.WorldPosition);
-            var sx = worldPoints.Select(i => i[0]).Max(i => i) / 2;
-            var sy = worldPoints.Select(i => i[1]).Max(i => i) / 2;
-            var meanX = worldPoints.Select(i => i[0]).Mean();
-            var meanY = worldPoints.Select(i => i[1]).Mean();
-
-            var T = DenseMatrix.OfArray(new double[,]
-            {
-                {1 / sx, 0, - meanX / sx},
-                {0, 1 / sy, - meanY / sy},
-                {0, 0, 1}
-            });
-
-            var unityPoints = correspondences.Select(i => i.UnityPosition);
-            var sx_prime = unityPoints.Select(i => i[0]).Max(i => i) / 2;
-            var sy_prime = unityPoints.Select(i => i[1]).Max(i => i) / 2;
-            var meanX_prime = unityPoints.Select(i => i[0]).Mean();
-            var meanY_prime = unityPoints.Select(i => i[1]).Mean();
-
-            var T_prime = DenseMatrix.OfArray(new double[,]
-            {
-                {1 / sx_prime, 0, - meanX_prime / sx_prime},
-                {0, 1 / sy_prime, - meanY_prime / sy_prime},
-                {0, 0, 1}
-            });
-
-            var result = new List<(Vector<double>, Vector<double>)>();
-            foreach (var correspondence in correspondences)
-            {
-                var homogeneousPointWorld = DenseVector.OfArray(new double[] { correspondence.WorldPosition[0], correspondence.WorldPosition[1], 1 });
-                var homogeneousTransformedPointWorld = T * homogeneousPointWorld;
-
-                var homogeneousPointUnity = DenseVector.OfArray(new double[] { correspondence.UnityPosition[0], correspondence.UnityPosition[1], 1 });
-                var homogeneousTransformedPointUnity = T_prime * homogeneousPointUnity;
-
-                //if (new[] { homogeneousTransformedPointUnity[1], homogeneousTransformedPointUnity[0], homogeneousTransformedPointWorld[1], homogeneousTransformedPointWorld[0] }.Any(i => i < -1 || i > 1))
-                //{
-
-                //}
-
-                result.Add((homogeneousTransformedPointWorld, homogeneousTransformedPointUnity));
-            }
-
-            return (result, T, T_prime);
-        }
-
-        private int RansacIterations(float inlierProbability, int samplesPerIteration, float successProbability)
-        {
-            return (int)Math.Ceiling(Math.Log(1 - successProbability, 1 - Math.Pow(inlierProbability, samplesPerIteration)));
-        }
-
-        private void CalculateHomographySimple()
-        {
-            if (!(calibrationVectors?.Count >= 4))
-            {
-                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
-            }
-
-            var cv = calibrationVectors;
-
-            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[,]
-            {
-                {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;
-            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();
-        }
-
         public Vector3 CalculateUnityPosition(Person person)
         {
             return WorldPositionToUnityPosition(person.WorldPosition);
@@ -322,26 +102,13 @@ namespace Assets.StreetLight.Scripts
 
         private Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
         {
-            var vector = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });
+            var homogeneousWorldPosition = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });
 
-            var output = homography * vector;
+            var output = homography * homogeneousWorldPosition;
             var scaledOutput = output / output[2];
             var resultVector = new Vector3((float)scaledOutput[0], 0, (float)scaledOutput[1]);
 
             return resultVector;
         }
-
-        /// <summary>
-        /// For internal debugging only.
-        /// </summary>
-        private void testCalibration()
-        {
-            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];
-            }
-        }
     }
 }