Exemplo n.º 1
0
        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);
        }
Exemplo n.º 2
0
        /// <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;
        }