public static List <Vector3d> TransformPoints(this Matrix4d matrix, List <Vector3d> a)
{
if (a == null || a.Count == 0)
{
return(null);
}
List <Vector3d> b = new List <Vector3d>();
double[,] matrixdouble = matrix.ToDoubleArray();
for (int i = 0; i < a.Count; i++)
{
Vector3d p1 = a[i];
//Does not work with pointers...
//this.LandmarkTransform.InternalTransformPoint(PointerUtils.GetIntPtr(p1), PointerUtils.GetIntPtr(p2));
double[] pointFloat = new double[3] {
p1.X, p1.Y, p1.Z
};
double[] pointReturn = Matrix4dExtension.TransformPointdouble(pointFloat, matrixdouble);
b.Add(new Vector3d(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(int i, int j, ref double bestResultMeanDistance, ref double meanDistance, ref Matrix4d myMatrixBestResult, PointCloudVertices sourceAxes)
{
PointCloudVertices targetAxes = InvertAxes(pointCloudTargetCentered, pointCloudTargetCentered.PCAAxes, j);
Matrix4d myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
//Matrix4d myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
//-----------------------
//for check - should give TargetPCVectors
List <Vector3d> resultAxes = Matrix4dExtension.TransformPoints(myMatrix, PointCloudVertices.ToVectors(sourceAxes));
resultAxes = resultAxes.Subtract(PointCloudVertices.ToVectors(targetAxes));
List <Vector3d> myPointsResult = myMatrix.TransformPoints(PointCloudVertices.ToVectors(pointCloudSourceCentered));
PointCloudVertices myPointsResultTemp = PointCloudVertices.FromVectors(myPointsResult);
PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela_Parallel(ref myPointsResultTemp, pointCloudTargetCentered, -1);
//PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);
double trace = myMatrix.Trace;
meanDistance = kdtree.MeanDistance;
//double trace = kdtree.MeanDistance;
//double meanDistance = myMatrix.Trace;
//Check:
System.Diagnostics.Debug.WriteLine(" in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G") + " : Trace: " + trace.ToString("G"));
if (meanDistance < bestResultMeanDistance)
{
myMatrixBestResult = myMatrix;
bestResultMeanDistance = meanDistance;
pointCloudResultBest = PointCloudVertices.FromVectors(myPointsResult);
}
pointCloudResult = PointCloudVertices.FromVectors(myPointsResult);
pointCloudTargetKDTree = myPointCloudTargetTemp;
}