private KDTreeVertex Helper_CreateTree(List <Vertex> pointsTarget)
{
KDTreeVertex kdTree = new KDTreeVertex();
if (!FixedTestPoints)
{
if (KDTreeVertex.KDTreeMode == KDTreeMode.Stark)
{
kdTree.BuildKDTree_Stark(pointsTarget);
}
else if (KDTreeVertex.KDTreeMode == KDTreeMode.Rednaxala)
{
kdTree.BuildKDTree_Rednaxela(pointsTarget);
}
//else if(KDTreeMode == KDTreeMode.BruteForce)
}
return(kdTree);
}
private KDTreeVertex Helper_CreateTree(PointCloudVertices pointsTarget)
{
KDTreeVertex kdTree = new KDTreeVertex();
kdTree.KDTreeMode = ICPSettings.KDTreeMode;
if (!ICPSettings.FixedTestPoints)
{
if (kdTree.KDTreeMode == KDTreeMode.Stark)
{
kdTree.BuildKDTree_Stark(pointsTarget);
}
else if (kdTree.KDTreeMode == KDTreeMode.Rednaxela || kdTree.KDTreeMode == KDTreeMode.Rednaxela_ExcludePoints)
{
kdTree.BuildKDTree_Rednaxela(pointsTarget);
}
//else if(KDTreeMode == KDTreeMode.BruteForce)
}
return(kdTree);
}
public List <Vertex> PerformICP()
{
List <Vertex> PT = Vertices.CopyVertices(PTarget);
List <Vertex> PS = Vertices.CopyVertices(PSource);
Vertex pSOrigin = null;
Vertex pTOrigin = null;
if (ResetVertexToOrigin)
{
pTOrigin = Vertices.ResetVertexToOrigin(PT);
pSOrigin = Vertices.ResetVertexToOrigin(PS);
}
int keepOnlyPoints = 0;
if (DistanceOptimization)
{
keepOnlyPoints = 3;
}
int iter = 0;
try
{
if (!CheckSourceTarget(PT, PS))
{
return(null);
}
List <Vertex> pointsTarget = Vertices.CopyVertices(PT);
List <Vertex> pointsSource = Vertices.CopyVertices(PS);
this.Matrix = Matrix4d.Identity;
double oldMeanDistance = 0;
KDTreeVertex kdTreee = Helper_CreateTree(pointsTarget);
for (iter = 0; iter < MaximumNumberOfIterations; iter++)
{
if (NormalsCheck)
{
CalculateNormals(pointsSource, pointsTarget);
}
if (SimulatedAnnealing)
{
if (Helper_ICP_Iteration_SA(PT, PS, kdTreee, keepOnlyPoints))
{
break;
}
}
else
{
if (Helper_ICP_Iteration(ref pointsTarget, ref pointsSource, PT, PS, kdTreee, keepOnlyPoints))
{
break;
}
}
oldMeanDistance = MeanDistance;
Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));
}
Debug.WriteLine("--------****** Solution of ICP after : " + iter.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000"));
PTransformed = MathUtils.TransformPoints(PS, Matrix);
//re-reset vector
if (ResetVertexToOrigin)
{
Vertices.AddVector(PTransformed, pTOrigin);
//Vertices.AddVector(PSource, pSOrigin);
}
DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, this.PSource, PTransformed, PTarget);
return(PTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + iter.ToString() + " : " + err.Message);
return(null);
}
}
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);
}
/// <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 bool Helper_ICP_Iteration(ref List <Vertex> pointsTarget, ref List <Vertex> pointsSource, List <Vertex> PT, List <Vertex> PS, KDTreeVertex kdTree, int keepOnlyPoints)
{
if (!Helper_FindNeighbours(ref pointsTarget, ref pointsSource, kdTree, keepOnlyPoints))
{
return(true);
}
Matrix4d myMatrix = Helper_FindTransformationMatrix(pointsTarget, pointsSource);
List <Vertex> myPointsTransformed = MathUtils.TransformPoints(pointsSource, myMatrix);
if (SimulatedAnnealing)
{
this.Matrix = myMatrix;
double totaldist = PointUtils.CalculateTotalDistance(pointsTarget, myPointsTransformed);
this.MeanDistance = totaldist / Convert.ToDouble(pointsTarget.Count);
//new set:
pointsSource = myPointsTransformed;
pointsTarget = Vertices.CopyVertices(PT);
}
else
{
Matrix4d.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
double totaldist = PointUtils.CalculateTotalDistance(pointsTarget, myPointsTransformed);
this.MeanDistance = totaldist / Convert.ToDouble(pointsTarget.Count);
//Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));
if (MeanDistance < this.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
{
return(true);
}
Helper_SetNewInterationSets(ref pointsTarget, ref pointsSource, PT, PS);
}
return(false);
}
private bool Helper_FindNeighbours(ref List <Vertex> pointsTarget, ref List <Vertex> pointsSource, KDTreeVertex kdTree, int keepOnlyPoints)
{
if (!FixedTestPoints)
{
if (KDTreeVertex.KDTreeMode == KDTreeMode.Stark)
{
pointsTarget = kdTree.FindNearest_Stark(pointsSource, pointsTarget);
}
else if (KDTreeVertex.KDTreeMode == KDTreeMode.Rednaxala)
{
pointsTarget = kdTree.FindNearest_Rednaxela(pointsSource, pointsTarget, keepOnlyPoints);
}
else if (KDTreeVertex.KDTreeMode == KDTreeMode.BruteForce)
{
pointsTarget = kdTree.FindNearest_BruteForce(pointsSource, pointsTarget);
}
if (NormalsCheck)
{
//adjust normals - because of Search, the number of PointTarget my be different
int pointsRemoved = 0;
for (int i = pointsTarget.Count - 1; i >= 0; i--)
{
Vector3d vT = pointsTarget[i].Vector;
Vector3d vS = pointsSource[i].Vector;
//double angle = Vector3d.CalculateAngle(vT, vS);
int indexVec = pointsTarget[i].IndexInModel;
Vector3d vTNormal = NormalsTarget[pointsTarget[i].IndexInModel];
//Vector3d vTNormal = NormalsTarget[i];
Vector3d vSNormal = NormalsSource[i];
double angle = vTNormal.AngleInDegrees(vSNormal);
//double angle = vT.AngleInDegrees(vS);
if (Math.Abs(angle) > 30)
{
pointsTarget.RemoveAt(i);
pointsSource.RemoveAt(i);
pointsRemoved++;
}
}
Debug.WriteLine("--NormalCheck: Removed a total of: " + pointsRemoved.ToString());
}
if (pointsTarget.Count != pointsSource.Count)
{
MessageBox.Show("Error finding neighbours, found " + pointsTarget.Count.ToString() + " out of " + pointsSource.Count.ToString());
return(false);
}
}
else
{
//adjust number of points - for the case if there are outliers
int min = pointsSource.Count;
if (pointsTarget.Count < min)
{
min = pointsTarget.Count;
pointsSource.RemoveRange(pointsTarget.Count, pointsSource.Count - min);
}
else
{
pointsTarget.RemoveRange(pointsSource.Count, pointsTarget.Count - min);
}
}
return(true);
}
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);
}
}
private void CalculateNormals(PointCloud pointsSource, PointCloud pointsTarget, KDTreeVertex kdTreee)
{
if (ICPSettings.Normal_RemovePoints || ICPSettings.Normal_SortPoints)
{
CalculateNormals(pointsSource, pointsTarget);
kdTreee.NormalsSource = this.normalsSource;
kdTreee.NormalsTarget = this.normalsTarget;
if (ICPSettings.Normal_RemovePoints)
{
kdTreee.Normals_RemovePoints = true;
}
if (ICPSettings.Normal_SortPoints)
{
kdTreee.Normals_SortPoints = true;
}
}
}
/// <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);
}
private bool Helper_FindNeighbours(ref PointCloudVertices pointsSource, ref PointCloudVertices pointsTarget, KDTreeVertex kdTree, float angleThreshold)
{
if (!ICPSettings.FixedTestPoints)
{
if (ICPSettings.KDTreeMode == KDTreeMode.Stark)
{
pointsTarget = kdTree.FindNearest_Stark(pointsSource, pointsTarget);
}
else if (ICPSettings.KDTreeMode == KDTreeMode.Rednaxela)
{
pointsTarget = kdTree.FindNearest_Rednaxela_Parallel(ref pointsSource, pointsTarget, angleThreshold);
//pointsTarget = kdTree.FindNearest_Rednaxela(ref pointsSource, pointsTarget, angleThreshold);
}
else if (ICPSettings.KDTreeMode == KDTreeMode.Rednaxela_ExcludePoints)
{
pointsTarget = kdTree.FindNearest_Rednaxela_ExcludePoints(pointsSource, pointsTarget, false);
}
else if (ICPSettings.KDTreeMode == KDTreeMode.BruteForce)
{
pointsTarget = kdTree.FindNearest_BruteForce(pointsSource, pointsTarget);
}
}
else
{
//adjust number of points - for the case if there are outliers
int min = pointsSource.Count;
if (pointsTarget.Count < min)
{
min = pointsTarget.Count;
pointsSource.RemoveRange(pointsTarget.Count, pointsSource.Count - min);
}
else
{
pointsTarget.RemoveRange(pointsSource.Count, pointsTarget.Count - min);
}
}
return(true);
}
