public void Camera_Pose_And_Triangulation_From_Fundamental()
{
Matrix <double> p;
Matrix <double> p1;
Matrix <double> e;
CameraPoseAndTriangulationFromFundamentalResult result;
#region Case 1
p = new Matrix <double>(new double[3, 4]
{
{ 1, 0, 0, 0 },
{ 0, 1, 0, 0 },
{ 0, 0, 1, 0 }
});
p1 = new Matrix <double>(new double[3, 4]
{
{ 0.9870124281645626, 0.03547270126454876, -0.1566785055892807, -0.0008631055249466995 },
{ -0.03193350943559628, 0.9991760497758493, 0.0250494017784099, -0.02589733540347029 },
{ 0.1574379802456221, -0.01972077633678765, 0.9873319468936744, -0.9996642351649142 }
});
e = new Matrix <double>(new double[3, 3]
{
{ -0.4052652752518787, 12.45233689948151, -0.01348616168894345 },
{ -11.47229370932243, -0.4564241889679639, 1.831549257782737 },
{ 0.297551532077682, 0.001072849756781113, -0.04743653296950803 }
});
var p1Init = new Matrix <double>(3, 4);
p1Init.SetIdentity();
result = OpenCvUtilities.CameraPoseAndTriangulationFromFundamental(_calibration, Utils.GetKeyPointsVector(_trackedFeatures2), Utils.GetKeyPointsVector(_bootstrapKp), p1Init);
Assert.AreEqual(196.532, result.Min, 0.1);
Assert.AreEqual(428.096, result.Max, 0.1);
CollectionAssert.AreEqual(e.Data, result.Esential.Data, _matrixComparer);
CollectionAssert.AreEqual(p.Data, result.P1.Data, _matrixComparer);
CollectionAssert.AreEqual(p1.Data, result.P2.Data, _matrixComparer);
Assert.That(result.Result, Is.EqualTo(true));
#endregion
}
public void Bootstrap_Track_Logic_Test()
{
var capture = new Capture($@"{TestCaseProjectPath}\Videos\cube2.avi");
//var capture = new Capture(@"C:\Users\zakharov\Documents\Repos\Mine\Rc\src\RubiksCube.OpenCV.TestCase\Videos\cube2.avi");
for (int i = 0; i < 40; i++)
{
capture.QueryFrame();
}
var prevGray = capture.QueryFrame();
CvInvoke.CvtColor(prevGray, prevGray, ColorConversion.Bgr2Gray);
var currentGray = capture.QueryFrame();
CvInvoke.CvtColor(currentGray, currentGray, ColorConversion.Bgr2Gray);
var bootstrapKp = new VectorOfKeyPoint();
new ORBDetector().DetectRaw(prevGray, bootstrapKp);
var trackedFeatures = new VectorOfKeyPoint(bootstrapKp.ToArray());
//-------------------------------------------------------------------------
var pointComparer = Comparer <PointF> .Create((p1, p2) => Math.Abs(p1.X - p2.X) < 0.0001f && Math.Abs(p1.Y - p2.Y) < 0.0001f? 0 : 1);
var point3DComparer = Comparer <MCvPoint3D32f> .Create((p1, p2) => Math.Abs(p1.X - p2.X) < 0.0001f && Math.Abs(p1.Y - p2.Y) < 0.0001f && Math.Abs(p1.Z - p2.Z) < 0.0001f? 0 : 1);
var matrixComparer = Comparer <double> .Create((x, y) => Math.Abs(x - y) < 0.0001f? 0 : 1);
for (int i = 41; i <= 95; i++)
{
var bootstrapPointsBeforeOpticalFlowCplusPlus = GetPoints($"I = {i}txt - Bootstrap points before optical flow.txt");
var trackedPointsBeforeOpticalFlowCplusPlus = GetPoints($"I = {i}txt - Tracked points before optical flow.txt");
var bootstrapPointsAfterOpticalFlowCplusPlus = GetPoints($"I = {i}txt - Bootstrap points after optical flow.txt");
var trackedPointsAfterOpticalFlowCplusPlus = GetPoints($"I = {i}txt - Tracked points after optical flow.txt");
var bootstrapPointsAfterHomographyCplusPlus = GetPoints($"I = {i}txt - Bootstrap points after homography.txt");
var trackedPointsAfterHomographyCplusPlus = GetPoints($"I = {i}txt - Tracked points after homography.txt");
var homographyCplusPlus = Getmatrix3X3($"I = {i}txt - Homography.txt");
var homographyMaskCplusPlus = GetByteVector($"I = {i}txt - Homography mask.txt");
var corners = new VectorOfPointF();
var status = new VectorOfByte();
var errors = new VectorOfFloat();
CollectionAssert.AreEqual(Utils.GetPointsVector(bootstrapKp).ToArray(), bootstrapPointsBeforeOpticalFlowCplusPlus.ToArray(), pointComparer);
CollectionAssert.AreEqual(Utils.GetPointsVector(trackedFeatures).ToArray(), trackedPointsBeforeOpticalFlowCplusPlus.ToArray(), pointComparer);
CvInvoke.CalcOpticalFlowPyrLK(prevGray, currentGray, Utils.GetPointsVector(trackedFeatures), corners,
status, errors, new Size(11, 11), 3, new MCvTermCriteria(30, 0.01));
currentGray.CopyTo(prevGray);
if (CvInvoke.CountNonZero(status) < status.Size * 0.8)
{
throw new Exception("Tracking failed.");
}
trackedFeatures = Utils.GetKeyPointsVector(corners);
Utils.KeepVectorsByStatus(ref trackedFeatures, ref bootstrapKp, status);
CollectionAssert.AreEqual(Utils.GetPointsVector(bootstrapKp).ToArray(), bootstrapPointsAfterOpticalFlowCplusPlus.ToArray(), pointComparer);
CollectionAssert.AreEqual(Utils.GetPointsVector(trackedFeatures).ToArray(), trackedPointsAfterOpticalFlowCplusPlus.ToArray(), pointComparer);
if (trackedFeatures.Size != bootstrapKp.Size)
{
const string error = "Tracked features vector size is not equal to bootstrapped one.";
throw new Exception(error);
}
//verify features with a homography
var inlierMask = new VectorOfByte();
var homography = new Mat();
if (trackedFeatures.Size > 4)
{
CvInvoke.FindHomography(Utils.GetPointsVector(trackedFeatures), Utils.GetPointsVector(bootstrapKp), homography, HomographyMethod.Ransac, 0.99,
inlierMask);
}
var homographyMatrix = new Matrix <double>(homography.Rows, homography.Cols, homography.DataPointer);
CollectionAssert.AreEqual(homographyMatrix.Data, homographyCplusPlus.Data, matrixComparer);
int inliersNum = CvInvoke.CountNonZero(inlierMask);
CollectionAssert.AreEqual(inlierMask.ToArray(), homographyMaskCplusPlus.ToArray());
if (inliersNum != trackedFeatures.Size && inliersNum >= 4 && !homography.IsEmpty)
{
Utils.KeepVectorsByStatus(ref trackedFeatures, ref bootstrapKp, inlierMask);
}
else if (inliersNum < 10)
{
throw new Exception("Not enough features survived homography.");
}
CollectionAssert.AreEqual(Utils.GetPointsVector(bootstrapKp).ToArray(), bootstrapPointsAfterHomographyCplusPlus.ToArray(), pointComparer);
CollectionAssert.AreEqual(Utils.GetPointsVector(trackedFeatures).ToArray(), trackedPointsAfterHomographyCplusPlus.ToArray(), pointComparer);
var bootstrapKpOrig = new VectorOfKeyPoint(bootstrapKp.ToArray());
var trackedFeaturesOrig = new VectorOfKeyPoint(trackedFeatures.ToArray());
//TODO: Compare all these to c++ version
//Attempt at 3D reconstruction (triangulation) if conditions are right
var rigidT = CvInvoke.EstimateRigidTransform(Utils.GetPointsVector(trackedFeatures).ToArray(), Utils.GetPointsVector(bootstrapKp).ToArray(), false);
var matrix = new Matrix <double>(rigidT.Rows, rigidT.Cols, rigidT.DataPointer);
if (CvInvoke.Norm(matrix.GetCol(2)) > 100)
{
var points3DCplusPlus = GetPoints3d($"I = {i}txt - 3d points.txt");
var eigenvectorsCplusPlus = Getmatrix3X3($"I = {i}txt - eigenvectors.txt");
var normalOfPlaneCplusPlus = GetDoubleArray($"I = {i}txt - normal of plane.txt");
//camera motion is sufficient
var p1Init = new Matrix <double>(3, 4);
p1Init.SetIdentity();
var result = OpenCvUtilities.CameraPoseAndTriangulationFromFundamental(_calibration, trackedFeatures, bootstrapKp, p1Init);
trackedFeatures = result.FilteredTrackedFeaturesKp;
bootstrapKp = result.FilteredBootstrapKp;
if (result.Result)
{
double pToPlaneTrashCplusPlus = GetDouble($"I = {i}txt - p_to_plane_thresh.txt");
int numInliersCplusPlus = GetInt($"I = {i}txt - num inliers.txt");
var statusArrCplusPlus = GetByteVector($"I = {i}txt - status arr.txt");
var trackedFeatures3D = result.TrackedFeatures3D;
CollectionAssert.AreEqual(trackedFeatures3D.ToArray(), points3DCplusPlus.ToArray(), point3DComparer);
//var trackedFeatures3Dm = Utils.Get3dPointsMat(trackedFeatures3D);
var tf3D = new double[trackedFeatures3D.Size, 3];
var trackedFeatures3Dm = new Matrix <double>(trackedFeatures3D.Size, 3);
for (int k = 0; k < trackedFeatures3D.Size; k++)
{
trackedFeatures3Dm[k, 0] = trackedFeatures3D[k].X;
trackedFeatures3Dm[k, 1] = trackedFeatures3D[k].Y;
trackedFeatures3Dm[k, 2] = trackedFeatures3D[k].Z;
tf3D[k, 0] = trackedFeatures3D[k].X;
tf3D[k, 1] = trackedFeatures3D[k].Y;
tf3D[k, 2] = trackedFeatures3D[k].Z;
}
var eigenvectors = new Mat();
var mean = new Mat();
CvInvoke.PCACompute(trackedFeatures3Dm, mean, eigenvectors);
var eigenvectorsMatrix = new Matrix <double>(eigenvectors.Rows, eigenvectors.Cols, eigenvectors.DataPointer);
CollectionAssert.AreEqual(eigenvectorsMatrix.Data, eigenvectorsCplusPlus.Data, matrixComparer);
var method = PrincipalComponentMethod.Center;
var pca = new PrincipalComponentAnalysis(method);
pca.Learn(tf3D.ToJagged());
var meanMatrix = new Matrix <double>(mean.Rows, mean.Cols, mean.DataPointer);
CollectionAssert.AreEqual(meanMatrix.Data.ToJagged()[0], pca.Means, matrixComparer);
int numInliers = 0;
var normalOfPlane = eigenvectorsMatrix.GetRow(2).ToUMat().ToMat(AccessType.Fast);
CvInvoke.Normalize(normalOfPlane, normalOfPlane);
var normalOfPlaneMatrix = new Matrix <double>(normalOfPlane.Rows, normalOfPlane.Cols, normalOfPlane.DataPointer);
var normalOfPlaneArray = new[] { normalOfPlaneMatrix[0, 0], normalOfPlaneMatrix[0, 1], normalOfPlaneMatrix[0, 2] };
CollectionAssert.AreEqual(normalOfPlaneArray, normalOfPlaneCplusPlus, matrixComparer);
double pToPlaneThresh = Math.Sqrt(pca.Eigenvalues.ElementAt(2));
Assert.AreEqual(pToPlaneTrashCplusPlus, pToPlaneThresh, 0.0001);
var statusArray = new byte[trackedFeatures3D.Size];
for (int k = 0; k < trackedFeatures3D.Size; k++)
{
var t1 = new double[] { trackedFeatures3D[k].X, trackedFeatures3D[k].Y, trackedFeatures3D[k].Z };
var t2 = t1.Subtract(pca.Means);
var w = new Matrix <double>(new[, ] {
{ t2[0], t2[1], t2[2] }
});
double d = Math.Abs(normalOfPlane.Dot(w));
if (d < pToPlaneThresh)
{
numInliers++;
statusArray[k] = 1;
}
}
Assert.AreEqual(numInliersCplusPlus, numInliers);
var statusVector = new VectorOfByte(statusArray);
CollectionAssert.AreEqual(statusArrCplusPlus.ToArray(), statusVector.ToArray());
bool bootstrapping = numInliers / (double)trackedFeatures3D.Size < 0.75;
if (!bootstrapping)
{
//enough features are coplanar, keep them and flatten them on the XY plane
Utils.KeepVectorsByStatus(ref trackedFeatures, ref trackedFeatures3D, statusVector);
//the PCA has the major axes of the plane
var projected = new Mat();
CvInvoke.PCAProject(trackedFeatures3Dm, mean, eigenvectors, projected);
var projectedMatrix = new Matrix <double>(projected.Rows, projected.Cols, projected.DataPointer);
projectedMatrix.GetCol(2).SetValue(0);
projectedMatrix.CopyTo(trackedFeatures3Dm);
}
else
{
bootstrapKp = bootstrapKpOrig;
trackedFeatures = trackedFeaturesOrig;
}
}
}
currentGray = capture.QueryFrame();
CvInvoke.CvtColor(currentGray, currentGray, ColorConversion.Bgr2Gray);
}
}