/// <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 PointCloud pointsSourceSubset, ref PointCloud pointsTargetSubset, int myNumberPoints,
LandmarkTransform myLandmarkTranform, PointCloud pointsTarget, PointCloud pointsSource, int maxNumberOfIterations)
{
try
{
if (CheckSourceTarget(pointsTarget, pointsSource))
{
return(null);
}
pointsTargetSubset = PointCloud.CloneAll(pointsTarget);
pointsSourceSubset = PointCloud.CloneAll(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);
}
PointCloud.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);
}
}
public IterativeClosestPointTransform()//:base(PointerUtils.GetIntPtr(new double[3]), true, true)
{
Reset();
this.PSource = null;
this.PTarget = null;
this.LandmarkTransform = new LandmarkTransform();
this.NumberOfIterations = 0;
this.MeanDistance = 0.0f;
}
public IterativeClosestPointTransform()//:base(PointerUtils.GetIntPtr(new float[3]), true, true)
{
KDTree = new KDTreeKennell();
//KDTree = new KDTreeJeremyC();
//KDTree = new KDTreeBruteForce();
Reset();
this.PSource = null;
this.PTarget = null;
this.LandmarkTransform = new LandmarkTransform();
this.NumberOfIterations = 0;
}
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 TryoutPoints(PointCloud pointsTarget, PointCloud pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
res.PointsTarget = RandomUtils.ExtractPoints(pointsTarget, res.RandomIndices);
res.PointsSource = RandomUtils.ExtractPoints(pointsSource, res.RandomIndices);
//transform:
MathUtilsVTK.FindTransformationMatrix(res.PointsSource, res.PointsTarget, myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
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);
}
public static Matrix4d TryoutPointsSA(List <Vertex> pointsTarget, List <Vertex> pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
//transform:
MatrixUtilsNew.FindTransformationMatrix(Vertices.ConvertToVector3dList(res.PointsSource), Vertices.ConvertToVector3dList(res.PointsTarget), myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
public static Matrix4d TryoutPointsSA(PointCloudVertices pointsTarget, PointCloudVertices pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
//transform:
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(res.PointsSource), PointCloudVertices.ToVectors(res.PointsTarget), myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
private static Matrix4d TryoutNewPoint(int iPoint, List <Vertex> pointsTarget, List <Vertex> pointsSource, List <Vertex> pointsTargetTrial, List <Vertex> pointsSourceTrial, LandmarkTransform myLandmarkTransform)
{
Vertex p1 = pointsTarget[iPoint];
Vertex p2 = pointsSource[iPoint];
pointsTargetTrial.Add(p1);
pointsSourceTrial.Add(p2);
MatrixUtilsNew.FindTransformationMatrix(Vertices.ConvertToVector3dList(pointsSourceTrial), Vertices.ConvertToVector3dList(pointsTargetTrial), myLandmarkTransform);//, accumulate);
Matrix4d myMatrix = myLandmarkTransform.Matrix;
return(myMatrix);
}
public static void FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, LandmarkTransform myLandmarkTransform)
{
myLandmarkTransform.Matrix = new Matrix4d();
myLandmarkTransform.SourceLandmarks = pointsSource;
myLandmarkTransform.TargetLandmarks = pointsTarget;
myLandmarkTransform.Update();
}
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);
}
