PCL点云处理库一个可能的bug

利用PCL 1.8.1中的pcl::eigen33()方法计算特征值和特征向量,得到的最大特征值对应的特征向量似乎不稳定。

将点云平移一个很大的位置之后,该特征向量就不全对了。

复制代码
if (kdtree.radiusSearch(searchPoint, s_radius, nn_indices, nn_dists) > 0)
                {
                    //计算PCA,保存主方向
                    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
                    Eigen::Vector4f xyz_centroid;
                    if (pcl::computeMeanAndCovarianceMatrix(*cloud, nn_indices, covariance_matrix, xyz_centroid) == 0)
                    {
                        continue;
                    }
                    EIGEN_ALIGN16 Eigen::Matrix3f eigen_vector3;
                    EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
                    //计算特征值和特征向量
                    pcl::eigen33(covariance_matrix, eigen_vector3, eigen_values);
                    SortEigenValuesAndVectors(eigen_vector3, eigen_values);
                    double eig_val1 = eigen_values[0];//最大
                    double eig_val2 = eigen_values[1];
                    double eig_val3 = eigen_values[2];
                    Eigen::Vector3f normal;
                    normal[0] = eigen_vector3(0, 2);//其它的Cell
                    normal[1] = eigen_vector3(1, 2);
                    normal[2] = eigen_vector3(2, 2);
                    Eigen::Vector3f direction;
                    direction[0] = eigen_vector3(0, 0);//其它的Cell
                    direction[1] = eigen_vector3(1, 0);
                    direction[2] = 0;// eigen_vector3(2, 0);
                    direction.normalize();
                    double gx = direction.x();
                    double gy = direction.y();
                }
复制代码

后改用CloudCompare中的计算方法,就稳定了。奇怪!

复制代码
CCVector3d Psum(0, 0, 0);
                    for (unsigned ix = 0; ix < nn_indices.size(); ++ix)
                    {
                        pcl::PointXYZ pt= cloud->at(nn_indices[ix]);
                        Psum.x += pt.x;
                        Psum.y += pt.y;
                        Psum.z += pt.z;
                    }

                    CCVector3 G(static_cast<PointCoordinateType>(Psum.x / nn_indices.size()),
                        static_cast<PointCoordinateType>(Psum.y / nn_indices.size()),
                        static_cast<PointCoordinateType>(Psum.z / nn_indices.size()));

                    double mXX = 0.0;
                    double mYY = 0.0;
                    double mZZ = 0.0;
                    double mXY = 0.0;
                    double mXZ = 0.0;
                    double mYZ = 0.0;
                    //for each point in the cell
                    for (unsigned ix = 0; ix < nn_indices.size(); ++ix)
                    {
                        
                        pcl::PointXYZ pt = cloud->at(nn_indices[ix]);
                        CCVector3 CellP(pt.x, pt.y, pt.z);
                        CCVector3 P = CellP - G;

                        mXX += static_cast<double>(P.x)*P.x;
                        mYY += static_cast<double>(P.y)*P.y;
                        mZZ += static_cast<double>(P.z)*P.z;
                        mXY += static_cast<double>(P.x)*P.y;
                        mXZ += static_cast<double>(P.x)*P.z;
                        mYZ += static_cast<double>(P.y)*P.z;
                    }
                    //协方差矩阵对称
                    CCLib::SquareMatrixd covMat(3);
                    covMat.m_values[0][0] = mXX / nn_indices.size();
                    covMat.m_values[1][1] = mYY / nn_indices.size();
                    covMat.m_values[2][2] = mZZ / nn_indices.size();
                    covMat.m_values[1][0] = covMat.m_values[0][1] = mXY / nn_indices.size();
                    covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ / nn_indices.size();
                    covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ / nn_indices.size();

                    CCLib::SquareMatrixd eigVectors;
                    std::vector<double> eigValues;
                    if (!Jacobi<double>::ComputeEigenValuesAndVectors(covMat, eigVectors, eigValues))
                    {
                        continue;//failure
                    }
                    Jacobi<double>::SortEigenValuesAndVectors(eigVectors, eigValues);
                    //compute eigen value
                    double eig_val1 = eigValues[0];
                    double eig_val2 = eigValues[1];
                    double eig_val3 = eigValues[2];//排序之后,e0最大,e2最小
                    double uOther[3];
                    Jacobi<double>::GetEigenVector(eigVectors, 0, uOther);
                    CCVector3 direction(uOther[0], uOther[1], uOther[2]);
                    double gx = direction.x;
                    double gy = direction.y;
复制代码

 

 

 

posted @   太一吾鱼水  阅读(245)  评论(0编辑  收藏  举报
相关博文:
阅读排行:
· 震惊!C++程序真的从main开始吗?99%的程序员都答错了
· 【硬核科普】Trae如何「偷看」你的代码?零基础破解AI编程运行原理
· 单元测试从入门到精通
· 上周热点回顾(3.3-3.9)
· Vue3状态管理终极指南:Pinia保姆级教程
点击右上角即可分享
微信分享提示