123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347 |
- using Assets.StreetLight.Interfaces;
- using Assets.StreetLight.Poco;
- using MathNet.Numerics.LinearAlgebra;
- using MathNet.Numerics.LinearAlgebra.Double;
- using MathNet.Numerics.Statistics;
- using Newtonsoft.Json;
- using System;
- using System.Collections.Generic;
- using System.Globalization;
- using System.IO;
- using System.Linq;
- using System.Security.Cryptography;
- using System.Threading;
- 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();
- }
- private List<HomographyCorrespondence> LoadSampleCorrespondences()
- {
- var p1 = File.ReadAllLines(Path.Combine(Application.streamingAssetsPath, "p1.csv"));
- var p2 = File.ReadAllLines(Path.Combine(Application.streamingAssetsPath, "p2.csv"));
- var correspondences = new List<HomographyCorrespondence>();
- int length = p1.Length;
- for (int i = 0; i < length; i++)
- {
- var p1Coordinates = p1[i].Split(',').Select(i => double.Parse(i, CultureInfo.InvariantCulture)).ToArray();
- var p2Coordinates = p2[i].Split(',').Select(i => double.Parse(i, CultureInfo.InvariantCulture)).ToArray();
- correspondences.Add(new HomographyCorrespondence(
- DenseVector.OfArray(p1Coordinates),
- DenseVector.OfArray(p2Coordinates)));
- }
- return correspondences;
- }
- /// <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 lines = calibrationVectors.Select(v => $"{v.WorldPosition.x.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)} {v.WorldPosition.z.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)} {v.UnityPosition.x.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)} {v.UnityPosition.z.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)}");
- var pairsString = string.Join(Environment.NewLine, lines);
- File.WriteAllText(@"C:\Git\git.tk.informatik.tu-darmstadt.de\StreetLight\Assets\StreamingAssets\pairs.csv", pairsString);
- var homographyString = File.ReadAllLines(@"C:\Git\git.tk.informatik.tu-darmstadt.de\StreetLight\Assets\StreamingAssets\homography.csv");
- var values = homographyString.Select(l => l.Split(' ').Select(s => double.Parse(s, CultureInfo.InvariantCulture)).ToArray()).ToArray();
- double[,] array = new double[3, 3];
- for (int i = 0; i < 3; i++)
- {
- for (int j = 0; j < 3; j++)
- {
- array[i, j] = values[i][j];
- }
- }
- homography = DenseMatrix.OfArray(array);
- return;
- 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();
- correspondences = LoadSampleCorrespondences();
- 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 indices = new[] { 96, 93, 63, 118 };
- var sample = correspondences.Where((_, index) => indices.Contains(index)).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>();
- currentH.MapInplace(q => Math.Round(q, 15));
- var inverseH = currentH.Inverse();
- var matrixString = string.Join(Environment.NewLine, currentH.EnumerateRows().Select(row => string.Join(" ", row.Enumerate().Select(number => number.ToString("0." + new string('#', 50), CultureInfo.InvariantCulture)))));
- File.WriteAllText(@"C:\Users\Nick\Desktop\currentH.csv", matrixString);
- var calc = new HomographyCalculator();
- //var foo = calc.InverseMatrix(currentH);
- 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 sx = worldPoints.Select(i => i[0]).Max(i => i) / 2;
- var sy = worldPoints.Select(i => i[1]).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 / sx, 0, - meanX / sx},
- {0, 1 / sy, - meanY / sy},
- {0, 0, 1}
- });
- var unityPoints = correspondences.Select(i => i.UnityPosition);
- var sx_prime = unityPoints.Select(i => i[0]).Max(i => i) / 2;
- var sy_prime = unityPoints.Select(i => i[1]).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 / sx_prime, 0, - meanX_prime / sx_prime},
- {0, 1 / sy_prime, - meanY_prime / sy_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];
- }
- }
- }
- }
|