点云配准

Pointcloud_Registeration

解析网站

​  我们称一对点云数据集的配准问题为两两配准（pairwise registration 或 pair-wise registration）。通常通过应用一个估计得到的表示平移和旋转的4×4刚体变换矩阵来使一个点云数据集精确地与另一个点云数据集（目标数据集）进行完美配准。

步骤：

（1）首先从两个数据集中按照同样的关键点选取标准，提取关键点。
（2）对选择的所有关键点分别计算其特征描述子。
（3）结合特征描述子在两个数据集中的坐标的位置，以两者之间特征和位置的相似度为基础，来估算它们的对应关系，初步估计对应点对。
（4）假定数据是有噪声的，除去对配准有影响的错误的对应点对。
（5）利用剩余的正确对应关系来估算刚体变换，完成配准。

查找对应估计

​ 假设我们已经得到由两次扫描的点云数据获得的两组特征向量，在此基础上，我们必须找到相似特征再确定数据的重叠部分，然后才能进行配准。根据特征的类型，PCL使用不同方法来搜索特征之间的对应关系。
  进行点匹配时（使用点的xyz三维坐标作为特征值），针对有序点云数据和无序点云数据有如下不同的处理策略。
  ∙ 穷举配准（brute force matching）
  ∙ kd-树最近邻查询（FLANN）
  ∙ 在有序点云数据的图像空间中查找
  ∙ 在无序点云数据的索引空间中查找

进行特征匹配时（不使用点的坐标，而是某些由查询点邻域确定的特征，如法向量、局部或全局形状直方图等），有以下几种方法。
  ∙ 穷举配准
  ∙ kd-树最近邻查询（FLANN）

对应关系去除

​ 使用随机采样一致性（RANSAC，Random Sample Consensus）估计或者其他方法剔除错误对应关系，最终使用的对应关系数量只使用一定比例的对应关系，这样即可以提高变换矩阵的估计精度，也可以提高配准速度。

变换矩阵估算

​ 估算变换矩阵步骤如下。
  （1）在对应关系的基础上评估一些错误的度量标准。
  （2）在摄像机位姿（运动估算）和最小化错误度量标准下估算一个（刚体）变换。
  （3）优化点的结构。
  （4）使用刚体变换把源旋转/平移到于目标所在的同一坐标系下，用所有点、点的一个子集或者关键点运行一个内部ICP循环。
  （5）进行迭代，直到符合收敛性判断标准为止。

ICP配准

​ ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度。ICP处理流程分为4个主要步骤：（1）对原始点云数据进行采样；（2）确定初始对应点集；（3）去除错误对应点对；（4）坐标变换的求解。

采样一致性初始对齐算法

​ 配准算法从精度上分两类，一种是初始的变换矩阵的粗略估计，另一种是像ICP一样的精确的变换矩阵估计。对于初始的变换矩阵粗略估计，贪婪的初始配准方法工作量很大，它使用了点云数据旋转不变的特性。此外，因为这是一个贪婪算法，所以有可能只能得到局部最优解。因此我们采用采样一致性方法，试图保持相同的对应关系的几何关系而不必尝试了解有限个对应关系的所有组合。相反，我们从候选对应关系中进行大量的采样并通过以下的步骤对它们中的每一个进行排名。

（1）从P中选择s个样本点，同时确定他们的配对距离大于用户设定的最小值dmin。
  （2）对于每个样本点，在Q中找到满足直方图和样本点直方图相似的点存入一个列表中。从这些点中随机选择一些代表采样点的对应关系。
  （3）计算通过采样点定义的刚体变换和其对应变换，计算点云的度量错误来评价转换的质量。

我们计划通过查看非常大量的对应关系，快速找到一个好的变换。
  重复这三个步骤，直至取得储存了最佳度量错误，并使用暴力配准部分数据。最后使用一个 Levenberg-Marquardt算法进行非线性局部优化。

（Nirenberg-Marquardt算法是最优化算法中的一种。最优化是寻找使得函数值最小的参数向量。根据求导数的方法，可分为2大类。第一类，若f具有解析函数形式，知道x后求导数速度快。第二类，使用数值差分来求导数。根据使用模型不同，分为非约束最优化、约束最优化、最小二乘最优化。它是使用最广泛的非线性最小二乘算法，利用梯度求最大（小）值的算法，形象的说，属于“爬山”法的一种。它同时具有梯度法和牛顿法的优点。当λ很小时，步长等于牛顿法步长，当λ很大时，步长约等于梯度下降法的步长。它的关键是用模型函数 f 对待估参数向量p在其领域内做线性近似，忽略掉二阶以上的导数项，从而转化为线性最小二乘问题，它具有收敛速度快等优点。解析

实例解析：

1. ICP配准

​ 在代码中使用ICP迭代最近点算法，程序随机生成一个点云作为源点云，并将其沿X轴平移后作为目标点云，然后利用ICP估计源到目标的刚体变换矩阵，中间对所有的信息都打印到终端中。创建一个 pcl::PointCloud<pcl::PointXYZ>实例Final对象，存储配准变换后的点云，应用ICP算法后，IterativeClosestPoint能够保存结果点云集，如果这两个点云匹配正确的话（也就是说仅仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云)，那么icp.hasConverged() = 1（true），然后会输出最终变换矩阵的适合性评估和一些相关的信息。

• 首先创建一个工作空间iterative_closest_point：mkdir -p iterative_closest_point/src
• ​ 在iterative_closest_point/src路径下，创建一个文件并命名为iterative_closest_point.cpp

#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>   //可视化类相关头文件

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;

using namespace std;

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

/* 处理点云的方便的结构定义 方便对点云以文件名和点云对象进行成对处理管理，在配准过程中，可以同时接受多个点云文件输入，程序从第一个文件开始，连续的两两配准处理，然后存储配准后的点云文件。*/
struct PCD
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
    std::string f_name;
    PCD() : cloud (new pcl::PointCloud<pcl::PointXYZ>) {};
};
struct PCDComparator
{
    bool operator () (const PCD& p1, const PCD& p2)
    {
        return (p1.f_name < p2.f_name);
    }
};

/* 以< 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;
    }
};


/* 在可视化窗口的第一视点显示源点云和目标点云 */
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
    p->removePointCloud ("vp1_target");
    p->removePointCloud ("vp1_source");
    PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);
    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");
    PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");
    if (!tgt_color_handler.isCapable ())
    {
        PCL_WARN ("Cannot create curvature color handler!");
    }
    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();
}


/** 加载一组我们想要匹配在一起的PCD文件
  * argc: 是参数的数量 (pass from main ())
  * argv: 实际的命令行参数 (pass from main ())
  * models: 点云数据集的合成矢量
  */
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 ())
        {
            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;
            m.f_name = argv[i];
            pcl::io::loadPCDFile (argv[i], *m.cloud);
            std::vector<int> indices;
            pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices); //从点云中移除NAN点
            models.push_back (m);
        }
    }
}


/**匹配一对点云数据集并且返回结果
  *cloud_src：源点云
  *cloud_tgt：目标点云
  *output：输出的配准结果的源点云
  *final_transform：源点云和目标点云之间的转换矩阵
  */
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<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> ());
    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);

    MyPointRepresentation point_representation;
    //调整'curvature'尺寸权重以便使它和x, y, z平衡
    float alpha[4] = {1.0, 1.0, 1.0, 1.0};
    point_representation.setRescaleValues (alpha);

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

    /* 配准 */
    pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;  //配准对象
    reg.setTransformationEpsilon (1e-6);    //设置收敛判断条件，越小精度越大，收敛也越慢
    reg.setMaxCorrespondenceDistance (0.1); //将两个对应关系之间的(src<->tgt)最大距离设置为10厘米，需要根据数据集大小来调整
    reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));  //设置点表示
    reg.setInputCloud (points_with_normals_src); //设置源点云
    reg.setInputTarget (points_with_normals_tgt);  //设置目标点云

/* 因为这是一个指导实例，我们希望显示配准的迭代过程，为达该目的，我们实现手动迭代（本例中是30次），在每次迭代过程中，我们记录并累积由ICP返回的变换。如果迭代N次找到的变换和迭代N-1次中找到的变换之间的差异小于传给ICP的变换收敛阈值，我们选择源与目标之间更靠近的对应点距离阈值来改善配准过程。最后，在配准结果视口对迭代的最新结果进行刷新显示。*/

    /* 在一个循环中运行相同的最优化并且使结果可视化 */
    Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
    PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
    
    reg.setMaximumIterations (2); //ICP在内部进行计算时，限制其最大的迭代次数为2，即每迭代两次就认为收敛，停止内部迭代。
    
    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);
        Ti = reg.getFinalTransformation () * Ti;      //在每一个迭代之间累积转换

        /* 如果这次转换和之前转换之间的差异小于阈值，则通过减小最大对应距离来改善程序 */
        if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
        {
            reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
            prev = reg.getLastIncrementalTransformation ();
        }

        showCloudsRight(points_with_normals_tgt, points_with_normals_src);  //可视化当前状态
    }

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

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

    /* 把目标点云转换回源框架 */
    pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
    p->removePointCloud ("source");
    p->removePointCloud ("target");
    PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
    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;
}

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

int main (int argc, char** argv)
{
    /* 加载数据 */
    std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
    loadData (argc, argv, data);

    /* 检查用户输入 */
    if (data.empty ())
    {
        PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", 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 ());

    /* 创建一个PCL可视化对象 */
    p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
    p->createViewPort (0.0, 0, 0.5, 1.0, vp_1;//用左半窗口创建视口vp_1
    p->createViewPort (0.5, 0, 1.0, 1.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);

        pcl::transformPointCloud (*temp, *result, GlobalTransform); //把当前的两两配对转换到全局变换
        GlobalTransform = pairTransform * GlobalTransform;          //更新全局变换

       /* 保存转换到第一个点云坐标系下的当前配准后的两组点云result到文件i.pcd */
        std::stringstream ss;
        ss << i << ".pcd";
        pcl::io::savePCDFile (ss.str (), *result, true);
    }
}