前言

在众多点云处理算法中，其中关于平面拟合的算法十分广泛。本篇内容主要是希望总结归纳各类点云平面拟合算法，并且将代码进行梳理保存。

环境：

VS2019 + PCL1.11.1

1.RANSAC

使用ransac对平面进行拟合是非常常见的用法，PCL库中就有RANSAC拟合平面的实现代码，而且还集成了 两种拟合平面的代码。
方法一：

/// <summary>
/// 使用PCL中集成的RANSAC方法进行平面拟合
/// </summary>
/// <param name="cloud_in">输入待拟合的点云</param>
/// <param name="inliers">RANSAC拟合得到的内点</param>
/// <param name="coefficients">得到的平面方程参数</param>
/// <param name="iterations">平面拟合最大迭代次数</param>
/// <param name="threshold">RANSAC拟合算法距离阈值</param>
void RANSAC(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, std::vector<int>& inliers, Eigen::VectorXf& coefficients,
    const int& iterations, const double& threshold)
{
    inliers.clear();                                          // 用于存放内点索引的vector
    pcl::shared_ptr<pcl::SampleConsensusModelPlane<pcl::PointXYZ>> model(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud_in)); //定义待拟合平面的model，并使用待拟合点云初始化
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model);  // 定义RANSAC算法模型
    ransac.setDistanceThreshold(threshold);                   // 设定阈值
    ransac.setMaxIterations(iterations);                      // 设置最大迭代次数
    ransac.setNumberOfThreads(10);                            // 设置线程数量
    ransac.computeModel();                                    // 拟合 
    ransac.getInliers(inliers);                               // 获取内点索引
    Eigen::VectorXf params;                                   // 第一次得到的平面方程
    ransac.getModelCoefficients(params);                      // 获取拟合平面参数，对于平面ax+by_cz_d=0，params分别按顺序保存a,b,c,d
    model->optimizeModelCoefficients(inliers, params, coefficients);      // 优化平面方程参数
    std::vector<double> vDistance;                            // 用于存储每个点到拟合平面的距离的vector容器
    model->getDistancesToModel(coefficients, vDistance);      // 得到每个点到平面的距离的集合
    std::cout << "params: " << params[0] << ", " << params[1] << ", " << params[2] << ", " << params[3] << std::endl;
    std::cout << "coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", " << coefficients[3] << std::endl;
}

上述代码需要注意的是，一定需要model->optimizeModelCoefficients(inliers, params, coefficients); 通过这一句代码去优化平面参数。优化前后差别很大，如下图所示：

方法二：

/// <summary>
/// 使用PCL中集成的用于点云分割的RANSAC方法进行平面拟合
/// </summary>
/// <param name="cloud_in">输入待拟合的点云</param>
/// <param name="inliers">RANSAC拟合得到的内点</param>
/// <param name="coefficients">得到的平面方程参数</param>
/// <param name="iterations">平面拟合最大迭代次数</param>
/// <param name="threshold">RANSAC拟合算法距离阈值</param>
void SEG_RANSAC(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, pcl::PointIndices::Ptr& inliers, Eigen::VectorXf& coefficients,
    const int& iterations, const double& threshold)
{
    if (inliers == nullptr) inliers.reset(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients_m(new pcl::ModelCoefficients);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(iterations);                     // 设置最大迭代次数
    seg.setDistanceThreshold(threshold);                  // 设定阈值
    seg.setNumberOfThreads(10);                           // 设置线程数量
    seg.setInputCloud(cloud_in);
    seg.segment(*inliers, *coefficients_m);

    coefficients.resize(4);
    coefficients[0] = coefficients_m->values[0]; coefficients[1] = coefficients_m->values[1];
    coefficients[2] = coefficients_m->values[2]; coefficients[3] = coefficients_m->values[3];
    std::cout << "SEG coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", " << coefficients[3] << std::endl;
}

方法二其实也提到了优化平面参数，seg.setOptimizeCoefficients(true);这句代码就是比较关键的，需要加上。
上述两种方法的运行结果如下，从结果和运行时间来看，两种方法几乎一致。

两种方法中都有一个设置线程的功能setNumberOfThreads(10); 但是这句话根本没有起到作用，下图中有提示[pcl::RandomSampleConsensus::computeModel] Parallelization is requested, but OpenMP 3.1 is not available! Continuing without parallelization.其实在VS中已经开启了openmp，也包含了头文件#include <omp.h>，只不过版本不对应，该问题也查阅了很久，没查到解决办法。

VS中开启openmp加速的配置方法：

关于方法二：
其实使用pcl::SACSegmentationpcl::PointXYZ seg进行平面拟合时，除了上面所给出的代码中那几个常规参数设置外，还有几个参数：

 // 设置角度容差（EpsAngle）。它定义了平面模型的法向量与给定轴之间的最大允许角度差异。只有当模型类型（setModelType）设置为SACMODEL_NORMAL_PARALLEL_PLANE时才有效。
seg_pos.setEpsAngle();
// 设置用于分割的轴向量。它定义了与平面模型垂直的方向。只有当模型类型（setModelType）设置为SACMODEL_NORMAL_PARALLEL_PLANE时才有效。
seg_pos.setAxis();
// Set the maximum distance allowed when drawing random samples
seg_pos.setSamplesMaxDist();

关于setAxis()，其说明是Set the axis along which we need to search for a model perpendicular to.简单理解就是找到一个模型能够垂直于这个轴。那么显然这个参数是为拟合平面而存在的。那么setModelType就需要设置为pcl::SACMODEL_NORMAL_PARALLEL_PLANE，意思是设置分割模型为带约束的平面。既然设置了轴，那么就要设置运行容差的角度setEpsAngle()。关于不同模型的含义请点击。
上述这两种参数设置，这里给出了相应的例子。关于这种用法，可以应用在提前使用了PCA或者其他方法得到了想要拟合平面的法向，使用这个法向进行约束可以得到更加精确的平面。个人认为这个提前设置的法向必须要足够精确，否则反而会干扰平面方程的拟合过程。

第三个参数则是seg_pos.setSamplesMaxDist();一般情况下都会认为同一平面上的点都会比较接近。那么就可以使用该方法对其进行设置。下面的例程来源于这里。第一参数就是搜索半径。这其实也有个问题，通常一个平面点云，小范围内都是非常平整的。假如有两块区域相离比较远，可能拟合出来的平面并不一定很重合。所以如果采用了这种方法进行限定，那么有必要将距离阈值设置的稍微大一点，否则提取得到的内点可能会残缺。

#include <pcl/search/kdtree.h> 
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_copy);
seg.setSamplesMaxDist(3, tree);

2.最小二乘法拟合平面

除了RANSAC之外，另一种十分常见的拟合方程的方法是最小二乘法，这里就不过多介绍相关的理论知识，这里提到了相关的代码和原理

当然还是要贴上封装的C++代码实现：

/// <summary>
/// 使用最小二乘法拟合平面：Ax + By + Cz + D = 0 */   //拉格朗日乘子法 https://zhuanlan.zhihu.com/p/390294059
/// </summary>
/// <param name="xjData">输入待拟合平面的点云</param>
/// <param name="coefficients">输出拟合的平面方程</param>
/// <returns>输入点云点数符合要求，返回true，否则返回false</returns>
bool LeastSquare(const pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>& xjData, Eigen::VectorXf& coefficients)
{
    int count = xjData->points.size();
    if (count < 3) return false;

    double meanX = 0, meanY = 0, meanZ = 0;
    double meanXX = 0, meanYY = 0, meanZZ = 0;
    double meanXY = 0, meanXZ = 0, meanYZ = 0;
    for (int i = 0; i < count; i++)
    {
        meanX += xjData->points[i].x;
        meanY += xjData->points[i].y;
        meanZ += xjData->points[i].z,

        meanXX += xjData->points[i].x * xjData->points[i].x;
        meanYY += xjData->points[i].y * xjData->points[i].y;
        meanZZ += xjData->points[i].z * xjData->points[i].z;

        meanXY += xjData->points[i].x * xjData->points[i].y;
        meanXZ += xjData->points[i].x * xjData->points[i].z;
        meanYZ += xjData->points[i].y * xjData->points[i].z;
    }
    meanX /= count; meanY /= count; meanZ /= count;
    meanXX /= count; meanYY /= count; meanZZ /= count;
    meanXY /= count; meanXZ /= count; meanYZ /= count;

    /* eigenvector */
    Eigen::Matrix3d eMat;
    eMat(0, 0) = meanXX - meanX * meanX; eMat(0, 1) = meanXY - meanX * meanY; eMat(0, 2) = meanXZ - meanX * meanZ;
    eMat(1, 0) = meanXY - meanX * meanY; eMat(1, 1) = meanYY - meanY * meanY; eMat(1, 2) = meanYZ - meanY * meanZ;
    eMat(2, 0) = meanXZ - meanX * meanZ; eMat(2, 1) = meanYZ - meanY * meanZ; eMat(2, 2) = meanZZ - meanZ * meanZ;
    Eigen::EigenSolver<Eigen::Matrix3d> xjMat(eMat);           // 求取矩阵特征值和特征向量的函数EigenSolver
    Eigen::Matrix3d eValue = xjMat.pseudoEigenvalueMatrix();   // 获取矩阵伪特征值 3*3
    Eigen::Matrix3d eVector = xjMat.pseudoEigenvectors();      // 获取矩阵伪特征向量 3*1

    /* the eigenvector corresponding to the minimum eigenvalue */
    double v1 = eValue(0, 0); double v2 = eValue(1, 1); double v3 = eValue(2, 2);
    int minNumber = 0;
    if ((abs(v2) <= abs(v1)) && (abs(v2) <= abs(v3)))
    {
        minNumber = 1;
    }
    if ((abs(v3) <= abs(v1)) && (abs(v3) <= abs(v2)))
    {
        minNumber = 2;
    }
    double A = eVector(0, minNumber); double B = eVector(1, minNumber); double C = eVector(2, minNumber);
    double length = sqrt(A * A + B * B + C * C);
    A /= length; B /= length; C /= length;
    double D = -(A * meanX + B * meanY + C * meanZ);

    /* result */
    if (C < 0)
    {
        A *= -1.0; B *= -1.0; C *= -1.0; D *= -1.0;
    }

    coefficients.resize(4);
    coefficients[0] = A; coefficients[1] = B; coefficients[2] = C; coefficients[3] = D;

    std::cout << "LS coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", " << coefficients[3] << std::endl;
}

同一份点云，使用最小二乘法拟合结果如下：

最小二乘法拟合平面方程和上述RANSAC拟合结果还是有些差别的，但是RANSAC运行时间约为0.02s，而最小二乘法运行时间需要0.0013s，最小二乘法运行时间要比RANSAC快很多。如果针对一份没有太多噪点，很平滑干净的点云，可以使用最小二乘法去拟合，但是如果是有噪点的点云，追求准确率的情况下，则需要使用RANSAC进行拟合。

3.SVD分解的方法求平面方程

可以通过Eigen实现使用SVD分解的方法进行平面方程求解

/// <summary>
/// 通过SVD分解的方法求拟合平面
/// </summary>
/// <param name="pcl_cloud">输入待拟合平面点云</param>
/// <param name="coefficients">输出拟合平面的参数</param>
void PlaneEigenSVD(const pcl::PointCloud<pcl::PointXYZ>& pcl_cloud, Eigen::VectorXf& coefficients)
{
    if (pcl_cloud.points.size() < 3) return;
    // Convert PCL point cloud to Eigen matrix
    Eigen::Matrix<float, 3, Eigen::Dynamic> coord(3, pcl_cloud.size());
    for (size_t i = 0; i < pcl_cloud.size(); ++i)
    {
        coord.col(i) << pcl_cloud[i].x, pcl_cloud[i].y, pcl_cloud[i].z;
    }

    // Calculate centroid
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(pcl_cloud, centroid);

    // Subtract centroid
    coord.row(0).array() -= centroid(0);
    coord.row(1).array() -= centroid(1);
    coord.row(2).array() -= centroid(2);

    // Perform singular value decomposition (SVD)
    Eigen::JacobiSVD<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>> svd(coord, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Vector3f plane_normal = svd.matrixU().rightCols<1>();

    // Create PCL ModelCoefficients
    coefficients.resize(4);
    coefficients[0] = plane_normal(0);
    coefficients[1] = plane_normal(1);
    coefficients[2] = plane_normal(2);
    coefficients[3] = -(plane_normal(0) * centroid(0) + plane_normal(1) * centroid(1) + plane_normal(2) * centroid(2));
    if (coefficients[2] < 0) coefficients = -coefficients;

    std::cout << "SVD coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", " << coefficients[3] << std::endl;

    return;
}

运行结果如下：

得到的结果其实和最小二乘法拟合得到的结果一样，但是耗时更长一些。

更新SVD分解的另一种写法，代码如下：

/// <summary>
/// 使用Eigen SVD分解获取平面方程
/// </summary>
/// <param name="cloud_in">输入点云</param>
/// <param name="coefficients">得到的平面方程</param>
/// <returns>return true表示拟合成功，return false表示拟合失败</returns>
bool LeastSquareByEigen(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, Eigen::VectorXf& coefficients)
{
    if (cloud_in == nullptr) return false;
    if (cloud_in->points.size() < 10) return false;
    // copy coordinates to  matrix in Eigen format
    size_t num_atoms = cloud_in->points.size();
    Eigen::Matrix< Eigen::Vector3f::Scalar, Eigen::Dynamic, Eigen::Dynamic > coord(3, num_atoms);
    for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = cloud_in->points[i].getVector3fMap();

    // calculate centroid
    Eigen::Vector3f centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());

    // subtract centroid
    coord.row(0).array() -= centroid(0); coord.row(1).array() -= centroid(1); coord.row(2).array() -= centroid(2);

    // we only need the left-singular matrix here
    //  http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
    auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Vector3f plane_normal = svd.matrixU().rightCols<1>();
    
    coefficients.resize(4);
    coefficients[0] = plane_normal(0);
    coefficients[1] = plane_normal(1);
    coefficients[2] = plane_normal(2);
    coefficients[3] = -(plane_normal(0) * centroid(0) + plane_normal(1) * centroid(1) + plane_normal(2) * centroid(2));

    if (coefficients[2] < 0) coefficients = -coefficients;

    // Normalize the coefficients
    float norm = coefficients.head<3>().norm();
    coefficients /= norm;
    std::cout << "LeastSquareByEigen coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", " << coefficients[3] << std::endl;
    
    return true;
}

4.霍夫变换进行平面拟合

其实使用霍夫变换去拟合几何模型也是一件非常常见的方法，但是目前没有相关代码进行测试验证，如果后续有找到实现方法也会更新在这里。