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); } }