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