使用RANSAC算法基于几何模型分割图像

  RanSaC算法（随机采样一致）原本是用于数据处理的一种经典算法，其作用是在大量噪声情况下，提取物体中特定的成分。下图是对RanSaC算法效果的说明。图中有一些点显然是满足某条直线的，另外有一团点是纯噪声。目的是在大量噪声的情况下找到直线方程，此时噪声数据量是直线的3倍。
  

　　如果用最小二乘法是无法得到这样的效果的，直线大约会在图中直线偏上一点。关于随机采样一致性算法的原理，在wiki百科上讲的很清楚，甚至给出了伪代码和matlab,C代码。见网址https://en.wikipedia.org/wiki/RANSAC. 我想换一个不那么严肃或者说不那么学术的方式来解释这个算法。

　　实际上这个算法就是从一堆数据里挑出自己最心仪的数据。所谓心仪当然是有个标准（目标的形式:满足直线方程？满足圆方程？以及能容忍的误差e）。平面中确定一条直线需要2点，确定一个圆则需要3点。随机采样算法，其实就和小女生找男朋友差不多。

1. 2. 从人群中随便找个男生，看看他条件怎么样，然后和他谈恋爱，（平面中随机找两个点，拟合一条直线，并计算在容忍误差e中有多少点满足这条直线）
   3. 第二天，再重新找个男生，看看他条件怎么样，和男朋友比比，如果更好就换新的（重新随机选两点，拟合直线，看看这条直线是不是能容忍更多的点，如果是则记此直线为结果）
   4. 第三天，重复第二天的行为（循环迭代）
   5. 终于到了某个年龄，和现在的男朋友结婚（迭代结束，记录当前结果）

　　显然，如果一个女生按照上面的方法找男朋友，最后一定会嫁一个好的（我们会得到心仪的分割结果）。只要这个模型在直观上存在，该算法就一定有机会把它找到。优点是噪声可以分布的任意广，噪声可以远大于模型信息。
　　这个算法有两个缺点，第一，必须先指定一个合适的容忍误差e。第二，必须指定迭代次数作为收敛条件。

　　综合以上特性，本算法非常适合从杂乱点云中检测某些具有特殊外形的物体。

PCL中的使用：

平面方程参数估计

常用的平面方程是平面的法线式：ax+by+cz=d，其中a^2+b^2+c^2=1，d>0，（a,b,c）为平面法矢，d为原点至平面的距离，这四个参数可以确定一个平面。

 //存放局内点索引
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  //创建分割对象
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  //选择模型系数是否需要优化
  seg.setOptimizeCoefficients (true);
  //设置模型类型
  seg.setModelType (pcl::SACMODEL_PLANE);
  //设置模型估计方法
  seg.setMethodType (pcl::SAC_RANSAC);
  //设置最大迭代次数
  seg.setMaxIterations(10000);
  //距离阈值（离散点到模型表面距离）
  seg.setDistanceThreshold (0.01);
  //输入点云
  seg.setInputCloud (cloud);
  //计算模型参数和得到符合此模型的局内点索引
  seg.segment (*inliers, *coefficients);
  //输出平面模型的四个参数
  std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3] << std::endl;

其他模型的估计只要改变ModelType就可以了， 除了平面以外，PCL几乎支持所有的几何形状。作为点云分割的基础算法，RanSaC很强大且必收敛，可以作为机器人抓取，识别等后续任务的前处理。

下面我贴的代码是去除点云里大片的平面部分所用的，因为有个桌子，这样可以去掉桌子的表面，就可以提取桌子上面的东西了，不然桌子上的东西点云是连续的。

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
#include <string>






int
main(int argc, char** argv)
{
// Read in the cloud data
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f_1(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
	reader.read("table_scene_lms400.pcd", *cloud);
	std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*


// Create the filtering object: downsample the dataset using a leaf size of 1cm
	pcl::VoxelGrid<pcl::PointXYZ> vg;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_filtered);
	std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*
	pcl::copyPointCloud(*cloud_filtered, *cloud_f_1);


// Create the segmentation object for the planar model and set all the parameters
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PCDWriter writer;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(100);
	seg.setDistanceThreshold(0.02);


	int i = 0, nr_points = (int)cloud_filtered->points.size();
	while (cloud_filtered->points.size() > 0.1 * nr_points)
	{
		// Segment the largest planar component from the remaining cloud
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}


		// Extract the planar inliers from the input cloud
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);


		// Write the planar inliers to disk
		extract.filter(*cloud_plane);
		std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;


		// Remove the planar inliers, extract the rest
		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered = cloud_f;
	}


	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));


	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("Radius: 0.01", 10, 10, "v1 text", v1);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_f_1,"sample cloud1", v1);


	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
	viewer->addText("Radius: 0.1", 10, 10, "v2 text", v2);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_filtered, "sample cloud2", v2);


	viewer->initCameraParameters();


	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}


	return (0);
}