/// <summary>
/// A single ICP Iteration
/// </summary>
/// <param name="pointsTarget"></param>
/// <param name="pointsSource"></param>
/// <param name="PT"></param>
/// <param name="PS"></param>
/// <param name="kdTree"></param>
/// <returns></returns>
private void Single_ICP_Iteration(float angleThreshold)
{
try
{
PointsResultKDTree = null;
//for geometric objects:
if (this.ICPSettings.FixedTestPoints)
{
PointsResultKDTree = this.pointsTarget.Clone();
}
else
{
PointsResultKDTree = this.KDTree.FindClosestPointCloud_Parallel(pointsSource);
}
Matrix4 myMatrix = Helper_FindTransformationMatrix(PointsResultKDTree);
if (myMatrix.CheckNAN())
{
return;
}
//overall matrix
Matrix4.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
//transform points:
pointsTransformed = MathUtilsVTK.TransformPoints(this.PSource, Matrix);
//for the "shuffle" effect (point order of source and target is different)
if (this.ICPSettings.ShuffleEffect)
{
CheckDuplicates_SetInteractionSet();
}
else
{
SetNewSet();
}
this.MeanDistance = PointCloud.MeanDistance(pointsSource, PointsResultKDTree);
if (MeanDistance < ICPSettings.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
{
return;
}
//check: is this really needed ??
//for the "shuffle" effect (point order of source and target is different)
//if (this.ICPSettings.ShuffleEffect)
//{
// SetNewSet();
//}
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Single_ICP_Iteration: " + err.Message);
}
}
