PCL学习：采样一致性算法

在计算机视觉领域广泛的使用各种不同的采样一致性参数估计算法用于排除错误的样本，样本不同对应的应用不同，例如剔除错误的配准点对，分割出处在模型上的点集，PCL中以随机采样一致性算法（RANSAC）为核心，同时实现了五种类似与随机采样一致形算法的随机参数估计算法，例如随机采样一致性算法（RANSAC）最大似然一致性算法（MLESAC），最小中值方差一致性算法（LMEDS）等，所有估计参数算法都符合一致性原则。在PCL中设计的采样一致性算法的应用主要就是对点云进行分割，根据设定的不同的几个模型，估计对应的几何参数模型的参数，在一定容许的范围内分割出在模型上的点云。

(1) RANSAC随机采样一致性算法的介绍

RANSAC是“RANdom SAmple Consensus（随机抽样一致）”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。

数据分两种：有效数据（inliers）和无效数据（outliers）。偏差不大的数据称为有效数据，偏差大的数据是无效数据。如果有效数据占大多数，无效数据只是少量时，我们可以通过最小二乘法或类似的方法来确定模型的参数和误差；如果无效数据很多（比如超过了50%的数据都是无效数据），最小二乘法就失效了，我们需要新的算法

一个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。简单的最小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概率还足够高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。

（a) 包含很多局外点的数据集 (b) RANSAC找到的直线（局外点并不影响结果）

(2) 最小中值法（LMedS）

LMedS的做法很简单，就是从样本中随机抽出N个样本子集，使用最大似然（通常是最小二乘）对每个子集计算模型参数和该模型的偏差，记录该模型参数及子集中所有样本中偏差居中的那个样本的偏差（即Med偏差），最后选取N个样本子集中Med偏差最小的所对应的模型参数作为我们要估计的模型参数。

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
	// --------------------------------------------
	// -----Open 3D viewer and add point cloud-----
	// --------------------------------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
	//viewer->addCoordinateSystem (1.0, "global");
	viewer->initCameraParameters();
	return (viewer);
}
/*
对点云进行初始化，并对其中一个点云填充点云数据作为处理前的的原始点云，其中大部分点云数据是基于设定的圆球和平面模型计算
而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的，作为局外点。
*/
int
main(int argc, char** argv)
{
	// 初始化点云对象
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点

																					 // 填充点云数据
	cloud->width = 500;                    //设置填充点云数目
	cloud->height = 1;                     //设置为无序点云
	cloud->is_dense = false;
	cloud->points.resize(cloud->width * cloud->height);
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
		{
			//根据命令行参数用x^2+y^2+Z^2=1设置一部分点云数据，此时点云组成1/4个球体作为内点
			cloud->points[i].x =  rand() / (RAND_MAX + 1.0);
			cloud->points[i].y = rand() / (RAND_MAX + 1.0);
			if (i % 5 == 0)
				cloud->points[i].z = rand() / (RAND_MAX + 1.0);   //此处对应的点为局外点
			else if (i % 2 == 0)
				cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
					- (cloud->points[i].y * cloud->points[i].y));
			else
				cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
					- (cloud->points[i].y * cloud->points[i].y));
		}
		else
		{ //用x+y+z=1设置一部分点云数据，此时地拿云组成的菱形平面作为内点
			cloud->points[i].x = rand() / (RAND_MAX + 1.0);
			cloud->points[i].y = rand() / (RAND_MAX + 1.0);
			if (i % 2 == 0)
				cloud->points[i].z = rand() / (RAND_MAX + 1.0);   //对应的局外点
			else
				cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
		}
	}

	std::vector<int> inliers;  //存储局内点集合的点的索引的向量

							   //创建随机采样一致性对象
	pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
		model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));    //针对球模型的对象
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
		model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));   //针对平面模型的对象
	if (pcl::console::find_argument(argc, argv, "-f") >= 0)
	{  //根据命令行参数，来随机估算对应平面模型，并存储估计的局内点
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
		ransac.setDistanceThreshold(.01);    //与平面距离小于0.01 的点称为局内点考虑
		ransac.computeModel();                   //执行随机参数估计
		ransac.getInliers(inliers);                 //存储估计所得的局内点
	}
	else if (pcl::console::find_argument(argc, argv, "-sf") >= 0)
	{
		//根据命令行参数  来随机估算对应的圆球模型，存储估计的内点
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
		ransac.setDistanceThreshold(.01);
		ransac.computeModel();
		ransac.getInliers(inliers);
	}

	// 复制估算模型的所有的局内点到final中
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

	// 创建可视化对象并加入原始点云或者所有的局内点

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
		viewer = simpleVis(final);
	else
		viewer = simpleVis(cloud);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return 0;
}

.\random_sample_consensus.exe

.\random_sample_consensus.exe -f

.\random_sample_consensus.exe -s

.\random_sample_consensus.exe -sf

PCL学习：采样一致性算法

猜你喜欢