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