[代码实践] 使用 PCA 进行平面和线特征拟合

简介

在 LOAM 系列的激光 SLAM 项目中，必不可少的一个环节是找到每一帧点云中的平面特征和线特征，而原作者用来拟合特征的其中一个方法是 PCA，这篇博客主要是通过写一个简单的 PCA 函数并应用在平面特征和线特征的点云下并对其生成的特征向量进行可视化，来观察 PCA 如何对平面和线特征进行拟合。博客用到的代码在：github

PCA （主成分分析）过程

PCA 的原理推导文章在网上有很多，本篇博客主要为代码实践，因此对 PCA 的原理推导不做详细介绍。PCA 的大致思想为：在给定数据集的中心找到一组正交基，使得数据在这一组正交基主成分对应基底分布最散。利用这个结果我们可以做很多事情，例如将点投影到主成分对应的方向上，实现数据的降维等等。而这篇博客主要是利用其来找到线特征的方向向量或平面特征的法向量。

PCA 的过程本身很简单，对一个点云进行 PCA 的过程大致分为：

对点云去中心化
求解点云的协方差矩阵
对协方差矩阵进行特征分解
分解后得到 3 个特征值，以及对应的特征向量

对平面点或者线点进行 PCA

结合 PCA 的思想对平面或线特征的主成分进行分析：

对平面特征而言，一个3 维平面的点只会在两个方向分布，那么不难想象如果对平面进行主成分分析，一定会有一个特征值显著地小，在该特征值对应的基底上，平面点几乎看不出来区别，可以看出来该基底为平面的法向量
对线特征而言，线特征的点只会分布在该线特征的方向量附近，因此如果对线特征进行主成分分析，一定会有一个特征值显著地大，而在另外两个特征值对应的方向，点几乎看不出来区别，因此最大的特征值对应的特征向量为该直线的方向向量

实现

对点云进行 PCA 的实现比较简单，如下所示：

template <class PointType>
void PCA(boost::shared_ptr<pcl::PointCloud<PointType>>& cloud_ptr,
         Eigen::Vector3f& center,
         Eigen::Vector3f& eigen_values,
         Eigen::Matrix3f& eigen_vectors) {
    Eigen::Vector3f sum = Eigen::Vector3f::Zero();
    Eigen::Vector3f mean;

    int n = cloud_ptr->points.size();

    for (const auto& pt : cloud_ptr->points) {
        sum.x() += pt.x;
        sum.y() += pt.y;
        sum.z() += pt.z;
    }

    mean = sum / n;

    Eigen::Matrix3f covariances;
    for (const auto& pt : cloud_ptr->points) {
        Eigen::Vector3f pt_center{pt.x - mean.x(), pt.y - mean.y(), pt.z - mean.z()};
        covariances += pt_center * pt_center.transpose();
    }

    covariances /= n;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> saes(covariances);

    center        = mean;
    eigen_values  = saes.eigenvalues();
    eigen_vectors = saes.eigenvectors();
}

在一阵点云中截取一个平面和一个线特征，分别进行 PCA，将得到的三个特征向量按照特征值大小比例延伸得到结果如下，可以看到结果符合我们的预测。