基于PCL库的通过ICP匹配多幅点云方法

前言
Code
Result

前言

PCL库中有很多配准的方式，主要都是基于ICP

ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P = {Pi, i = 0,1, 2,…,k}及U = {Ui,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k ≥ n。配准过程就是求取 2 个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：
(1)计算最近点，即对于集合U中的每一个点，在集合P中都找出距该点最近的对应点，设集合P中由这些对应点组成的新点集为Q = {qi,i = 0,1,2,…,n}。
(2)采用最小均方根法，计算点集 U 与 Q 之间的配准，使得到配准变换矩阵 R，T，其中R是 3×3 的旋转矩阵，T 是 3×1 的平移矩阵。
(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U1，即U1 = RU + T
(4)计算U1与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U1替换U，重复上述步骤。

SLAM中点云的地图构建，本质就是把点云拼在一起，形成的点云模型。
那么如何把点云拼在一起呢，方法有很多，例如ICP、NDT
点云拼接，点云配准其实是一个意思，都是把不同位置的点云通过重叠部分的信息，变换到同一个位置。

在这篇博客中实现了PCL基本ICP算法的点云拼接

本次使用PCL中的ICPNL（IterativeClosestPointNonLinear）算法来实现多幅点云的拼接

功能的实现思想就是对所有点云进行变换，使得都与第一个点云在统一的坐标系中，在每个连贯的有重叠的点云之间找到最佳的变换，并积累这些变换到全部的点云。

Code

///

#include <boost/make_shared.hpp>//boost 指针相关头文件
#include <pcl/point_types.h>//点云类型定义
#include <pcl/point_cloud.h>//点云类
#include <pcl/point_representation.h>//点相关表示
#include <pcl/io/pcd_io.h>//pcd文件打开存储类
#include <pcl/filters/voxel_grid.h>//体素滤波
#include <pcl/filters/filter.h>//滤波
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/registration/icp.h>//icp类
#include <pcl/registration/icp_nl.h>//非线性icp类
#include <pcl/registration/transforms.h>//变换矩阵
#include <pcl/visualization/pcl_visualizer.h>//可视化

相关头文件功能已注释

///

//定义点、点云  的类型
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;

设置点云类型
///

//创建可视化工具
pcl::visualization::PCLVisualizer *p;
//定义左右视点
int vp_1, vp_2;

创建可视化工具分成左右两个视点左边显示匹配的两帧原始数据直接拼一起的效果右边显示拼接过程与最终效果
///

//声明一个结构体 里面可以存放文件名和对应的点云
struct PCD
{PointCloud::Ptr cloud;//存储对应的点云std::string f_name;//存储对应的文件名PCD():cloud(new PointCloud) {};//结构体构造函数
};

声明一个结构体里面可以存放文件名和对应的点云
///

//以< x, y, z, curvature >形式定义一个新的点
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:MyPointRepresentation (){//定义尺寸值nr_dimensions_ = 4;}
//覆盖copyToFloatArray方法来定义我们的特征矢量virtual void copyToFloatArray (const PointNormalT &p, float * out) const{// < x, y, z, curvature >out[0] = p.x;out[1] = p.y;out[2] = p.z;out[3] = p.curvature;}
};

PCL的点的表示相关以< x, y, z, curvature >形式定义一个新的点

class pcl::PointRepresentation< PointT > 这个给主要提供将一个点转换成n维向量的方法
这是一个抽象类，子类必须在构造函数中设置 nr_dimensions_为合适的值，并且提供纯虚函数 copyToFloatArray（）方法的实现

定义的类可以调用的父类（ pcl::PointRepresentation）的函数有
getNumberOfDimensions() 返回点的向量表示中的维数。也就是 nr_dimensions_ 的值
setRescaleValues(const float * rescale_array) 设置点的缩放值这个在后面会用到
///

void loadData(int argc, char **argv,std::vector<PCD, Eigen::aligned_allocator<PCD> > &models )
{std::string extension (".pcd");//声明一个字符串  要读取文件的后缀名//循环读指令 指定的文件  第1个参数是执行程序，第二个参数是pcd文件 所有i从1开始  for(int i=1;i<argc;i++){std::string fname = std::string (argv[i]);//提取第i个pcd文件的名字//对文件的名字的长度判断大小if(fname.size ()<= extension.size ()){ continue;}//跳出本次循环//将文件中的大写字母转成小写  如果有的话std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);//检查文件的名字 后缀 是否 为 pcdif (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0){/*加载点云并保存在总体的模型列表中*/PCD m;//声明一个PCD结构体m.f_name = argv[i];//赋值文件名if(pcl::io::loadPCDFile(argv[i], *m.cloud)==-1)//取点云{//没有读到  输出信息  std::cout<<"路径不对，没读到pcd文件：  "<<argv[i]<<std::endl;continue;//跳出本次循环}//从点云中移除NAN点std::vector<int> indices;pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);models.push_back (m);//保存在总体的模型列表中}}
}

从PCD文件中读取点云
过程在代码里面基本每行都注释了
主要就是判断设置的文件名字是否正确，然后读取，保存在总体的模型列表中
///

/** 在可视化窗口的第一视点显示源点云和目标点云
**/
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{p->removePointCloud ("vp1_target");p->removePointCloud ("vp1_source");pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0);p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1);p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);PCL_INFO ("Press q to begin the registration.\n");p-> spin();
}/**在可视化窗口的第二视点显示源点云和目标点云**/
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{p->removePointCloud ("source");p->removePointCloud ("target");pcl::visualization::PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");if (!tgt_color_handler.isCapable ())PCL_WARN ("Cannot create curvature color handler!");pcl::visualization::PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature");if (!src_color_handler.isCapable ())PCL_WARN ("Cannot create curvature color handler!");p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);p->spinOnce();
}

左右的可视化窗口显示程序

///
下面是ICP的两两校准函数

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

匹配一对点云数据集并且返还结果
参数 cloud_src 是源点云
参数 cloud_src 是目标点云
参数output输出的配准结果的源点云
参数final_transform是在来源和目标之间的转换

    /*下采样*///为了一致性和高速的下采样//注意：为了大数据集需要允许这项PointCloud::Ptr src (new PointCloud);//下采样后的源点云PointCloud::Ptr tgt (new PointCloud);//下采样后的目标点云pcl::VoxelGrid<PointT> 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<PointT, PointNormalT> norm_est;//法向量估计pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());//kdtreenorm_est.setSearchMethod (tree);norm_est.setKSearch (30);//估计源点云的各点法向量norm_est.setInputCloud (src);norm_est.compute (*points_with_normals_src);//src中的xyz覆盖掉points_with_normals_src中的xyz值，然后把xyz+normal的信息给points_with_normals_srcpcl::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提供点匹配的特征

///

    //我们自定义点的表示（以上定义）MyPointRepresentation point_representation;//调整'curvature'尺寸权重以便使它和x, y, z平衡float alpha[4] = {1.0, 1.0, 1.0, 1.0};point_representation.setRescaleValues (alpha);

设置点的表示向量，并设置缩放

///

    /*配准*/pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;//icpnl对象// 设置 两次变换矩阵之间的停止差值 (默认 2)reg.setTransformationEpsilon (1e-6);//将两个对应关系之间的(src<->tgt)最大距离设置为10厘米// 设置最大的对应关系距离为 多少 单位m， （对应距离大于此距离将被忽略） reg.setMaxCorrespondenceDistance (15); //设置点表示  继承与  Registration 类的  setPointRepresentation 的方法 功能 提供指向 PointRepresentation 的 boost 共享指针，以便在比较点时使用。reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));reg.setInputCloud (points_with_normals_src);reg.setInputTarget (points_with_normals_tgt);reg.setMaximumIterations (2);//设置最大迭代次数  因为以手动方式设置的很小

进行配准的相关设置

x容差
对应距离
点的表示
输入、目标点云
迭代次数

    //Ti 本次迭代的转换矩阵  prev上次迭代的转换矩阵  targetToSource 目标点云到源点云的变换Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;PointCloudWithNormals::Ptr reg_result = points_with_normals_src;//本次迭代结合的点云//设置一个初始的估计 位姿矩阵Eigen::Matrix4f  T_init = Eigen::Matrix4f::Identity ();T_init<<1,0,0, 0,0,1,0,0,0,0,1,0,0,0,0,1;

设置相关的矩阵

///

    //手动循环30次for (int i = 0; i < 30; ++i){//打印迭代的第几次PCL_INFO ("Iteration Nr. %d.\n", i);//为了可视化的目的保存点云points_with_normals_src = reg_result;//更新上一次结合的点云//估计reg.setInputCloud (points_with_normals_src);reg.align (*reg_result,T_init);//得到结合的点云//在每一个迭代之间累积转换Ti = reg.getFinalTransformation () * Ti;//如果这次转换和之前转换之间的差异小于阈值//则通过减小最大对应距离来改善程序if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ()){reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);prev = reg.getLastIncrementalTransformation ();std::cout<<"reduce max correspond distance"<<std::endl;}//可视化当前状态showCloudsRight(points_with_normals_tgt, points_with_normals_src);}// 得到目标点云到源点云的变换targetToSource = Ti.inverse();//把目标点云转换回源点云坐标系pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);//可视化最终结果状态p->removePointCloud ("source");p->removePointCloud ("target");pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);p->addPointCloud (output, cloud_tgt_h, "target", vp_2);p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);PCL_INFO ("Press q to continue the registration.\n");p->spin ();p->removePointCloud ("source"); p->removePointCloud ("target");*output += *cloud_src;//将源点云加的结果上  即 返回拼接的点云final_transform = targetToSource;//返回的 目标点云转换回源点云的坐标变换
}

手动循环30次
进行估计
在每一个迭代之间累积转换
得到目标点云到源点云的变换
将源点云加的结果上即返回拼接的点云

///

int main (int argc, char** argv)

下面是main函数

    //加载数据  std::vector<PCD, Eigen::aligned_allocator<PCD> > data;//data是PCD的数据类型，第二个参数是动态分配内存loadData (argc, argv, data);//根据用户指令 读取数据 存在data中

加载数据

    //创建一个PCL可视化对象p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");p->createViewPort (0.0, 0, 0.5, 1.0, vp_1);p->createViewPort (0.5, 0, 1.0, 1.0, vp_2);

创建PCL可视化对象

    //result是 统一转换到第一个pcd坐标系系的 点云   source 是源点云 上一帧  target是目标点云 当前帧PointCloud::Ptr result (new PointCloud), source, target;PointCloud::Ptr all_result (new PointCloud);//GlobalTransform 是保存的两个pcd结合转到 第一帧的坐标变换   //pairTransform 是两帧pcd之间的坐标变换Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;

声明相关变量

///

    //循环处理每个pcdfor (size_t i = 1; i < data.size (); ++i){//从data中分别提取 点云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 ());//执行对准pairAlign (source, target, temp, pairTransform, true);//把当前的两两配对转换到全局变换pcl::transformPointCloud (*temp, *result, GlobalTransform);*all_result += *result;//update the global transform更新全局变换GlobalTransform = pairTransform * GlobalTransform;//保存配准对，转换到第一个点云框架中std::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *all_result, true);}

循环两两处理每个pcd
把当前的两两配对转换到全局变换
保存配准对
///

Result

基于PCL库的通过ICP匹配多幅点云方法相关推荐

学习PCL库：PCL库中的IO模块介绍
公众号致力于点云处理,SLAM,三维视觉,高精地图等领域相关内容的干货分享,欢迎各位加入,有兴趣的可联系dianyunpcl@163.com.未经作者允许请勿转载,欢迎各位同学积极分享和交流. IO模 ...
3D视觉学习计划之PCL库的基础知识
3D视觉学习计划之PCL库的基础知识一.PCL库的概述 PCL是一个大型跨平台开源C++编程库,它在吸收了前人点云相关研究基础上建立起来,实现了大量点云相关的通用算法和高效数据结构,涉及到点云获取. ...
十九.在ROS系统基于点云和视觉图像数据融合构建3D点云场景
一. 背景介绍现在很多智能导航场景都涉及到激光(毫米波,固态等)雷达和相机视觉信息融合,这里激光雷达一般都是指多线激光雷达,16线,64线,甚至更多线数. 但多线激光雷达动不动数万的价格,让很多技术 ...
随笔：送给初次使用PCL库的小伙伴
写在前面: PCL库,之前在使用他的时候,只是各种掉库,觉得自己会调库了,根据案例可以跑出来自己想要的结果(比如计算一个点云的边界,使用RANSAC拟合三维直线等)觉得自己就是掌握了,其实并不然,这样 ...
一起做激光SLAM：ICP匹配用于闭环检测
编辑丨古月居目标利用ICP进行闭环检测,完成闭环. 预期效果:通过闭环检测完成起止闭环,下图为加入闭环前后. rosbag数据: https://pan.baidu.com/s/1o-noUxgV ...
PCL库官方教程01
PCL 库概览介绍 PCL 库的组件,简短说明各个模块的功能及其之间的交互. 概述 PCL 被拆分为多个模块化库.主要模块如下图所示: Filters 滤波器下图给出了一个去除噪声的例子.由于测量 ...
利用PCL库构建Mesh三维模型
从两张任意拍摄的一对图像(得有大部分重合面积)和相机内参矩阵开始,重建出基于Mesh的三维模型,美观又实用,还不赶快学起来.本文也是记录一下自己学习过程,废话较多,请多包涵,主要代码已注释,请自行下载 ...
[转载] PCL库相关资源的查找和使用Tip
前言学习PCL库的两个月左右时间里,从刚开始的无从下手到慢慢地摸清一些门道,走了很多弯路,也遇到过很多困惑,逐渐有了一些如何查找与利用PCL库学习资源的心得.所以把这些心得记录下来,一是为了给自己以 ...

基于PCL库的通过ICP匹配多幅点云方法

基于PCL库的通过ICP匹配多幅点云方法

前言

Code

Result

基于PCL库的通过ICP匹配多幅点云方法相关推荐

最新文章

热门文章