public PointCloud PerformICP()
{
//float convergenceThreshold = PTarget.BoundingBoxMaxFloat * ICPSettings.ConvergenceThreshold;
//if (ICPSettings.ResetVector3ToOrigin)
//{
//}
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Before build");
}
KDTree.Build(this.PTarget);
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Build Tree");
}
try
{
if (!CheckSourceTarget(PTarget, this.PSource))
{
return(null);
}
pointsTarget = PointCloud.CloneAll(PTarget);
pointsSource = PointCloud.CloneAll(PSource);
this.Matrix = Matrix4.Identity;
float oldMeanDistance = 0;
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Start ICP");
}
for (NumberOfIterations = 0; NumberOfIterations < ICPSettings.MaximumNumberOfIterations; NumberOfIterations++)
{
//kdTreee.NormalsSource = this.normalsSource;
float angleThreshold = Convert.ToSingle(this.startAngleForNormalsCheck - 5) * (1.0f - this.NumberOfIterations * 1.0f / this.ICPSettings.MaximumNumberOfIterations) + 5;
Single_ICP_Iteration(angleThreshold);
//Debug.WriteLine("--------------Iteration: " + NumberOfIterations.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000") );
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Iteration ");
}
if (MeanDistance < ICPSettings.ThresholdConvergence)
//if (Math.Abs(oldMeanDistance - MeanDistance) < convergenceThreshold)
{
Debug.WriteLine("Convergence reached - changes under: " + ICPSettings.ThresholdConvergence.ToString());
break;
}
oldMeanDistance = MeanDistance;
}
//shuffle effect - set points source to other order
//PS = pointsSource;//reordered for shuffle effect
//this.PSource = pointsSource;
if (this.ICPSettings.ShuffleEffect)
{
PTarget = pointsTarget;
//PS = pointsSource;//reordered for shuffle effect
//this.PSource = pointsSource;
PointsTransformed = this.pointsTransformed;
}
else
{
PointsTransformed = MathUtilsVTK.TransformPoints(this.PSource, Matrix);
}
//ignore ICP result if the convergence results is bad
if (MeanDistance > ICPSettings.ThresholdIgnoreICP)
{
Debug.WriteLine("ICP RESULT IGNORED - TOO BAD " + this.MeanDistance.ToString("0.0000"));
return(this.PTarget);
}
if (this.pointsTransformed != null)
{
//DebugWriteUtils.WriteTestOutputVector3("Solution of ICP", Matrix, pointsSource, pointsTransformed, pointsTarget);
}
else
{
//no convergence - write matrix
this.Matrix.Print("Cumulated Matrix ");
}
pointsTarget = PTarget;
float thresh = this.MeanDistance;
if (ICPSettings.SingleSourceTargetMatching)
{
PointsTransformed = this.PointsResultKDTree;// this.pointsTransformed;
PointCloud.AddVector3(PointsTransformed, centroidSource);
}
else
{
PointsTransformed = PointCloud.CalculateMergedPoints(this.pointsTransformed, this.pointsTarget, this.KDTree, ICPSettings.ThresholdMergedPoints, out PointsAdded, this.ICPSettings.ChangeColorsOfMergedPoints);
PointCloud.AddVector3(PointsTransformed, centroidSource);
}
Debug.WriteLine("--------****** Solution of ICP after : " + NumberOfIterations.ToString() + " iterations, Mean Distance: " + MeanDistance.ToString("0.00000000000") + " Matrix trace : " + this.Matrix.Trace.ToString("0.00") + " - points added : " + PointsAdded.ToString());
//not the best solution but ...
PointsTransformed.SetDefaultIndices();
PointsTransformed.Name = "Result Cloud";
PointsTransformed.FileNameLong = "ResultCloud.obj";
return(PointsTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + NumberOfIterations.ToString() + " : " + err.Message);
return(null);
}
}