/// <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 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);
}
}