private PointCloudVertices CalculateMergedPoints(PointCloudVertices pResult, PointCloudVertices pTarget)
{
PointCloudVertices pMerged = PointCloudVertices.CloneVertices(pResult);
for (int i = 0; i < pTarget.Count; i++)
{
pMerged.Add(pTarget[i]);
}
//for(int i = 0; i < pMerged.Count; i++)
//{
// Vertex vMerged = pMerged[i];
// Vertex vSource = pTarget[i];
// if(vMerged.Vector.X == vSource.Vector.X && vMerged.Vector.Y == vSource.Vector.Y)
// {
// if(vMerged.Vector.Z == 0)
// {
// vMerged.Vector.Z = vSource.Vector.Z;
// }
// if (vMerged.Color == System.Drawing.Color.Black)
// {
// vMerged.Color = vSource.Color;
// }
// }
//}
return(pMerged);
}
public PointCloudVertices AlignPointClouds_SVD_WithShuflleEffect(bool axesRotateEffect, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget)
{
try
{
if (pointCloudSource == null || pointCloudTarget == null || pointCloudSource.Count == 0 || pointCloudTarget.Count == 0)
{
System.Diagnostics.Debug.WriteLine("PCA - please check point clouds ");
return(null);
}
this.Matrix = Matrix4d.Identity;
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
PrepareTargetTree(pointCloudTarget);
PointCloudVertices myPointCloudIteration = PointCloudVertices.CloneVertices(pointCloudSource);
for (int i = 0; i < MaxmimumIterations; i++)
{
double meanDistance = SVD_Iteration(myPointCloudIteration);
System.Diagnostics.Debug.WriteLine("-->> Iteration " + i.ToString() + " : Mean Distance : " + meanDistance.ToString("G") + ": duration: " + GlobalVariables.TimeSpanString());
if (meanDistance < thresholdConvergence)
{
break;
}
myPointCloudIteration = pointCloudResultBest;
}
//final check:
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
//"Shuffle" effect - the target points are in other order after kdtree search:
//The mean distance calculated again, as check (was calculated before in the kdTree routine)
MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTargetKDTree);
System.Diagnostics.Debug.WriteLine("-->> TO CHECK: PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
//MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
//System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget);
pointCloudResult = Matrix.TransformPoints(pointCloudSource);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
//MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
//System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
//for display later:
}
catch
{
System.Windows.Forms.MessageBox.Show("Error aligning point cloud");
}
return(pointCloudResult);
}
public static double Test5_CubeScale_Inhomogenous(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(50);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.ScaleByVertex(myPCLSource, new Vertex(1, 2, 3));
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_CubeScale_Uniform(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(50);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.ScaleByFactor(myPCLSource, 0.2f);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
//private static int cubeSize ;
//private static PointCloud pointCloudTarget;
//private static PointCloud pointCloudSource;
// private static PointCloud vectorsResult;
public static double Test1_Translation(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult)
{
myPCLTarget = PointCloudVertices.CreateSomePoints();
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.Translate(myPCLSource, 10, 3, 8);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_Cube8Shuffle_1Milion(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_RegularGrid_Empty(cubeSize, 409);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.ShuffleRandom(myPCLSource);
myPCLResult = IterativeClosestPointTransform.Instance.PerformICP(myPCLSource, myPCLTarget);
return(IterativeClosestPointTransform.Instance.MeanDistance);
}
public static double Test5_Cube8Shuffle(ref PointCloudVertices myPCLTarget, ref PointCloudVertices myPCLSource, ref PointCloudVertices myPCLResult, float cubeSize)
{
myPCLTarget = PointCloudVertices.CreateCube_Corners(cubeSize);
myPCLSource = PointCloudVertices.CloneVertices(myPCLTarget);
PointCloudVertices.ShuffleTest(myPCLSource);
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 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);
}
