/// <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 void Single_ICP_Iteration(float angleThreshold)
{
try
{
PointsResultKDTree = null;
//for geometric objects:
if (this.ICPSettings.FixedTestPoints)
{
PointsResultKDTree = this.pointsTarget.Clone();
}
else
{
PointsResultKDTree = this.KDTree.FindClosestPointCloud_Parallel(pointsSource);
}
Matrix4 myMatrix = Helper_FindTransformationMatrix(PointsResultKDTree);
if (myMatrix.CheckNAN())
{
return;
}
//overall matrix
Matrix4.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
//transform points:
pointsTransformed = MathUtilsVTK.TransformPoints(this.PSource, Matrix);
//for the "shuffle" effect (point order of source and target is different)
if (this.ICPSettings.ShuffleEffect)
{
CheckDuplicates_SetInteractionSet();
}
else
{
SetNewSet();
}
this.MeanDistance = PointCloud.MeanDistance(pointsSource, PointsResultKDTree);
if (MeanDistance < ICPSettings.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
{
return;
}
//check: is this really needed ??
//for the "shuffle" effect (point order of source and target is different)
//if (this.ICPSettings.ShuffleEffect)
//{
// SetNewSet();
//}
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Single_ICP_Iteration: " + err.Message);
}
}
private static float TransformPoints(ref PointCloud myPointsTransformed, PointCloud pointsTarget, PointCloud pointsSource, Matrix4 myMatrix)
{
myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSource, myMatrix);
float meanDistance = PointCloud.MeanDistance(pointsTarget, myPointsTransformed);
return(meanDistance);
}
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(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 TryoutPointsSA(PointCloud pointsTarget, PointCloud pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
//transform:
MathUtilsVTK.FindTransformationMatrix(res.PointsSource, res.PointsTarget, myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
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);
}
public static Matrix4d TryoutPoints(PointCloudVertices pointsTarget, PointCloudVertices pointsSource, ICPSolution res, LandmarkTransform myLandmarkTransform)
{
res.PointsTarget = RandomUtils.ExtractPoints(pointsTarget, res.RandomIndices);
res.PointsSource = RandomUtils.ExtractPoints(pointsSource, res.RandomIndices);
//transform:
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(res.PointsSource), PointCloudVertices.ToVectors(res.PointsTarget), myLandmarkTransform);//, accumulate);
res.Matrix = myLandmarkTransform.Matrix;
return(res.Matrix);
}
public void GetScale(float[] scale)
{
float[,] U = new float[3, 3];
float[,] VT = new float[3, 3];
for (int i = 0; i < 3; i++)
{
U[0, i] = Matrix[0, i];
U[1, i] = Matrix[1, i];
U[2, i] = Matrix[2, i];
}
MathUtilsVTK.SingularValueDecomposition3x3(U, U, scale, VT);
}
private Matrix4d Helper_FindTransformationMatrix(PointCloudVertices pointsSource, PointCloudVertices pointsTarget)
{
Matrix4d myMatrix;
if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn)
{
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), ICPSettings.ICPVersion);
}
return(myMatrix);
}
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);
}
private float CheckNewPointDistance(int iPoint, Matrix4 myMatrix, PointCloud pointsTarget, PointCloud pointsSource)
{
Vector3 p1 = pointsTarget.Vectors[iPoint];
Vector3 p2 = pointsSource.Vectors[iPoint];
PointCloud tempPointReference = new PointCloud();
PointCloud tempPointToBeMatched = new PointCloud();
tempPointReference.AddVector(p1);
tempPointToBeMatched.AddVector(p2);
PointCloud tempPointRotate = MathUtilsVTK.TransformPoints(tempPointToBeMatched, myMatrix);
float dist = PointCloud.MeanDistance(tempPointReference, tempPointRotate);
return(dist);
}
private Matrix4 Helper_FindTransformationMatrixOld(PointCloud pointsSource, PointCloud pointsTarget)
{
Matrix4 myMatrix;
if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn)
{
MathUtilsVTK.FindTransformationMatrix(pointsSource, pointsTarget, this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
myMatrix = SVD_Float.FindTransformationMatrix(pointsSource, pointsTarget, ICPSettings.ICPVersion);
}
return(myMatrix);
}
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);
}
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);
}
private Matrix4 Helper_FindTransformationMatrix(PointCloud resultKDTree)
{
Matrix4 myMatrix;
if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn)
{
MathUtilsVTK.FindTransformationMatrix(pointsSource, resultKDTree, this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
if (ICPSettings.IgnoreFarPoints)
{
myMatrix = SVD.FindTransformationMatrix_MinimumDistance(pointsSource, resultKDTree, ICPSettings.ICPVersion, this.MeanDistance).ToMatrix4();
}
else
{
myMatrix = SVD.FindTransformationMatrix(pointsSource, resultKDTree, ICPSettings.ICPVersion).ToMatrix4();
}
}
return(myMatrix);
}
public void GetOrientationWXYZ(float[] wxyz)
{
int i;
Matrix3 ortho = new Matrix3();
for (i = 0; i < 3; i++)
{
ortho[0, i] = Matrix[0, i];
ortho[1, i] = Matrix[1, i];
ortho[2, i] = Matrix[2, i];
}
if (ortho.Determinant < 0)
{
ortho[0, i] = -ortho[0, i];
ortho[1, i] = -ortho[1, i];
ortho[2, i] = -ortho[2, i];
}
float[,] orthoArray = ortho.ToFloatArray();
MathUtilsVTK.Matrix3x3ToQuaternion(orthoArray, wxyz);
// calc the return value wxyz
float mag = Convert.ToSingle(Math.Sqrt(wxyz[1] * wxyz[1] + wxyz[2] * wxyz[2] + wxyz[3] * wxyz[3]));
if ((int)mag != 0)
{
wxyz[0] = Convert.ToSingle(2 * Math.Acos(wxyz[0]) / MathBase.DegreesToRadians);
wxyz[1] /= mag;
wxyz[2] /= mag;
wxyz[3] /= mag;
}
else
{
wxyz[0] = 0.0f;
wxyz[1] = 0.0f;
wxyz[2] = 0.0f;
wxyz[3] = 1.0f;
}
}
public bool Update()
{
/*
* The solution is based on
* Berthold K. P. Horn (1987),
* "Closed-form solution of absolute orientation using unit quaternions,"
* Journal of the Optical Society of America A, 4:629-642
*/
// Original python implementation by David G. Gobbi
if (this.SourceLandmarks == null || this.TargetLandmarks == null)
{
//Identity Matrix
this.Matrix = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW);
return(false);
}
// --- compute the necessary transform to match the two sets of landmarks ---
//int N_PTS = this.SourceLandmarks.Count;
int N_PTS = Math.Min(this.SourceLandmarks.Count, this.TargetLandmarks.Count);
//if (N_PTS != this.TargetLandmarks.Count)
//{
// System.Diagnostics.Debug.WriteLine("Error: Source and Target Landmarks contain a different number of points");
// return false;
//}
// -- if no points, stop here
if (N_PTS == 0)
{
//Identity Matrix
this.Matrix = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW);
return(false);
}
float[] source_centroid = { 0, 0, 0 };
float[] target_centroid = { 0, 0, 0 };
FindCentroids(N_PTS, source_centroid, target_centroid);
///-------------------------------
// -- if only one point, stop right here
if (N_PTS == 1)
{
this.Matrix = new Matrix4(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW);
Matrix[0, 3] = target_centroid[0] - source_centroid[0];
this.Matrix[1, 3] = target_centroid[1] - source_centroid[1];
this.Matrix[2, 3] = target_centroid[2] - source_centroid[2];
return(true);
}
// -- build the 3x3 matrix M --
float[,] M = new float[3, 3];
float[,] AAT = new float[3, 3];
for (int i = 0; i < 3; i++)
{
AAT[i, 0] = M[i, 0] = 0.0F; // fill M with zeros
AAT[i, 1] = M[i, 1] = 0.0F;
AAT[i, 2] = M[i, 2] = 0.0F;
}
int pt;
for (pt = 0; pt < N_PTS; pt++)
{
float scale = 0F;
float[,] N = CreateMatrixForDiag(pt, source_centroid, target_centroid, M, ref scale);
float[,] eigenvectorData = new float[4, 4];
//float *eigenvectors[4],eigenvalues[4];
float[,] eigenvectors = new float[4, 4];
float[] eigenvalues = new float[4];
MathUtilsVTK.JacobiN(N, 4, eigenvalues, eigenvectors);
//calculates this.Matrix:
CreateMatrixOutOfDiagonalizationResult(eigenvectors, eigenvalues, N_PTS, source_centroid, target_centroid, scale);
}
return(true);
}
private void ChooseFirstFourEigenvalues(ref float w, ref float x, ref float y, ref float z, float[,] eigenvectors, float[] eigenvalues, int N_PTS)
{
// first: if points are collinear, choose the quaternion that
// results in the smallest rotation.
if (eigenvalues[0] == eigenvalues[1] || N_PTS == 2)
{
Vector3 s0 = this.SourceLandmarks[0];
Vector3 t0 = this.TargetLandmarks[0];
Vector3 s1 = this.SourceLandmarks[1];
Vector3 t1 = this.TargetLandmarks[1];
float[] ds = new float[3];
float[] dt = new float[3];
float rs = 0;
float rt = 0;
for (int i = 0; i < 3; i++)
{
ds[i] = s1[i] - s0[i]; // vector between points
rs += ds[i] * ds[i];
dt[i] = t1[i] - t0[i];
rt += dt[i] * dt[i];
}
// normalize the two vectors
rs = Convert.ToSingle(Math.Sqrt(rs));
ds[0] /= rs; ds[1] /= rs; ds[2] /= rs;
rt = Convert.ToSingle(Math.Sqrt(rt));
dt[0] /= rt; dt[1] /= rt; dt[2] /= rt;
// take dot & cross product
w = ds[0] * dt[0] + ds[1] * dt[1] + ds[2] * dt[2];
x = ds[1] * dt[2] - ds[2] * dt[1];
y = ds[2] * dt[0] - ds[0] * dt[2];
z = ds[0] * dt[1] - ds[1] * dt[0];
float r = Convert.ToSingle(Math.Sqrt(x * x + y * y + z * z));
float theta = Convert.ToSingle(Math.Atan2(r, w));
// construct quaternion
w = Convert.ToSingle(Math.Cos(theta / 2));
if (r != 0)
{
r = Convert.ToSingle(Math.Sin(theta / 2) / r);
x = x * r;
y = y * r;
z = z * r;
}
else // rotation by 180 degrees: special case
{
// rotate around a vector perpendicular to ds
MathUtilsVTK.Perpendiculars(ds, dt, null, 0);
r = Convert.ToSingle(Math.Sin(theta / 2f));
x = dt[0] * r;
y = dt[1] * r;
z = dt[2] * r;
}
}
else // points are not collinear
{
w = eigenvectors[0, 0];
x = eigenvectors[1, 0];
y = eigenvectors[2, 0];
z = eigenvectors[3, 0];
}
}
/// <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 PointCloud PerformICP_Stitching()
{
int iPoint = 0;
try
{
PointCloud pointsTarget = null;
PointCloud pointsSource = null;
ICPSolution res = CalculateStartSolution(ref pointsSource, ref pointsTarget, ICPSettings.NumberOfStartTrialPoints, this.LandmarkTransform, this.PTarget, this.PSource, ICPSettings.MaximumNumberOfIterations);
if (res == null)
{
return(null);
}
Matrix4 myMatrix = res.Matrix;
float oldMeanDistance = 0;
//now try all points and check if outlier
for (iPoint = (pointsTarget.Count - 1); iPoint >= 0; iPoint--)
{
float 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)
{
PointCloud pointsTargetTrial = PointCloud.CloneAll(res.PointsTarget);
PointCloud pointsSourceTrial = PointCloud.CloneAll(res.PointsSource);
myMatrix = TryoutNewPoint(iPoint, pointsTarget, pointsSource, pointsTargetTrial, pointsSourceTrial, this.LandmarkTransform);
PointCloud myPointsTransformed = MathUtilsVTK.TransformPoints(pointsSourceTrial, myMatrix);
this.MeanDistance = PointCloud.MeanDistance(pointsTargetTrial, myPointsTransformed);
// this.MeanDistance = totaldist / Convert.ToSingle(pointsTargetTrial.Count);
//DebugWriteUtils.WriteTestOutputVector3("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.WriteTestOutputVector3("************* 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.WriteTestOutputVector3("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);
}
//Matrix4 newMatrix = accumulate.GetMatrix();
//this.Matrix = newMatrix;
}
public PointCloud PerformICP()
{
//float convergenceThreshold = PTarget.BoundingBoxMaxFloat * ICPSettings.ConvergenceThreshold;
//if (ICPSettings.ResetVector3ToOrigin)
//{
//}
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Before build");
}
KDTree.Build(this.PTarget);
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Build Tree");
}
try
{
if (!CheckSourceTarget(PTarget, this.PSource))
{
return(null);
}
pointsTarget = PointCloud.CloneAll(PTarget);
pointsSource = PointCloud.CloneAll(PSource);
this.Matrix = Matrix4.Identity;
float oldMeanDistance = 0;
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Start ICP");
}
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;
Single_ICP_Iteration(angleThreshold);
//Debug.WriteLine("--------------Iteration: " + NumberOfIterations.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000") );
if (ICPSettings.LogLevel > 0)
{
GlobalVariables.ShowLastTimeSpan("Iteration ");
}
if (MeanDistance < ICPSettings.ThresholdConvergence)
//if (Math.Abs(oldMeanDistance - MeanDistance) < convergenceThreshold)
{
Debug.WriteLine("Convergence reached - changes under: " + ICPSettings.ThresholdConvergence.ToString());
break;
}
oldMeanDistance = MeanDistance;
}
//shuffle effect - set points source to other order
//PS = pointsSource;//reordered for shuffle effect
//this.PSource = pointsSource;
if (this.ICPSettings.ShuffleEffect)
{
PTarget = pointsTarget;
//PS = pointsSource;//reordered for shuffle effect
//this.PSource = pointsSource;
PointsTransformed = this.pointsTransformed;
}
else
{
PointsTransformed = MathUtilsVTK.TransformPoints(this.PSource, Matrix);
}
//ignore ICP result if the convergence results is bad
if (MeanDistance > ICPSettings.ThresholdIgnoreICP)
{
Debug.WriteLine("ICP RESULT IGNORED - TOO BAD " + this.MeanDistance.ToString("0.0000"));
return(this.PTarget);
}
if (this.pointsTransformed != null)
{
//DebugWriteUtils.WriteTestOutputVector3("Solution of ICP", Matrix, pointsSource, pointsTransformed, pointsTarget);
}
else
{
//no convergence - write matrix
this.Matrix.Print("Cumulated Matrix ");
}
pointsTarget = PTarget;
float thresh = this.MeanDistance;
if (ICPSettings.SingleSourceTargetMatching)
{
PointsTransformed = this.PointsResultKDTree;// this.pointsTransformed;
PointCloud.AddVector3(PointsTransformed, centroidSource);
}
else
{
PointsTransformed = PointCloud.CalculateMergedPoints(this.pointsTransformed, this.pointsTarget, this.KDTree, ICPSettings.ThresholdMergedPoints, out PointsAdded, this.ICPSettings.ChangeColorsOfMergedPoints);
PointCloud.AddVector3(PointsTransformed, centroidSource);
}
Debug.WriteLine("--------****** Solution of ICP after : " + NumberOfIterations.ToString() + " iterations, Mean Distance: " + MeanDistance.ToString("0.00000000000") + " Matrix trace : " + this.Matrix.Trace.ToString("0.00") + " - points added : " + PointsAdded.ToString());
//not the best solution but ...
PointsTransformed.SetDefaultIndices();
PointsTransformed.Name = "Result Cloud";
PointsTransformed.FileNameLong = "ResultCloud.obj";
return(PointsTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + NumberOfIterations.ToString() + " : " + err.Message);
return(null);
}
}
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);
}
}
public void GetOrientation(float[] orientation, Matrix4 amatrix)
{
int i;
// convenient access to matrix
//float[] matrixElement = amatrix;
//float[,] ortho = new float[3, 3];
Matrix3 ortho = new Matrix3();
for (i = 0; i < 3; i++)
{
ortho[0, i] = amatrix[0, i];
ortho[1, i] = amatrix[1, i];
ortho[2, i] = amatrix[2, i];
}
if (ortho.Determinant < 0)
{
ortho[0, 2] = -ortho[0, 2];
ortho[1, 2] = -ortho[1, 2];
ortho[2, 2] = -ortho[2, 2];
}
float[,] orthoArray = ortho.ToFloatArray();
MathUtilsVTK.Orthogonalize3x3(orthoArray, orthoArray);
// first rotate about y axis
float x2 = ortho[2, 0];
float y2 = ortho[2, 1];
float z2 = ortho[2, 2];
float x3 = ortho[1, 0];
float y3 = ortho[1, 1];
float z3 = ortho[1, 2];
float d1 = Convert.ToSingle(Math.Sqrt(x2 * x2 + z2 * z2));
float cosTheta;
float sinTheta;
if (d1 < AXIS_EPSILON)
{
cosTheta = 1.0f;
sinTheta = 0.0f;
}
else
{
cosTheta = z2 / d1;
sinTheta = x2 / d1;
}
float theta = Convert.ToSingle(Math.Atan2(sinTheta, cosTheta));
orientation[1] = -theta / Convert.ToSingle(MathBase.DegreesToRadians);
// now rotate about x axis
float d = Convert.ToSingle(Math.Sqrt(x2 * x2 + y2 * y2 + z2 * z2));
float sinPhi;
float cosPhi;
if (d < AXIS_EPSILON)
{
sinPhi = 0.0f;
cosPhi = 1.0f;
}
else if (d1 < AXIS_EPSILON)
{
sinPhi = y2 / d;
cosPhi = z2 / d;
}
else
{
sinPhi = y2 / d;
cosPhi = (x2 * x2 + z2 * z2) / (d1 * d);
}
float phi = Convert.ToSingle(Math.Atan2(sinPhi, cosPhi));
orientation[0] = Convert.ToSingle(phi / MathBase.DegreesToRadians);
// finally, rotate about z
float x3p = x3 * cosTheta - z3 * sinTheta;
float y3p = -sinPhi * sinTheta * x3 + cosPhi * y3 - sinPhi * cosTheta * z3;
float d2 = Convert.ToSingle(Math.Sqrt(x3p * x3p + y3p * y3p));
float cosAlpha;
float sinAlpha;
if (d2 < AXIS_EPSILON)
{
cosAlpha = 1.0f;
sinAlpha = 0.0f;
}
else
{
cosAlpha = y3p / d2;
sinAlpha = x3p / d2;
}
float alpha = Convert.ToSingle(Math.Atan2(sinAlpha, cosAlpha));
orientation[2] = alpha / MathBase.DegreesToRadians;
}
