PCL中采样一致性算法
PCL中采样一致性算法
- 1 随机采样一致性相关概念及算法
-
- 1.1 RANSAC随机采样一致性算法
- 1.2 LMedS最小中值方差估计算法
- 1.3 PCL中Sample Consensus模块支持的几何模型
-
- 1.3.1 SACMODEL_PLANE模型
- 1.3.2 SACMODEL_LINE模型
- 1.3.3 SACMODEL_CIRCLE2D模型
- 1.3.4 SACMODEL_CIRCLE3D模型
- 1.3.5 SACMODEL_SPHERE模型
- 1.3.6 SACMODEL_CYLINDER模型
- 1.3.7 SACMODEL_CONE模型
- 1.3.8 SACMODEL_TORUS模型
- 1.3.9 SACMODEL_PARALLEL_LINE模型
- 1.3.10 SACMODEL_PERPENDICULAR_PLANE模型
- 1.3.11 SACMODEL_NORMAL_PLANE模型
- 1.3.12 SACMODEL_PARALLEL_PLANE模型
- 1.3.13 SACMODEL_NORMAL_PARALLEL_PLANE模型
- 1.4 PCL中Sample Consensus模块及类介绍
- 2 采样一致性入门级实例解析：PCL中如何使用随机采样一致性模型
- 3 采样一致性精通级实例解析：两两点云获取设备自动标定
在计算机视觉领域广泛应用各种不同的采样一致性参数估计算法，用于排除错误的样本，样本不同，对应的应用则不同，例如别除错误的配准点对，分割出处在模型上的点等。PCL中以随机采样一致性算法(
RANSAC)为核心，同时实现了五种类似于随机采样一致性估计参数算法的随机参数估计算法，例如随机采样一致性估计(
RANSAC)、最大似然一致性估计(
MLESAC)、最小中值方差一致性估计(
LMEDS)等，所有的估计参数算法都符合一致性准则。本文涉及的基于采样一致性算法的应用主要是对点云进行分割，根据不同的设定几何模型，估计对应的几何模型的参数，在一定允许误差范围内分割出在模型上的点云，但本文并非讲述点云分割，主要是对采样一致性进行相关的介绍。目前PCL中支持的几何模型分割有空间平面、直线、二维或三维圆周、圆球、锥体等，如下图所示。随机采样一致性估计的另一应用就是点云的配准。
本文首先对随机采样一致性模块相关概念及算法进行简介，其次对PCL的随机采样一致性相关模块及类进行简单介绍，最后通过应用实例来展示如何对PCL中随机采样一致性模块进行灵活运用。
1 随机采样一致性相关概念及算法
1.1 RANSAC随机采样一致性算法
RANSAC是一种随机参数估计算法。RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时该样本点属于模型内样本点(inliers)，文中简称局内点或内点，否则为模型外样本点(outliers)，文中简称局外点或外点，记录下当前的inliers的个数，然后重复这一过程。每一次重复，都记录当前最佳的模型参数，所谓最佳，即inliers的个数最多，此时对应的inliers个数为best_ninliers。每次选代的末尾，都会根据期望的误差率、best_ninliers、总样本个数、当前选代次数，计算一个迭代结束评判因子，据此决定是否迭代结束。迭代结束后，最佳模型参数就是最终的模型参数估计值。
RANSAC理论上可以剔除outliers的影响，并得到全局最优的参数估计。但是RANSAC有两个问题，首先在每次代中都要区分inliers和outliers，因此需要事先设定阈值，当模型具有明显的物理意义时，这个值还比较容易设定，但是若模型比较抽象时，这个阈值就不那么容易设定了，而且固定阈值不适用于样本动态变化的应用；第二个问题是，RANSAC的迭代次数是运行期决定的，不能预知迭代的确切次数（当然选代次数的范围是可以预测的），除此之外，RANSAC只能从一个特定数据集中估计一个模型，当两个（或者更多个）存在时，RANSAC不能找到别的模型。
下图展示了RANSAC算法在二维数据集中的简单应用。左侧的图像形象地表示了一组既包含局内点又包含局外点的数据集。右侧的图像中，所有的局外点都表示为红色，局内点表示为蓝色，蓝色线就是基于RANSAC得到的结果，此例中我们尝试适应数据的模型就是一条线。
1.2 LMedS最小中值方差估计算法
LMedS也是一种随机参数估计算法。LMedS也从样本中随机抽选出一个样本子集，使用
最小方差估计算法对子集计算模型参数，然后计算所有样本与该模型的偏差。但是与RANSAC算法不同的是，LMedS记录的是所有样本中，偏差值居中的那个样本的偏差 [称为Med偏差（这也是LMedS中Med的由来）]，以及本次计算得到的模型参数。由于这一变化LMedS不需要预先设定阈值来区分inliers和outliers。重复前面的过程N次，从N个Med偏差中挑选出最小的一个，其对应的模型参数就是最终的模型参数估计值。其中选代次数N是由样本子集中样本的个数、期望的模型误差、事先估计的样本中outliers的比例所决定的。
LMedS理论上也可以剔除outliers的影响，并得到全局最优的参数估计，而且克服了RANSAC的两个缺点（虽然LMedS也需要实现设定样本中outliers的比例，但这个数字比较容易设定）。但是当outliers在样本中所占比例达到或超过50%时，LMedS就无能为力了！这与LMedS每次选代记录的是Med偏差值有关。
1.3 PCL中Sample Consensus模块支持的几何模型
1.3.1 SACMODEL_PLANE模型
SACMODEL_PLANE模型：定义为平面模型，共设置4个参数[normal_x normal_y normal_z d]，其中（normal_x, normal_y, normal_z）为Hessian范式中法向量的坐标及常量 d d d值， a x + b y + c z + d = 0 ax+by+cz+d=0 ax+by+cz+d=0，从点云中分割提取的内点都处在估计参数对应的平面上或与平面距离在一定范围内。
1.3.2 SACMODEL_LINE模型
SACMODEL_LINE模型：定义为直线模型，共设置6个参数[point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z]，其中（point_on_line.x, point_on_line.y, point_on_line.z）为直线上一点的三维坐标，（line_direction.x, line_direction.y, line_direction.z）为直线方向向量，从点云中分割提取的内点都处在估计参数对应直线上或与直线距离在一定范内。
1.3.3 SACMODEL_CIRCLE2D模型
SACMODEL_CIRCLE2D模型：定义为二维圆的圆周模型，共设置3个参数[center.x center.y radius]，其中（center.x, center.y）为圆周中心二维坐标，radius为圆周半径，从点云中分割提取的内点都处在估计参数对应的圆周上或距离圆周边线的距离在一定范围内。
1.3.4 SACMODEL_CIRCLE3D模型
SACMODEL_CIRCLE3D模型：定义为三维圆的圆周模型，共设置7个参数[center.x, center.y, center.z, radius, normal.x, normal.y, normal.z]，其中（center.x, center.y, center.z）为圆周中心三维坐标，radius为圆周半径，（normal.x, normal.y, normal.z）为三维圆所在平面的法向量坐标，从点云中分割提取的内点都处在估计参数对应的圆周上或距离圆周边线的距离在一定范围内。
1.3.5 SACMODEL_SPHERE模型
SACMODEL_SPHERE模型：定义为三维球体模型，共设置4个参数[center.x center.y center.z radius]，其中（center.x center.y center.z）为球体中心的三维坐标，radius为球体半径，从点云中分割提取的内点都处在估计参数对应的球体上或距离球体边线的距离在一定范围内。
1.3.6 SACMODEL_CYLINDER模型
SACMODEL_CYLINDER模型：定义为圆柱体模型，共设置7个参数[point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]，其中，（point_on_axis.x point_on_axis.y point_on_axis.z）为轴线上点的三维坐标，（axis_direction.x axis_direction.y axis_direction.z）为轴线方向向量的三维坐标，radius为圆柱体半径，从点云分割提取的内点都在估计参数对应的圆柱体上或距离柱体边线的距离在一定范围内。
1.3.7 SACMODEL_CONE模型
SACMODEL_CONE模型：定义为圆锥模型，共设置7个参数[apex.x, apex.y, apex.z, axis_direction.x, axis_direction.y, axis_direction.z, opening_angle]，其中（apex.x, apex.y, apex.z）为顶点的三维坐标，（axis_direction.x, axis_direction.y, axis_direction.z）为轴线方向向量的三维坐标，opening_angle为锥角的大小，从点云分割提取的内点都在估计参数对应的圆锥体上或距离锥体边线的距离在一定范围内。
1.3.8 SACMODEL_TORUS模型
SACMODEL_TORUS模型：定义为圆环面模型，尚未实现。
1.3.9 SACMODEL_PARALLEL_LINE模型
SACMODEL_PARALLEL_LINE模型：定义为有条件限制的直线模型，在规定的最大角度偏差限制下，直线模型与给定轴线平行，其参数设置参见SACMODEL_LINE模型。
1.3.10 SACMODEL_PERPENDICULAR_PLANE模型
SACMODEL_PERPENDICULAR_PLANE模型：定义为有条件限制的平面模型，在规定的最大角度偏差限制下，平面模型与给定轴线垂直，参数设置参见SACMODEL_PLANE模型。
1.3.11 SACMODEL_NORMAL_PLANE模型
SACMODEL_NORMAL_PLANE模型：定义为有条件限制的平面模型，在规定的最大角度偏差限制下，每一个局内点的法线必须与估计的平面模型的法线平行，参数设置参SACMODEL_PLANE模型。
1.3.12 SACMODEL_PARALLEL_PLANE模型
SACMODEL_PARALLEL_PLANE模型：定义为有条件限制的平面模型，在规定的最大角度偏差限制下，平面模型与给定的轴线平行，参数设置参见SACMODEL_PLANE模型。
1.3.13 SACMODEL_NORMAL_PARALLEL_PLANE模型
SACMODEL_NORMAL_PARALLEL_PLANE模型：定义为有条件限制的平面模型，在法线约束来下，三维平面模型必须与用户设定的轴线平行，参数设置见SACMODEL_PLANE模型。
1.4 PCL中Sample Consensus模块及类介绍
PCL库实现了随机采样一致性及其泛化估计算法，以及例如平面、柱面等各种常见几何模型，用不同的估计算法和不同的几何模型自由结合估算点云中隐含的具体几何模型的系数，实现对点云中所处的几何模型的分割。线、平面、柱面和球面等模型已经在PCL库中实现，平面模型经常被应用到常见的室内平面分割提取中，比如墙、地板、桌面，其他模型常应用到根据几何结构检测识别和分割物体中（比如用一个圆柱体模型分割出一个杯子），类和函数的具体说明请参考官网。
2 采样一致性入门级实例解析：PCL中如何使用随机采样一致性模型
本小节将学习如何使用RandomSampleConsensus类获得点云的拟合平面及球面模型。
首先创建一个工作空间random_sample_consensus，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p random_sample_consensus/src
接着，在random_sample_consensus/src路径下，创建一个文件并命名为random_sample_consensus.cpp，拷贝如下代码：
#include 【解释说明】
下面我们对上面的源代码关键语句进行解析。首先对两个点云进行定义初始化，并对其中一个点云填充点云数据，作为处理前的原始点云，其中，大部分点云数据是基于设定的圆球和平面模型计算而得到的坐标值，作为局内点；有五分之一的点云数据是被随机放置的，作为局外点。
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // 存储源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); // 存储提取的局内点
/* 填充点云数据 */
cloud->width = 5000; // 设置点云数目
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设置一部分点云数据，此时点云组成四分之一个球体作为内点 */
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 % 5 == 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); // 与球面距离小于0.01的点作为局内点考虑
ransac.computeModel(); // 执行随机参数估计
ransac.getInliers(inliers); // 存储估计所得的局内点
}
最后，复制适合模型的局内点到final点云中，然后根据命令行参数在三维窗口中显示原始点云或者估计得到的局内点组成的点云。
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final); // 复制估算模型的所有局内点到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));
}
【编译和运行程序】
在工作空间根目录random_sample_consensus下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(random_sample_consensus)
find_package(PCL 1.2 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable (${
PROJECT_NAME}_node src/random_sample_consensus.cpp)
target_link_libraries (${
PROJECT_NAME}_node ${
PCL_LIBRARIES})
在工作空间根目录random_sample_consensus下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件random_sample_consensus_node，首先不加任何参数运行该可执行文件：
./random_sample_consensus_node
没有任何参数的情况下，执行上述命令后三维窗口显示创建的原始点云（含有局外点和局内点），如下图所示，很明显为一个带有噪声的菱形平面，噪声点呈立方体状，主要是因为我们在创建点云时利用的随机数只产生在（0，1）范围内。
接下来使用参数-f运行程序进行测试：
./random_sample_consensus_node -f
执行上述命令后三维窗口显示的效果如下图所示，可以看到此点云不含有局外点立方体点云集合，程序只显示利用随机估计模型处理得到的局内点，符合我们选择的特定模型（此处为平面模型）。
下面使用参数-s运行程序进行测试：
./random_sample_consensus_node -s
此时，程序运行的是产生圆球相关的测试，如下图所示，很明显点云由四分之一圆球面上的点集和一个立方体内的点集组成，此点集作为圆球模型测试的原始点云。
最后，使用参数-sf运行程序进行测试：
./random_sample_consensus_node -sf
运行结果如下图所示，与原始点云相比，立方体内的点都被作为局外点而被剔除掉，而估计所得的局内点只包含球面上的点集。
3 采样一致性精通级实例解析：两两点云获取设备自动标定
经过前面的学习，我们已经学会了如何利用PCL的相关类应用RANSAC算法检测形状，为了帮助大家理解RANSAC算法在点云中的实际应用，本案例应用RANSAC算法实现基于球形靶标的自动标定方法，具体实现流程如下：坐标系 C 1 C_{1} C1和 C 2 C_{2} C2分别表示两个深度摄像头获取的点云数据的局部坐标系。3个在摄像头公共视场中半径不同的小圆球作为标定时待识别标靶物，圆球圆心（ Q 1 Q_{1} Q1， Q 2 Q_{2} Q2， Q 3 Q_{3} Q3）空间坐标用来计算坐标系之间的变换矩阵，需要在两个深度摄像头获取的点云数据中检测并识别出各个圆球。检测识别出3个圆球后，即在两组坐标系中拥有两组对应的3个坐标点，这样就可根据变换矩阵求解方程，来估计出变换矩阵 T T T，实现双深度摄像头的标定。
首先创建一个工作空间matrix_ransac，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p matrix_ransac/src
接着，在matrix_ransac/src路径下，创建一个文件并命名为matrix_ransac.cpp，拷贝如下代码：
#include 【解释说明】
本实例包括数据获取模块、直通滤波模块、平面检测及删除模块、圆球检测模块、刚体
变换矩阵估计模块，下面针对各个模块对源代码进行梳理和解析。
数据获取模块
本实例是利用支架固定双深度摄像头( Xtion Pro)，从不同方向采集包含三个小球的场景点云。读者可以根据实际情况自行搭建获取环境，获取数据时应使双摄像头尽可能多地采集圆球表面点云，也可以使用激光雷达或者其他硬件设备获取环境。
直通滤波以及下采样模块
通过深度摄像头采集得到的原始场景点云通常包含Z轴方向（垂直于摄像头镜面的方向）2m以外的点云数据，而这些点云数据通常不是我们需要的，所以为了提高后续处理的效率，本实例应用直通滤波器滤除2m以外的点云数据。然后，利用VoxelGrid滤波器对直通滤波之后的点云数据进行下采样，从而提高接下来平面检测以及球面检测的效率。
平面检测及删除模块
获取的直通滤波后的场景有近似于平面形状的地面点云，本实例利用SACSegmentation类检测地面所在的平面点云，并将其删除。利用SACSegmentation的setModelType函数设置模型类型为平面，并利用setMethodType函数设置方法为“随机采样一致性”。
圆球检测模块
删除场景地面点云数据后，创建SACSegmentation对象用于检测圆球，并设置包括检测形状和方法的SACSegmentation类相关参数。
/* 开始检测球面，此时cloud_filtered为去除点云中平面集合的点云 */
pcl::SACSegmentation<pcl::PointXYZRGB> seg2;
pcl::PointIndices::Ptr tmpinliers2 (new pcl::PointIndices);
std::vector <pcl::ModelCoefficients> coefficients_all;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_sphere (new pcl::PointCloud<pcl::PointXYZRGB> ());
seg2.setOptimizeCoefficients (true);
seg2.setModelType (pcl::SACMODEL_SPHERE);
seg2.setMethodType (pcl::SAC_RANSAC);
seg2.setMaxIterations (100);
seg2.setDistanceThreshold (0.01);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f2 (new pcl::PointCloud<pcl::PointXYZRGB>);
然后利用如下代码循环检测场景内的圆球点云：即利用SACSegmentation类使用随机采样一致性检测到一个圆球点云并记录该圆球球心后，删除该圆球点云，继续利用采样一致性算法检测，直到把三个圆球表面点云都检测到，并记录三个圆球的球心。
/* 完成三个球的球心检测，其在局部坐标系内的坐标存储在coefficients_all,输入的全局坐标在xyz_all */
for(i=0;i<3;i++)//循环次数控制球个数
{
// Segment the largest planar component from the remaining cloud
pcl::ModelCoefficients coefficients2;
seg2.setInputCloud (cloud_filtered);
seg2.segment (*tmpinliers2, coefficients2);
coefficients_all.push_back(coefficients2);
if (tmpinliers2->indices.size () == 0)
{
std::cout << "Could not estimate a sphere model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract2;
extract2.setInputCloud (cloud_filtered);
extract2.setIndices (tmpinliers2);
extract2.setNegative (false);
// Write the planar inliers to disk
extract2.filter (*cloud_sphere);
viewer.showCloud(cloud_sphere); //可视化显示最后一个小球
std::cout << "PointCloud representing the sphere component: " << cloud_sphere->points.size () << " data points." << std::endl;
std::cout << coefficients2 << std::endl;//打印各个球的四个参数
// Remove the planar inliers, extract the rest
extract2.setNegative (true);
extract2.filter (*cloud_f2);
cloud_filtered = cloud_f2;
}
刚体变换矩阵估计模块
利用双深度摄像头分别采集包含圆球的场景点云后，经过上述模块的处理，得到了在两个摄像头局部坐标系下三个小球球心的坐标，接下来基于三组对应点，利用TransformationFromCorrespondences类估计变换矩阵。利用TransformationFromCorrespondences类的add函数添加对应点，利用getTransformation函数获取变换矩阵的结果，实现双深度摄像头的标定。
【编译和运行程序】
在工作空间根目录matrix_ransac下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(matrix_ransac)
find_package(PCL 1.6 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable (${
PROJECT_NAME}_node src/matrix_ransac.cpp)
target_link_libraries (${
PROJECT_NAME}_node ${
PCL_LIBRARIES})
在工作空间根目录matrix_ransac下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件matrix_ransac_node，运行该可执行文件：
./matrix_ransac_node
此时即可获取双深度摄像头局部坐标系之间的变换矩阵，需要注意的是检测的精度与效果同圆球材质和采集距离有关。