private static ICPSolution IterateStartPoints(PointCloud pointsSource, PointCloud pointsTarget, int myNumberPoints, LandmarkTransform myLandmarkTransform, int maxNumberOfIterations) { int maxIterationPoints = pointsSource.Count; int currentIteration = 0; try { if (myNumberPoints > pointsSource.Count) { myNumberPoints = pointsSource.Count; } List <ICPSolution> solutionList = new List <ICPSolution>(); for (currentIteration = 0; currentIteration < maxNumberOfIterations; currentIteration++) { ICPSolution res = ICPSolution.SetRandomIndices(myNumberPoints, maxIterationPoints, solutionList); res.Matrix = TryoutPoints(pointsTarget, pointsSource, res, myLandmarkTransform);//, accumulate); res.PointsTransformed = MathUtilsVTK.TransformPoints(res.PointsSource, res.Matrix); res.MeanDistance = PointCloud.MeanDistance(res.PointsTarget, res.PointsTransformed); //res.MeanDistance = totaldist / Convert.ToSingle(res.PointsSource.Count); solutionList.Add(res); } if (solutionList.Count > 0) { solutionList.Sort(new ICPSolutionComparer()); RemoveSolutionIfMatrixContainsNaN(solutionList); if (solutionList.Count == 0) { System.Windows.Forms.MessageBox.Show("No start solution could be found !"); } Debug.WriteLine("Solutions found after: " + currentIteration.ToString() + " iterations, number of solution " + solutionList.Count.ToString()); if (solutionList.Count > 0) { ICPSolution result = solutionList[0]; //write solution to debug ouput //System.Diagnostics.Debug.WriteLine("Solution of start sequence is: "); DebugWriteUtils.WriteTestOutputVector3("Solution of start sequence", result.Matrix, result.PointsSource, result.PointsTransformed, result.PointsTarget); return(result); } } return(null); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in IterateStartPoints of ICP at: " + currentIteration.ToString() + " : " + err.Message); return(null); } }
private PointCloud ICPOnPoints_WithSubset(PointCloud mypointCloudTarget, PointCloud myPCLToBeMatched, PointCloud myPointsTargetSubset, PointCloud mypointsSourceSubset) { List <Vector3> myVectorsTransformed = null; PointCloud myPCLTransformed = null; try { Matrix4 m; PerformICP(mypointsSourceSubset, myPointsTargetSubset); myVectorsTransformed = PointsTransformed.ListVectors; m = Matrix; //DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset); //extend points: //myPointsTransformed = icpSharp.TransformPointsToPointsData(mypointsSourceSubset, m); //----------------------------- //DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset); //now with all other points as well... myVectorsTransformed = new List <Vector3>(); myPCLTransformed = m.TransformPoints(myPCLToBeMatched); //write all results in debug output DebugWriteUtils.WriteTestOutputVector3("Soluation of Points With Subset", m, myPCLToBeMatched, myPCLTransformed, mypointCloudTarget); } catch (Exception err) { System.Diagnostics.Debug.WriteLine("Error in ICP : " + err.Message); return(null); } //for output: return(myPCLTransformed); }
public PointCloud PerformICP_Stitching() { int iPoint = 0; try { PointCloud pointsTarget = null; PointCloud pointsSource = null; ICPSolution res = CalculateStartSolution(ref pointsSource, ref pointsTarget, ICPSettings.NumberOfStartTrialPoints, this.LandmarkTransform, this.PTarget, this.PSource, ICPSettings.MaximumNumberOfIterations); if (res == null) { return(null); } Matrix4 myMatrix = res.Matrix; float oldMeanDistance = 0; //now try all points and check if outlier for (iPoint = (pointsTarget.Count - 1); iPoint >= 0; iPoint--) { float distanceOfNewPoint = CheckNewPointDistance(iPoint, myMatrix, pointsTarget, pointsSource); ////experimental ////--compare this distance to: //pointsTargetTrial.Add[pointsTargetTrial.Count, p1[0], p1[1], p1[2]); //pointsSourceTrial.Add[pointsSourceTrial.Count, p2[0], p2[1], p2[2]); //PointCloud tempPointRotateAll = TransformPoints(pointsSourceTrial, myMatrix, pointsSourceTrial.Count); //dist = CalculateTotalDistance(pointsTargetTrial, tempPointRotateAll); //DebugWriteUtils.WriteTestOutput(myMatrix, pointsSourceTrial, tempPointRotateAll, pointsTargetTrial, pointsTargetTrial.Count); Debug.WriteLine("------>ICP Iteration Trial: " + iPoint.ToString() + " : Mean Distance: " + distanceOfNewPoint.ToString()); if (Math.Abs(distanceOfNewPoint - res.MeanDistance) < ICPSettings.ThresholdOutlier) { PointCloud pointsTargetTrial = PointCloud.CloneAll(res.PointsTarget); PointCloud pointsSourceTrial = PointCloud.CloneAll(res.PointsSource); myMatrix = TryoutNewPoint(iPoint, pointsTarget, pointsSource, pointsTargetTrial, pointsSourceTrial, this.LandmarkTransform); PointCloud myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSourceTrial, myMatrix); this.MeanDistance = PointCloud.MeanDistance(pointsTargetTrial, myPointsTransformed); // this.MeanDistance = totaldist / Convert.ToSingle(pointsTargetTrial.Count); //DebugWriteUtils.WriteTestOutputVector3("Iteration " + iPoint.ToString(), myMatrix, pointsSourceTrial, myPointsTransformed, pointsTargetTrial); //could also remove this check... if (Math.Abs(oldMeanDistance - this.MeanDistance) < ICPSettings.ThresholdOutlier) { res.PointsTarget = pointsTargetTrial; res.PointsSource = pointsSourceTrial; res.Matrix = myMatrix; res.PointsTransformed = myPointsTransformed; oldMeanDistance = this.MeanDistance; //Debug.WriteLine("************* Point OK : "); DebugWriteUtils.WriteTestOutputVector3("************* Point OK :", myMatrix, res.PointsSource, myPointsTransformed, res.PointsTarget); } //remove point from point list pointsTarget.RemoveAt(iPoint); pointsSource.RemoveAt(iPoint); } } this.Matrix = res.Matrix; //System.Diagnostics.Debug.WriteLine("Solution of ICP is : "); DebugWriteUtils.WriteTestOutputVector3("Solution of ICP", Matrix, res.PointsSource, res.PointsTransformed, res.PointsTarget); pointsTransformed = res.PointsTransformed; return(pointsTransformed); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Update ICP at point: " + iPoint.ToString() + " : " + err.Message); return(null); } //Matrix4 newMatrix = accumulate.GetMatrix(); //this.Matrix = newMatrix; }