StreetLight/Assets/StreetLight/Scripts/PositionCalculator.cs

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
{
    // World to Unity Position
    private List<Tuple<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];
        }
    }
}