public PointCloudVertices FindNearest_BruteForceOld(PointCloudVertices vSource, PointCloudVertices vTarget)
{
PointCloudVertices nearestNeighbours = new PointCloudVertices();
int iMax = 10;
PointCloudVertices tempTarget = PointCloudVertices.CopyVertices(vTarget);
for (int i = 0; i < vSource.Count; i++)
{
//BuildKDTree_Standard(tempTarget);
Vertex p = vSource[i];
// Perform a nearest neighbour search around that point.
KDTreeRednaxela.NearestNeighbour <EllipseWrapper> pIter = null;
pIter = KdTree_Rednaxela.FindNearest_EuclidDistance(new float[] { Convert.ToSingle(p.Vector.X), Convert.ToSingle(p.Vector.Y), Convert.ToSingle(p.Vector.Z) }, iMax, -1);
while (pIter.MoveNext())
{
// Get the ellipse.
//var pEllipse = pIter.Current;
EllipseWrapper wr = pIter.CurrentPoint;
nearestNeighbours.Add(wr.Vertex);
tempTarget.RemoveAt(pIter.CurrentIndex);
break;
}
}
return(nearestNeighbours);
}
/// <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 PointCloudVertices pointsSourceSubset, ref PointCloudVertices pointsTargetSubset, int myNumberPoints,
LandmarkTransform myLandmarkTranform, PointCloudVertices pointsTarget, PointCloudVertices pointsSource, int maxNumberOfIterations)
{
try
{
if (CheckSourceTarget(pointsTarget, pointsSource))
{
return(null);
}
pointsTargetSubset = PointCloudVertices.CopyVertices(pointsTarget);
pointsSourceSubset = PointCloudVertices.CopyVertices(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);
}
PointCloudVertices.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);
}
}
private PointCloudVertices Helper_SetNewInterationSets(ref PointCloudVertices pointsSource, ref PointCloudVertices pointsTarget, PointCloudVertices PS, PointCloudVertices PT)
{
PointCloudVertices myPointsTransformed = MathUtilsVTK.TransformPoints(PS, Matrix);
this.Matrix.TransformVectorList(normalsSource);
pointsSource = myPointsTransformed;
pointsTarget = PointCloudVertices.CopyVertices(PT);
return(myPointsTransformed);
}
/// <summary>
/// assume - vectors are mass - centered!
/// </summary>
/// <param name="pointCloud"></param>
/// <param name="axesVectors"></param>
/// <param name="i"></param>
/// <returns></returns>
private PointCloudVertices InvertAxes(PointCloudVertices pointCloud, PointCloudVertices axesVectors, int i)
{
PointCloudVertices resultList = PointCloudVertices.CopyVertices(axesVectors);
if (i == -1)
{
return(resultList);
}
Vector3d v = resultList[i].Vector.Negate();
resultList[i] = new Vertex(resultList[i].IndexInModel, v);
return(resultList);
}
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);
}
////}
///// <summary>
///// PCA are center of mass - centered
///// </summary>
///// <param name="pointCloud"></param>
///// <param name="myCentroid"></param>
//private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered)
//{
// pointCloud.CentroidPCA = Centroid;
// pointCloud.PCAAxes = new PointCloud();
// for (int i = 0; i < 3; i++)
// {
// Vector3d v = VT.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV[i];
// double d = v.Length;
// Vertex ve = new Vertex(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
////}
////}
////}
///// <summary>
///// PCA are center of mass - centered
///// </summary>
///// <param name="pointCloud"></param>
///// <param name="myCentroid"></param>
//private void AssignPCVectors(PointCloud pointCloud, PointCloud mypointCloudSourceCentered)
//{
// pointCloud.CentroidPCA = Centroid;
// pointCloud.PCAAxes = new PointCloud();
// for (int i = 0; i < 3; i++)
// {
// Vector3d v = VT_NotNormalized.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV_NotNormalized[i];
// double d = v.Length;
// Vertex ve = new Vertex(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
//}
private static PointCloudVertices CalculateResults(Matrix3d Ub, Matrix3d Ua, PointCloudVertices pointCloudSource, Vector3d centroidB, Vector3d centroidA)
{
Matrix3d R;
Matrix3d.Mult(ref Ub, ref Ua, out R);
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.Rotate(pointCloudResult, R);
Vector3d t = centroidB - R.MultiplyVector(centroidA);
//Vertices.AddVector(pointCloudResult, t);
return(pointCloudResult);
}
public PointCloudVertices FindNearest_BruteForce(PointCloudVertices source, PointCloudVertices target)
{
PointCloudVertices result = new PointCloudVertices();
List <int> indicesTargetFound = new List <int>();
PointCloudVertices tempTarget = PointCloudVertices.CopyVertices(target);
for (int i = source.Count - 1; i >= 0; i--)
{
BuildKDTree_Stark(tempTarget);
int indexNearest = KdTree_Stark.FindNearest(source[i]);
result.Add(target[indexNearest]);
tempTarget.RemoveAt(indexNearest);
}
return(result);
}
public static PointCloudVertices RotateToOriginAxes(PointCloudVertices mypointCloudSource)
{
PCA pca = new PCA();
pca.PCA_OfPointCloud(mypointCloudSource);
Matrix3d R = new Matrix3d();
PointCloudVertices mypointCloudResult = PointCloudVertices.CopyVertices(mypointCloudSource);
R = R.Rotation_ToOriginAxes(mypointCloudResult.PCAAxes);
PointCloudVertices.Rotate(mypointCloudResult, R);
pca.PCA_OfPointCloud(mypointCloudResult);
mypointCloudResult.Path = mypointCloudSource.Path;
mypointCloudResult.FilaName = mypointCloudSource.FilaName;
return(mypointCloudResult);
}
public PointCloudVertices AlignToCenter(PointCloudVertices pointCloudSource)
{
pointCloudSourceCentered = CalculatePCA_Internal(pointCloudSource);
Matrix3d R = new Matrix3d();
R = R.RotationChangeBasis(PointCloudVertices.ToVectors(pointCloudSource.PCAAxes));
PointCloudVertices pointCloudResult = PointCloudVertices.CopyVertices(pointCloudSource);
PointCloudVertices.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloudVertices.Rotate(pointCloudResult, R);
pointCloudResultCentered = CalculatePCA_Internal(pointCloudResult);
return(pointCloudResult);
}
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);
}
}
/// <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;
}
