/// <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 void Single_ICP_Iteration(float angleThreshold) { try { PointsResultKDTree = null; //for geometric objects: if (this.ICPSettings.FixedTestPoints) { PointsResultKDTree = this.pointsTarget.Clone(); } else { PointsResultKDTree = this.KDTree.FindClosestPointCloud_Parallel(pointsSource); } Matrix4 myMatrix = Helper_FindTransformationMatrix(PointsResultKDTree); if (myMatrix.CheckNAN()) { return; } //overall matrix Matrix4.Mult(ref myMatrix, ref this.Matrix, out this.Matrix); //transform points: pointsTransformed = MathUtilsVTK.TransformPoints(this.PSource, Matrix); //for the "shuffle" effect (point order of source and target is different) if (this.ICPSettings.ShuffleEffect) { CheckDuplicates_SetInteractionSet(); } else { SetNewSet(); } this.MeanDistance = PointCloud.MeanDistance(pointsSource, PointsResultKDTree); if (MeanDistance < ICPSettings.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance) { return; } //check: is this really needed ?? //for the "shuffle" effect (point order of source and target is different) //if (this.ICPSettings.ShuffleEffect) //{ // SetNewSet(); //} } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Single_ICP_Iteration: " + err.Message); } }
private static float TransformPoints(ref PointCloud myPointsTransformed, PointCloud pointsTarget, PointCloud pointsSource, Matrix4 myMatrix) { myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSource, myMatrix); float meanDistance = PointCloud.MeanDistance(pointsTarget, myPointsTransformed); return(meanDistance); }
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(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 TryoutPointsSA(PointCloud pointsTarget, PointCloud pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform) { //transform: MathUtilsVTK.FindTransformationMatrix(res.PointsSource, res.PointsTarget, myLandmarkTransform);//, accumulate); res.Matrix = myLandmarkTransform.Matrix; return(res.Matrix); }
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); }
public static Matrix4d TryoutPoints(PointCloudVertices pointsTarget, PointCloudVertices pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform) { res.PointsTarget = RandomUtils.ExtractPoints(pointsTarget, res.RandomIndices); res.PointsSource = RandomUtils.ExtractPoints(pointsSource, res.RandomIndices); //transform: MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(res.PointsSource), PointCloudVertices.ToVectors(res.PointsTarget), myLandmarkTransform);//, accumulate); res.Matrix = myLandmarkTransform.Matrix; return(res.Matrix); }
public void GetScale(float[] scale) { float[,] U = new float[3, 3]; float[,] VT = new float[3, 3]; for (int i = 0; i < 3; i++) { U[0, i] = Matrix[0, i]; U[1, i] = Matrix[1, i]; U[2, i] = Matrix[2, i]; } MathUtilsVTK.SingularValueDecomposition3x3(U, U, scale, VT); }
private Matrix4d Helper_FindTransformationMatrix(PointCloudVertices pointsSource, PointCloudVertices pointsTarget) { Matrix4d myMatrix; if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn) { MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), this.LandmarkTransform); myMatrix = LandmarkTransform.Matrix; } else { myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), ICPSettings.ICPVersion); } return(myMatrix); }
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); }
private float CheckNewPointDistance(int iPoint, Matrix4 myMatrix, PointCloud pointsTarget, PointCloud pointsSource) { Vector3 p1 = pointsTarget.Vectors[iPoint]; Vector3 p2 = pointsSource.Vectors[iPoint]; PointCloud tempPointReference = new PointCloud(); PointCloud tempPointToBeMatched = new PointCloud(); tempPointReference.AddVector(p1); tempPointToBeMatched.AddVector(p2); PointCloud tempPointRotate = MathUtilsVTK.TransformPoints(tempPointToBeMatched, myMatrix); float dist = PointCloud.MeanDistance(tempPointReference, tempPointRotate); return(dist); }
private Matrix4 Helper_FindTransformationMatrixOld(PointCloud pointsSource, PointCloud pointsTarget) { Matrix4 myMatrix; if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn) { MathUtilsVTK.FindTransformationMatrix(pointsSource, pointsTarget, this.LandmarkTransform); myMatrix = LandmarkTransform.Matrix; } else { myMatrix = SVD_Float.FindTransformationMatrix(pointsSource, pointsTarget, ICPSettings.ICPVersion); } return(myMatrix); }
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); }
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); }
private Matrix4 Helper_FindTransformationMatrix(PointCloud resultKDTree) { Matrix4 myMatrix; if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn) { MathUtilsVTK.FindTransformationMatrix(pointsSource, resultKDTree, this.LandmarkTransform); myMatrix = LandmarkTransform.Matrix; } else { if (ICPSettings.IgnoreFarPoints) { myMatrix = SVD.FindTransformationMatrix_MinimumDistance(pointsSource, resultKDTree, ICPSettings.ICPVersion, this.MeanDistance).ToMatrix4(); } else { myMatrix = SVD.FindTransformationMatrix(pointsSource, resultKDTree, ICPSettings.ICPVersion).ToMatrix4(); } } return(myMatrix); }
public void GetOrientationWXYZ(float[] wxyz) { int i; Matrix3 ortho = new Matrix3(); for (i = 0; i < 3; i++) { ortho[0, i] = Matrix[0, i]; ortho[1, i] = Matrix[1, i]; ortho[2, i] = Matrix[2, i]; } if (ortho.Determinant < 0) { ortho[0, i] = -ortho[0, i]; ortho[1, i] = -ortho[1, i]; ortho[2, i] = -ortho[2, i]; } float[,] orthoArray = ortho.ToFloatArray(); MathUtilsVTK.Matrix3x3ToQuaternion(orthoArray, wxyz); // calc the return value wxyz float mag = Convert.ToSingle(Math.Sqrt(wxyz[1] * wxyz[1] + wxyz[2] * wxyz[2] + wxyz[3] * wxyz[3])); if ((int)mag != 0) { wxyz[0] = Convert.ToSingle(2 * Math.Acos(wxyz[0]) / MathBase.DegreesToRadians); wxyz[1] /= mag; wxyz[2] /= mag; wxyz[3] /= mag; } else { wxyz[0] = 0.0f; wxyz[1] = 0.0f; wxyz[2] = 0.0f; wxyz[3] = 1.0f; } }
public bool Update() { /* * The solution is based on * Berthold K. P. Horn (1987), * "Closed-form solution of absolute orientation using unit quaternions," * Journal of the Optical Society of America A, 4:629-642 */ // Original python implementation by David G. Gobbi if (this.SourceLandmarks == null || this.TargetLandmarks == null) { //Identity Matrix this.Matrix = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW); return(false); } // --- compute the necessary transform to match the two sets of landmarks --- //int N_PTS = this.SourceLandmarks.Count; int N_PTS = Math.Min(this.SourceLandmarks.Count, this.TargetLandmarks.Count); //if (N_PTS != this.TargetLandmarks.Count) //{ // System.Diagnostics.Debug.WriteLine("Error: Source and Target Landmarks contain a different number of points"); // return false; //} // -- if no points, stop here if (N_PTS == 0) { //Identity Matrix this.Matrix = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW); return(false); } float[] source_centroid = { 0, 0, 0 }; float[] target_centroid = { 0, 0, 0 }; FindCentroids(N_PTS, source_centroid, target_centroid); ///------------------------------- // -- if only one point, stop right here if (N_PTS == 1) { this.Matrix = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW); Matrix[0, 3] = target_centroid[0] - source_centroid[0]; this.Matrix[1, 3] = target_centroid[1] - source_centroid[1]; this.Matrix[2, 3] = target_centroid[2] - source_centroid[2]; return(true); } // -- build the 3x3 matrix M -- float[,] M = new float[3, 3]; float[,] AAT = new float[3, 3]; for (int i = 0; i < 3; i++) { AAT[i, 0] = M[i, 0] = 0.0F; // fill M with zeros AAT[i, 1] = M[i, 1] = 0.0F; AAT[i, 2] = M[i, 2] = 0.0F; } int pt; for (pt = 0; pt < N_PTS; pt++) { float scale = 0F; float[,] N = CreateMatrixForDiag(pt, source_centroid, target_centroid, M, ref scale); float[,] eigenvectorData = new float[4, 4]; //float *eigenvectors[4],eigenvalues[4]; float[,] eigenvectors = new float[4, 4]; float[] eigenvalues = new float[4]; MathUtilsVTK.JacobiN(N, 4, eigenvalues, eigenvectors); //calculates this.Matrix: CreateMatrixOutOfDiagonalizationResult(eigenvectors, eigenvalues, N_PTS, source_centroid, target_centroid, scale); } return(true); }
private void ChooseFirstFourEigenvalues(ref float w, ref float x, ref float y, ref float z, float[,] eigenvectors, float[] eigenvalues, int N_PTS) { // first: if points are collinear, choose the quaternion that // results in the smallest rotation. if (eigenvalues[0] == eigenvalues[1] || N_PTS == 2) { Vector3 s0 = this.SourceLandmarks[0]; Vector3 t0 = this.TargetLandmarks[0]; Vector3 s1 = this.SourceLandmarks[1]; Vector3 t1 = this.TargetLandmarks[1]; float[] ds = new float[3]; float[] dt = new float[3]; float rs = 0; float rt = 0; for (int i = 0; i < 3; i++) { ds[i] = s1[i] - s0[i]; // vector between points rs += ds[i] * ds[i]; dt[i] = t1[i] - t0[i]; rt += dt[i] * dt[i]; } // normalize the two vectors rs = Convert.ToSingle(Math.Sqrt(rs)); ds[0] /= rs; ds[1] /= rs; ds[2] /= rs; rt = Convert.ToSingle(Math.Sqrt(rt)); dt[0] /= rt; dt[1] /= rt; dt[2] /= rt; // take dot & cross product w = ds[0] * dt[0] + ds[1] * dt[1] + ds[2] * dt[2]; x = ds[1] * dt[2] - ds[2] * dt[1]; y = ds[2] * dt[0] - ds[0] * dt[2]; z = ds[0] * dt[1] - ds[1] * dt[0]; float r = Convert.ToSingle(Math.Sqrt(x * x + y * y + z * z)); float theta = Convert.ToSingle(Math.Atan2(r, w)); // construct quaternion w = Convert.ToSingle(Math.Cos(theta / 2)); if (r != 0) { r = Convert.ToSingle(Math.Sin(theta / 2) / r); x = x * r; y = y * r; z = z * r; } else // rotation by 180 degrees: special case { // rotate around a vector perpendicular to ds MathUtilsVTK.Perpendiculars(ds, dt, null, 0); r = Convert.ToSingle(Math.Sin(theta / 2f)); x = dt[0] * r; y = dt[1] * r; z = dt[2] * r; } } else // points are not collinear { w = eigenvectors[0, 0]; x = eigenvectors[1, 0]; y = eigenvectors[2, 0]; z = eigenvectors[3, 0]; } }
/// <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 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; }
public PointCloud PerformICP() { //float convergenceThreshold = PTarget.BoundingBoxMaxFloat * ICPSettings.ConvergenceThreshold; //if (ICPSettings.ResetVector3ToOrigin) //{ //} if (ICPSettings.LogLevel > 0) { GlobalVariables.ShowLastTimeSpan("Before build"); } KDTree.Build(this.PTarget); if (ICPSettings.LogLevel > 0) { GlobalVariables.ShowLastTimeSpan("Build Tree"); } try { if (!CheckSourceTarget(PTarget, this.PSource)) { return(null); } pointsTarget = PointCloud.CloneAll(PTarget); pointsSource = PointCloud.CloneAll(PSource); this.Matrix = Matrix4.Identity; float oldMeanDistance = 0; if (ICPSettings.LogLevel > 0) { GlobalVariables.ShowLastTimeSpan("Start ICP"); } 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; Single_ICP_Iteration(angleThreshold); //Debug.WriteLine("--------------Iteration: " + NumberOfIterations.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000") ); if (ICPSettings.LogLevel > 0) { GlobalVariables.ShowLastTimeSpan("Iteration "); } if (MeanDistance < ICPSettings.ThresholdConvergence) //if (Math.Abs(oldMeanDistance - MeanDistance) < convergenceThreshold) { Debug.WriteLine("Convergence reached - changes under: " + ICPSettings.ThresholdConvergence.ToString()); break; } oldMeanDistance = MeanDistance; } //shuffle effect - set points source to other order //PS = pointsSource;//reordered for shuffle effect //this.PSource = pointsSource; if (this.ICPSettings.ShuffleEffect) { PTarget = pointsTarget; //PS = pointsSource;//reordered for shuffle effect //this.PSource = pointsSource; PointsTransformed = this.pointsTransformed; } else { PointsTransformed = MathUtilsVTK.TransformPoints(this.PSource, Matrix); } //ignore ICP result if the convergence results is bad if (MeanDistance > ICPSettings.ThresholdIgnoreICP) { Debug.WriteLine("ICP RESULT IGNORED - TOO BAD " + this.MeanDistance.ToString("0.0000")); return(this.PTarget); } if (this.pointsTransformed != null) { //DebugWriteUtils.WriteTestOutputVector3("Solution of ICP", Matrix, pointsSource, pointsTransformed, pointsTarget); } else { //no convergence - write matrix this.Matrix.Print("Cumulated Matrix "); } pointsTarget = PTarget; float thresh = this.MeanDistance; if (ICPSettings.SingleSourceTargetMatching) { PointsTransformed = this.PointsResultKDTree;// this.pointsTransformed; PointCloud.AddVector3(PointsTransformed, centroidSource); } else { PointsTransformed = PointCloud.CalculateMergedPoints(this.pointsTransformed, this.pointsTarget, this.KDTree, ICPSettings.ThresholdMergedPoints, out PointsAdded, this.ICPSettings.ChangeColorsOfMergedPoints); PointCloud.AddVector3(PointsTransformed, centroidSource); } Debug.WriteLine("--------****** Solution of ICP after : " + NumberOfIterations.ToString() + " iterations, Mean Distance: " + MeanDistance.ToString("0.00000000000") + " Matrix trace : " + this.Matrix.Trace.ToString("0.00") + " - points added : " + PointsAdded.ToString()); //not the best solution but ... PointsTransformed.SetDefaultIndices(); PointsTransformed.Name = "Result Cloud"; PointsTransformed.FileNameLong = "ResultCloud.obj"; return(PointsTransformed); } catch (Exception err) { System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + NumberOfIterations.ToString() + " : " + err.Message); return(null); } }
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); } }
public void GetOrientation(float[] orientation, Matrix4 amatrix) { int i; // convenient access to matrix //float[] matrixElement = amatrix; //float[,] ortho = new float[3, 3]; Matrix3 ortho = new Matrix3(); for (i = 0; i < 3; i++) { ortho[0, i] = amatrix[0, i]; ortho[1, i] = amatrix[1, i]; ortho[2, i] = amatrix[2, i]; } if (ortho.Determinant < 0) { ortho[0, 2] = -ortho[0, 2]; ortho[1, 2] = -ortho[1, 2]; ortho[2, 2] = -ortho[2, 2]; } float[,] orthoArray = ortho.ToFloatArray(); MathUtilsVTK.Orthogonalize3x3(orthoArray, orthoArray); // first rotate about y axis float x2 = ortho[2, 0]; float y2 = ortho[2, 1]; float z2 = ortho[2, 2]; float x3 = ortho[1, 0]; float y3 = ortho[1, 1]; float z3 = ortho[1, 2]; float d1 = Convert.ToSingle(Math.Sqrt(x2 * x2 + z2 * z2)); float cosTheta; float sinTheta; if (d1 < AXIS_EPSILON) { cosTheta = 1.0f; sinTheta = 0.0f; } else { cosTheta = z2 / d1; sinTheta = x2 / d1; } float theta = Convert.ToSingle(Math.Atan2(sinTheta, cosTheta)); orientation[1] = -theta / Convert.ToSingle(MathBase.DegreesToRadians); // now rotate about x axis float d = Convert.ToSingle(Math.Sqrt(x2 * x2 + y2 * y2 + z2 * z2)); float sinPhi; float cosPhi; if (d < AXIS_EPSILON) { sinPhi = 0.0f; cosPhi = 1.0f; } else if (d1 < AXIS_EPSILON) { sinPhi = y2 / d; cosPhi = z2 / d; } else { sinPhi = y2 / d; cosPhi = (x2 * x2 + z2 * z2) / (d1 * d); } float phi = Convert.ToSingle(Math.Atan2(sinPhi, cosPhi)); orientation[0] = Convert.ToSingle(phi / MathBase.DegreesToRadians); // finally, rotate about z float x3p = x3 * cosTheta - z3 * sinTheta; float y3p = -sinPhi * sinTheta * x3 + cosPhi * y3 - sinPhi * cosTheta * z3; float d2 = Convert.ToSingle(Math.Sqrt(x3p * x3p + y3p * y3p)); float cosAlpha; float sinAlpha; if (d2 < AXIS_EPSILON) { cosAlpha = 1.0f; sinAlpha = 0.0f; } else { cosAlpha = y3p / d2; sinAlpha = x3p / d2; } float alpha = Convert.ToSingle(Math.Atan2(sinAlpha, cosAlpha)); orientation[2] = alpha / MathBase.DegreesToRadians; }