在众多点云处理算法中,其中关于平面拟合的算法十分广泛。本篇内容主要是希望总结归纳各类点云平面拟合算法,并且将代码进行梳理保存。
VS2019 + PCL1.11.1
使用ransac对平面进行拟合是非常常见的用法,PCL库中就有RANSAC拟合平面的实现代码,而且还集成了 两种拟合平面的代码。
方法一:
///
/// 使用PCL中集成的RANSAC方法进行平面拟合
///
/// 输入待拟合的点云
/// RANSAC拟合得到的内点
/// 得到的平面方程参数
/// 平面拟合最大迭代次数
/// RANSAC拟合算法距离阈值
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); 通过这一句代码去优化平面参数。优化前后差别很大,如下图所示:
方法二:
///
/// 使用PCL中集成的用于点云分割的RANSAC方法进行平面拟合
///
/// 输入待拟合的点云
/// RANSAC拟合得到的内点
/// 得到的平面方程参数
/// 平面拟合最大迭代次数
/// RANSAC拟合算法距离阈值
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
除了RANSAC之外,另一种十分常见的拟合方程的方法是最小二乘法,这里就不过多介绍相关的理论知识,这里提到了相关的代码和原理
当然还是要贴上封装的C++代码实现:
///
/// 使用最小二乘法拟合平面:Ax + By + Cz + D = 0 */ //拉格朗日乘子法 https://zhuanlan.zhihu.com/p/390294059
///
/// 输入待拟合平面的点云
/// 输出拟合的平面方程
/// 输入点云点数符合要求,返回true,否则返回false
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进行拟合。
可以通过Eigen实现使用SVD分解的方法进行平面方程求解
///
/// 通过SVD分解的方法求拟合平面
///
/// 输入待拟合平面点云
/// 输出拟合平面的参数
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;
}
运行结果如下:
得到的结果其实和最小二乘法拟合得到的结果一样,但是耗时更长一些。
其实使用霍夫变换去拟合几何模型也是一件非常常见的方法,但是目前没有相关代码进行测试验证,如果后续有找到实现方法也会更新在这里。