Esempio n. 1
0
        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);
        }
Esempio 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(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;
        }