/// <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); } }
public IterativeClosestPointTransform()//:base(PointerUtils.GetIntPtr(new double[3]), true, true) { Reset(); this.PSource = null; this.PTarget = null; this.LandmarkTransform = new LandmarkTransform(); this.NumberOfIterations = 0; this.MeanDistance = 0.0f; }
public IterativeClosestPointTransform()//:base(PointerUtils.GetIntPtr(new float[3]), true, true) { KDTree = new KDTreeKennell(); //KDTree = new KDTreeJeremyC(); //KDTree = new KDTreeBruteForce(); Reset(); this.PSource = null; this.PTarget = null; this.LandmarkTransform = new LandmarkTransform(); this.NumberOfIterations = 0; }
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 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); }
private static Matrix4 TryoutNewPoint(int iPoint, PointCloud pointsTarget, PointCloud pointsSource, PointCloud pointsTargetTrial, PointCloud pointsSourceTrial, LandmarkTransform myLandmarkTransform) { Vector3 p1 = pointsTarget.Vectors[iPoint]; Vector3 p2 = pointsSource.Vectors[iPoint]; pointsTargetTrial.AddVector(p1); pointsSourceTrial.AddVector(p2); MathUtilsVTK.FindTransformationMatrix(pointsSourceTrial, pointsTargetTrial, myLandmarkTransform);//, accumulate); Matrix4 myMatrix = myLandmarkTransform.Matrix; return(myMatrix); }
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); }
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); }
private static Matrix4d TryoutNewPoint(int iPoint, List <Vertex> pointsTarget, List <Vertex> pointsSource, List <Vertex> pointsTargetTrial, List <Vertex> pointsSourceTrial, LandmarkTransform myLandmarkTransform) { Vertex p1 = pointsTarget[iPoint]; Vertex p2 = pointsSource[iPoint]; pointsTargetTrial.Add(p1); pointsSourceTrial.Add(p2); MatrixUtilsNew.FindTransformationMatrix(Vertices.ConvertToVector3dList(pointsSourceTrial), Vertices.ConvertToVector3dList(pointsTargetTrial), myLandmarkTransform);//, accumulate); Matrix4d myMatrix = myLandmarkTransform.Matrix; return(myMatrix); }
public static void FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, LandmarkTransform myLandmarkTransform) { myLandmarkTransform.Matrix = new Matrix4d(); myLandmarkTransform.SourceLandmarks = pointsSource; myLandmarkTransform.TargetLandmarks = pointsTarget; myLandmarkTransform.Update(); }
private static Matrix4d TryoutNewPoint(int iPoint, PointCloudVertices pointsTarget, PointCloudVertices pointsSource, PointCloudVertices pointsTargetTrial, PointCloudVertices pointsSourceTrial, LandmarkTransform myLandmarkTransform) { Vertex p1 = pointsTarget[iPoint]; Vertex p2 = pointsSource[iPoint]; pointsTargetTrial.Add(p1); pointsSourceTrial.Add(p2); MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSourceTrial), PointCloudVertices.ToVectors(pointsTargetTrial), myLandmarkTransform);//, accumulate); Matrix4d myMatrix = myLandmarkTransform.Matrix; return(myMatrix); }