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);
}
private void CheckPCA(Matrix4d myMatrix4d, PointCloud sourceAxes, PointCloud targetAxes)
{
//-----------------------
//for check - transform sourceAxes - should give targetAxis: i.w. resultList should contains only zeros
PointCloud resultAxes = myMatrix4d.TransformPoints(sourceAxes);
resultAxes.SubtractCloud(targetAxes);
float fMax;
if (!resultAxes.CheckCloud(this.ThresholdConvergence, out fMax))
{
System.Windows.Forms.MessageBox.Show("SW Error in SVD.FindTransformationMatrix, difference should be zero, is: " + fMax.ToString());
}
}
/// <summary>
/// calculates Matrix for alignment of sourceAxes and targetAxes; sets pointCloudResult
/// </summary>
/// <param name="i"></param>
/// <param name="j"></param>
/// <param name="bestResultMeanDistance"></param>
/// <param name="meanDistance"></param>
/// <param name="myMatrixBestResult"></param>
/// <param name="sourceAxes"></param>
private void SVD_ForTwoPointCloudAlignment(int i, int j, ref double bestResultMeanDistance, ref double meanDistance, ref Matrix4d myMatrixBestResult, PointCloudVertices sourceAxes)
{
PointCloudVertices targetAxes = InvertAxes(pointCloudTargetCentered, pointCloudTargetCentered.PCAAxes, j);
Matrix4d myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
//Matrix4d myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
//-----------------------
//for check - should give TargetPCVectors
List <Vector3d> resultAxes = Matrix4dExtension.TransformPoints(myMatrix, PointCloudVertices.ToVectors(sourceAxes));
resultAxes = resultAxes.Subtract(PointCloudVertices.ToVectors(targetAxes));
List <Vector3d> myPointsResult = myMatrix.TransformPoints(PointCloudVertices.ToVectors(pointCloudSourceCentered));
PointCloudVertices myPointsResultTemp = PointCloudVertices.FromVectors(myPointsResult);
PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela_Parallel(ref myPointsResultTemp, pointCloudTargetCentered, -1);
//PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);
double trace = myMatrix.Trace;
meanDistance = kdtree.MeanDistance;
//double trace = kdtree.MeanDistance;
//double meanDistance = myMatrix.Trace;
//Check:
System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G") + " : Trace: " + trace.ToString("G"));
if (meanDistance < bestResultMeanDistance)
{
myMatrixBestResult = myMatrix;
bestResultMeanDistance = meanDistance;
pointCloudResultBest = PointCloudVertices.FromVectors(myPointsResult);
}
pointCloudResult = PointCloudVertices.FromVectors(myPointsResult);
pointCloudTargetKDTree = myPointCloudTargetTemp;
}
/// <summary>
/// calculates Matrix for alignment of sourceAxes and targetAxes; sets pointCloudResult
/// </summary>
/// <param name="i"></param>
/// <param name="j"></param>
/// <param name="bestResultMeanDistance"></param>
/// <param name="meanDistance"></param>
/// <param name="myMatrixBestResult"></param>
/// <param name="sourceAxes"></param>
private void SVD_ForTwoPointCloudAlignment_Double(int i, int j, ref float meanDistance, ref Matrix4 myMatrixBestResult, PointCloud sourceAxes)
{
PointCloud targetAxes = InvertAxes(pcTargetCentered, pcTargetCentered.PCAAxes, j);
//Matrix4 myMatrix = SVD_Float.FindTransformationMatrix(sourceAxes, targetAxes, ICP_VersionUsed.Scaling_Umeyama);
Matrix4d myMatrix4d = SVD.FindTransformationMatrix_WithoutCentroids(sourceAxes, targetAxes, ICP_VersionUsed.Umeyama);
//CheckPCA(myMatrix4d, sourceAxes, targetAxes);
PointCloud myResultPC = myMatrix4d.TransformPoints(pcSourceCentered);
//--------------
pcTreeResult = KDTree.FindClosestPointCloud_Parallel(myResultPC);
meanDistance = KDTree.MeanDistance;
//PointCloud myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);
System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G"));
if (meanDistance < bestResultMeanDistance)
{
myMatrixBestResult = myMatrix4d.ToMatrix4();
bestResultMeanDistance = meanDistance;
pcResultBest = myResultPC;
}
pcResult = myResultPC;
}
