做100个网站挂广告联盟,wordpress 知更鸟 公告,凡科网页版,郑州做网站推广地址前言
在众多点云处理算法中#xff0c;其中关于平面拟合的算法十分广泛。本篇内容主要是希望总结归纳各类点云平面拟合算法#xff0c;并且将代码进行梳理保存。
环境#xff1a;
VS2019 PCL1.11.1
1.RANSAC
使用ransac对平面进行拟合是非常常见的用法#xff0c;PCL…前言
在众多点云处理算法中其中关于平面拟合的算法十分广泛。本篇内容主要是希望总结归纳各类点云平面拟合算法并且将代码进行梳理保存。
环境
VS2019 PCL1.11.1
1.RANSAC
使用ransac对平面进行拟合是非常常见的用法PCL库中就有RANSAC拟合平面的实现代码而且还集成了 两种拟合平面的代码。 方法一
/// summary
/// 使用PCL中集成的RANSAC方法进行平面拟合
/// /summary
/// param namecloud_in输入待拟合的点云/param
/// param nameinliersRANSAC拟合得到的内点/param
/// param namecoefficients得到的平面方程参数/param
/// param nameiterations平面拟合最大迭代次数/param
/// param namethresholdRANSAC拟合算法距离阈值/param
void RANSAC(const pcl::PointCloudpcl::PointXYZ::Ptr cloud_in, std::vectorint inliers, Eigen::VectorXf coefficients,const int iterations, const double threshold)
{inliers.clear(); // 用于存放内点索引的vectorpcl::shared_ptrpcl::SampleConsensusModelPlanepcl::PointXYZ model(new pcl::SampleConsensusModelPlanepcl::PointXYZ(cloud_in)); //定义待拟合平面的model并使用待拟合点云初始化pcl::RandomSampleConsensuspcl::PointXYZ ransac(model); // 定义RANSAC算法模型ransac.setDistanceThreshold(threshold); // 设定阈值ransac.setMaxIterations(iterations); // 设置最大迭代次数ransac.setNumberOfThreads(10); // 设置线程数量ransac.computeModel(); // 拟合 ransac.getInliers(inliers); // 获取内点索引Eigen::VectorXf params; // 第一次得到的平面方程ransac.getModelCoefficients(params); // 获取拟合平面参数对于平面axby_cz_d0params分别按顺序保存a,b,c,dmodel-optimizeModelCoefficients(inliers, params, coefficients); // 优化平面方程参数std::vectordouble 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 namecloud_in输入待拟合的点云/param
/// param nameinliersRANSAC拟合得到的内点/param
/// param namecoefficients得到的平面方程参数/param
/// param nameiterations平面拟合最大迭代次数/param
/// param namethresholdRANSAC拟合算法距离阈值/param
void SEG_RANSAC(const pcl::PointCloudpcl::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::SACSegmentationpcl::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加速的配置方法
2.最小二乘法拟合平面
除了RANSAC之外另一种十分常见的拟合方程的方法是最小二乘法这里就不过多介绍相关的理论知识这里提到了相关的代码和原理
当然还是要贴上封装的C代码实现
/// summary
/// 使用最小二乘法拟合平面Ax By Cz D 0 */ //拉格朗日乘子法 https://zhuanlan.zhihu.com/p/390294059
/// /summary
/// param namexjData输入待拟合平面的点云/param
/// param namecoefficients输出拟合的平面方程/param
/// returns输入点云点数符合要求返回true否则返回false/returns
bool LeastSquare(const pcl::shared_ptrpcl::PointCloudpcl::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::EigenSolverEigen::Matrix3d xjMat(eMat); // 求取矩阵特征值和特征向量的函数EigenSolverEigen::Matrix3d eValue xjMat.pseudoEigenvalueMatrix(); // 获取矩阵伪特征值 3*3Eigen::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 namepcl_cloud输入待拟合平面点云/param
/// param namecoefficients输出拟合平面的参数/param
void PlaneEigenSVD(const pcl::PointCloudpcl::PointXYZ pcl_cloud, Eigen::VectorXf coefficients)
{if (pcl_cloud.points.size() 3) return;// Convert PCL point cloud to Eigen matrixEigen::Matrixfloat, 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 centroidEigen::Vector4f centroid;pcl::compute3DCentroid(pcl_cloud, centroid);// Subtract centroidcoord.row(0).array() - centroid(0);coord.row(1).array() - centroid(1);coord.row(2).array() - centroid(2);// Perform singular value decomposition (SVD)Eigen::JacobiSVDEigen::Matrixfloat, Eigen::Dynamic, Eigen::Dynamic svd(coord, Eigen::ComputeThinU | Eigen::ComputeThinV);Eigen::Vector3f plane_normal svd.matrixU().rightCols1();// Create PCL ModelCoefficientscoefficients.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;
}运行结果如下 得到的结果其实和最小二乘法拟合得到的结果一样但是耗时更长一些。
4.霍夫变换进行平面拟合
其实使用霍夫变换去拟合几何模型也是一件非常常见的方法但是目前没有相关代码进行测试验证如果后续有找到实现方法也会更新在这里。
