public PointCloud AlignPointClouds_SVD(PointCloud pointCloudSource, PointCloud pointCloudTarget)
{
try
{
if (pointCloudSource == null || pointCloudTarget == null || pointCloudSource.Count == 0 || pointCloudTarget.Count == 0)
{
System.Diagnostics.Debug.WriteLine("PCA - please check point clouds ");
return(null);
}
this.Matrix = Matrix4.Identity;
// pointCloudSourceCentered = ShiftByCenterOfMass(pointCloudSource);
pcSourceCentered = CalculatePCA_Internal(pointCloudSource);
PrepareTargetTree(pointCloudTarget);
PointCloud myPointCloudIteration = PointCloud.CloneAll(pointCloudSource);
for (int i = 0; i < MaxmimumIterations; i++)
{
float meanDistance = SVD_Iteration(myPointCloudIteration);
System.Diagnostics.Debug.WriteLine("-->> Iteration " + i.ToString() + " : Mean Distance : " + meanDistance.ToString("G") + ": duration: " + GlobalVariables.TimeSpanString());
//myPointCloudIteration = pcResultBest;
if (meanDistance < ThresholdConvergence)
{
break;
}
}
//final check:
//this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget);
pcResult = Matrix.TransformPoints(pointCloudSource);
pcTreeResult = KDTree.FindClosestPointCloud_Parallel(pcResult);
MeanDistance = KDTree.MeanDistance;
pcTreeResult = KDTree.FindClosestPointCloud_Parallel(pcResultBest);
MeanDistance = KDTree.MeanDistance;
//"Shuffle" effect - the target points are in other order after kdtree search:
//The mean distance calculated again, as check (was calculated before in the kdTree routine)
System.Diagnostics.Debug.WriteLine("-->> TO CHECK: PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
//MeanDistance = PointCloud.MeanDistance(pointCloudResult, pointCloudTarget);
//System.Diagnostics.Debug.WriteLine("-->> PCA (SVD) - Final Mean Distance : " + MeanDistance.ToString("G"));
this.Matrix = AdjustSourceTargetByTranslation(Matrix, pointCloudSource, pointCloudTarget);
pcResult = Matrix.TransformPoints(pointCloudSource);
pcResultCentered = CalculatePCA_Internal(pcResult);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error aligning point cloud" + err.Message);
}
return(pcResult);
}
private List <Vector3> TransformPointsAfterPCA(List <Vector3> listVector3)
{
PointCloud pc = PointCloud.FromListVector3(listVector3);
List <Vector3> listResult = PointCloud.CloneAll(pc).ListVectors;
Matrix3 R = Matrix3.Mult(U, VT);
listResult = TransformPoints(listResult, R);
return(listResult);
}
/// <summary>
/// assume - vectors are mass - centered!
/// </summary>
/// <param name="pointCloud"></param>
/// <param name="axesVectors"></param>
/// <param name="i"></param>
/// <returns></returns>
private PointCloud InvertAxes(PointCloud pointCloud, PointCloud axesVectors, int i)
{
PointCloud resultC = PointCloud.CloneAll(axesVectors);
if (i == -1)
{
return(resultC);
}
resultC.Vectors[i] = resultC.Vectors[i].Negate();
return(resultC);
}
public PointCloud AlignPointClouds_OneVector(PointCloud pointCloudSource, PointCloud pointCloudTarget, int vectorNumberSource, int vectorNumberTarget)
{
//-------------------
pcSourceCentered = CalculatePCA_Internal(pointCloudSource);
//second object:
//-----------
pcTargetCentered = CalculatePCA_Internal(pointCloudTarget);
//Vector3 v = TargetPCVectors[vectorNumberTarget];
//v.X = -v.X;
//v.Y = -v.Y;
//v.Z = -v.Z;
//TargetPCVectors[vectorNumberTarget] = v;
Matrix3 R = new Matrix3();
//R = R.RotationOneVectorToAnother(TargetPCVectors[vectorNumber], SourcePCVectors[vectorNumber]);
R = R.RotationOneVectorToAnother(pointCloudSource.PCAAxes[vectorNumberSource].Vector, pointCloudTarget.PCAAxes[vectorNumberTarget].Vector);
//R.CheckRotationMatrix();
//
//test:
//Vector3 testV = R.MultiplyVector(sourceV);
PointCloud pointCloudResult = PointCloud.CloneAll(pointCloudSource);
PointCloud.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloud.Rotate(pointCloudResult, R);
PointCloud.AddVectorToAll(pointCloudResult, pointCloudTarget.CentroidVector);
pcResultCentered = CalculatePCA_Internal(pointCloudResult);
MeanDistance = PointCloud.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++)
// {
// Vector3 v = VT.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV[i];
// float d = v.Length;
// Vector3 ve = new Vector3(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++)
// {
// Vector3 v = VT_NotNormalized.ExtractColumn(i);
// //v = v * Math.Sqrt(EV[i]);
// v = v * EV_NotNormalized[i];
// float d = v.Length;
// Vector3 ve = new Vector3(i, v);
// pointCloud.PCAAxes.Add(ve);
// }
// mypointCloudSourceCentered.PCAAxes = pointCloud.PCAAxes;
//}
private static PointCloud CalculateResults(Matrix3 Ub, Matrix3 Ua, PointCloud pointCloudSource, Vector3 centroidB, Vector3 centroidA)
{
Matrix3 R;
Matrix3.Mult(ref Ub, ref Ua, out R);
PointCloud pointCloudResult = PointCloud.CloneAll(pointCloudSource);
PointCloud.Rotate(pointCloudResult, R);
Vector3 t = centroidB - R.MultiplyVector(centroidA);
//Vertices.AddVector(pointCloudResult, t);
return(pointCloudResult);
}
public static PointCloud RotateToOriginAxes(PointCloud mypointCloudSource)
{
PCA pca = new PCA();
pca.PCA_OfPointCloud(mypointCloudSource);
Matrix3 R = new Matrix3();
PointCloud mypointCloudResult = PointCloud.CloneAll(mypointCloudSource);
R = R.Rotation_ToOriginAxes(mypointCloudResult.PCAAxes);
PointCloud.Rotate(mypointCloudResult, R);
pca.PCA_OfPointCloud(mypointCloudResult);
mypointCloudResult.Path = mypointCloudSource.Path;
mypointCloudResult.Name = mypointCloudSource.Name;
return(mypointCloudResult);
}
public PointCloud AlignToCenter(PointCloud pointCloudSource)
{
pcSourceCentered = CalculatePCA_Internal(pointCloudSource);
Matrix3 R = new Matrix3();
R = R.RotationChangeBasis(pointCloudSource.PCAAxes.ListVectors);
PointCloud pointCloudResult = PointCloud.CloneAll(pointCloudSource);
PointCloud.SubtractVectorRef(pointCloudResult, pointCloudSource.CentroidVector);
PointCloud.Rotate(pointCloudResult, R);
pcResultCentered = CalculatePCA_Internal(pointCloudResult);
return(pointCloudResult);
}
