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范式中法向量的坐标及常量ddd值，ax+by+cz+d=0ax+by+cz+d=0ax+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 <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)
{/* 打开3D viewer，然后添加点云数据 */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);viewer->initCameraParameters ();return (viewer);
}int main(int argc, char** argv)
{srand(time(NULL));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));elsecloud->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);elsecloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);}}std::vector<int> inliers; // 存储局内点集合的点的索引的向量/* 创建随机采样一致性对象 */pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptrmodel_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud)); // 针对球模型的对象pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptrmodel_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); // 存储估计所得的局内点}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);elseviewer = simpleVis(cloud);while (!viewer->wasStopped ()){viewer->spinOnce (100);boost::this_thread::sleep (boost::posix_time::microseconds (100000));}return 0;
}
【解释说明】
下面我们对上面的源代码关键语句进行解析。首先对两个点云进行定义初始化，并对其中一个点云填充点云数据，作为处理前的原始点云，其中，大部分点云数据是基于设定的圆球和平面模型计算而得到的坐标值，作为局内点；有五分之一的点云数据是被随机放置的，作为局外点。
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));elsecloud->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);elsecloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);}
}
接下来，创建一个用于存储点云中局内点位置的整数型向量，作为点索引的向量，这里我们使用一个平面或者球面随机估计模型建立随机采样一致性对象，并根据命令行参数分别以点云作为输入，来进行随机参数估计，存储局内点。
std::vector<int> inliers; // 存储局内点集合的点的索引的向量/* 创建随机采样一致性对象 */
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptrmodel_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud)); // 针对球模型的对象
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptrmodel_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);
elseviewer = 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算法实现基于球形靶标的自动标定方法，具体实现流程如下：坐标系C1C_{1}C1和C2C_{2}C2分别表示两个深度摄像头获取的点云数据的局部坐标系。3个在摄像头公共视场中半径不同的小圆球作为标定时待识别标靶物，圆球圆心（Q1Q_{1}Q1，Q2Q_{2}Q2，Q3Q_{3}Q3）空间坐标用来计算坐标系之间的变换矩阵，需要在两个深度摄像头获取的点云数据中检测并识别出各个圆球。检测识别出3个圆球后，即在两组坐标系中拥有两组对应的3个坐标点，这样就可根据变换矩阵求解方程，来估计出变换矩阵TTT，实现双深度摄像头的标定。
首先创建一个工作空间matrix_ransac，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p matrix_ransac/src
接着，在matrix_ransac/src路径下，创建一个文件并命名为matrix_ransac.cpp，拷贝如下代码：
#include <fstream>
#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.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>
#include <pcl/cuda/io/cloud_to_pcl.h>
#include <pcl/cuda/io/disparity_to_cloud.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/cuda/time_cpu.h>
//#include <boost/shared_ptr.hpp>
#include <pcl/visualization/cloud_viewer.h>
#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 <vector>#include <pcl/common/transforms.h>
#include <pcl/common/transformation_from_correspondences.h>#include <pcl/filters/passthrough.h>using pcl::cuda::PointCloudAOS;
using pcl::cuda::Device;class KinectViewerCuda
{public:KinectViewerCuda (bool downsample) : viewer ("KinectGrabber"), downsample_(downsample),global_(false) {}void cloud_cb_ (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float constant){pcl::PointCloud<pcl::PointXYZRGB>::Ptr final (new pcl::PointCloud<pcl::PointXYZRGB>);PointCloudAOS<Device>::Ptr data;{pcl::cuda::ScopeTimeCPU t ("time:"); d2c.compute<Device> (depth_image, image, constant, data, downsample_);}pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB>);pcl::cuda::toPCL (*data, *output);cloud_=output;pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered0 (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_filteredz (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_filteredx (new pcl::PointCloud<pcl::PointXYZRGB>);/* 滤除距离远于2米的点云，便于后续处理效率和质量 */pcl::PassThrough<pcl::PointXYZRGB> pass;pass.setInputCloud (output);pass.setFilterFieldName ("z");pass.setFilterLimits (0.0, 1.7);pass.filter (*cloud_filteredz);pass.setInputCloud(cloud_filteredz);pass.setFilterFieldName ("x");pass.setFilterLimits (-0.3, 0.3);pass.filter (*cloud_filteredx);pass.setInputCloud(cloud_filteredx);pass.setFilterFieldName ("y");pass.setFilterLimits (-0.3, 0.3);pass.filter (*cloud_filtered0);/* 处理前对点云进行下采样 */pcl::VoxelGrid<pcl::PointXYZRGB> vg;pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); // 经过下采样处理后的点云vg.setInputCloud (cloud_filtered0);vg.setLeafSize (0.001f, 0.001f, 0.001f);vg.filter (*cloud_filtered);std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;/* 检测剔除掉处在平面上的点云 */pcl::SACSegmentation<pcl::PointXYZRGB> seg;pcl::PointIndices::Ptr tmpinliers (new pcl::PointIndices);pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());seg.setOptimizeCoefficients (true);seg.setModelType (pcl::SACMODEL_PLANE);seg.setMethodType (pcl::SAC_RANSAC);seg.setMaxIterations (100);seg.setDistanceThreshold (0.05);pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);int i=0, nr_points = (int) cloud_filtered->points.size ();while (cloud_filtered->points.size () > 0.3 * nr_points){/* 从剩余点云中分割出最大平面成分 */seg.setInputCloud (cloud_filtered);seg.segment (*tmpinliers, *coefficients);//std::cout << *coefficients << std::endl; 打印平面的四个参数if (tmpinliers->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 cloudpcl::ExtractIndices<pcl::PointXYZRGB> extract;extract.setInputCloud (cloud_filtered);extract.setIndices (tmpinliers);extract.setNegative (false);// Write the planar inliers to diskextract.filter (*cloud_plane);std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;// Remove the planar inliers, extract the restextract.setNegative (true);extract.filter (*cloud_f);cloud_filtered = cloud_f;}output=cloud_filtered;/* 开始检测球面，此时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>);/* 输入全局坐标 */for(i=0; i<3; i++){if(global_==false && i==0){std::cout<<"\n请依次输入最近的球的球心对应的全局坐标位置x y z之间用空格或回车\n"<<std::endl;std::cin>>xyz.x;std::cin>>xyz.y;std::cin>>xyz.z;std::cout<<" x="<<xyz.x<<" y="<<xyz.y<<" z="<<xyz.z<<std::endl;std::cin.clear();}//最近的球if(global_==false && i==1){std::cout<<"\n请依次输入较远的球的球心对应的全局坐标位置x y z之间用空格或回车\n"<<std::endl;std::cin>>xyz.x;std::cin>>xyz.y;std::cin>>xyz.z;std::cout<<" x="<<xyz.x<<" y="<<xyz.y<<" z="<<xyz.z<<std::endl;std::cin.clear();}//第二近的球if(global_==false && i==2){std::cout<<"\n请依次输入最远的球的球心对应的全局坐标位置x y z之间用空格或回车\n"<<std::endl;std::cin>>xyz.x;std::cin>>xyz.y;std::cin>>xyz.z;std::cout<<" x="<<xyz.x<<" y="<<xyz.y<<" z="<<xyz.z<<std::endl;std::cin.clear();global_=true;}//最远的球xyz_all.push_back(xyz);}/* 完成三个球的球心检测，其在局部坐标系内的坐标存储在coefficients_all,输入的全局坐标在xyz_all */for(i=0;i<3;i++)//循环次数控制球个数{// Segment the largest planar component from the remaining cloudpcl::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 cloudpcl::ExtractIndices<pcl::PointXYZRGB> extract2;extract2.setInputCloud (cloud_filtered);extract2.setIndices (tmpinliers2);extract2.setNegative (false);// Write the planar inliers to diskextract2.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 restextract2.setNegative (true);extract2.filter (*cloud_f2);cloud_filtered = cloud_f2;}/* 开始进行变换矩阵估计 */Eigen::Matrix4f transformationCorrespondence; pcl::TransformationFromCorrespondences transformationFromCorr; for ( i =0;i<coefficients_all.size();i++) { Eigen::Vector3f from(coefficients_all.at(i).values[0], coefficients_all.at(i).values[1],coefficients_all.at(i).values[2]); Eigen::Vector3f to (xyz_all.at(i).x, xyz_all.at(i).y,xyz_all.at(i).z); transformationFromCorr.add(from, to, 1.0);//all the same weight } transformationCorrespondence= transformationFromCorr.getTransformation().matrix(); std::cout<< "\ntransformation from corresponding points is \n"<<transformationCorrespondence<<std::endl; std::cout<< "\n如果您认为标定数据正确，则输入y，则会提示保存数据，否则输入n。\n"<<std::endl; char userin;std::cin.clear();std::cin.get(userin);if(userin=='y'||userin=='Y'){std::string filename;std::cin.clear();std::cout<< "\n请输入文件名。\n"<<std::endl;std::cin>>filename;std::ofstream myfile( filename, std::ios::binary);myfile.write((char *)transformationCorrespondence.data(),transformationCorrespondence.size()*sizeof(float));myfile.close();}else{std::cout<< "\n程序继续。\n"<<std::endl; }}void run (const std::string& device_id){pcl::Grabber* interface = new pcl::OpenNIGrabber(device_id);boost::function<void (const boost::shared_ptr<openni_wrapper::Image>& image, const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image, float)>f = boost::bind (&KinectViewerCuda::cloud_cb_, this, _1, _2, _3);boost::signals2::connection c = interface->registerCallback (f);interface->start ();while (true){pcl_sleep (1);}interface->stop ();}pcl::cuda::DisparityToCloud d2c;pcl::visualization::CloudViewer viewer;boost::mutex mutex_;bool downsample_;pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_;bool global_;std::vector <pcl::PointXYZ> xyz_all;pcl::PointXYZ xyz;
};int main (int argc, char** argv)
{std::string device_id = "#1";int downsample = false;if (argc >= 2){device_id = argv[1];}if (argc >= 3){downsample = atoi (argv[2]);}KinectViewerCuda v (downsample);v.run (device_id);std::cout<< "/n正常退出/n"<<std::endl; return 0;
}
【解释说明】
本实例包括数据获取模块、直通滤波模块、平面检测及删除模块、圆球检测模块、刚体
变换矩阵估计模块，下面针对各个模块对源代码进行梳理和解析。
数据获取模块
本实例是利用支架固定双深度摄像头( 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 cloudpcl::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 cloudpcl::ExtractIndices<pcl::PointXYZRGB> extract2;extract2.setInputCloud (cloud_filtered);extract2.setIndices (tmpinliers2);extract2.setNegative (false);// Write the planar inliers to diskextract2.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 restextract2.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
此时即可获取双深度摄像头局部坐标系之间的变换矩阵，需要注意的是检测的精度与效果同圆球材质和采集距离有关。
PCL中采样一致性算法相关推荐
- PCL采样一致性算法
在计算机视觉领域广泛的使用各种不同的采样一致性参数估计算法用于排除错误的样本,样本不同对应的应用不同,例如剔除错误的配准点对,分割出处在模型上的点集,PCL中以随机采样一致性算法(RANSAC)为核心 ...
- PCL学习笔记5-sample consensus采样一致性算法
PCL几种采样方法 - Being_young - 博客园 PCL常见采样方法 下采样 Downsampling 通过构造一个三维体素栅格,用每个体素内所有点的重心近似该体素其他点,达到滤波的效果,这 ...
- 点云PCL学习笔记-分割segmentation-RANSAC随机采样一致性算法欧式聚类提取
随机采样一致性算法RANSAC 程序实例参考网址: https://pcl.readthedocs.io/projects/tutorials/en/latest/random_sample_cons ...
- PCL学习：随机采样一致性算法（RANSAC）
此文是在他人的文章上进行了补充完善.另外代码部分是在Ziv Yaniv的c++实现上重新实现了一次,加了中文注释,修正了一个错误.便于理解算法实现. 随机采样一致性算法,RANSAC是"RA ...
- Python-pcl 随机采样一致性算法
RANSAC 随机采样一致性算法 RANSAC是一种随机参数估计算法.RANSAC从样本中随机抽选出一个样本子集,使用最小方差估计算法对这个子集计算模型参数,然后计算所有样本与该模型的偏差,在使用一个 ...
- PCL 点云分割与分类 Segmentation RANSAC随机采样一致性 平面模型分割 欧氏距离分割 区域聚类分割算法 最小分割算法 超体聚类 渐进式形态学滤波器
点云分割 博文末尾支持二维码赞赏哦 _ 点云分割是根据空间,几何和纹理等特征对点云进行划分, 使得同一划分内的点云拥有相似的特征,点云的有效分割往往是许多应用的前提, 例如逆向工作,CAD领域对零件的 ...
- PCL中RANSAC模型的使用
RANSAC算法是什么 RANSAC算法于1981年由Fischler和Bolles提出,全程是RANdom SAmple Consensus,一般中文翻译为"随机抽样一致性算法" ...
- 【转】分布式一致性算法：Raft 算法（Raft 论文翻译）
编者按:这篇文章来自简书的一个位博主Jeffbond,读了好几遍,翻译的质量比较高,原文链接:分布式一致性算法:Raft 算法(Raft 论文翻译),版权一切归原译者. 同时,第6部分的集群成员变更读 ...
- 分布式一致性算法：Raft 算法
由于微信字数的限制,此处给出的并非全文,请拉到页面最下方点击阅读原文查看完整版. Raft 算法是可以用来替代 Paxos 算法的分布式一致性算法,而且 raft 算法比 Paxos 算法更易懂且更容 ...
- 分布式一致性算法——Paxos 和 Raft 算法
写在前面 本文隶属于专栏<100个问题搞定大数据理论体系>,该专栏为笔者原创,引用请注明来源,不足和错误之处请在评论区帮忙指出,谢谢! 本专栏目录结构和参考文献请见100个问题搞定大数据理 ...
最新文章
- C#编码实践：使用委托和特性调用指定函数
- 基于互联网大脑架构的阿里巴巴未来趋势分析【系列2】
- qt opencv cmake配置 单纯小白
- 磁盘格式化，磁盘挂载，手动增加swap空间
- Redis中缓存Lua 脚本
- (二)stm32之中断配置
- 《C++并发编程实战》——1.1 什么是并发
- 斗鱼tv 服务器响应失败,斗鱼tv打不开怎么办 斗鱼直播打不开得解决办法
- 计算机图形学(闫令琪博士课程答疑)-Shading（二）
- Snipaste--截屏--贴图
- 常威来了（变量和常量）简单易懂
- Distill文章-A gentle introduction to graph Neural Networks（图神经网络是怎么构造的）
- JAVA IO流（内存流、管道流、打印流）
- 视频教程-2020年软考系统集成项目管理工程师应用技术软考视频教程-软考
- 微信小程序期末大作业，体育新闻小程序
- Gear 在 Polkadot 网络中的作用是什么？
- JavaScript高级程序设计 第4版 -- 操作符
- 基于纬创的流水线物料摆放分析与优化
- python实现堆排序算法_Python实现的堆排序算法示例
- 想在Java中把PPT转化为PDF吗？教你用Aspose.Slides轻松搞定！