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