/// <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 PointCloudVertices Helper_ICP_Iteration(ref PointCloudVertices pointsSource, ref PointCloudVertices pointsTarget, PointCloudVertices PT, PointCloudVertices PS, KDTreeVertex kdTree, float angleThreshold)
{
//Take care - might return less points than originally, since NormalsCheck or TreeStark remove points
if (!Helper_FindNeighbours(ref pointsSource, ref pointsTarget, kdTree, angleThreshold))
{
return(null);
}
Matrix4d myMatrix = Helper_FindTransformationMatrix(pointsSource, pointsTarget);
if (myMatrix.CheckNAN())
{
return(null);
}
PointCloudVertices myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSource, myMatrix);
//DebugWriteUtils.WriteTestOutputVertex("Iteration Result", myMatrix, pointsSource, myPointsTransformed, pointsTarget);
if (ICPSettings.SimulatedAnnealing)
{
this.Matrix = myMatrix;
this.MeanDistance = PointCloudVertices.MeanDistance(pointsTarget, myPointsTransformed);
//new set:
pointsSource = myPointsTransformed;
pointsTarget = PointCloudVertices.CopyVertices(PT);
}
else
{
Matrix4d.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
//DebugWriteUtils.WriteMatrix("Cumulated Matrix", Matrix);
//for the "shuffle" effect (point order of source and target is different)
if (!this.ICPSettings.ShuffleEffect)
{
myPointsTransformed = Helper_SetNewInterationSets(ref pointsSource, ref pointsTarget, PS, PT);
}
else
{
CheckDuplicates(ref myPointsTransformed, pointsSource, pointsTarget, PS, PT);
}
this.MeanDistance = PointCloudVertices.MeanDistance(pointsTarget, myPointsTransformed);
//Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));
if (MeanDistance < ICPSettings.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
{
return(myPointsTransformed);
}
//for the "shuffle" effect (point order of source and target is different)
if (this.ICPSettings.ShuffleEffect)
{
myPointsTransformed = Helper_SetNewInterationSets(ref pointsSource, ref pointsTarget, PS, PT);
}
this.pointsTransformed = myPointsTransformed;
}
return(null);
}
