StreetLight/Assets/StreetLight/Scripts/PositionCalculator.cs

using Assets.StreetLight.Poco;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using UnityEngine;

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

        public PositionCalculator(double[,] homographyArray)
        {
            homography = DenseMatrix.OfArray(homographyArray);
        }

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

            var output = homography * homogeneousWorldPosition;
            var scaledOutput = output / output[2];
            var resultVector = new Vector3((float)scaledOutput[0], 0, (float)scaledOutput[1]);

            return resultVector;
        }
    }
}