public List <Vertex> PerformICP()
{
List <Vertex> PT = Vertices.CopyVertices(PTarget);
List <Vertex> PS = Vertices.CopyVertices(PSource);
Vertex pSOrigin = null;
Vertex pTOrigin = null;
if (ResetVertexToOrigin)
{
pTOrigin = Vertices.ResetVertexToOrigin(PT);
pSOrigin = Vertices.ResetVertexToOrigin(PS);
}
int keepOnlyPoints = 0;
if (DistanceOptimization)
{
keepOnlyPoints = 3;
}
int iter = 0;
try
{
if (!CheckSourceTarget(PT, PS))
{
return(null);
}
List <Vertex> pointsTarget = Vertices.CopyVertices(PT);
List <Vertex> pointsSource = Vertices.CopyVertices(PS);
this.Matrix = Matrix4d.Identity;
double oldMeanDistance = 0;
KDTreeVertex kdTreee = Helper_CreateTree(pointsTarget);
for (iter = 0; iter < MaximumNumberOfIterations; iter++)
{
if (NormalsCheck)
{
CalculateNormals(pointsSource, pointsTarget);
}
if (SimulatedAnnealing)
{
if (Helper_ICP_Iteration_SA(PT, PS, kdTreee, keepOnlyPoints))
{
break;
}
}
else
{
if (Helper_ICP_Iteration(ref pointsTarget, ref pointsSource, PT, PS, kdTreee, keepOnlyPoints))
{
break;
}
}
oldMeanDistance = MeanDistance;
Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));
}
Debug.WriteLine("--------****** Solution of ICP after : " + iter.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000"));
PTransformed = MathUtils.TransformPoints(PS, Matrix);
//re-reset vector
if (ResetVertexToOrigin)
{
Vertices.AddVector(PTransformed, pTOrigin);
//Vertices.AddVector(PSource, pSOrigin);
}
DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, this.PSource, PTransformed, PTarget);
return(PTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + iter.ToString() + " : " + err.Message);
return(null);
}
}
