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