private KDTreePoint3D Helper_CreateTree(IList <Point3D> pointsTarget)
{
KDTreePoint3D kdTree = new KDTreePoint3D();
kdTree.AddPoint(pointsTarget);
return(kdTree);
}
public Matrix4d PerformICP()
{
IList <Point3D> PT = PTarget.Copy();
IList <Point3D> PS = PSource.Copy();
Point3D pSOrigin = null;
Point3D pTOrigin = null;
if (ResetPoint3DToOrigin)
{
pTOrigin = PT.ResetToOrigin();
pSOrigin = PS.ResetToOrigin();
}
int keepOnlyPoints = 0;
if (DistanceOptimization)
{
keepOnlyPoints = 3;
}
int iter = 0;
try
{
if (!CheckSourceTarget(PT, PS))
{
return(Matrix4d.Identity);
}
IList <Point3D> pointsTarget = PT.Copy();
IList <Point3D> pointsSource = PS.Copy();
this.Matrix = Matrix4d.Identity;
Matrix4d bestMatrix = Matrix4d.Mult(this.Matrix, 1f);
double bestMeanDistance = (double)(pointsTarget.CalculateTotalDistance(pointsSource) / Convert.ToDouble(pointsTarget.Count));
double oldMeanDistance = 0;
KDTreePoint3D kdTreee = Helper_CreateTree(pointsTarget);
for (iter = 0; iter < MaximumNumberOfIterations; iter++)
{
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"));
if (bestMeanDistance > MeanDistance)
{
bestMeanDistance = MeanDistance;
bestMatrix = Matrix4d.Mult(this.Matrix, 1f);
}
else
{
return(bestMatrix);
}
Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : BEST: " + bestMeanDistance.ToString("0.00000000000"));
}
Debug.WriteLine("--------****** Solution of ICP after : " + iter.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000"));
return(bestMatrix);
//PTransformed = MathUtils.TransformPoints(PS, Matrix);
////re-reset vector
//if (ResetPoint3DToOrigin)
//{
// PTransformed.AddVector(pTOrigin);
// //Vertices.AddVector(PSource, pSOrigin);
//}
////DebugWriteUtils.WriteTestOutputPoint3D("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(Matrix4d.Identity);
}
}
private bool Helper_ICP_Iteration_SA(IList <Point3D> PT, IList <Point3D> PS, KDTreePoint3D 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 = 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++)
{
IList <Point3D> 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 IList <Point3D> pointsTarget, ref IList <Point3D> pointsSource, IList <Point3D> PT, IList <Point3D> PS, KDTreePoint3D kdTree, int keepOnlyPoints)
{
if (!Helper_FindNeighbours(ref pointsTarget, ref pointsSource, kdTree, keepOnlyPoints))
{
return(true);
}
Matrix4d myMatrix = Helper_FindTransformationMatrix(pointsTarget, pointsSource);
IList <Point3D> myPointsTransformed = MathUtils.TransformPoints(pointsSource, myMatrix);
if (SimulatedAnnealing)
{
this.Matrix = myMatrix;
double totaldist = pointsTarget.CalculateTotalDistance(myPointsTransformed);
this.MeanDistance = (double)(totaldist / Convert.ToDouble(pointsTarget.Count));
//new set:
pointsSource = myPointsTransformed;
pointsTarget = PT.Copy();
}
else
{
Matrix4d.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
double totaldist = pointsTarget.CalculateTotalDistance(myPointsTransformed);
this.MeanDistance = (double)(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 IList <Point3D> pointsTarget, ref IList <Point3D> pointsSource, KDTreePoint3D kdTree, int keepOnlyPoints)
{
if (!FixedTestPoints)
{
pointsTarget = kdTree.FindNearest(pointsSource, pointsTarget, keepOnlyPoints);
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);
}
