StreetLight/Assets/StreetLight/Scripts/PositionCalculator.cs

using Assets.StreetLight.Interfaces;
using Assets.StreetLight.Poco;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using MathNet.Numerics.Statistics;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Security.Cryptography;
using UnityEngine;
using Random = System.Random;

namespace Assets.StreetLight.Scripts
{
    public class PositionCalculator
    {
        private List<CalibrationPoint> calibrationVectors;
        private Matrix<double> homography;

        public PositionCalculator(List<CalibrationPoint> calibrationVectors)
        {
            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 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();

            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 (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>();
            var inverseH = currentH.Inverse();

            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)
        {
            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);
        }

        private (ICollection<(Vector<double>, Vector<double>)>, Matrix<double>, Matrix<double>) ConditionPoints(IEnumerable<HomographyCorrespondence> correspondences)
        {
            var worldPoints = correspondences.Select(i => i.WorldPosition);
            var s = worldPoints.Select(i => i.Count).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 / s, 0, - meanX / s},
                {0, 1 / s, - meanY / s},
                {0, 0, 1}
            });

            var unityPoints = correspondences.Select(i => i.UnityPosition);
            var s_prime = unityPoints.Select(i => i.Count).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 / s_prime, 0, - meanX_prime / s_prime},
                {0, 1 / s_prime, - meanY_prime / s_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);
        }

        private Vector3 WorldPositionToUnityPosition(Vector3 worldPosition)
        {
            var vector = DenseVector.OfArray(new double[] { worldPosition.x, worldPosition.z, 1 });

            var output = homography * vector;
            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];
            }
        }
    }
}