如何逐步匹配多幅点云(1)

转载 2016年08月30日 14:45:15

【原文:http://www.pclcn.org/study/shownews.php?lang=cn&id=78

本章演示了迭代最近点算法的使用,以便逐步地对一系列点云进行两两匹配。它的思想是对所有的点云进行变换,使得都与第一个点云在统一坐标系中。在每个连贯的有重叠的点云之间找到最佳的变换,并累积这些变换到全部的点云。能够进行ICP算法的点云需要粗略的预匹配(如:在一个机器人的量距内或在地图框架内),并且一个点云与另一个点云需要有重叠部分。

代码

首先,在PCLPoint Cloud Learning)中国协助发行的书[1]提供光盘的第13章例2文件夹中,打开名为pairwise_incremental_registration.cpp的代码文件,同文件夹下可以找到相关的测试点云文件capture000[1-5].pcd

解释说明

首先快速看一下相关类和数据结构的头文件声明声明下面语句包含所有我们使用的类的定义的头文件。

#include                     //boost指针相关头文件

#include                         //点类型定义相关头文件

#include                         //点云类相关头文件

#include               //点表示相关头文件

#include                           //pcd文件打开存储类相关头文件

#include                 //基于体素网格化的滤波类相关头文件

#include                     //滤波相关头文件

#include                 //法线特征相关头文件

#include                   //icp类相关头文件

#include                //非线性icp类相关头文件

#include           //变换矩阵类相关头文件

#include    //可视化类相关头文件

为了可视化目的,方便用户直观的观察到配准前后结果以及配准过程,创建全局可视化对象变量,并定义左右视点分别显示配准前和配准后的结果点云。

pcl::visualization::PCLVisualizer *p;//创建可视化对象

int vp_1, vp_2;                        //定义存储左右视点ID

声明一个结构体,方便对点云以文件名和点云对象进行成对处理管理,在配准过程中,可以同时接受多个点云文件输入,程序从第一个文件开始,连续的两两配准处理,然后存储配准后的点云文件。

struct PCD

{

  PointCloud::Ptr cloud;

  std::string f_name;

PCD() : cloud (new PointCloud) {};

};

定义一个新的点表示。

//< x, y, z, curvature >形式定义一个新的点表示

classMyPointRepresentation:public pcl::PointRepresentation <PointNormalT>

{

using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:

  MyPointRepresentation ()

  {

    nr_dimensions_ =4;//定义点的维度

  }

//重载copyToFloatArray方法来将点转化为4维数组

virtual void copyToFloatArray (const PointNormalT &p, float* out) const

  {

out[0= p.x;

out[1= p.y;

out[2= p.z;

out[3= p.curvature;

  }

};

整个配准过程分几步,来看看如何组织我们的函数,主函数检查用户输入,在点云矢量data中加载了用户输入的所有点云数据,建立两个视口的可视化对象,左边显示未配准的源和目标点云,右边显示配准后的源和目标点云。在为一对点云找到变换矩阵后,将目标点云变换到源点云坐标系下,并将源点云与变换后的目标点云存储到一点云文件,同时用此次找到的变换矩阵更新全局变换,用于将后续的点云都变换到与第一个输入点云同一坐标系下。

int main (int argc, char** argv)

{

  std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //  存储管理所有打开的点云

  loadData (argc, argv, data);// 加载所有点云文件到data

if (data.empty ())//检查用户输入

  {

    PCL_ERROR ("Syntax is: %s [*]", argv[0]);

    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");

    PCL_INFO ("Example: %s `rospack find pcl`/test/bun0.pcd `rospack find pcl`/test/bun4.pcd", argv[0]);

return (-1);

  }

  PCL_INFO ("Loaded %d datasets.", (int)data.size ());

  p =new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");//创建一个PCL可视化对象

  p->createViewPort (0.000.51.0, vp_1);           //用左半窗口创建视口vp_1

  p->createViewPort (0.501.01.0, vp_2);           //用右半窗口创建视口vp_2

  PointCloud::Ptr result (new PointCloud), source, target;

  Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;

for (size_t i =1; i < data.size (); ++i)             //循环处理所有点云

  {

    source = data[i-1].cloud;                             //连续配准

    target = data[i].cloud;                               //相邻两组点云

    showCloudsLeft(source, target);                      //可视化为配准的源和目标点云

    PointCloud::Ptr temp (new PointCloud);

PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());

//调用子函数完成一组点云的配准,temp返回配准后两组点云在第一组点云坐标下的点云,pairTransform返回从目标点云target到源点云source的变换矩阵。

pairAlign (source, target, temp, pairTransform, true);

//把当前的两两配对后的点云temp转换到全局坐标系下(第一个输入点云的坐标系)返回result

    pcl::transformPointCloud (*temp, *result, GlobalTransform);

//用当前的两组点云之间的变换更新全局变换

    GlobalTransform = pairTransform * GlobalTransform;

//保存转换到第一个点云坐标下的当前配准后的两组点云result到文件i.pcd

std::stringstream ss;

ss<< i <<".pcd";

pcl::io::savePCDFile (ss.str (), *result, true);

整体的程序框架了解后,我们来分别对不同的子函数实现进行分析。加载数据非常简单,我们迭代其他程序的参数,检查每一个参数是否指向一个pcd文件,如果是,创建一个添加到点云矢量data中的PCD对象。

void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models)

{

std::string extension (".pcd");

for (int i =1; i < argc; i++) //第一个参数为命令本身,所以从第二个参数开始解析

  {

std::string fname = std::string (argv[i]);

if (fname.size () <= extension.size ())    //pcd文件名至少为5个字符大小字符串

continue;

    std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);

if (fname.compare (fname.size () - extension.size (), extension.size (), extension)==0)//检查参数是否为一个pcd后缀的文件

    {

//加载点云并保存在总体的点云列表中

      PCD m;

      m.f_name = argv[i];

pcl::io::loadPCDFile (argv[i], *m.cloud);

//从点云中移除NAN

      std::vector<int> indices;

      pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);

      models.push_back (m);

...

现在我们开始进行实际匹配,由子函数pairAlign具体实现,其中参数有输入一组需要配准的点云,以及是否进行下采样的设置项,其他参数输出配准后的点云及变换矩阵。

void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample =false)

首先,我们可以选择对点云进行下采样,这对于大型数据集是非常实用的,可以加快后期的处理计算速度,然后计算法线,法线输入用于可视化。

//下采样

PointCloud::Ptr src (new PointCloud);         //存储滤波后的源点云

  PointCloud::Ptr tgt (new PointCloud);       //存储滤波后的目标点云

  pcl::VoxelGrid grid;                 //滤波处理对象

if (downsample)

  {

    grid.setLeafSize (0.05, 0.05, 0.05);      //设置滤波处理时采用的体素大小

    grid.setInputCloud (cloud_src);

    grid.filter (*src);

    grid.setInputCloud (cloud_tgt);

    grid.filter (*tgt);

  }

else

  {

src = cloud_src;

tgt = cloud_tgt;

  }

//计算点云法线

  PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);

  PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);

  pcl::NormalEstimation norm_est; //点云法线估计对象

  pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ());

  norm_est.setSearchMethod (tree);                        //设置估计对象采用的搜索对象

  norm_est.setKSearch (30);                                //设置估计时进行搜索用的k

  norm_est.setInputCloud (src);

  norm_est.compute (*points_with_normals_src);           //下面分别估计源和目标点云法线

  pcl::copyPointCloud (*src, *points_with_normals_src);

  norm_est.setInputCloud (tgt);

  norm_est.compute (*points_with_normals_tgt);

  pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

一切都准备好了,可以进行配准了,创建ICP对象,设置它的参数,以需要匹配的两个点云作为输入,用户使用时,参数的设置需要根据自己的数据集进行调整。

// 配准

  pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; //配准对象

  reg.setTransformationEpsilon (1e-6);//设置收敛判断条件,越小精度越大,收敛也越慢

//将两个点云中的对应点对之间的(src<->tgt)最大距离设置为10厘米,大于此值的点对不考虑。

//注意:根据你的数据集大小来调整

  reg.setMaxCorrespondenceDistance (0.1); 

//设置点表示

  reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation>(point_representation));

  reg.setInputCloud (points_with_normals_src);    //设置源点云

  reg.setInputTarget (points_with_normals_tgt);   //设置目标点云

因为这是一个指导实例,我们希望显示匹配过程的迭代过程,为了这个目的,ICP在内部进行计算时,限制其最大的迭代次数为2,即每迭代两次就认为收敛,停止内部迭代。

  reg.setMaximumIterations (2);                     //设置最大迭代次数

         手动迭代(本例中是30次),每手动迭代一次,在配准结果视口对迭代的最新的结果进行刷新显示。

for (int i =0; i <30++i)

  {

[...]

    points_with_normals_src = reg_result;

    reg.setInputCloud (points_with_normals_src);

    reg.align (*reg_result);

[...]

  }

在每次迭代过程中,我们记录并积累由ICP返回的变换:

Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;

[...]

for (int i =0; i <30++i)

  {

[...]

    Ti = reg.getFinalTransformation () * Ti;

[...]

  }

如果迭代N次找到的变换和迭代N-1中找到的变换之间的差异小于传给ICP的变换收敛阈值,我们选择源与目标之间更靠近的对应点距离阈值来改善配准过程。

for (int i =0; i <30++i)

  {

[...]

if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())

      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () -0.001);

prev= reg.getLastIncrementalTransformation ();

[...]

  }

一旦找到最优的变换,ICP返回的变换是从源点云到目标点云的变换矩阵,我们求逆变换得到从目标点云到源点云的变换矩阵,并应用到目标点云,变换后的目标点云然后添加到源点云中,并且将点云和变换矩阵一起返回到主函数。

  targetToSource = Ti.inverse();// 得到目标点云到源点云的变换

//把目标点云变换到源点云坐标系下

  pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);

[...]

*output +=*cloud_src;

    final_transform = targetToSource;

    未完待续,敬请关注“如何逐步匹配多幅点云”的编译和运行程序和结果


相关文章推荐

基于深度学习的图像语义分割技术概述之4常用方法

4 深度学习图像分割的常用方法 深度学习在多种高级计算机视觉任务中的成功—特别是监督CNNs(Convolutional Neural Networks,卷积神经网络)在图像分类、对象检测方面的成功...

如何逐步匹配多幅点云

/* \author Radu Bogdan Rusu * adaptation Raphael Favier*/ #include #include #include #include #i...

利用PCL(Point Cloud Library)进行点云拼接

本文翻译自官网教程 近期在做一个关于三维扫描的项目,需要用到点云拼接,从而接触到PCL。PCL是一个类似于OpenCV的开源库,只是OpenCV提供的是对二维图像的处理方法,而PCL提供了很多三维点云...

使用正态分布变换进行配准

#include #include #include #include #include #include #include int main(int argc, char** a...

Ubuntu安装Kinect驱动(openni、NITE、Sensor)及遇到的问题

一、前言先说一下博主的运行环境: Ubuntu kylin14.04 kinect第一代 另外还要注意的是openni、Sensor、NITE这三个包的版本必须匹配才能正常运行,我的版本分别是:...

【Matlab Computer Vision System ToolBox】学习笔记-1-点云配准流程 | 特征匹配

本系列博客将介绍Matlab中机器视觉工具箱的应用,更多内容见Matlab官方文档。 1. PointCloud Registration Workflow -点云配准流程 2. Bluran ...

关于点云的匹配算法

  • 2017年11月07日 10:05
  • 818B
  • 下载

点云匹配ICP

  • 2014年06月14日 21:15
  • 114KB
  • 下载

点云匹配和ICP算法概述

【原文:http://www.cnblogs.com/yhlx125/p/4955337.html】 Iterative Closest Point (ICP) [1][2][3] is ...

彩色点云初始匹配出现色彩丢失的问题

我在进行点云SAC初始匹配时,出现了点云色彩丢失的问题。如下图所示:我用了两种方法:一种是直接对PointXYZRGB格式的点云进行SAC配准,得到配准后的图像。另一种是以PointXYZ格式载入点云...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:如何逐步匹配多幅点云(1)
举报原因:
原因补充:

(最多只允许输入30个字)