RANSAC
)为核心,同时实现了五种类似于随机采样一致性估计参数算法的随机参数估计算法,例如随机采样一致性估计(
RANSAC
)、最大似然一致性估计(
MLESAC
)、最小中值方差一致性估计(
LMEDS
)等,所有的估计参数算法都符合一致性准则。本文涉及的基于采样一致性算法的应用主要是对点云进行分割,根据不同的设定几何模型,估计对应的几何模型的参数,在一定允许误差范围内分割出在模型上的点云,但本文并非讲述点云分割,主要是对采样一致性进行相关的介绍。目前PCL中支持的几何模型分割有空间平面、直线、二维或三维圆周、圆球、锥体等,如下图所示。随机采样一致性估计的另一应用就是点云的配准。
本文首先对随机采样一致性模块相关概念及算法进行简介,其次对PCL的随机采样一致性相关模块及类进行简单介绍,最后通过应用实例来展示如何对PCL中随机采样一致性模块进行灵活运用。
RANSAC
是一种随机参数估计算法。RANSAC
从样本中随机抽选出一个样本子集,使用最小方差估计算法对这个子集计算模型参数,然后计算所有样本与该模型的偏差,再使用一个预先设定好的阈值与偏差比较,当偏差小于阈值时该样本点属于模型内样本点(inliers
),文中简称局内点或内点,否则为模型外样本点(outliers
),文中简称局外点或外点,记录下当前的inliers
的个数,然后重复这一过程。每一次重复,都记录当前最佳的模型参数,所谓最佳,即inliers
的个数最多,此时对应的inliers
个数为best_ninliers
。每次选代的末尾,都会根据期望的误差率、best_ninliers
、总样本个数、当前选代次数,计算一个迭代结束评判因子,据此决定是否迭代结束。迭代结束后,最佳模型参数就是最终的模型参数估计值。
RANSAC
理论上可以剔除outliers
的影响,并得到全局最优的参数估计。但是RANSAC
有两个问题,首先在每次代中都要区分inliers
和outliers
,因此需要事先设定阈值,当模型具有明显的物理意义时,这个值还比较容易设定,但是若模型比较抽象时,这个阈值就不那么容易设定了,而且固定阈值不适用于样本动态变化的应用;第二个问题是,RANSAC
的迭代次数是运行期决定的,不能预知迭代的确切次数(当然选代次数的范围是可以预测的),除此之外,RANSAC
只能从一个特定数据集中估计一个模型,当两个(或者更多个)存在时,RANSAC
不能找到别的模型。
下图展示了RANSAC
算法在二维数据集中的简单应用。左侧的图像形象地表示了一组既包含局内点又包含局外点的数据集。右侧的图像中,所有的局外点都表示为红色,局内点表示为蓝色,蓝色线就是基于RANSAC
得到的结果,此例中我们尝试适应数据的模型就是一条线。
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
偏差值有关。
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,从点云中分割提取的内点都处在估计参数对应的平面上或与平面距离在一定范围内。
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)
为直线方向向量,从点云中分割提取的内点都处在估计参数对应直线上或与直线距离在一定范内。
SACMODEL_CIRCLE2D
模型:定义为二维圆的圆周模型,共设置3个参数[center.x center.y radius]
,其中(center.x, center.y)
为圆周中心二维坐标,radius
为圆周半径,从点云中分割提取的内点都处在估计参数对应的圆周上或距离圆周边线的距离在一定范围内。
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)
为三维圆所在平面的法向量坐标,从点云中分割提取的内点都处在估计参数对应的圆周上或距离圆周边线的距离在一定范围内。
SACMODEL_SPHERE
模型:定义为三维球体模型,共设置4个参数[center.x center.y center.z radius]
,其中(center.x center.y center.z)
为球体中心的三维坐标,radius
为球体半径,从点云中分割提取的内点都处在估计参数对应的球体上或距离球体边线的距离在一定范围内。
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
为圆柱体半径,从点云分割提取的内点都在估计参数对应的圆柱体上或距离柱体边线的距离在一定范围内。
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
为锥角的大小,从点云分割提取的内点都在估计参数对应的圆锥体上或距离锥体边线的距离在一定范围内。
SACMODEL_TORUS模型:定义为圆环面模型,尚未实现。
SACMODEL_PARALLEL_LINE
模型:定义为有条件限制的直线模型,在规定的最大角度偏差限制下,直线模型与给定轴线平行,其参数设置参见SACMODEL_LINE
模型。
SACMODEL_PERPENDICULAR_PLANE
模型:定义为有条件限制的平面模型,在规定的最大角度偏差限制下,平面模型与给定轴线垂直,参数设置参见SACMODEL_PLANE
模型。
SACMODEL_NORMAL_PLANE
模型:定义为有条件限制的平面模型,在规定的最大角度偏差限制下,每一个局内点的法线必须与估计的平面模型的法线平行,参数设置参SACMODEL_PLANE
模型。
SACMODEL_PARALLEL_PLANE
模型:定义为有条件限制的平面模型,在规定的最大角度偏差限制下,平面模型与给定的轴线平行,参数设置参见SACMODEL_PLANE
模型。
SACMODEL_NORMAL_PARALLEL_PLANE
模型:定义为有条件限制的平面模型,在法线约束来下,三维平面模型必须与用户设定的轴线平行,参数设置见SACMODEL_PLANE
模型。
PCL库实现了随机采样一致性及其泛化估计算法,以及例如平面、柱面等各种常见几何模型,用不同的估计算法和不同的几何模型自由结合估算点云中隐含的具体几何模型的系数,实现对点云中所处的几何模型的分割。线、平面、柱面和球面等模型已经在PCL库中实现,平面模型经常被应用到常见的室内平面分割提取中,比如墙、地板、桌面,其他模型常应用到根据几何结构检测识别和分割物体中(比如用一个圆柱体模型分割出一个杯子),类和函数的具体说明请参考官网。
本小节将学习如何使用RandomSampleConsensus
类获得点云的拟合平面及球面模型。
首先创建一个工作空间random_sample_consensus
,然后再在工作空间创建一个文件夹src
用于存放源代码:
mkdir -p random_sample_consensus/src
接着,在random_sample_consensus/src
路径下,创建一个文件并命名为random_sample_consensus.cpp
,拷贝如下代码:
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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));
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); // 存储估计所得的局内点
}
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));
}
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));
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
运行结果如下图所示,与原始点云相比,立方体内的点都被作为局外点而被剔除掉,而估计所得的局内点只包含球面上的点集。
经过前面的学习,我们已经学会了如何利用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
#include
#include
#include
#include
#include
//#include
#include
//#include
#include
#include
#include
#include
#include
#include
#include
//#include
#include
#include
//#include
#include
//#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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 cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (tmpinliers);
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;
}
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 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;
}
/* 开始进行变换矩阵估计 */
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 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
此时即可获取双深度摄像头局部坐标系之间的变换矩阵,需要注意的是检测的精度与效果同圆球材质和采集距离有关。