private Matrix4d AdjustSourceTargetByTranslation(Matrix4d myMatrixFound, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget)
{
Matrix3d R = myMatrixFound.ExtractMatrix3d();
Vector3d T = SVD.CalculateTranslation(pointCloudSource.CentroidVector, pointCloudTarget.CentroidVector, R);
myMatrixFound = myMatrixFound.AddTranslation(T);
return(myMatrixFound);
}
private void SVD_OfPointCloud(PointCloudVertices pointsSource, bool normalsCovariance)
{
//calculate correlation matrix
Matrix3d C = PointCloudVertices.CovarianceMatrix(pointsSource, normalsCovariance);
SVD.Eigenvalues_Helper(C);
EV = SVD.EV;
VT = SVD.VT;
U = SVD.U;
}
private void SVD_ForListVectorsMassCentered(List <Vector3d> pointsSource, bool normalsCovariance)
{
//calculate correlation matrix
Matrix3d C = PointCloudVertices.CovarianceMatrix(pointsSource, normalsCovariance);
SVD.Eigenvalues_Helper(C);
EV = SVD.EV;
VT = SVD.VT;
U = SVD.U;
V = Matrix3d.Transpose(VT);
}
/// <summary>
/// calculates Matrix for alignment of sourceAxes and targetAxes; sets pointCloudResult
/// </summary>
/// <param name="i"></param>
/// <param name="j"></param>
/// <param name="bestResultMeanDistance"></param>
/// <param name="meanDistance"></param>
/// <param name="myMatrixBestResult"></param>
/// <param name="sourceAxes"></param>
private void SVD_ForTwoPointCloudAlignment(int i, int j, ref double bestResultMeanDistance, ref double meanDistance, ref Matrix4d myMatrixBestResult, PointCloudVertices sourceAxes)
{
PointCloudVertices targetAxes = InvertAxes(pointCloudTargetCentered, pointCloudTargetCentered.PCAAxes, j);
Matrix4d myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
//Matrix4d myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
//-----------------------
//for check - should give TargetPCVectors
List <Vector3d> resultAxes = Matrix4dExtension.TransformPoints(myMatrix, PointCloudVertices.ToVectors(sourceAxes));
resultAxes = resultAxes.Subtract(PointCloudVertices.ToVectors(targetAxes));
List <Vector3d> myPointsResult = myMatrix.TransformPoints(PointCloudVertices.ToVectors(pointCloudSourceCentered));
PointCloudVertices myPointsResultTemp = PointCloudVertices.FromVectors(myPointsResult);
PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela_Parallel(ref myPointsResultTemp, pointCloudTargetCentered, -1);
//PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);
double trace = myMatrix.Trace;
meanDistance = kdtree.MeanDistance;
//double trace = kdtree.MeanDistance;
//double meanDistance = myMatrix.Trace;
//Check:
System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G") + " : Trace: " + trace.ToString("G"));
if (meanDistance < bestResultMeanDistance)
{
myMatrixBestResult = myMatrix;
bestResultMeanDistance = meanDistance;
pointCloudResultBest = PointCloudVertices.FromVectors(myPointsResult);
}
pointCloudResult = PointCloudVertices.FromVectors(myPointsResult);
pointCloudTargetKDTree = myPointCloudTargetTemp;
}
public Matrix4d CalculateRegistrationMatrix(PointCloud pcOriginal)
{
Vector3d v = new Vector3d();
Matrix4d myMatrix = Matrix4d.Identity;
if (this.GLrender.RenderableObjects.Count > 1)
{
RenderableObject o = this.GLrender.RenderableObjects[0];
PointCloud pc1 = o.PointCloud;
myMatrix = SVD.FindTransformationMatrix(v.ArrayVector3ToList(pcOriginal.Vectors), v.ArrayVector3ToList(pc1.Vectors), ICP_VersionUsed.Scaling_Umeyama);
//myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(v.ArrayVector3ToList(pc1.Vectors), v.ArrayVector3ToList(pc2.Vectors), ICP_VersionUsed.Scaling_Umeyama);
}
else
{
System.Windows.Forms.MessageBox.Show("Please load two Point Clouds first");
}
return(myMatrix);
}
public static Matrix4d FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3d centroidTarget = pointsTarget.CalculateCentroid();
List <Vector3d> pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3d centroidSource = pointsSource.CalculateCentroid();
List <Vector3d> pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3d T = SVD.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);
return(myMatrix);
}
public List <Vertex> PerformICP_New()
{
int iter = 0;
try
{
if (!CheckSourceTarget(PTarget, PSource))
{
return(null);
}
if (ResetVertexToOrigin)
{
Vertices.ResetVertexToOrigin(PTarget);
Vertices.ResetVertexToOrigin(PSource);
}
List <Vertex> pointsTargetIterate = Vertices.CopyVertices(PTarget);
List <Vertex> pointsSourceIterate = Vertices.CopyVertices(PSource);
Matrix4d myMatrix;
if (!FixedTestPoints)
{
BuildKDTree(pointsTargetIterate, pointsSourceIterate);
}
this.Matrix = Matrix4d.Identity;
double oldMeanDistance = 0;
for (iter = 0; iter < MaximumNumberOfIterations; iter++)
{
//pointsSourceIterate = PSource;
if (!FixedTestPoints)
{
pointsTargetIterate = SearchNearestNeighbours(pointsSourceIterate, pointsTargetIterate);
if (pointsTargetIterate.Count != pointsSourceIterate.Count)
{
MessageBox.Show("Error finding neighbours, found " + pointsSourceIterate.Count.ToString() + " out of " + pointsTargetIterate.ToString());
break;
}
}
List <Vertex> myPointsTransformed = null;
if (ICPVersion == ICP_VersionUsed.Quaternions)
{
TransformPointsUtils.FindTransformationMatrix(Vertices.Vector3dListFromVertexList(pointsSourceIterate), Vertices.Vector3dListFromVertexList(pointsTargetIterate), this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
myPointsTransformed = MathUtils.TransformPoints(pointsSourceIterate, myMatrix);
DebugWriteUtils.WriteTestOutputVertex("Quaternion Method", myMatrix, pointsSourceIterate, myPointsTransformed, pointsTargetIterate);
}
else
{
myMatrix = SVD.FindTransformationMatrix(Vertices.Vector3dListFromVertexList(pointsTargetIterate), Vertices.Vector3dListFromVertexList(pointsSourceIterate), ICPVersion);
//DebugWriteUtils.WriteMatrix("Solution with scaling", trialM);
myPointsTransformed = MathUtils.TransformPoints(pointsSourceIterate, myMatrix);
DebugWriteUtils.WriteTestOutputVertex("New Method WITH Scale", myMatrix, pointsSourceIterate, myPointsTransformed, pointsTargetIterate);
}
double totaldist = PointUtils.CalculateTotalDistance(pointsTargetIterate, myPointsTransformed);
this.MeanDistance = totaldist / Convert.ToDouble(pointsTargetIterate.Count);
Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));
//accumulate resulting matrix
Matrix4d.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
if (MeanDistance < this.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
{
break;
}
oldMeanDistance = MeanDistance;
DebugWriteUtils.WriteMatrix("Concatenated matrix", Matrix);
myPointsTransformed = MathUtils.TransformPoints(PSource, myMatrix);
//Matrix4.Mult(ref preMatrix, ref myMatrix, out preMatrix);
pointsSourceIterate = myPointsTransformed;
pointsTargetIterate = Vertices.CopyVertices(PTarget);
}
Debug.WriteLine("--------****** Solution of ICP after : " + iter.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000"));
this.PTransformed = MathUtils.TransformPoints(PSource, Matrix);
DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, PSource, PTransformed, PTarget);
return(PTransformed);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + iter.ToString() + " : " + err.Message);
return(null);
}
}
