StreetLight/Assets/StreetLight/Scripts/PositionCalculator.cs

using Assets.StreetLight.Interfaces;
using Assets.StreetLight.Poco;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
using System.Collections.Generic;
using UnityEngine;

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

        public PositionCalculator(List<CalibrationPoint> calibrationVectors)
        {
            this.calibrationVectors = calibrationVectors;

            CalculateHomographySimple();
        }

        private void CalculateHomographySVD()
        {
            if (!(calibrationVectors?.Count >= 4))
            {
                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
            }
        }

        private void CalculateHomographyRansac()
        {
            if (!(calibrationVectors?.Count >= 4))
            {
                throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
            }

            // TODO: RANSAC with conditioning
            var inlierThreshold = 5.0;
            var inlierProbability = 0.25;
            var samplesPerInteration = 4;
            var confidence = 0.99;

            throw new NotImplementedException();
        }

        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];
            }
        }
    }
}