public PointCloudVertices FindNearest_BruteForceOld(PointCloudVertices vSource, PointCloudVertices vTarget) { PointCloudVertices nearestNeighbours = new PointCloudVertices(); int iMax = 10; PointCloudVertices tempTarget = PointCloudVertices.CopyVertices(vTarget); for (int i = 0; i < vSource.Count; i++) { //BuildKDTree_Standard(tempTarget); Vertex p = vSource[i]; // Perform a nearest neighbour search around that point. KDTreeRednaxela.NearestNeighbour <EllipseWrapper> pIter = null; pIter = KdTree_Rednaxela.FindNearest_EuclidDistance(new float[] { Convert.ToSingle(p.Vector.X), Convert.ToSingle(p.Vector.Y), Convert.ToSingle(p.Vector.Z) }, iMax, -1); while (pIter.MoveNext()) { // Get the ellipse. //var pEllipse = pIter.Current; EllipseWrapper wr = pIter.CurrentPoint; nearestNeighbours.Add(wr.Vertex); tempTarget.RemoveAt(pIter.CurrentIndex); break; } } return(nearestNeighbours); }
/// <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 PointCloudVertices pointsSourceSubset, ref PointCloudVertices pointsTargetSubset, int myNumberPoints, LandmarkTransform myLandmarkTranform, PointCloudVertices pointsTarget, PointCloudVertices pointsSource, int maxNumberOfIterations) { try { if (CheckSourceTarget(pointsTarget, pointsSource)) { return(null); } pointsTargetSubset = PointCloudVertices.CopyVertices(pointsTarget); pointsSourceSubset = PointCloudVertices.CopyVertices(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); } PointCloudVertices.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 PointCloudVertices Helper_SetNewInterationSets(ref PointCloudVertices pointsSource, ref PointCloudVertices pointsTarget, PointCloudVertices PS, PointCloudVertices PT) { PointCloudVertices myPointsTransformed = MathUtilsVTK.TransformPoints(PS, Matrix); this.Matrix.TransformVectorList(normalsSource); pointsSource = myPointsTransformed; pointsTarget = PointCloudVertices.CopyVertices(PT); return(myPointsTransformed); }
/// <summary> /// assume - vectors are mass - centered! /// </summary> /// <param name="pointCloud"></param> /// <param name="axesVectors"></param> /// <param name="i"></param> /// <returns></returns> private PointCloudVertices InvertAxes(PointCloudVertices pointCloud, PointCloudVertices axesVectors, int i) { PointCloudVertices resultList = PointCloudVertices.CopyVertices(axesVectors); if (i == -1) { return(resultList); } Vector3d v = resultList[i].Vector.Negate(); resultList[i] = new Vertex(resultList[i].IndexInModel, v); return(resultList); }
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); }
////} ///// <summary> ///// PCA are center of mass - centered ///// </summary> ///// <param name="pointCloud"></param> ///// <param name="myCentroid"></param> //private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered) //{ // pointCloud.CentroidPCA = Centroid; // pointCloud.PCAAxes = new PointCloud(); // for (int i = 0; i < 3; i++) // { // Vector3d v = VT.ExtractColumn(i); // //v = v * Math.Sqrt(EV[i]); // v = v * EV[i]; // double d = v.Length; // Vertex ve = new Vertex(i, v); // pointCloud.PCAAxes.Add(ve); // } // mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes; ////} ////} ////} ///// <summary> ///// PCA are center of mass - centered ///// </summary> ///// <param name="pointCloud"></param> ///// <param name="myCentroid"></param> //private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered) //{ // pointCloud.CentroidPCA = Centroid; // pointCloud.PCAAxes = new PointCloud(); // for (int i = 0; i < 3; i++) // { // Vector3d v = VT_NotNormalized.ExtractColumn(i); // //v = v * Math.Sqrt(EV[i]); // v = v * EV_NotNormalized[i]; // double d = v.Length; // Vertex ve = new Vertex(i, v); // pointCloud.PCAAxes.Add(ve); // } // mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes; //} private static PointCloudVertices CalculateResults(Matrix3d Ub, Matrix3d Ua, PointCloudVertices pointCloudSource, Vector3d centroidB, Vector3d centroidA) { Matrix3d R; Matrix3d.Mult(ref Ub, ref Ua, out R); PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource); PointCloudVertices.Rotate(pointCloudResult, R); Vector3d t = centroidB - R.MultiplyVector(centroidA); //Vertices.AddVector(pointCloudResult, t); return(pointCloudResult); }
public PointCloudVertices FindNearest_BruteForce(PointCloudVertices source, PointCloudVertices target) { PointCloudVertices result = new PointCloudVertices(); List <int> indicesTargetFound = new List <int>(); PointCloudVertices tempTarget = PointCloudVertices.CopyVertices(target); for (int i = source.Count - 1; i >= 0; i--) { BuildKDTree_Stark(tempTarget); int indexNearest = KdTree_Stark.FindNearest(source[i]); result.Add(target[indexNearest]); tempTarget.RemoveAt(indexNearest); } return(result); }
public static PointCloudVertices RotateToOriginAxes(PointCloudVertices mypointCloudSource) { PCA pca = new PCA(); pca.PCA_OfPointCloud(mypointCloudSource); Matrix3d R = new Matrix3d(); PointCloudVertices mypointCloudResult = PointCloudVertices.CopyVertices(mypointCloudSource); R = R.Rotation_ToOriginAxes(mypointCloudResult.PCAAxes); PointCloudVertices.Rotate(mypointCloudResult, R); pca.PCA_OfPointCloud(mypointCloudResult); mypointCloudResult.Path = mypointCloudSource.Path; mypointCloudResult.FilaName = mypointCloudSource.FilaName; return(mypointCloudResult); }
public PointCloudVertices AlignToCenter(PointCloudVertices pointCloudSource) { pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource); Matrix3d R = new Matrix3d(); R = R.RotationChangeBasis(PointCloudVertices.ToVectors(pointCloudSource.PCAAxes)); PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource); PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector); PointCloudVertices.Rotate(pointCloudResult, R); pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult); return(pointCloudResult); }
public PointCloudVertices PerformICP() { double convergenceThreshold = PTarget.BoundingBoxMaxFloat * ICPSettings.ConvergenceThreshold; PointCloudVertices PT = PointCloudVertices.CopyVertices(PTarget); PointCloudVertices PS = PointCloudVertices.CopyVertices(PSource); Vertex pSOrigin = new Vertex(0, new Vector3d(0, 0, 0)); Vertex pTOrigin = new Vertex(0, new Vector3d(0, 0, 0)); PointCloudVertices myPointsTransformed = null; ICPSettings.ResetVertexToOrigin = false; if (ICPSettings.ResetVertexToOrigin) { pTOrigin = PointCloudVertices.ResetCentroid(PT, true); pSOrigin = PointCloudVertices.ResetCentroid(PS, true); } KDTreeVertex kdTreee = Helper_CreateTree(PT); kdTreee.DistanceOptimization = this.ICPSettings.DistanceOptimization; CalculateNormals(PS.ToPointCloud(), PT.ToPointCloud(), kdTreee); try { if (!CheckSourceTarget(PT, PS)) { return(null); } PointCloudVertices pointsTarget = PointCloudVertices.CopyVertices(PT); PointCloudVertices pointsSource = PointCloudVertices.CopyVertices(PS); this.Matrix = Matrix4d.Identity; double oldMeanDistance = 0; for (NumberOfIterations = 0; NumberOfIterations < ICPSettings.MaximumNumberOfIterations; NumberOfIterations++) { kdTreee.NormalsSource = this.normalsSource; float angleThreshold = Convert.ToSingle(this.startAngleForNormalsCheck - 5) * (1.0f - this.NumberOfIterations * 1.0f / this.ICPSettings.MaximumNumberOfIterations) + 5; if (ICPSettings.SimulatedAnnealing) { if (Helper_ICP_Iteration_SA(PS, PT, kdTreee, angleThreshold)) { break; } } else { myPointsTransformed = Helper_ICP_Iteration(ref pointsSource, ref pointsTarget, PT, PS, kdTreee, angleThreshold); if (myPointsTransformed != null) { break; } } Debug.WriteLine("--------------Iteration: " + NumberOfIterations.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000") + ": duration: " + GlobalVariables.TimeSpanString()); if (Math.Abs(oldMeanDistance - MeanDistance) < convergenceThreshold) { Debug.WriteLine("Convergence reached - changes under: " + convergenceThreshold.ToString()); break; } oldMeanDistance = MeanDistance; } Debug.WriteLine("--------****** Solution of ICP after : " + NumberOfIterations.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000")); //if number of Iteration if (myPointsTransformed == null) { myPointsTransformed = pointsTransformed; } if (this.ICPSettings.ShuffleEffect) { PT = pointsTarget; PTransformed = myPointsTransformed; } else { PTransformed = MathUtilsVTK.TransformPoints(PS, Matrix); } //re-reset vector if (ICPSettings.ResetVertexToOrigin) { PointCloudVertices.AddVertex(PTransformed, pTOrigin); } //DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, this.PSource, PTransformed, PTarget); if (myPointsTransformed != null) { DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, pointsSource, myPointsTransformed, pointsTarget); } else { //no convergence - write matrix this.Matrix.Print("Cumulated Matrix "); } PMerged = CalculateMergedPoints(PTransformed, PT); return(PTransformed); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + NumberOfIterations.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; }