Пример #1
0
        private void SVD_OfPointCloud_Double(PointCloud pointsSource, bool normalsCovariance)
        {
            //calculate correlation matrix

            //SVD.Eigenvalues_Helper(C);
            //Matrix3d C = PointCloud.CovarianceMatrix_Double(pointsSource, normalsCovariance);

            Matrix3d C = PointCloud.CovarianceMatrix3d(pointsSource, normalsCovariance);

            SVD.Eigenvalues_Helper(C);


            EV = new Vector3(Convert.ToSingle(SVD.EV.X), Convert.ToSingle(SVD.EV.Y), Convert.ToSingle(SVD.EV.Z));
            VT = SVD.VT.ToMatrix3();
            U  = SVD.U.ToMatrix3();
        }
Пример #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_Double(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);

            Matrix4d myMatrix4d = SVD.FindTransformationMatrix_WithoutCentroids(sourceAxes, targetAxes, ICP_VersionUsed.Umeyama);


            //CheckPCA(myMatrix4d, sourceAxes, targetAxes);


            PointCloud myResultPC = myMatrix4d.TransformPoints(pcSourceCentered);


            //--------------
            pcTreeResult = KDTree.FindClosestPointCloud_Parallel(myResultPC);
            meanDistance = KDTree.MeanDistance;


            //PointCloud myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);


            System.Diagnostics.Debug.WriteLine("   in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G"));

            if (meanDistance < bestResultMeanDistance)
            {
                myMatrixBestResult     = myMatrix4d.ToMatrix4();
                bestResultMeanDistance = meanDistance;
                pcResultBest           = myResultPC;
            }



            pcResult = myResultPC;
        }