public static double Test6_Bunny_PCA(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
string path = AppDomain.CurrentDomain.BaseDirectory + "TestData";
Model model3DTarget = new Model(path + "\\bunny.obj");
myPCLTarget = model3DTarget.PointCloudVertices;
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = new Matrix3d();
//ICP converges with a rotation of
//R = R.RotationXYZ(60, 60, 60);
R = R.RotationXYZDegrees(124, 124, 124);
PointCloudVertices.Rotate(myPCLSource, R);
PCA pca = new PCA();
myPCLResult = pca.AlignPointClouds_SVD(myPCLSource, myPCLTarget);
if (pca.MeanDistance > 1e-5)
{
myPCLResult = pca.AlignPointClouds_SVD(myPCLResult, myPCLTarget);
}
//myPCLResult = pca.AlignPointClouds_SVD(myPCLResult, myPCLTarget);
//myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLResult, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test6_Bunny_ExpectedError(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
string path = AppDomain.CurrentDomain.BaseDirectory + "TestData";
Model model3DTarget = new Model(path + "\\bunny.obj");
myPCLTarget = model3DTarget.PointCloudVertices;
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = new Matrix3d();
//ICP converges with a rotation of
//R = R.RotationXYZ(60, 60, 60);
R = R.RotationXYZRadiants(65, 65, 65);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test7_Face_KnownTransformation_PCA_15000(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
string path = AppDomain.CurrentDomain.BaseDirectory + "Models\\UnitTests";
Model model3DTarget = new Model(path + "\\KinectFace_1_15000.obj");
myPCLTarget = model3DTarget.PointCloudVertices;
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.ScaleByFactor(myPCLSource, 0.9f);
Matrix3d R = new Matrix3d();
R = R.RotationXYZDegrees(60, 60, 60);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.Translate(myPCLSource, 0.3f, 0.5f, -0.4f);
PCA pca = new PCA();
myPCLResult = pca.AlignPointClouds_SVD(myPCLSource, myPCLTarget);
myPCLSource = myPCLResult;
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_CubeRotate45(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloud.CreateCube_RandomPointsOnPlanes(cubeSize, 10).ToPointCloudVertices();
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = CreateAndPrintMatrix(45, 45, 45);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_CubeRotate(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(50);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = new Matrix3d();
R = R.RotationXYZDegrees(90, 124, -274);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test2_RotationX30Degrees(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
//myPCLTarget = Vertices.CreateSomePoints();
myPCLTarget = PointCloudVertices.CreateCube_Corners(50);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = Matrix3d.CreateRotationX(30);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public PointCloudVertices AlignPointClouds_OneVector(PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget, int vectorNumberSource, int vectorNumberTarget)
{
//-------------------
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
//second object:
//-----------
pointCloudTargetCentered = CalculatePCA_Internal(pointCloudTarget);
//Vector3d v = TargetPCVectors[vectorNumberTarget];
//v.X = -v.X;
//v.Y = -v.Y;
//v.Z = -v.Z;
//TargetPCVectors[vectorNumberTarget] = v;
Matrix3d R = new Matrix3d();
//R = R.RotationOneVectorToAnother(TargetPCVectors[vectorNumber], SourcePCVectors[vectorNumber]);
R = R.RotationOneVertexToAnother(pointCloudSource.PCAAxes[vectorNumberSource], pointCloudTarget.PCAAxes[vectorNumberTarget]);
//R.CheckRotationMatrix();
//
//test:
//Vector3d testV = R.MultiplyVector(sourceV);
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloudVertices.Rotate(pointCloudResult, R);
PointCloudVertices.AddVector(pointCloudResult, pointCloudTarget.CentroidVector);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
System.Diagnostics.Debug.WriteLine("-->> PCA (V) - Mean Distance : " + MeanDistance.ToString("0.000000"));
return(pointCloudResult);
}
public static double Test5_Cube8RotateShuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(cubeSize);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = CreateAndPrintMatrix(45, 45, 45);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ShuffleTest(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test10_Cube26p_RotateShuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_RegularGrid_Empty(cubeSize, 2);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = CreateAndPrintMatrix(65, -123, 35);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ShuffleRandom(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
////}
///// <summary>
///// PCA are center of mass - centered
///// </summary>
///// <param name="pointCloud"></param>
///// <param name="myCentroid"></param>
//private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered)
//{
// pointCloud.CentroidPCA = Centroid;
// pointCloud.PCAAxes = new PointCloud();
// for (int i = 0; i < 3; i++)
// {
// Vector3d v = VT.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV[i];
// double d = v.Length;
// Vertex ve = new Vertex(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
////}
////}
////}
///// <summary>
///// PCA are center of mass - centered
///// </summary>
///// <param name="pointCloud"></param>
///// <param name="myCentroid"></param>
//private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered)
//{
// pointCloud.CentroidPCA = Centroid;
// pointCloud.PCAAxes = new PointCloud();
// for (int i = 0; i < 3; i++)
// {
// Vector3d v = VT_NotNormalized.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV_NotNormalized[i];
// double d = v.Length;
// Vertex ve = new Vertex(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
//}
private static PointCloudVertices CalculateResults(Matrix3d Ub, Matrix3d Ua, PointCloudVertices pointCloudSource, Vector3d centroidB, Vector3d centroidA)
{
Matrix3d R;
Matrix3d.Mult(ref Ub, ref Ua, out R);
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.Rotate(pointCloudResult, R);
Vector3d t = centroidB - R.MultiplyVector(centroidA);
//Vertices.AddVector(pointCloudResult, t);
return(pointCloudResult);
}
public static double Test10_Cube8pRotateTranslateScaleShuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_RegularGrid_Empty(cubeSize, 1);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.Translate(myPCLSource, cubeSize * 1.2f, -cubeSize * 2.5f, cubeSize * 2);
PointCloudVertices.ScaleByFactor(myPCLSource, 0.2f);
Matrix3d R = CreateAndPrintMatrix(65, -123, 35);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ShuffleTest(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_Cube8TranslateRotateShuffleNew(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(cubeSize);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
//Model3D myModel = Example3DModels.Cuboid("Cuboid", 20f, 40f, 100, System.Drawing.Color.White, null);
Matrix3d R = CreateAndPrintMatrix(65, -123, 35);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.Translate(myPCLSource, cubeSize * 1.2f, -cubeSize * 2.5f, cubeSize * 2);
PointCloudVertices.ShuffleTest(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test8_CubeOutliers_Rotate(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
//myPCLTarget = Vertices.CreateCube_Corners(50);
Model myModel = Example3DModels.Cuboid("Cuboid", 20f, 40f, 100, System.Drawing.Color.White, null);
myPCLTarget = myModel.PointCloudVertices;
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
Matrix3d R = new Matrix3d();
R = R.RotationXYZDegrees(30, 30, 30);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.CreateOutliers(myPCLSource, 5);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static PointCloudVertices RotateToOriginAxes(PointCloudVertices mypointCloudSource)
{
PCA pca = new PCA();
pca.PCA_OfPointCloud(mypointCloudSource);
Matrix3d R = new Matrix3d();
PointCloudVertices mypointCloudResult = PointCloudVertices.CopyVertices(mypointCloudSource);
R = R.Rotation_ToOriginAxes(mypointCloudResult.PCAAxes);
PointCloudVertices.Rotate(mypointCloudResult, R);
pca.PCA_OfPointCloud(mypointCloudResult);
mypointCloudResult.Path = mypointCloudSource.Path;
mypointCloudResult.FilaName = mypointCloudSource.FilaName;
return(mypointCloudResult);
}
public PointCloudVertices AlignToCenter(PointCloudVertices pointCloudSource)
{
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
Matrix3d R = new Matrix3d();
R = R.RotationChangeBasis(PointCloudVertices.ToVectors(pointCloudSource.PCAAxes));
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloudVertices.Rotate(pointCloudResult, R);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
return(pointCloudResult);
}
public static double Test6_Bunny(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
string path = AppDomain.CurrentDomain.BaseDirectory + "TestData";
Model model3DTarget = new Model(path + "\\bunny.obj");
myPCLTarget = model3DTarget.PointCloudVertices;
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.Translate(myPCLSource, -0.15f, 0.05f, 0.02f);
Matrix3d R = new Matrix3d();
R = R.RotationXYZDegrees(30, 30, 30);
PointCloudVertices.Rotate(myPCLSource, R);
PointCloudVertices.ScaleByFactor(myPCLSource, 0.8f);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_CuboidRotate(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
double cubeSize = 1;
double cubeSizeY = 2;
int numberOfPoints = 3;
Model myModel = Example3DModels.Cuboid("Cuboid", cubeSize, cubeSizeY, numberOfPoints, System.Drawing.Color.White, null);
myPCLTarget = myModel.PointCloudVertices;
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
//Vertices.Translate(myPCLSource, 220, -300, 127);
//Vertices.ScaleByFactor(myPCLSource, 0.2);
Matrix3d R = new Matrix3d();
R = R.RotationXYZDegrees(90, 124, -274);
PointCloudVertices.Rotate(myPCLSource, R);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
