private void SVD_OfPointCloud_Double(PointCloud pointsSource, bool normalsCovariance)
{
//calculate correlation matrix
//SVD.Eigenvalues_Helper(C);
//Matrix3d C = PointCloud.CovarianceMatrix_Double(pointsSource, normalsCovariance);
Matrix3d C = PointCloud.CovarianceMatrix3d(pointsSource, normalsCovariance);
SVD.Eigenvalues_Helper(C);
EV = new Vector3(Convert.ToSingle(SVD.EV.X), Convert.ToSingle(SVD.EV.Y), Convert.ToSingle(SVD.EV.Z));
VT = SVD.VT.ToMatrix3();
U = SVD.U.ToMatrix3();
}
/// <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_Double(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);
Matrix4d myMatrix4d = SVD.FindTransformationMatrix_WithoutCentroids(sourceAxes, targetAxes, ICP_VersionUsed.Umeyama);
//CheckPCA(myMatrix4d, sourceAxes, targetAxes);
PointCloud myResultPC = myMatrix4d.TransformPoints(pcSourceCentered);
//--------------
pcTreeResult = KDTree.FindClosestPointCloud_Parallel(myResultPC);
meanDistance = KDTree.MeanDistance;
//PointCloud myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);
System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G"));
if (meanDistance < bestResultMeanDistance)
{
myMatrixBestResult = myMatrix4d.ToMatrix4();
bestResultMeanDistance = meanDistance;
pcResultBest = myResultPC;
}
pcResult = myResultPC;
}
