private Matrix4 AdjustSourceTargetByTranslation(Matrix4 myMatrixFound, PointCloud pointCloudSource, PointCloud pointCloudTarget)
{
Matrix3 R = myMatrixFound.ExtractMatrix3();
Vector3 T = SVD_Float.CalculateTranslation(pointCloudSource.CentroidVector, pointCloudTarget.CentroidVector, R);
myMatrixFound = myMatrixFound.AddTranslation(T);
return(myMatrixFound);
}
private void SVD_ForListVectorsMassCentered(List <Vector3> pointsSource, bool normalsCovariance)
{
//calculate correlation matrix
Matrix3 C = PointCloud.CovarianceMatrix(pointsSource, normalsCovariance);
SVD_Float.Eigenvalues_Helper(C);
EV = SVD_Float.EV;
VT = SVD_Float.VT;
U = SVD_Float.U;
V = Matrix3.Transpose(VT);
}
private void SVD_OfPointCloud_Float(PointCloud pointsSource, bool normalsCovariance)
{
//calculate correlation matrix
//SVD.Eigenvalues_Helper(C);
//Matrix3d C = PointCloud.CovarianceMatrix_Double(pointsSource, normalsCovariance);
Matrix3 C = PointCloud.CovarianceMatrix3(pointsSource, normalsCovariance);
SVD_Float.Eigenvalues_Helper(C);
EV = SVD_Float.EV;
VT = SVD_Float.VT;
U = SVD_Float.U;
}
public Matrix4 CalculateRegistrationMatrix(PointCloud pcOriginal)
{
//Vector3 v = new Vector3();
Matrix4 myMatrix = Matrix4.Identity;
if (this.GLrender.RenderableObjects.Count > 1)
{
RenderableObject o = this.GLrender.RenderableObjects[0];
PointCloud pc1 = o.PointCloud;
myMatrix = SVD_Float.FindTransformationMatrix(pcOriginal, pc1, ICP_VersionUsed.Zinsser);
//myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(v.ArrayVector3ToList(pc1.Vectors), v.ArrayVector3ToList(pc2.Vectors), ICP_VersionUsed.Scaling_Zinsser);
}
else
{
System.Windows.Forms.MessageBox.Show("Please load two Point Clouds first");
}
return(myMatrix);
}
public static Matrix4 FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3 R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3 T = SVD_Float.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4 myMatrix = new Matrix4();
myMatrix = myMatrix.PutTheMatrix4together(T, R);
return(myMatrix);
}
/// <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_Float(int i, int j, ref float meanDistance, ref Matrix4 myMatrixBestResult, PointCloud sourceAxes)
{
PointCloud targetAxes = InvertAxes(pcTargetCentered, pcTargetCentered.PCAAxes, j);
//Matrix4 myMatrix = SVD_Float.FindTransformationMatrix(sourceAxes, targetAxes, ICP_VersionUsed.Scaling_Umeyama);
Matrix4 myMatrix = SVD_Float.FindTransformationMatrix_WithoutCentroids(sourceAxes, targetAxes, ICP_VersionUsed.Umeyama);
//-----------------------
//for check - should give TargetPCVectors
List <Vector3> resultAxes = Matrix4Extension.TransformPoints(myMatrix, sourceAxes.ListVectors);
resultAxes = resultAxes.Subtract(targetAxes.ListVectors);
PointCloud myResultPC = myMatrix.TransformPoints(pcSourceCentered);
//--------------
pcTreeResult = KDTree.FindClosestPointCloud_Parallel(myResultPC);
meanDistance = KDTree.MeanDistance;
pcTreeResult = KDTree.BuildAndFindClosestPoints(myResultPC, this.pcTargetCentered, false);
meanDistance = KDTree.MeanDistance;
pcTreeResult = KDTree.FindClosestPointCloud_Parallel(myResultPC);
meanDistance = KDTree.MeanDistance;
pcTreeResult = KDTree.BuildAndFindClosestPoints(this.pcTargetCentered, this.pcTargetCentered, false);
meanDistance = KDTree.MeanDistance;
pcTreeResult = KDTree.BuildAndFindClosestPoints(this.pcSourceCentered, this.pcTargetCentered, false);
meanDistance = KDTree.MeanDistance;
pcTreeResult = KDTree.BuildAndFindClosestPoints(myResultPC, this.pcTargetCentered, false);
meanDistance = KDTree.MeanDistance;
//PointCloud myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);
float trace = myMatrix.Trace;
//float trace = kdtree.MeanDistance;
//float meanDistance = myMatrix.Trace;
//Check:
System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G"));
if (meanDistance < bestResultMeanDistance)
{
myMatrixBestResult = myMatrix;
bestResultMeanDistance = meanDistance;
pcResultBest = myResultPC;
}
pcResult = myResultPC;
}
