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