PCL求取点云的均值、协方差、PCA

PCL求取点云的均值、协方差、PCA

原理

1. 均值
2. 协方差
   • 正常的协方差
   • 归一化的，协方差除以总共的点云个数
3. PCA
   可以理解为讲协方差分解成三个轴的法向量，就是SVD获取了特征向量与特征值
   • 特征向量：反应了三维物体三个轴的方向，vPCA的行向量就是特征向量。三行代表三个。
   • 特征值：大小反应各个轴的分布情况，DigV大的对应的特征向量是主轴，小的对应的特征向量可能是平面法向量。使用eigen计算

代码实现

#include <iomanip>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/common/common.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d_omp.h> //使用OMP需要添加的头文件
#include <pcl/filters/passthrough.h>    //分块使用
#include <pcl/common/transforms.h>
int main(int argc, char **argv)
{
    //加载整体地图
    pcl::PointCloud<pcl::PointXYZL>::Ptr pointCloudLabelPtr(new pcl::PointCloud<pcl::PointXYZL>); //所有的点云地图
    std::string frameFile = "/home/wgq/PrioriMapLoc/test/testdata/000253.pcd";
    pcl::io::loadPCDFile<pcl::PointXYZL>(frameFile, *pointCloudLabelPtr);
    std::cout << "frameFile 加载:" << frameFile << "点云数量数:" << pointCloudLabelPtr->size() << std::endl;

    pcl::PointXYZL min, max;
    pcl::getMinMax3D(*pointCloudLabelPtr, min, max);
    std::cout << "x:[" << min.x << "," << max.x << "]" << std::endl;
    std::cout << "y:[" << min.y << "," << max.y << "]" << std::endl;
    std::cout << "z:[" << min.z << "," << max.z << "]" << std::endl;

    //计算均值mean和协方差cov
    {
        Eigen::Vector4f mean;
        Eigen::Matrix3f cov;
        pcl::compute3DCentroid(*pointCloudLabelPtr, mean);            //输入点云，输出均值
        pcl::computeCovarianceMatrix(*pointCloudLabelPtr, mean, cov); //输入点云、均值，输出协方差矩阵
        std::cout << "mean:" << mean.transpose() << std::endl
                  << "cov:" << cov << std::endl
                  << "cov1:" << cov / pointCloudLabelPtr->size() << std::endl
                  << std::endl;
    }
    {
        Eigen::Vector4f mean;
        Eigen::Matrix3f cov;
        pcl::computeMeanAndCovarianceMatrix(*pointCloudLabelPtr, cov, mean); //输入点云，输出均值，协方差矩阵
        std::cout << "mean:" << mean.transpose() << std::endl
                  << "cov1:" << cov << std::endl
                  << std::endl;
    }

    // PCA计算主成分
    {
        Eigen::Vector4f mean;
        Eigen::Matrix3f cov;
        pcl::compute3DCentroid(*pointCloudLabelPtr, mean);                      //输入点云，输出均值
        pcl::computeCovarianceMatrixNormalized(*pointCloudLabelPtr, mean, cov); //输入点云、均值，输出归一化的协方差矩阵，1/n 为总点数
        std::cout << "mean:" << mean.transpose() << std::endl
                  << "cov1:" << cov << std::endl
                  << std::endl;

        Eigen::Matrix3f vPCA;                               //特征向量，行向量排列，第一行对应的特征值最小
        Eigen::Vector3f DigV;                               //特征值，其中特征值最小的为第一个
        Eigen::Matrix3f DigM = Eigen::Matrix3f::Identity(); //对角矩阵
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(cov, Eigen::ComputeEigenvectors);
        vPCA = eigen_solver.eigenvectors();
        DigV = eigen_solver.eigenvalues();

        std::cout << "vPCA:" << vPCA.transpose() << std::endl
                  << "DigV:" << DigV.transpose() << std::endl
                  << std::endl;

        DigM(0, 0) = DigV.x();
        DigM(1, 1) = DigV.y();
        DigM(2, 2) = DigV.z();
        // SVD分解，特征值最大的特征向量值对应主轴，最小值对应法向量如果是平面
        std::cout << "cov = vPCA*Dig*vPCAT" << vPCA * DigM * vPCA.transpose() << std::endl;
    }

    return 0;
}