PCL RANSAC 多平面提取

文章目录

RANSAC
- 原理
- 讲解
源码阅读
- 代码规范
- SAC多方法多模型的实现
实际使用
- 无序点云多平面提取代码
- 有序点云多平面提取

RANSAC

RANSAC是非常经典的模型拟合方法，原理简单易于实现，且实际应用效果非常良好，即使有大量离群值，也能准确的估计出模型参数。

原理

随机采样一致性算法（RANSAC）首先随机选点，接着计算模型参数，然后计算当前模型内点个数。不断更新包含内点数最大的模型以及迭代次数 $n$ 。迭代结束后，内点数最大的模型为结果

讲解

以2维直线拟合来举例

首先随机选取可以估计出模型的最少点数对模型参数进行计算：对于直线来说，两点可以确定一条直线，所以需要随机选择两个点。对于平面，则需要选择3个点。
统计内点个数： 计算所有数据点到模型的距离，如果距离小于设定的阈值 $\tau$ 就认为是内点（inliers），否则认为是外点（outliers），统计当前模型内点个数并与最优模型的内点数比较，如果当前模型更多则更新最优模型。
更新迭代次数：尽管迭代次数会作为一个超参数用来初始化RANSAC方法，但随着迭代的进行，RANSAC会根据当前模型的内点率来更新迭代次数，以达到更快的收敛。简单介绍一下更新方法：
先定义几个符号：
$e$ ：外点率
$s$ ：用来计算模型参数的随机采样点数
$N$ ：迭代次数
$p$ ：在 $N$ 次迭代中，至少有一次不会采样到离群点的概率
$1-p\ \ \ N次采样都没出现完美采样的概率\\ (1-e)^s\ \ \ s个点都是内点的概率\\ 1-(1-e)^s\ \ \ s个点中至少有一个点是外点的概率\\ \therefore (1-(1-e)^s)^N=1-p\\ \therefore N=\frac{log(1-p)}{log (1-(1-e)^s)}$

源码阅读

PCL是一个非常优秀的C++开源库，阅读PCL的源码可以帮助我们学习到很多东西。看源码最快的方式肯定是通过调试一行行看，但DLL是没办法进行内部调试的，需要安装对应的pdb文件，并在Debug模式下运行才可以。如果不知道如何安装，可以看之前我写的pcl配置教程，里边有说。

需要注意的是，本节的内容是我个人对感兴趣地方的阅读笔记，而不是详细的教程，如果大家感兴趣，还是推荐自己去看源码。

代码规范

SACSegmentation类包含了使用SAC方法需要的所有接口

使用using来列出子类中使用到的基类方法与成员变量

 class SACSegmentation : public PCLBase<PointT>
  {
    
    
    using PCLBase<PointT>::initCompute;
    using PCLBase<PointT>::deinitCompute;

     public:
      using PCLBase<PointT>::input_;
      using PCLBase<PointT>::indices_;

对长类型名，使用using来设置别名

 using PointCloud = pcl::PointCloud<PointT>;
 using PointCloudPtr = typename PointCloud::Ptr;
 using PointCloudConstPtr = typename PointCloud::ConstPtr;
 using SearchPtr = typename pcl::search::Search<PointT>::Ptr;

 using SampleConsensusPtr = typename SampleConsensus<PointT>::Ptr;
 using SampleConsensusModelPtr = typename SampleConsensusModel<PointT>::Ptr;

为什么有的别名需要写typename，有的不写？
可以发现需要加typename的都是嵌套类型，因为它的确切类型需要等到模板实例化时才被确定，为了让编译器知道这是一个类型，而不是一个叫Ptr的静态成员变量，因此需要加typename

不是所有东西都要暴露在外

inline void 
setModelType (int model) {
    
     model_type_ = model; }
/** \brief Get the type of SAC model used. */
inline int 
getModelType () const {
    
     return (model_type_); }
....

virtual void 
segment (PointIndices &inliers, ModelCoefficients &model_coefficients);

可以看到暴露在外的只有两种东西，一个是对参数的读取和设置接口，另一个是执行SAC计算的函数

成员变量命名会加上_，来提高清晰度，并有效避免命名冲突

/** \brief The type of model to use (user given parameter). */
int model_type_;

/** \brief The type of sample consensus method to use (user given parameter). */
int method_type_;

SAC多方法多模型的实现

RANSAC只是SAC的一种实现方式，除此以外，还有MSAC、RRANSAC、MLESAC、PROSAC等方法；并且SAC也可以拟合多种模型，圆柱、平面、直线等。要是我自己写，肯定是在程序运行时候，输入两个整数，这两个整数对应方法和模型的选择，然后switch套switch，直接写出依托答辩。所以看看PCL是如何实现的

通过setMethodType()和setModelType()可以完成对方法和模型的设置。

//method_types.h
namespace pcl
{
    
    
  const static int SAC_RANSAC  = 0;
  const static int SAC_LMEDS   = 1;
  const static int SAC_MSAC    = 2;
  ...
}

//model_types.h
namespace pcl
{
    
    
  enum SacModel
  {
    
    
    SACMODEL_PLANE,
    SACMODEL_LINE,
    SACMODEL_CIRCLE2D,
	...
  };
}

执行segment()函数时会调用initSACModel()和initSAC()来对完成对模型和方法的初始化

virtual bool 
initSACModel (const int model_type);

virtual void 
nitSAC (const int method_type);

用了两个虚函数，我们知道虚函数的作用是，可以使用基类的指针来调用子类的方法

pcl::SACSegmentation<PointT>::initSAC (const int method_type)
{
    
    
  switch (method_type)
  {
    
    
    case SAC_RANSAC:
    default:
    {
    
    
      sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
      break;
    }
    //....其他方法就不写了
}
pcl::SACSegmentation<PointT>::initSACModel (const int model_type)
{
    
    
  switch (model_type)
  {
    
    
    case SACMODEL_PLANE:
    {
    
    
      model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_, random_));
      break;
    }
	//........ 其他的模型就不写了
}

model_的类型是SampleConsensusModelPtr，sac_的类型是SampleConsensusPtr，是两个抽象类，定义了一些纯虚函数，强制继承类需要实现的一些方法：

class SampleConsensus
{
    
    
	virtual bool 
    computeModel (int debug_verbosity_level = 0) = 0;
    //......等等
}
class SampleConsensusModel
{
    
    
 	virtual void 
    getSamples (int &iterations, Indices &samples)

	virtual bool
    computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const = 0;
	
	virtual void
    getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const = 0;
 	//......等等
}

看ransac.h和sac_model_plane.h可知，RandomSampleConsensus是SampleConsensus的子类，SampleConsensusModelPlane是SampleConsensusModel的子类。

所以回看上边的两句代码：

sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_, random_));

作用一目了然，就是将基类指针指向了子类，执行时就会根据设置的方法和模型，来调用到子类所定义的不同实现。

实际使用

无序点云多平面提取代码

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/io/ply_io.h>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>

//#include <pcl/search/kdtree.h> //下一小节使用

int main() {
    
    

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPLYFile("D:\\welding_project\\sim_project\\simulation_project\\ply\\vrep_cloud.ply", *cloud_in);

	pcl::SACSegmentation<pcl::PointXYZ> seg(true);
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(10);
	seg.setDistanceThreshold(0.3);

	//pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);//下一小节使用

	pcl::ExtractIndices<pcl::PointXYZ> extract;

	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	std::vector<pcl::ModelCoefficients> coeffs(3);
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> planes(3);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);

	*cloud_copy = *cloud_in;
	for (int i = 0; i < 3; i++) {
    
    
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_remaining(new pcl::PointCloud<pcl::PointXYZ>);
		seg.setInputCloud(cloud_copy);
		//tree->setInputCloud(cloud_copy);//下一小节使用
		//seg.setSamplesMaxDist(3, tree);//下一小节使用
		seg.segment(*inliers, coeffs[i]);

		extract.setInputCloud(cloud_copy);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_temp);
		extract.setNegative(true);
		extract.filter(*cloud_remaining);

		planes[i] = cloud_temp;
		cloud_copy = cloud_remaining;
	}

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));
	int v1 = 1;
	viewer->createViewPort(0, 0, 1, 1, v1);
	viewer->setBackgroundColor(1, 1, 1, v1);
	viewer->addPointCloud(cloud_in, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_in, 255, 255, 0), "cloud_in");
	viewer->addPointCloud(planes[0], pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(planes[0], 255, 0, 0), "plane1");
	viewer->addPointCloud(planes[1], pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(planes[1], 0, 255, 0), "plane2");
	viewer->addPointCloud(planes[2], pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(planes[2], 0, 0, 255), "plane3");

	while (!viewer->wasStopped()) {
    
    
		viewer->spinOnce(10);
	}
}

黄色的是原始点云，但没被提取为平面的部分。

有序点云多平面提取

上边使用的测试数据是无序点云，但在测试有序点云的时候，上边代码的效果很差，下边的点云也是类似与上边的三个平面，只不过是从仿真环境种生成的仿真点云。

我猜测效果不好的原因如下，RANSAC随机选点其实随机的是点的序号，然后取出对应序号的点。对于有序点云，位于同一平面上的点序号都比较接近，想选出位于同一平面上的点概率相对较低，针对此问题我们可以将代码稍作修改，也就是添加上一节中的注释部分代码。

#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);

setSamplesMaxDist()这个函数的作用就是，选择第一个点之后，之后的点都在这个点的邻域里选择。

但是因为找近邻需要用到KNN，所以trade-off就是运行时间会增加