本文比较了点云处理中RANSAC（包括两种方法）、最小二乘法（SVD分解和基本计算）、霍夫变换以及LM算法在直线拟合中的应用，展示了它们的优缺点和性能差异。
前言
在点云处理领域，对应拟合直线是再正常不过的事情。本篇内容就是汇总不同的算法进行直线拟合，求取点到直线的距离，得到拟合直线的方向向量等。
1.RANSAC
使用RANSAC拟合直线当然是最常见的方法之一。PCL中依旧集成有两种拟合3D直线的RANSAC算法实现。
方法一代码如下：
/// <summary>
/// 通过PCL中集成的RANSAC算法进行直线拟合
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="inliers">直线的内点</param>
/// <param name="iterations">迭代次数</param>
/// <param name="threshold">距离阈值</param>
/// <param name="coefficients">直线方程</param>
/// <returns>return true表示拟合成功，return false表示失败</returns>
bool LineEstimateRANSAC(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::vector<int>& inliers, const int& iterations, const double& threshold,
Eigen::VectorXf& coefficients)
{
if (cloud == nullptr) return false;
if (cloud->points.size() < 10) return false;
inliers.clear();
Eigen::VectorXf params;
pcl::shared_ptr<pcl::SampleConsensusModelLine<pcl::PointXYZ>> model(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model);
ransac.setDistanceThreshold(threshold); // 设置距离阈值，即点到直线的距离超过该阈值时，被认为是外点 0.05
ransac.setMaxIterations(iterations);
ransac.setNumberOfThreads(10);
ransac.computeModel();
ransac.getInliers(inliers);
ransac.getModelCoefficients(params); // 获取拟合结果，即直线方程
model->optimizeModelCoefficients(inliers, params, coefficients);
model->selectWithinDistance(coefficients, threshold, inliers);
std::vector<double> vDistance;
model->getDistancesToModel(coefficients, vDistance); // 根据已经得到的直线方程，得到点云cloud中每个点到直线的距离
std::cout << "coefficients size : " << coefficients.size() << std::endl;
std::cout << "ransac Line params: " << params[0] << ", " << params[1] << ", " << params[2] << ", "
<< params[3] << ", " << params[4] << ", " << params[5] << std::endl;
std::cout << "ransac Line coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", "
<< coefficients[3] << ", " << coefficients[4] << ", " << coefficients[5] << std::endl;
return true;
}
与先前所提到的拟合平面方法一样，需要使用model->optimizeModelCoefficients(inliers, params, coefficients);去优化直线的参数。有一种用法是ransac.setDistanceThreshold(threshold1);使用阈值1，即threshold1去拟合直线，而可以通过model->selectWithinDistance(coefficients, threshold2, inliers);获取阈值2的内点。threshold1和threshold2可以设置成不一样的大小。依旧可以通过model->getDistancesToModel(coefficients, vDistance);得到点云cloud上每个点到直线的距离。
运行结果如下：
得到的直线方程参数里面其实是有6个值的，前三个值表示直线上一个点的坐标值，即x，y，z。而后三个值表示直线方程的方向向量。从上述运行结果来看，优化参数与否其实存在比较大的差异的。
方法二：
/// <summary>
/// 使用PCL库中集成的点云分割方法进行直线拟合
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="inliers">拟合直线内点</param>
/// <param name="iterations">最大迭代次数</param>
/// <param name="threshold">距离阈值</param>
/// <param name="coefficients">拟合直线方程</param>
/// <returns>return true表示拟合成功，return false表示失败</returns>
bool LineSegmentRANSAC(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::vector<int>& inliers, const int& iterations, const double& threshold,
Eigen::VectorXf& coefficients)
{
if (cloud == nullptr) return false;
if (cloud->points.size() < 10) return false;
inliers.clear();
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr modelcoefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZ> seg_pos;// 创建一个分割器
seg_pos.setOptimizeCoefficients(true); // 优化直线方程
seg_pos.setModelType(pcl::SACMODEL_LINE); // SACMODEL_LINE SACMODEL_PLANE注：在这里面修改模型名称
seg_pos.setMethodType(pcl::SAC_RANSAC); // 分割方法：随机采样法RANSAC
seg_pos.setMaxIterations(iterations); // 最大的迭代的次数
seg_pos.setNumberOfThreads(12); // 设置线程数量，这个需要OpenMP 3.1 否则无效，其实还是单线程运行
seg_pos.setDistanceThreshold(threshold); // 设置误差容忍范围
seg_pos.setInputCloud(cloud); // 输入点云
seg_pos.segment(*indices, *modelcoefficients); // 分割点云，得到内点和直线方程
inliers.assign(indices->indices.begin(), indices->indices.end());
coefficients.resize(6);
coefficients[0] = modelcoefficients->values[0]; coefficients[1] = modelcoefficients->values[1]; coefficients[2] = modelcoefficients->values[2];
coefficients[3] = modelcoefficients->values[3]; coefficients[4] = modelcoefficients->values[4]; coefficients[5] = modelcoefficients->values[5];
std::cout << "ransac seg Line coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", "
<< coefficients[3] << ", " << coefficients[4] << ", " << coefficients[5] << std::endl;
return true;
}
上述两种方法运行结果如下：
从结果来看，两种方法拟合的结果完全一致。而且setNumberOfThreads(12);依旧没起作用，原因是openmp的版本不符合要求。
2.最小二乘法拟合直线
Eigen中有相应的方法可以使用。通过SVD分解的方法求取直线代码如下：
/// <summary>
/// 使用Eigen SVD分解拟合直线方程
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="coefficients">得到拟合直线方程</param>
/// <returns>true表示拟合成功，false表示拟合失败</returns>
bool LineEstimateEigen(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, Eigen::VectorXf& coefficients)
{
if (cloud == nullptr) return false;
if (cloud->points.size() < 10) return false;
int number = cloud->points.size();
/*std::vector<Eigen::Vector3f> vEigen3f(number, Eigen::Vector3f(0, 0, 0));
for (size_t i = 0; i < number; ++i)
{
vEigen3f[i] = cloud->points[i].getVector3fMap();
}*/
// copy coordinates to matrix in Eigen format
size_t num_atoms = cloud->points.size();
Eigen::Matrix< Eigen::Vector3f::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3);
for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = cloud->points[i].getVector3fMap();
Eigen::Vector3f origin = centers.colwise().mean(); // 计算点云的质心（center of mass）作为直线上的一个点
Eigen::MatrixXf centered = centers.rowwise() - origin.transpose(); // 将点云坐标减去质心，以使质心成为坐标系的原点（(0, 0, 0)）
Eigen::MatrixXf cov = centered.adjoint() * centered; // 计算中心化后的点云的协方差矩阵（covariance matrix）
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(cov); // 对协方差矩阵进行特征值分解（Eigenvalue Decomposition）
Eigen::Vector3f axis = eig.eigenvectors().col(2).normalized(); // 提取特征向量矩阵的第三列（对应最大特征值的特征向量），并进行归一化
coefficients.resize(6);
coefficients[0] = origin[0]; coefficients[1] = origin[1]; coefficients[2] = origin[2];
coefficients[3] = axis[0]; coefficients[4] = axis[1]; coefficients[5] = axis[2];
std::cout << "coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", "
<< coefficients[3] << ", " << coefficients[4] << ", " << coefficients[5] << std::endl;
return true;
}
除了上述SVD分解的方法之外，当然还可以最基本的数学计算方法进行方程求解。代码如下：
/// <summary>
/// 使用最小二乘法拟合直线
/// </summary>
/// <param name="cloud">输入点云</param>
/// <param name="coefficients">拟合直线参数</param>
/// <returns>true表示拟合成功，false表示拟合失败</returns>
bool Fit3DLineByLeastSquares(const pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>& cloud, Eigen::VectorXf& coefficients)
{
if (cloud == nullptr || cloud->points.size() < 10) return false;
float sum_x = 0.;
float sum_y = 0.;
float sum_z = 0.;
float sum_xz = 0.;
float sum_yz = 0.;
float sum_z2 = 0.;
size_t n = cloud->points.size();
for (size_t i = 0; i < n; ++i)
{
sum_x += cloud->points[i].x;
sum_y += cloud->points[i].y;
sum_z += cloud->points[i].z;
sum_xz += cloud->points[i].x * cloud->points[i].z;
sum_yz += cloud->points[i].y * cloud->points[i].z;
sum_z2 += cloud->points[i].z * cloud->points[i].z;
}
float den = n * sum_z2 - sum_z * sum_z;
float k1 = (n * sum_xz - sum_x * sum_z) / den;
float b1 = (sum_x - k1 * sum_z) / n;
float k2 = (n * sum_yz - sum_y * sum_z) / den;
float b2 = (sum_y - k2 * sum_z) / n;
Eigen::Vector3f axis;
axis[0] = k1; axis[1] = k2; axis[2] = 1;
float norm = axis.norm();
axis /= norm;
Eigen::Vector3f origins;
origins[0] = sum_x / n; origins[1] = sum_y / n; origins[2] = sum_z / n;
coefficients.resize(6);
coefficients[0] = origins[0]; coefficients[1] = origins[1]; coefficients[2] = origins[2];
coefficients[3] = axis[0]; coefficients[4] = axis[1]; coefficients[5] = axis[2];
std::cout << "LS coefficients: " << coefficients[0] << ", " << coefficients[1] << ", " << coefficients[2] << ", "
<< coefficients[3] << ", " << coefficients[4] << ", " << coefficients[5] << std::endl;
return true;
}
上述两种方法计算结果如下：
两种方法运行结果依旧相同。但是RANSAC和最小二乘法计算得到的直线方程则存在明显差别。
3.霍夫变换检测直线
霍夫变换跟RANSAC、最小二乘法以及LM拟合直线都不太相同，霍夫变换更偏向于检测是否存在直线，如果没有直线，那么在设定好的参数下可能得不到直线方程。而其他方法拟合直线，只要是有点云，而且不那么稀疏的情况下，无论点云中是否真的存在直线，可能都会得到一个结果。所以相比于拟合，用检测这个词更加贴切一点。
有一篇2017年的文章《Iterative Hough Transform for Line Detection in 3D Point Clouds》，从题目来看就是使用迭代霍夫变换法在3D点云中检测直线。论文地址请点击。这篇论文已经在github上开源了代码。
使用代码如下，只需要将开源代码的hpp，cpp和ipp全部加入到项目中，然后#include "3d_line_detection.hpp"就可以运行，十分方便。
/// <summary>
/// 使用霍夫变换进行直线检测
/// </summary>
/// <param name="cloud">输入点云</param>
/// <returns>true表示检测成功，false表示检测失败</returns>
bool HoughTransformLineFit(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
if (cloud == nullptr) return false;
if (cloud->points.size() < 10) return false;
std::size_t numRangeBin = 12;
std::size_t sphereGranularity = 5;
std::size_t minNumVote = 300;
float distanceToLineThresh = 1;
HoughTransform::Param param({ numRangeBin,sphereGranularity,minNumVote,distanceToLineThresh });
HoughTransform transformer(param);
pcl::ModelCoefficients lineModelCoefficients;
HoughTransform::LineSegment3Ds lineSegments = transformer.run(cloud);
//std::cout << "lineSegments size: " << lineSegments.size() << std::endl;
for (int i = 0; i < lineSegments.size(); ++i)
{
lineModelCoefficients = lineSegments[i].coeffs();
//std::cout << "lineModelCoefficients size: " << lineModelCoefficients.values.size() << std::endl;
std::cout << "lineModelCoefficients: " << lineModelCoefficients.values[0] << ", " << lineModelCoefficients.values[1] << ", "
<< lineModelCoefficients.values[2] << ", " << lineModelCoefficients.values[3] << ", "
<< lineModelCoefficients.values[4] << ", " << lineModelCoefficients.values[5] << std::endl;
}
if (lineSegments.empty()) return false;
return true;
}
使用该算法检测直线，其效果与以下四个参数的设定关系很大。
std::size_t numRangeBin = 12;
std::size_t sphereGranularity = 5;
std::size_t minNumVote = 300;
float distanceToLineThresh = 1;
目前还没有仔细阅读论文，所以就先不介绍这些参数的含义了。现有的这个参数都是反复运行测试得到的结果。
可视化效果如下，其中黄色为霍夫变换得到的直线，而白色为原始点云。
4.LM拟合直线
Levenberg-Marquardt 最小二乘优化即LM算法。更像是一种牛顿迭代法。该算法原理和实现请参见。链接中给出的代码并不是拟合直线的，但是经过一些细微的修改，可以更改成用于拟合直线的代码。该算法一个很重要的特点是，它偏向于更加精细的求解，在开始计算前，需要给定一个初始直线方程参数。这样可能迭代的会更快一些。所以LM算法可以结合其他算法一起使用。
运行时间对比
对比如下，可以看出RANSAC方法二竟然比方法一要快。霍夫变换则是比较慢的。（不包含LM算法）
扫一扫
4085
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
