使用RANSAC进行平面模型分割

RANSAC

  • RANSAC(Random Sample Consensus),即随机采样一致性算法。它可以从一组包含“局外点”的观测数据集中,通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果;为了提高概率必须提高迭代次数。该算法最早由Fischler和Bolles于1981年提出。
  • 输入:一个数据集(点云),一个适用的模型(平面模型),一个可靠性参数(距离阈值)

算法过程


RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点,并用下述方法进行验证:

  1. 有一个模型适应于假设的局内点,即所有的未知参数都能从假设的局内点计算得出。
  2. 用1中得到的模型去测试所有的其它数据,如果某个点适用于估计的模型,认为它也是局内点。
  3. 如果有足够多的点被归类为假设的局内点,那么估计的模型就足够合理。
  4. 然后,用所有假设的局内点去重新估计模型,因为它仅仅被初始的假设局内点估计过。
  5. 最后,通过估计局内点与模型的错误率来评估模型。

这个过程被重复执行固定的次数,每次产生的模型要么因为局内点太少而被舍弃,要么因为比现有的模型更好而被选用。

平面模型的估计过程

1.输入:

  1. 数据:点云
  2. 模型:平面模型
  3. 可靠性参数:距离阈值

2.用于估计的系数:

平面方程:ax+by+cz+d=0,系数a,b,c,d

3.迭代过程

  • 随机选取三个点假设为内点,确定平面方程系数,计算其余点与该平面的距离,根据距离阈值判断是否满足估计的模型,将满足的加入内点,并计算内点个数
  • 将假设的内点重新估计模型系数
  • 重复执行以上两个步骤,若内点个数太少,将抛弃该模型,重新执行以上过程,若产生比现有模型更好的模型,则抛弃旧模型。反复迭代固定次数,得到最终模型

PCL应用

1.第一步:输入点云

#include <pcl/sample_consensus/method_types.h> //随机参数估计方法的头文件
#include <pcl/sample_consensus/model_types.h>//模型的定义,这里是平面模型
#include <pcl/segmentation/sac_segmentation.h>//采样一致性算法的具体方法
int main(int argc, char** argv)
{
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 // 设置点云属性,为随机点云,共15个点
 cloud->width = 15;
 cloud->height = 1;
 cloud->points.resize(cloud->width * cloud->height);
 // 随机设置点云的坐标,其中x,y值随机产生,z值恒为1,因此是z=1平面上散布的点 
 for (size_t i = 0; i < cloud->points.size(); ++i) {
 cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
 cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
 cloud->points[i].z = 1.0;
 }

 // 改变其中几个点的位置,使之脱离平面成为外点
 cloud->points[0].z = 2.0;
 cloud->points[3].z = -2.0;
 cloud->points[6].z = 4.0;

回显输入的点云数据

 std::cerr << "初始点云数据(共 "
 << cloud->points.size() << " 个点):"
 << std::endl;
 for (size_t i = 0; i < cloud->points.size(); ++i)
 std::cerr << i << " " << cloud->points[i].x
 << " \t" << cloud->points[i].y
 << "\t\t" << cloud->points[i].z
 << std::end;

2.第二步:指定模型和可靠性参数


 //创建模型参数对象,这里指的是平面模型的方程ax+by+cz+d=0的参数
 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
 //创建内点集合,用于存储分割得到的内点结果
 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
 // 创建分割对象
 pcl::SACSegmentation&amp;amp;amp;amp;lt;pcl::PointXYZ&amp;amp;amp;amp;gt; seg;
 // 设置模型系数需要优化
 seg.setOptimizeCoefficients(true);
 // 必须配置
 seg.setModelType(pcl::SACMODEL_PLANE);
 //指定当前模型是平面模型
 seg.setMethodType(pcl::SAC_RANSAC);
 //指定分割算法采用随机采样一致性算法
 seg.setDistanceThreshold(0.01);//设置距离阈值

 

 

3.第三步:开始分割

 seg.setInputCloud(cloud);//输入点云
 seg.segment(*inliers, *coefficients);//分割点云

4.第四步:查看输出的模型系数和内点

//如果内点个数为0,则分割失败 
 if (inliers->indices.size() == 0) {
 PCL_ERROR("对于给定数据集,无法估计出一个平面模型");
 return (-1);
 }
 std::cerr << "分割完毕,模型参数: " << coefficients->values[0] << " "
 << coefficients->values[1] << " "
 << coefficients->values[2] << " "
 << coefficients->values[3] << std::endl;
 std::cerr << "模型内点个数: "
 << inliers->indices.size() << std::endl;
 for (size_t i = 0; i < inliers->indices.size(); ++i)
 std::cerr << inliers->indices[i] << " "
 << cloud->points[inliers->indices[i]].x
 << " \t" << cloud->points[inliers->indices[i]].y
 << "\t\t" << cloud->points[inliers->indices[i]].z
 << std::endl; return (0);
}

PCL库源码摘要

计算模型系数abcd


pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients(
 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) {
 // Compute the plane coefficients from the 3 given points in a straightforward manner
 // calculate the plane normal 
 n = (p2 - p1) x(p3 - p1) = cross(p2 - p1, p3 - p1)model_coefficients.resize(4);
 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
 model_coefficients[3] = 0;
 // Normalizemodel_coefficients.normalize ();
 // ... + d = 0
 model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot(p0.matrix()));
}

 

计算点到平面的距离


template <typename PointT> voidpcl::SampleConsensusModelPlane<PointT>::getDistancesToModel
(
 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
 Eigen::Vector4f pt(input_->points[(*indices_)[i]].x,
 input_->points[(*indices_)[i]].y,
 input_->points[(*indices_)[i]].z,
 1);
 distances[i] = fabs(model_coefficients.dot(pt));
}

计算内点个数


for (size_t i = 0; i < indices_->size(); ++i) {
 // Calculate the distance from the point to the plane normal as the dot product 
 // D = (P-A).N/|N| 
 Eigen::Vector4f pt(input_->points[(*indices_)[i]].x,
 input_->points[(*indices_)[i]].y,
 input_->points[(*indices_)[i]].z,
 1);
 if (fabs(model_coefficients.dot(pt)) < threshold)
 nr_p++;
}

共有 0 条评论

发表评论

电子邮件地址不会被公开。