前言

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);

        //检查文件的名字 后缀 是否 为 pcd
        if (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> ());//kdtree
    norm_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_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提供点匹配的特征

///////////////////////////////////////////////////////////////////////////////////////////////////////////

    //我们自定义点的表示（以上定义）
    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;

声明相关变量

///////////////////////////////////////////////////////////////////////////////////////////////////////////

    //循环处理每个pcd
    for (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