private static double TransformPoints(ref PointCloudVertices myPointsTransformed, PointCloudVertices pointsTarget, PointCloudVertices pointsSource, Matrix4d myMatrix) { myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSource, myMatrix); double meanDistance = PointCloudVertices.MeanDistance(pointsTarget, myPointsTransformed); //double meanDistance = totaldist / Convert.ToSingle(pointsTarget.Count); return(meanDistance); }
private static ICPSolution IterateStartPoints(PointCloudVertices pointsSource, PointCloudVertices 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 = PointCloudVertices.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.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: " + currentIteration.ToString() + " : " + err.Message); return(null); } }
public PointCloudVertices AlignPointClouds_SVD_WithShuflleEffect(bool axesRotateEffect, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget) { try { if (pointCloudSource == null || pointCloudTarget == null || pointCloudSource.Count == 0 || pointCloudTarget.Count == 0) { System.Diagnostics.Debug.WriteLine("PCA - please check point clouds "); return(null); } this.Matrix = Matrix4d.Identity; pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource); PrepareTargetTree(pointCloudTarget); PointCloudVertices myPointCloudIteration = PointCloudVertices.CloneVertices(pointCloudSource); for (int i = 0; i < MaxmimumIterations; i++) { double meanDistance = SVD_Iteration(myPointCloudIteration); System.Diagnostics.Debug.WriteLine("-->> Iteration " + i.ToString() + " : Mean Distance : " + meanDistance.ToString("G") + ": duration: " + GlobalVariables.TimeSpanString()); if (meanDistance < thresholdConvergence) { break; } myPointCloudIteration = pointCloudResultBest; } //final check: pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult); //"Shuffle" effect - the target points are in other order after kdtree search: //The mean distance calculated again, as check (was calculated before in the kdTree routine) MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTargetKDTree); System.Diagnostics.Debug.WriteLine("-->> TO CHECK: PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G")); //MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget); //System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G")); this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget); pointCloudResult = Matrix.TransformPoints(pointCloudSource); pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult); //MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget); //System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G")); //for display later: } catch { System.Windows.Forms.MessageBox.Show("Error aligning point cloud"); } return(pointCloudResult); }
public PointCloudVertices AlignPointClouds_OneVector(PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget, int vectorNumberSource, int vectorNumberTarget) { //------------------- pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource); //second object: //----------- pointCloudTargetCentered = CalculatePCA_Internal(pointCloudTarget); //Vector3d v = TargetPCVectors[vectorNumberTarget]; //v.X = -v.X; //v.Y = -v.Y; //v.Z = -v.Z; //TargetPCVectors[vectorNumberTarget] = v; Matrix3d R = new Matrix3d(); //R = R.RotationOneVectorToAnother(TargetPCVectors[vectorNumber], SourcePCVectors[vectorNumber]); R = R.RotationOneVertexToAnother(pointCloudSource.PCAAxes[vectorNumberSource], pointCloudTarget.PCAAxes[vectorNumberTarget]); //R.CheckRotationMatrix(); // //test: //Vector3d testV = R.MultiplyVector(sourceV); PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource); PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector); PointCloudVertices.Rotate(pointCloudResult, R); PointCloudVertices.AddVector(pointCloudResult, pointCloudTarget.CentroidVector); pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult); MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget); System.Diagnostics.Debug.WriteLine("-->> PCA (V) - Mean Distance : " + MeanDistance.ToString("0.000000")); return(pointCloudResult); }
private double CheckNewPointDistance(int iPoint, Matrix4d myMatrix, PointCloudVertices pointsTarget, PointCloudVertices pointsSource) { Vertex p1 = pointsTarget[iPoint]; Vertex p2 = pointsSource[iPoint]; PointCloudVertices tempPointReference = new PointCloudVertices(); PointCloudVertices tempPointToBeMatched = new PointCloudVertices(); tempPointReference.Add(p1); tempPointToBeMatched.Add(p2); PointCloudVertices tempPointRotate = MathUtilsVTK.TransformPoints(tempPointToBeMatched, myMatrix); double dist = PointCloudVertices.MeanDistance(tempPointReference, tempPointRotate); return(dist); }
public PointCloudVertices FindNearest_Rednaxela_Parallel(ref PointCloudVertices mypointsSource, PointCloudVertices mypointsTarget, float angleThreshold) { this.pointsSource = mypointsSource; this.pointsTarget = mypointsTarget; int indexI = -1; try { PointCloudVertices pointsSourceNew = new PointCloudVertices(pointsSource); PointCloudVertices pointsResult = new PointCloudVertices(); FindNearest_Rednaxela_HelperParallel(); for (int i = 0; i < pointsSource.Count; i++) { indexI = i; Vertex v = pointsTarget[pointsSource[i].IndexKDTreeTarget]; pointsResult.Add(v); } this.MeanDistance = PointCloudVertices.MeanDistance(pointsResult, pointsSource); pointsSource = pointsSourceNew; if (pointsSource.Count != pointsResult.Count) { MessageBox.Show("Error finding neighbours, found " + pointsResult.Count.ToString() + " out of " + pointsSource.Count.ToString()); //return false; } mypointsSource = this.pointsSource; return(pointsResult); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Finding neighbors at: " + indexI.ToString() + " : " + err.Message); return(null); } }
public PointCloudVertices FindNearest_Rednaxela(ref PointCloudVertices mypointsSource, PointCloudVertices mypointsTarget, float angleThreshold) { this.pointsSource = mypointsSource; this.pointsTarget = mypointsTarget; int indexI = -1; try { PointCloudVertices pointsSourceNew = new PointCloudVertices(pointsSource); PointCloudVertices pointsResult = new PointCloudVertices(); FindNearest_Rednaxela_Helper(); for (int i = 0; i < listNeighbours.Count; i++) { indexI = i; List <Neighbours> mySublist = listNeighbours[i]; mySublist.Sort(new NeighboursComparer()); Vertex v = pointsTarget[mySublist[0].IndexTarget]; pointsResult.Add(v); } if (Normals_RemovePoints) { listNeighbours.Sort(new NeighboursListComparer()); int minPoints = Math.Min(10, listNeighbours.Count - 1); for (int i = 0; i < listNeighbours.Count; i++) { List <Neighbours> mySublist = listNeighbours[i]; int indexInsert = mySublist[0].IndexSource; Vertex vSource = pointsSource[mySublist[0].IndexSource]; Vertex vTarget = pointsTarget[mySublist[0].IndexTarget]; pointsSourceNew[indexInsert] = vSource; pointsResult[indexInsert] = vTarget; if (i > minPoints) { if (mySublist[0].Angle > angleThreshold) { pointsSourceNew[indexInsert] = null; pointsResult[indexInsert] = null; } } } for (int i = pointsSource.Count - 1; i >= 0; i--) { if (pointsSourceNew[i] == null) { pointsSourceNew.RemoveAt(i); pointsResult.RemoveAt(i); } } Debug.WriteLine("Remaining points : " + (pointsResult.Count * 1.0 / pointsTarget.Count * 100).ToString("0.00") + " %"); } this.MeanDistance = PointCloudVertices.MeanDistance(pointsResult, pointsSource); pointsSource = pointsSourceNew; if (pointsSource.Count != pointsResult.Count) { MessageBox.Show("Error finding neighbours, found " + pointsResult.Count.ToString() + " out of " + pointsSource.Count.ToString()); //return false; } mypointsSource = this.pointsSource; return(pointsResult); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Finding neighbors at: " + indexI.ToString() + " : " + err.Message); return(null); } }
/// <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); }
public PointCloudVertices PerformICP_Stitching() { int iPoint = 0; try { PointCloudVertices pointsTarget = null; PointCloudVertices pointsSource = null; ICPSolution res = CalculateStartSolution(ref pointsSource, ref pointsTarget, ICPSettings.NumberOfStartTrialPoints, this.LandmarkTransform, this.PTarget, this.PSource, ICPSettings.MaximumNumberOfIterations); if (res == null) { return(null); } Matrix4d myMatrix = res.Matrix; double oldMeanDistance = 0; //now try all points and check if outlier for (iPoint = (pointsTarget.Count - 1); iPoint >= 0; iPoint--) { double 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) { PointCloudVertices pointsTargetTrial = PointCloudVertices.CopyVertices(res.PointsTarget); PointCloudVertices pointsSourceTrial = PointCloudVertices.CopyVertices(res.PointsSource); myMatrix = TryoutNewPoint(iPoint, pointsTarget, pointsSource, pointsTargetTrial, pointsSourceTrial, this.LandmarkTransform); PointCloudVertices myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSourceTrial, myMatrix); this.MeanDistance = PointCloudVertices.MeanDistance(pointsTargetTrial, myPointsTransformed); // this.MeanDistance = totaldist / Convert.ToSingle(pointsTargetTrial.Count); DebugWriteUtils.WriteTestOutputVertex("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.WriteTestOutputVertex("************* 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.WriteTestOutputVertex("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); } //Matrix4d newMatrix = accumulate.GetMatrix(); //this.Matrix = newMatrix; }