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);
}
}
private PointCloud ICPOnPoints_WithSubset(PointCloud mypointCloudTarget, PointCloud myPCLToBeMatched, PointCloud myPointsTargetSubset, PointCloud mypointsSourceSubset)
{
List <Vector3> myVectorsTransformed = null;
PointCloud myPCLTransformed = null;
try
{
Matrix4 m;
PerformICP(mypointsSourceSubset, myPointsTargetSubset);
myVectorsTransformed = PointsTransformed.ListVectors;
m = Matrix;
//DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
//extend points:
//myPointsTransformed = icpSharp.TransformPointsToPointsData(mypointsSourceSubset, m);
//-----------------------------
//DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
//now with all other points as well...
myVectorsTransformed = new List <Vector3>();
myPCLTransformed = m.TransformPoints(myPCLToBeMatched);
//write all results in debug output
DebugWriteUtils.WriteTestOutputVector3("Soluation of Points With Subset", m, myPCLToBeMatched, myPCLTransformed, mypointCloudTarget);
}
catch (Exception err)
{
System.Diagnostics.Debug.WriteLine("Error in ICP : " + err.Message);
return(null);
}
//for output:
return(myPCLTransformed);
}
private List <Vertex> ICPOnPoints_WithSubset(List <Vertex> myVerticesTarget, List <Vertex> myVerticesToBeMatched, List <Vertex> myPointsTargetSubset, List <Vertex> mypointsSourceSubset)
{
List <Vector3d> myVectorsTransformed = null;
List <Vertex> myVerticesTransformed = null;
try
{
Matrix4d m;
PerformICP(myPointsTargetSubset, mypointsSourceSubset);
myVectorsTransformed = Vertices.ConvertToVector3dList(PTransformed);
m = Matrix;
//DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
//extend points:
//myPointsTransformed = icpSharp.TransformPointsToPointsData(mypointsSourceSubset, m);
//-----------------------------
//DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
//now with all other points as well...
myVectorsTransformed = new List <Vector3d>();
myVectorsTransformed = MathUtils.TransformPoints(Vertices.ConvertToVector3dList(myVerticesToBeMatched), m);
myVerticesTransformed = Vertices.ConvertVector3DListToVertexList(myVectorsTransformed);
//write all results in debug output
DebugWriteUtils.WriteTestOutputVertex("Soluation of Points With Subset", m, myVerticesToBeMatched, myVerticesTransformed, myVerticesTarget);
}
catch (Exception err)
{
System.Diagnostics.Debug.WriteLine("Error in ICP : " + err.Message);
return(null);
}
//for output:
return(myVerticesTransformed);
}
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 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);
}
}
/// <summary>
/// a simulated annealing like technique
/// </summary>
/// <param name="pointsTarget"></param>
/// <param name="pointsSource"></param>
/// <param name="myNumberPoints"></param>
/// <param name="myLandmarkTransform"></param>
/// <param name="maxSolutions"></param>
/// <returns></returns>
private static ICPSolution IterateSA(List <Vertex> pointsTarget, List <Vertex> pointsSource, int myNumberPoints, int maxSolutions)
{
int i = 0;
//int currentIteration = 0;
try
{
if (myNumberPoints > pointsTarget.Count)
{
myNumberPoints = pointsTarget.Count;
}
List <ICPSolution> solutionList = new List <ICPSolution>();
for (i = 0; i < maxSolutions; i++)
{
ICPSolution myTrial = ICPSolution.SetRandomIndices(myNumberPoints, pointsSource.Count, solutionList);
//myTrial.PointsTargetTrial = RandomUtils.ExtractPoints(pointsTarget, myTrial.RandomIndices);
myTrial.PointsSource = RandomUtils.ExtractPoints(pointsSource, myTrial.RandomIndices);
//myTrial.Matrix = TryoutPointsSA(pointsTarget, pointsSource, myTrial, myLandmarkTransform);//, accumulate);
myTrial.PointsTransformed = MathUtils.TransformPoints(myTrial.PointsSource, myTrial.Matrix);
double totaldist = PointUtils.CalculateTotalDistance(myTrial.PointsTarget, myTrial.PointsTransformed);
myTrial.MeanDistance = totaldist / Convert.ToDouble(myTrial.PointsSource.Count);
solutionList.Add(myTrial);
}
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: " + i.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: " + i.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);
}
}
