private void SetStartPoints(ref PointCloud points1, ref PointCloud points2, PointCloud pointsInput1, PointCloud pointsInput2)
{
List <int> randomIndices = RandomUtils.UniqueRandomIndices(3, pointsInput1.Count);
points1 = RandomUtils.ExtractPoints(pointsInput1, randomIndices);
points2 = RandomUtils.ExtractPoints(pointsInput2, randomIndices);
}
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);
}
public static Matrix4d TryoutPoints(List <Vertex> pointsTarget, List <Vertex> pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
res.PointsTarget = RandomUtils.ExtractPoints(pointsTarget, res.RandomIndices);
res.PointsSource = RandomUtils.ExtractPoints(pointsSource, res.RandomIndices);
//transform:
MatrixUtilsNew.FindTransformationMatrix(Vertices.ConvertToVector3dList(res.PointsSource), Vertices.ConvertToVector3dList(res.PointsTarget), myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
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 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);
}
}
