/// <summary> /// calculates a start solution set in total of "myNumberPoints" points /// </summary> /// <param name="pointsTargetSubset"></param> /// <param name="pointsSourceSubset"></param> /// <returns></returns> private static ICPSolution CalculateStartSolution(ref PointCloud pointsSourceSubset, ref PointCloud pointsTargetSubset, int myNumberPoints, LandmarkTransform myLandmarkTranform, PointCloud pointsTarget, PointCloud pointsSource, int maxNumberOfIterations) { try { if (CheckSourceTarget(pointsTarget, pointsSource)) { return(null); } pointsTargetSubset = PointCloud.CloneAll(pointsTarget); pointsSourceSubset = PointCloud.CloneAll(pointsSource); ICPSolution res = IterateStartPoints(pointsSourceSubset, pointsTargetSubset, myNumberPoints, myLandmarkTranform, maxNumberOfIterations); if (res == null) { System.Windows.Forms.MessageBox.Show("Could not find starting points for ICP Iteration - bad matching"); return(null); } PointCloud.RemoveEntriesByIndices(ref pointsSourceSubset, ref pointsTargetSubset, res.RandomIndices); return(res); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in CalculateStartSolution of ICP: " + err.Message); return(null); } }
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); } }
public static ICPSolution SetRandomIndices(int myNumberPoints, int maxNumber, List <ICPSolution> solutionList) { int i; List <int> randomIndices; try { //set trial points for (i = 0; i < 1000; i++) { try { randomIndices = ICPSolution.GetRandomIndices(maxNumber, myNumberPoints); if (ICPSolution.IndicesAreNew(randomIndices, solutionList)) { ICPSolution res = new ICPSolution(); res.RandomIndices = randomIndices; return(res); } } catch (Exception err) { MessageBox.Show("Error in SetRandomIndices " + err.Message); return(null); } } // MessageBox.Show("SetRandomIndices: No indices could be found!!"); // 1000 trials return(null); } catch (Exception err) { MessageBox.Show("Error in SetRandomIndices " + err.Message); return(null); } }
public static Matrix4 TryoutPoints(PointCloud pointsTarget, PointCloud pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform) { res.PointsTarget = RandomUtils.ExtractPoints(pointsTarget, res.RandomIndices); res.PointsSource = RandomUtils.ExtractPoints(pointsSource, res.RandomIndices); //transform: MathUtilsVTK.FindTransformationMatrix(res.PointsSource, res.PointsTarget, myLandmarkTransform);//, accumulate); res.Matrix = myLandmarkTransform.Matrix; return(res.Matrix); }
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; }
private bool Helper_ICP_Iteration_SA(List <Vertex> PT, List <Vertex> PS, KDTreeVertex kdTree, int keepOnlyPoints) { try { //first iteration if (solutionList == null) { solutionList = new List <ICPSolution>(); if (NumberOfStartTrialPoints > PS.Count) { NumberOfStartTrialPoints = PS.Count; } if (NumberOfStartTrialPoints == PS.Count) { NumberOfStartTrialPoints = PS.Count * 80 / 100; } if (NumberOfStartTrialPoints < 3) { NumberOfStartTrialPoints = 3; } for (int i = 0; i < MaxNumberSolutions; i++) { ICPSolution myTrial = ICPSolution.SetRandomIndices(NumberOfStartTrialPoints, PS.Count, solutionList); if (myTrial != null) { myTrial.PointsSource = RandomUtils.ExtractPoints(PS, myTrial.RandomIndices); solutionList.Add(myTrial); } } ////test.... ////maxNumberSolutions = 1; //ICPSolution myTrial1 = new ICPSolution(); //for (int i = 0; i < NumberPointsSolution; i++) //{ // myTrial1.RandomIndices.Add(i); //} //myTrial1.PointsSource = RandomUtils.ExtractPoints(PS, myTrial1.RandomIndices); //solutionList[0] = myTrial1; } for (int i = 0; i < solutionList.Count; i++) { List <Vertex> transformedPoints = null; ICPSolution myTrial = solutionList[i]; Helper_ICP_Iteration(ref myTrial.PointsTarget, ref myTrial.PointsSource, PT, PS, kdTree, keepOnlyPoints); myTrial.Matrix = Matrix4d.Mult(myTrial.Matrix, this.Matrix); myTrial.MeanDistanceSubset = this.MeanDistance; myTrial.MeanDistance = TransformPoints(ref transformedPoints, PT, PS, myTrial.Matrix); // solutionList[i] = myTrial; } if (solutionList.Count > 0) { solutionList.Sort(new ICPSolutionComparer()); RemoveSolutionIfMatrixContainsNaN(solutionList); if (solutionList.Count == 0) { System.Windows.Forms.MessageBox.Show("No solution could be found !"); } this.Matrix = solutionList[0].Matrix; this.MeanDistance = solutionList[0].MeanDistance; if (solutionList[0].MeanDistance < this.MaximumMeanDistance) { return(true); } } } catch (Exception err) { MessageBox.Show("Error in Helper_ICP_Iteration_SA: " + err.Message); return(false); } return(false); }
public static Matrix4d TryoutPointsSA(List <Vertex> pointsTarget, List <Vertex> pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform) { //transform: MatrixUtilsNew.FindTransformationMatrix(Vertices.ConvertToVector3dList(res.PointsSource), Vertices.ConvertToVector3dList(res.PointsTarget), myLandmarkTransform);//, accumulate); res.Matrix = myLandmarkTransform.Matrix; return(res.Matrix); }
/// <summary> /// a simulated annealing like technique /// </summary> /// <param name="pointsTarget"></param> /// <param name="pointsSource"></param> /// <param name="myNumberPoints"></param> /// <param name="myLandmarkTransform"></param> /// <param name="maxSolutions"></param> /// <returns></returns> private static ICPSolution IterateSA(List <Vertex> pointsTarget, List <Vertex> pointsSource, int myNumberPoints, int maxSolutions) { int i = 0; //int currentIteration = 0; try { if (myNumberPoints > pointsTarget.Count) { myNumberPoints = pointsTarget.Count; } List <ICPSolution> solutionList = new List <ICPSolution>(); for (i = 0; i < maxSolutions; i++) { ICPSolution myTrial = ICPSolution.SetRandomIndices(myNumberPoints, pointsSource.Count, solutionList); //myTrial.PointsTargetTrial = RandomUtils.ExtractPoints(pointsTarget, myTrial.RandomIndices); myTrial.PointsSource = RandomUtils.ExtractPoints(pointsSource, myTrial.RandomIndices); //myTrial.Matrix = TryoutPointsSA(pointsTarget, pointsSource, myTrial, myLandmarkTransform);//, accumulate); myTrial.PointsTransformed = MathUtils.TransformPoints(myTrial.PointsSource, myTrial.Matrix); double totaldist = PointUtils.CalculateTotalDistance(myTrial.PointsTarget, myTrial.PointsTransformed); myTrial.MeanDistance = totaldist / Convert.ToDouble(myTrial.PointsSource.Count); solutionList.Add(myTrial); } 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: " + i.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.WriteTestOutputVertex("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: " + i.ToString() + " : " + err.Message); return(null); } }
public static Matrix4d TryoutPointsSA(PointCloudVertices pointsTarget, PointCloudVertices pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform) { //transform: MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(res.PointsSource), PointCloudVertices.ToVectors(res.PointsTarget), myLandmarkTransform);//, accumulate); res.Matrix = myLandmarkTransform.Matrix; return(res.Matrix); }