public static List <Vector3> TransformPoints(this Matrix4 matrix, List <Vector3> a)
{
if (a == null || a.Count == 0)
{
return(null);
}
List <Vector3> b = new List <Vector3>();
float[,] matrixfloat = matrix.ToFloatArray();
for (int i = 0; i < a.Count; i++)
{
Vector3 p1 = a[i];
//Does not work with pointers...
//this.LandmarkTransform.InternalTransformPoint(PointerUtils.GetIntPtr(p1), PointerUtils.GetIntPtr(p2));
float[] pointFloat = new float[3] {
p1.X, p1.Y, p1.Z
};
float[] pointReturn = Matrix4Extension.TransformPointfloat(pointFloat, matrixfloat);
b.Add(new Vector3(pointReturn[0], pointReturn[1], pointReturn[2]));
}
return(b);
}
/// <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;
}