public PointCloudVertices PerformICP() { double convergenceThreshold = PTarget.BoundingBoxMaxFloat * ICPSettings.ConvergenceThreshold; PointCloudVertices PT = PointCloudVertices.CopyVertices(PTarget); PointCloudVertices PS = PointCloudVertices.CopyVertices(PSource); Vertex pSOrigin = new Vertex(0, new Vector3d(0, 0, 0)); Vertex pTOrigin = new Vertex(0, new Vector3d(0, 0, 0)); PointCloudVertices myPointsTransformed = null; ICPSettings.ResetVertexToOrigin = false; if (ICPSettings.ResetVertexToOrigin) { pTOrigin = PointCloudVertices.ResetCentroid(PT, true); pSOrigin = PointCloudVertices.ResetCentroid(PS, true); } KDTreeVertex kdTreee = Helper_CreateTree(PT); kdTreee.DistanceOptimization = this.ICPSettings.DistanceOptimization; CalculateNormals(PS.ToPointCloud(), PT.ToPointCloud(), kdTreee); try { if (!CheckSourceTarget(PT, PS)) { return(null); } PointCloudVertices pointsTarget = PointCloudVertices.CopyVertices(PT); PointCloudVertices pointsSource = PointCloudVertices.CopyVertices(PS); this.Matrix = Matrix4d.Identity; double oldMeanDistance = 0; 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; if (ICPSettings.SimulatedAnnealing) { if (Helper_ICP_Iteration_SA(PS, PT, kdTreee, angleThreshold)) { break; } } else { myPointsTransformed = Helper_ICP_Iteration(ref pointsSource, ref pointsTarget, PT, PS, kdTreee, angleThreshold); if (myPointsTransformed != null) { break; } } Debug.WriteLine("--------------Iteration: " + NumberOfIterations.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000") + ": duration: " + GlobalVariables.TimeSpanString()); if (Math.Abs(oldMeanDistance - MeanDistance) < convergenceThreshold) { Debug.WriteLine("Convergence reached - changes under: " + convergenceThreshold.ToString()); break; } oldMeanDistance = MeanDistance; } Debug.WriteLine("--------****** Solution of ICP after : " + NumberOfIterations.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000")); //if number of Iteration if (myPointsTransformed == null) { myPointsTransformed = pointsTransformed; } if (this.ICPSettings.ShuffleEffect) { PT = pointsTarget; PTransformed = myPointsTransformed; } else { PTransformed = MathUtilsVTK.TransformPoints(PS, Matrix); } //re-reset vector if (ICPSettings.ResetVertexToOrigin) { PointCloudVertices.AddVertex(PTransformed, pTOrigin); } //DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, this.PSource, PTransformed, PTarget); if (myPointsTransformed != null) { DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, pointsSource, myPointsTransformed, pointsTarget); } else { //no convergence - write matrix this.Matrix.Print("Cumulated Matrix "); } PMerged = CalculateMergedPoints(PTransformed, PT); return(PTransformed); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + NumberOfIterations.ToString() + " : " + err.Message); return(null); } }