private static double TransformPoints(ref PointCloudVertices myPointsTransformed, PointCloudVertices pointsTarget, PointCloudVertices pointsSource, Matrix4d myMatrix)
{
myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSource, myMatrix);
double meanDistance = PointCloudVertices.MeanDistance(pointsTarget, myPointsTransformed);
//double meanDistance = totaldist / Convert.ToSingle(pointsTarget.Count);
return(meanDistance);
}
private static ICPSolution IterateStartPoints(PointCloudVertices pointsSource, PointCloudVertices 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 = PointCloudVertices.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.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: " + currentIteration.ToString() + " : " + err.Message);
return(null);
}
}
public PointCloudVertices AlignPointClouds_SVD_WithShuflleEffect(bool axesRotateEffect, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget)
{
try
{
if (pointCloudSource == null || pointCloudTarget == null || pointCloudSource.Count == 0 || pointCloudTarget.Count == 0)
{
System.Diagnostics.Debug.WriteLine("PCA - please check point clouds ");
return(null);
}
this.Matrix = Matrix4d.Identity;
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
PrepareTargetTree(pointCloudTarget);
PointCloudVertices myPointCloudIteration = PointCloudVertices.CloneVertices(pointCloudSource);
for (int i = 0; i < MaxmimumIterations; i++)
{
double meanDistance = SVD_Iteration(myPointCloudIteration);
System.Diagnostics.Debug.WriteLine("-->> Iteration " + i.ToString() + " : Mean Distance : " + meanDistance.ToString("G") + ": duration: " + GlobalVariables.TimeSpanString());
if (meanDistance < thresholdConvergence)
{
break;
}
myPointCloudIteration = pointCloudResultBest;
}
//final check:
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
//"Shuffle" effect - the target points are in other order after kdtree search:
//The mean distance calculated again, as check (was calculated before in the kdTree routine)
MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTargetKDTree);
System.Diagnostics.Debug.WriteLine("-->> TO CHECK: PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
//MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
//System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget);
pointCloudResult = Matrix.TransformPoints(pointCloudSource);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
//MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
//System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
//for display later:
}
catch
{
System.Windows.Forms.MessageBox.Show("Error aligning point cloud");
}
return(pointCloudResult);
}
public PointCloudVertices AlignPointClouds_OneVector(PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget, int vectorNumberSource, int vectorNumberTarget)
{
//-------------------
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
//second object:
//-----------
pointCloudTargetCentered = CalculatePCA_Internal(pointCloudTarget);
//Vector3d v = TargetPCVectors[vectorNumberTarget];
//v.X = -v.X;
//v.Y = -v.Y;
//v.Z = -v.Z;
//TargetPCVectors[vectorNumberTarget] = v;
Matrix3d R = new Matrix3d();
//R = R.RotationOneVectorToAnother(TargetPCVectors[vectorNumber], SourcePCVectors[vectorNumber]);
R = R.RotationOneVertexToAnother(pointCloudSource.PCAAxes[vectorNumberSource], pointCloudTarget.PCAAxes[vectorNumberTarget]);
//R.CheckRotationMatrix();
//
//test:
//Vector3d testV = R.MultiplyVector(sourceV);
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloudVertices.Rotate(pointCloudResult, R);
PointCloudVertices.AddVector(pointCloudResult, pointCloudTarget.CentroidVector);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
MeanDistance = PointCloudVertices.MeanDistance(pointCloudResult, pointCloudTarget);
System.Diagnostics.Debug.WriteLine("-->> PCA (V) - Mean Distance : " + MeanDistance.ToString("0.000000"));
return(pointCloudResult);
}
private double CheckNewPointDistance(int iPoint, Matrix4d myMatrix, PointCloudVertices pointsTarget, PointCloudVertices pointsSource)
{
Vertex p1 = pointsTarget[iPoint];
Vertex p2 = pointsSource[iPoint];
PointCloudVertices tempPointReference = new PointCloudVertices();
PointCloudVertices tempPointToBeMatched = new PointCloudVertices();
tempPointReference.Add(p1);
tempPointToBeMatched.Add(p2);
PointCloudVertices tempPointRotate = MathUtilsVTK.TransformPoints(tempPointToBeMatched, myMatrix);
double dist = PointCloudVertices.MeanDistance(tempPointReference, tempPointRotate);
return(dist);
}
public PointCloudVertices FindNearest_Rednaxela_Parallel(ref PointCloudVertices mypointsSource, PointCloudVertices mypointsTarget, float angleThreshold)
{
this.pointsSource = mypointsSource;
this.pointsTarget = mypointsTarget;
int indexI = -1;
try
{
PointCloudVertices pointsSourceNew = new PointCloudVertices(pointsSource);
PointCloudVertices pointsResult = new PointCloudVertices();
FindNearest_Rednaxela_HelperParallel();
for (int i = 0; i < pointsSource.Count; i++)
{
indexI = i;
Vertex v = pointsTarget[pointsSource[i].IndexKDTreeTarget];
pointsResult.Add(v);
}
this.MeanDistance = PointCloudVertices.MeanDistance(pointsResult, pointsSource);
pointsSource = pointsSourceNew;
if (pointsSource.Count != pointsResult.Count)
{
MessageBox.Show("Error finding neighbours, found " + pointsResult.Count.ToString() + " out of " + pointsSource.Count.ToString());
//return false;
}
mypointsSource = this.pointsSource;
return(pointsResult);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Finding neighbors at: " + indexI.ToString() + " : " + err.Message);
return(null);
}
}
public PointCloudVertices FindNearest_Rednaxela(ref PointCloudVertices mypointsSource, PointCloudVertices mypointsTarget, float angleThreshold)
{
this.pointsSource = mypointsSource;
this.pointsTarget = mypointsTarget;
int indexI = -1;
try
{
PointCloudVertices pointsSourceNew = new PointCloudVertices(pointsSource);
PointCloudVertices pointsResult = new PointCloudVertices();
FindNearest_Rednaxela_Helper();
for (int i = 0; i < listNeighbours.Count; i++)
{
indexI = i;
List <Neighbours> mySublist = listNeighbours[i];
mySublist.Sort(new NeighboursComparer());
Vertex v = pointsTarget[mySublist[0].IndexTarget];
pointsResult.Add(v);
}
if (Normals_RemovePoints)
{
listNeighbours.Sort(new NeighboursListComparer());
int minPoints = Math.Min(10, listNeighbours.Count - 1);
for (int i = 0; i < listNeighbours.Count; i++)
{
List <Neighbours> mySublist = listNeighbours[i];
int indexInsert = mySublist[0].IndexSource;
Vertex vSource = pointsSource[mySublist[0].IndexSource];
Vertex vTarget = pointsTarget[mySublist[0].IndexTarget];
pointsSourceNew[indexInsert] = vSource;
pointsResult[indexInsert] = vTarget;
if (i > minPoints)
{
if (mySublist[0].Angle > angleThreshold)
{
pointsSourceNew[indexInsert] = null;
pointsResult[indexInsert] = null;
}
}
}
for (int i = pointsSource.Count - 1; i >= 0; i--)
{
if (pointsSourceNew[i] == null)
{
pointsSourceNew.RemoveAt(i);
pointsResult.RemoveAt(i);
}
}
Debug.WriteLine("Remaining points : " + (pointsResult.Count * 1.0 / pointsTarget.Count * 100).ToString("0.00") + " %");
}
this.MeanDistance = PointCloudVertices.MeanDistance(pointsResult, pointsSource);
pointsSource = pointsSourceNew;
if (pointsSource.Count != pointsResult.Count)
{
MessageBox.Show("Error finding neighbours, found " + pointsResult.Count.ToString() + " out of " + pointsSource.Count.ToString());
//return false;
}
mypointsSource = this.pointsSource;
return(pointsResult);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Finding neighbors at: " + indexI.ToString() + " : " + err.Message);
return(null);
}
}
/// <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);
}
public PointCloudVertices PerformICP_Stitching()
{
int iPoint = 0;
try
{
PointCloudVertices pointsTarget = null;
PointCloudVertices pointsSource = null;
ICPSolution res = CalculateStartSolution(ref pointsSource, ref pointsTarget, ICPSettings.NumberOfStartTrialPoints, this.LandmarkTransform, this.PTarget, this.PSource, ICPSettings.MaximumNumberOfIterations);
if (res == null)
{
return(null);
}
Matrix4d myMatrix = res.Matrix;
double oldMeanDistance = 0;
//now try all points and check if outlier
for (iPoint = (pointsTarget.Count - 1); iPoint >= 0; iPoint--)
{
double 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)
{
PointCloudVertices pointsTargetTrial = PointCloudVertices.CopyVertices(res.PointsTarget);
PointCloudVertices pointsSourceTrial = PointCloudVertices.CopyVertices(res.PointsSource);
myMatrix = TryoutNewPoint(iPoint, pointsTarget, pointsSource, pointsTargetTrial, pointsSourceTrial, this.LandmarkTransform);
PointCloudVertices myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSourceTrial, myMatrix);
this.MeanDistance = PointCloudVertices.MeanDistance(pointsTargetTrial, myPointsTransformed);
// this.MeanDistance = totaldist / Convert.ToSingle(pointsTargetTrial.Count);
DebugWriteUtils.WriteTestOutputVertex("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.WriteTestOutputVertex("************* 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.WriteTestOutputVertex("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);
}
//Matrix4d newMatrix = accumulate.GetMatrix();
//this.Matrix = newMatrix;
}
