|
@@ -20,14 +20,41 @@ namespace Assets.StreetLight.Scripts
|
|
CalculateHomographySimple();
|
|
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()
|
|
private void CalculateHomographySimple()
|
|
{
|
|
{
|
|
- if (calibrationVectors?.Count >= 4)
|
|
|
|
|
|
+ if (!(calibrationVectors?.Count >= 4))
|
|
{
|
|
{
|
|
- var cv = calibrationVectors;
|
|
|
|
|
|
+ throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ var cv = calibrationVectors;
|
|
|
|
|
|
- Matrix<double> matrixA = DenseMatrix.OfArray(new double[,]
|
|
|
|
- {
|
|
|
|
|
|
+ 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},
|
|
{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},
|
|
{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},
|
|
{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},
|
|
@@ -36,10 +63,10 @@ namespace Assets.StreetLight.Scripts
|
|
{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},
|
|
{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},
|
|
{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}
|
|
{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[,]
|
|
|
|
- {
|
|
|
|
|
|
+ Matrix<double> matrixB = DenseMatrix.OfArray(new double[,]
|
|
|
|
+ {
|
|
{cv[0].UnityPosition.x},
|
|
{cv[0].UnityPosition.x},
|
|
{cv[0].UnityPosition.z},
|
|
{cv[0].UnityPosition.z},
|
|
{cv[1].UnityPosition.x},
|
|
{cv[1].UnityPosition.x},
|
|
@@ -48,22 +75,17 @@ namespace Assets.StreetLight.Scripts
|
|
{cv[2].UnityPosition.z},
|
|
{cv[2].UnityPosition.z},
|
|
{cv[3].UnityPosition.x},
|
|
{cv[3].UnityPosition.x},
|
|
{cv[3].UnityPosition.z}
|
|
{cv[3].UnityPosition.z}
|
|
- });
|
|
|
|
|
|
+ });
|
|
|
|
|
|
- var lambda = (matrixA.Transpose() * matrixA).Inverse() * matrixA.Transpose() * matrixB;
|
|
|
|
- homography = DenseMatrix.OfArray(new double[,]
|
|
|
|
- {
|
|
|
|
|
|
+ var lambda = (matrixA.Transpose() * matrixA).Inverse() * matrixA.Transpose() * matrixB;
|
|
|
|
+ homography = DenseMatrix.OfArray(new double[,]
|
|
|
|
+ {
|
|
{ lambda[0,0], lambda[1,0], lambda[2,0]},
|
|
{ lambda[0,0], lambda[1,0], lambda[2,0]},
|
|
{ lambda[3,0], lambda[4,0], lambda[5,0]},
|
|
{ lambda[3,0], lambda[4,0], lambda[5,0]},
|
|
{ lambda[6,0], lambda[7,0], 1}
|
|
{ lambda[6,0], lambda[7,0], 1}
|
|
- });
|
|
|
|
|
|
+ });
|
|
|
|
|
|
- testCalibration();
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- throw new InvalidOperationException("Must have at least 4 correspondences to calculate a homography.");
|
|
|
|
- }
|
|
|
|
|
|
+ testCalibration();
|
|
}
|
|
}
|
|
|
|
|
|
public Vector3 CalculateUnityPosition(Person person)
|
|
public Vector3 CalculateUnityPosition(Person person)
|