pcl--第七节 点云配准

点云配准原理概述

点云配准需求场景

​ 随着计算机辅助设计技术的发展，通过实物模型产生数字模型的逆向工程技术获得了越来越广泛的应用，与此同时，硬件设备的日趋完善也为数字模型操作提供了足够的技术支持。

​ 由于三维扫描仪设备受到测量方式和被测物体形状的条件限制，一次扫描往往只能获取到局部的点云信息，进而需要进行多次扫描，然后每次扫描时得到的点云都有独立的坐标系，不可以直接进行拼接。在逆向工程、计算机视觉、文物数字化等领域中，由于点云的不完整、旋转错位、平移错位等，使得要得到完整点云就需要对多个局部点云进行配准。为了得到被测物体的完整数据模型,需要确定一个合适的坐标变换 ，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化等操作，这就是点云数据的配准。

点云配准方法

点云配准步骤上可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两个阶段。

粗配准是指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。常见粗配准算法：

• 基于特征匹配(PFH)的配准算法：
  • SAC-IA 采样一致性初始配准算法（Sample Consensus Initial Alignment）PCL库已实现，基于FPFH
• 基于穷举搜索的配准算法：
  • 4PCS 四点一致集配准算法（4-Point Congruent Set）
  • Super4PCS

精配准是指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。所以需要各种粗配准技术为ICP算法提供较好的位置，在迭代过程中确立正确对应点集能避免迭代陷入局部极值，决定了算法的收敛速度和最终的配准精度。最常见的精配准算法是ICP及其变种。

• ICP 迭代最近点算法（Iterative Cloest Point）
  • GICP
  • NICP
  • MBICP
• NDT 正态分布变换算法（Normal Distributions Transform）

其他配准：

• 依赖平台设备：将被测物体放在平台上，利用控制器对平台进行控制，使之按照指定角度转动，通过多次测量可以得到不同视角下的点云，由于提前获知了距离及角度信息，则可以直接对所有点云进行配准。

• 辅助标志点：通过在被测物体表面粘贴标签，将这些标签作为标志点，对多次测量得到的点云数据进行配准时，对这些有显著特征的标签进行识别配准，代替了对整体点云的配准，提高效率，精确度。

自动配准技术

​ 通常所说的点云配准就是指自动配准，点云自动配准技术是通过一定的算法或者统计学规律，利用计算机计算两块点云之间的错位，从而达到把两片点云自动配准的效果。本质上就是把不同坐标系中测量得到的数据点云进行坐标变换，从而得到整体的数据模型。

​ 即求得坐标变换参数 R（ 旋转矩阵）和 T（平移向量）,使得两视角下测得的三维数据经坐标变换后的距离最小。配准算法按照实现过程可以分为整体配准和局部配准。

PCL实现的配准算法

上图中给出了示例，使用倾斜的2D激光设备获取了六个独立数据集。由于每个单独的扫描仅代表周围世界的一小部分，因此必须找到将它们配准在一起的方法，从而创建完整的点云模型，如下图所示。

PCL提供的配准库算法是通过在给定的输入数据集中找到正确的点对应关系，并将每个单独的数据集转换为一致的全局坐标系的刚性变换。理想情况下，如果在输入数据集中完全知道点对应关系，则该配准范式可以轻松解决。这意味着一个数据集中选定的关键点列表必须与另一个数据集中的点列表“重合”。此外，如果估计的对应关系“完美”匹配，则注册问题具有封闭式解决方案。 PCL包含一组功能强大的算法，这些算法可以估算多组对应关系，排除不良对应关系，从而以可靠的方式估算转换关系方法。以下将分别描述它们：

两两配准

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

具体实现步骤如下 :

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

整个配准过程最重要的是关键点的提取以及关键点的特征描述，以确保对应估计的准确性和效率，这样才能保证后续流程中的刚体变换矩阵估计的无误性。接下来我们对单次迭代的每一步进行解读：

1.关键点提取

关键点是在场景中具有“特殊属性”的兴趣点，例如书的一角或书上写有“ PCL”的字母“ P”。 PCL中有许多不同的关键点提取技术，如NARF，SIFT和FAST。另外，您也可以将每个点或子集作为关键点。如果不进行关键点提取，直接“将两个kinect数据集执行对应估计”会产生的问题是：每帧中有300k点，因此可以有300k^2个对应关系，这个数量太庞大不利于计算。

2.特征描述符

基于找到的关键点，我们必须提取特征，在此我们封装解析点云数据并生成向量以相互比较。同样，有许多特征描述符提取技术可供选择，例如NARF，FPFH，BRIEF或SIFT。

3. 对应关系估计

对应关系估计(correspondences estimation)

假设我们已经得到由两次扫描的点云数据获得的两组特征向量，在此基础上，我们必须找到相似特征再确定数据的重叠部分才能进行配准。根据特征的类型，PCL 使用不同方法来搜索特征之间的对应关系。

• 进行点匹配时(使用点的 xyz 三维坐标作为特征值)，针对有序点云数据和无序点云数据有不同的处理策略：
  • 穷举配准( brute force matching）简称BFMatching，或称野蛮匹配。
  • kd-tree最近邻查询，Fast Library for Approximate Nearest Neighbors.( FLANN )。
  • 在有序点云数据的图像空间中查找。
  • 在无序点云 数据的索引空间中查找。
• 进行特征匹配时，有以下几种方法(不使用点的坐标，而是某些由查询点邻域确定的特征，如法向量、局部或全局形状直方图等)：
  1. 穷举配准( brute force matching）简称BFMatching，或称野蛮匹配。
  2. kd-tree最近邻查询，Fast Library for Approximate Nearest Neighbors.( FLANN )。
• 除了查询之外，对应估计也区分了两种类型：
  1. 直接对应估计(默认)：为点云 A 中的每一个点搜索点云 B 中的对应点,确认最终对应点对。
  2. “相互”( Reciprocal )对应估计 : 首先为点云 A 中的点到点云 B 搜索对应点 ，然后又从点云 B 到点云 A 搜索对应点，最后只取交集作为对应点对。

所有这些在 PCL 类设计和实现中都以函数的形式让用户可以自由设定和使用。

4. 对应关系去除

对应关系去除(correspondences rejection)

​ 由于噪声的影响，通常并不是所有估计的对应关系都是正确的 。由于错误的对应关系对于最终的刚体变换矩阵的估算会产生负面的影响，所以必须去除它们，于是我们使用随机采样一致性( Random Sample Consensus , RANSAC )估算或者其他方法剔除错误对应关系，最终只保留一定比例的对应关系，这样即能提高变换矩阵的估算精度也可以提高配准速度。​ 遇到有一对多对应关系的特例情况，即目标模型中的一个点在源中有若干个点与之对应。可以通过只取与其距离最近的对应点或者检查附近的其他匹配的滤波方法过滤掉其他伪对应关系。同样地针对对应关系的去除，PCL 有单独设计类与之对应 。

5. 变换矩阵估算

最后一步就是计算实际的转换关系

1. 根据对应关系评估误差值
2. 估算相机位姿之间的刚性变换，并最小化误差指标
3. 优化点的结构
4. 使用刚性变换将源旋转/变换到目标上
5. 迭代此过程直到满足预定的收敛标准

点云配准流程示例¶

使用迭代最近点算法(ICP)

#include 
#include 
#include 
#include 

int
main(int argc, char **argv) {
    // 定义输入和输出点云
    pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud);
    pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud);

    // 随机填充无序点云
    cloud_in->width = 5;
    cloud_in->height = 1;
    cloud_in->is_dense = false;
    cloud_in->points.resize(cloud_in->width * cloud_in->height);
    for (size_t i = 0; i < cloud_in->points.size(); ++i) {
        cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
              << std::endl;
    for (size_t i = 0; i < cloud_in->points.size(); ++i)
        std::cout << "    " <<
                  cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
                  cloud_in->points[i].z << std::endl;
    *cloud_out = *cloud_in;
    std::cout << "size:" << cloud_out->points.size() << std::endl;

    // 在点云上执行简单的刚性变换，将cloud_out中的x平移0.7f米，然后再次输出数据值。
    for (size_t i = 0; i < cloud_in->points.size(); ++i)
        cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
    // 打印这些点
    std::cout << "Transformed " << cloud_in->points.size() << " data points:"
              << std::endl;
    for (size_t i = 0; i < cloud_out->points.size(); ++i)
        std::cout << "    " << cloud_out->points[i].x << " " <<
                  cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;

    // 创建IterativeClosestPoint的实例
    // setInputSource将cloud_in作为输入点云
    // setInputTarget将平移后的cloud_out作为目标点云
    pcl::IterativeClosestPoint icp;
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

    // 创建一个 pcl::PointCloud实例 Final 对象,存储配准变换后的源点云,
    // 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话
    // （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp. hasConverged()= 1 (true),
    // 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。
    pcl::PointCloud Final;
    icp.align(Final);
    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
              icp.getFitnessScore() << std::endl;
    const pcl::Registration::Matrix4 &matrix = icp.getFinalTransformation();
    std::cout << matrix << std::endl;
    return (0);
}

 

多幅点云配准

#include   // for pcl::make_shared
#include 
#include 
#include 

#include 

#include 
#include 

#include 

#include 
#include 
#include 

#include 

点/点云
//#include 
//#include 
//#include 
pcd文件输入/输出
//#include 
滤波
//#include 
//#include 
特征
//#include 
配准
//#include  //ICP方法
//#include 
//#include 
可视化
//#include 



//命名空间
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

//定义类型的别名
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud 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  //继承关系
{
    using pcl::PointRepresentation::nr_dimensions_;
public:
    MyPointRepresentation()
    {
        //指定维数
        nr_dimensions_ = 4;
    }

    //重载函数copyToFloatArray，以定义自己的特征向量
    virtual void copyToFloatArray(const PointNormalT& p, float* out) const
    {
        //< x, y, z, curvature > 坐标xyz和曲率
        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"); //依据给定的ID。从屏幕中去除一个点云。參数是ID
    p->removePointCloud("vp1_source"); //
    PointCloudColorHandlerCustom tgt_h(cloud_target, 0, 255, 0); //目标点云绿色
    PointCloudColorHandlerCustom 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"); //依据给定的ID。从屏幕中去除一个点云。
    //參数是ID
    p->removePointCloud("target");
    PointCloudColorHandlerGenericField tgt_color_handler(cloud_target, "curvature"); //目标点云彩色句柄
    if (!tgt_color_handler.isCapable())
        PCL_WARN("Cannot create curvature color handler!");
    PointCloudColorHandlerGenericField 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 參数的数量（来自main()）
// 參数argv 參数的列表（来自main()）
// 參数models 点云数据集的结果向量
void loadData(int argc, char** argv, std::vector >& models)
{
    std::string extension(".pcd"); //声明并初始化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)
        {
            // 读取点云，并保存到models
            PCD m;
            m.f_name = argv[i];
            pcl::io::loadPCDFile(argv[i], *m.cloud); //读取点云数据
            //去除点云中的NaN点（xyz都是NaN）
            std::vector indices; //保存去除的点的索引
            pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices); //去除点云中的NaN点

            models.push_back(m);
        }
    }
}


//简单地配准一对点云数据，并返回结果
//參数cloud_src  源点云
//參数cloud_tgt  目标点云
//參数output     输出点云
//參数final_transform 成对变换矩阵
//參数downsample 是否下採样
void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f& final_transform, bool downsample = false)
{
    //
    //为了一致性和速度，下採样
    // \note enable this for large datasets
    PointCloud::Ptr src(new PointCloud); //创建点云指针
    PointCloud::Ptr tgt(new PointCloud);
    pcl::VoxelGrid grid; //VoxelGrid 把一个给定的点云，聚集在一个局部的3D网格上,并下採样和滤波点云数据
    if (downsample) //下採样
    {
        grid.setLeafSize(0.05, 0.05, 0.05); //设置体元网格的叶子大小
        //下採样 源点云
        grid.setInputCloud(cloud_src); //设置输入点云
        grid.filter(*src); //下採样和滤波，并存储在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()); //创建kd树，用于计算法向量的搜索方法
    norm_est.setSearchMethod(tree); //设置搜索方法
    norm_est.setKSearch(30); //设置近期邻的数量
    norm_est.setInputCloud(src); //设置输入云
    norm_est.compute(*points_with_normals_src); //计算法向量，并存储在points_with_normals_src
    pcl::copyPointCloud(*src, *points_with_normals_src); //复制点云（坐标）到points_with_normals_src（包括坐标和法向量）
    norm_est.setInputCloud(tgt); //这3行计算目标点云的法向量，同上
    norm_est.compute(*points_with_normals_tgt);
    pcl::copyPointCloud(*tgt, *points_with_normals_tgt);

    //创建一个 自己定义点表达方式的 实例
    MyPointRepresentation point_representation;
    //加权曲率维度。以和坐标xyz保持平衡
    float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
    point_representation.setRescaleValues(alpha); //设置缩放值（向量化点时使用）

    //创建非线性ICP对象 并设置參数
    pcl::IterativeClosestPointNonLinear reg; //创建非线性ICP对象（ICP变体，使用Levenberg-Marquardt最优化）
    reg.setTransformationEpsilon(1e-6); //设置容许的最大误差（迭代最优化）
    //***** 注意：依据自己数据库的大小调节该參数
    reg.setMaxCorrespondenceDistance(0.1);  //设置相应点之间的最大距离（0.1m）,在配准过程中，忽略大于该阈值的点
    reg.setPointRepresentation(std::make_shared(point_representation)); //设置点表达
    //设置源点云和目标点云
  //reg.setInputSource (points_with_normals_src); //版本号不符合。使用以下的语句
    reg.setInputSource(points_with_normals_src); //设置输入点云（待变换的点云）
    reg.setInputTarget(points_with_normals_tgt); //设置目标点云
    reg.setMaximumIterations(2); //设置内部优化的迭代次数

    // Run the same optimization in a loop and visualize the results
    Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
    PointCloudWithNormals::Ptr reg_result = points_with_normals_src; //用于存储结果（坐标+法向量）

    for (int i = 0; i < 30; ++i) //迭代
    {
        PCL_INFO("Iteration Nr. %d.\n", i); //命令行显示迭代的次数
        //保存点云，用于可视化
        points_with_normals_src = reg_result; //
        //预计
        //reg.setInputSource (points_with_normals_src);
        reg.setInputSource(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);
    }

    targetToSource = Ti.inverse(); //计算从目标点云到源点云的变换矩阵
    pcl::transformPointCloud(*cloud_tgt, *output, targetToSource); //将目标点云 变换回到 源点云帧

    p->removePointCloud("source"); //依据给定的ID，从屏幕中去除一个点云。參数是ID
    p->removePointCloud("target");
    PointCloudColorHandlerCustom cloud_tgt_h(output, 0, 255, 0); //设置点云显示颜色。下同
    PointCloudColorHandlerCustom 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");

    //add the source to the transformed target
    *output += *cloud_src; // 拼接点云图（的点）点数数目是两个点云的点数和

    final_transform = targetToSource; //终于的变换。目标点云到源点云的变换矩阵
}


//****************  入口函数  ************************
//主函数
int main(int argc, char** argv)
{
    //读取数据
    std::vector > data; //模型
    loadData(argc, argv, data); //读取pcd文件数据，定义见上面

    //检查用户数据
    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"); //能够使用多个文件
        return (-1);
    }
    PCL_INFO("Loaded %d datasets.", (int)data.size()); //显示读取了多少个点云文件

    //创建一个 PCLVisualizer 对象
    p = new pcl::visualization::PCLVisualizer(argc, argv, "Pairwise Incremental Registration example"); //p是全局变量
    p->createViewPort(0.0, 0, 0.5, 1.0, vp_1); //创建左视区
    p->createViewPort(0.5, 0, 1.0, 1.0, vp_2); //创建右视区

    //创建点云指针和变换矩阵
    PointCloud::Ptr result(new PointCloud), source, target; //创建3个点云指针，分别用于结果。源点云和目标点云
    //全局变换矩阵，单位矩阵。成对变换
      //逗号表达式，先创建一个单位矩阵，然后将成对变换 赋给 全局变换
    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 points) with %s (%d points).\n", data[i - 1].f_name.c_str(), source->points.size(), data[i].f_name.c_str(), target->points.size());

        //********************************************************
        //配准2个点云。函数定义见上面
        pairAlign(source, target, temp, pairTransform, true);
        //将当前的一对点云数据，变换到全局变换中。
        pcl::transformPointCloud(*temp, *result, GlobalTransform);
        //更新全局变换
        GlobalTransform = GlobalTransform * pairTransform;
        //********************************************************

        // 保存成对的配准结果。变换到第一个点云帧
        std::stringstream ss; //这两句是生成文件名称
        ss << i << ".pcd";
        pcl::io::savePCDFile(ss.str(), *result, true); //保存成对的配准结果

    }
}

 

 并没有完整运行出来，问题未解决

正态分布变换配准(NDT)¶

正态分布变换( Normal Distributions Transform )进行配准

用正态分布变换算法来确定两个大型点云（都超过 100 000个点）之间的刚体变换。正态分布变换算法是一个配准算法 , 它应用于 三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，更多关于正态分布变换算法的详细的信息，请看 Martin Magnusson 博士的博士毕业论文“The Three-Dimensional Normal Distributions Transform – an Efficient Representation for Registration, Surface Analysis, and Loop Detection.”。

#include 
#include 

#include 
#include 

#include 
#include 

#include 

using namespace std::chrono_literals;

int
main ()
{
  // Loading first scan of room.
  pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud);
  if (pcl::io::loadPCDFile ("room_scan1.pcd", *target_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;

  // Loading second scan of room from new perspective.
  pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud);
  if (pcl::io::loadPCDFile ("room_scan2.pcd", *input_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;

  // Filtering input scan to roughly 10% of original size to increase speed of registration.
  // 降采样，提升执行性能
  pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud);
  pcl::ApproximateVoxelGrid approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setInputCloud (input_cloud);
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from room_scan2.pcd" << std::endl;

  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform ndt;

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.01);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (0.1);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);

  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);

  // Setting point cloud to be aligned.
  ndt.setInputSource (filtered_cloud);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);

  // Set initial alignment estimate found using robot odometry.
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  // Calculating required rigid transform to align the input cloud to the target cloud.
  pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud);
  ndt.align (*output_cloud, init_guess);

  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // Transforming unfiltered, input cloud using found transform.
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // Saving transformed input cloud.
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // Initializing point cloud visualizer
  pcl::visualization::PCLVisualizer::Ptr
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);

  // Coloring and visualizing target cloud (red).
  pcl::visualization::PointCloudColorHandlerCustom
  target_color (target_cloud, 255, 0, 0);
  viewer_final->addPointCloud (target_cloud, target_color, "target cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "target cloud");

  // Coloring and visualizing transformed input cloud (green).
  pcl::visualization::PointCloudColorHandlerCustom
  output_color (output_cloud, 0, 255, 0);
  viewer_final->addPointCloud (output_cloud, output_color, "output cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "output cloud");

  // Starting visualizer
  viewer_final->addCoordinateSystem (1.0, "global");
  viewer_final->initCameraParameters ();

  // Wait until visualizer window is closed.
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    std::this_thread::sleep_for(100ms);
  }

  return (0);
}

代码详解¶

#include        // ndt配准文件头
#include // 滤波文件头

使用【正态分布变换算法】和【用来过滤数据的过滤器】对应的头文件，这个过滤器可以用其他过滤器来替换 , 但是使用体素网格过滤器( approximate voxel filter)处理结果较好。

// Loading first scan of room.
// 加载首次的房间扫描数据作为目标点云 target_cloud
pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud);
if (pcl::io::loadPCDFile ("room_scan1.pcd", *target_cloud) == -1)
{
    PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
    return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;

// Loading second scan of room from new perspective.
// 加载从新的视角得到的房间第二次扫描数据作为输入源点云 input_cloud
pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud);
if (pcl::io::loadPCDFile ("room_scan2.pcd", *input_cloud) == -1)
{
    PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
    return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;

以上代码加载了两个 PCD 文件到共享指针，配准操作是完成 【源点云input_cloud】到【目标点云target_cloud】坐标系变换矩阵的估算，即求出input_cloud变换到target_cloud的变换矩阵

// Filtering input scan to roughly 10% of original size to increase speed of registration.
// 过滤输入点云到约10%的原始大小，以提高配准速度。
pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud);
pcl::ApproximateVoxelGrid approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;

以上代码将过滤输入点云到约10%的原始大小，以提高配准速度。这里用任何其他均匀过滤器都可以，目标点云target_cloud不需要进行滤波处理，因为NDT算法在目标点云对应的体素Voxel网格数据结构计算时，不使用单个点，而是使用体素的点。即已做了降采样处理。

// 初始化正态分布变化NDT对象
pcl::NormalDistributionsTransform ndt;

Here we create the NDT algorithm with the default values. The internal data structures are not initialized until later.

//根据输入数据的尺度设置NDT相关参数
ndt.setTransformationEpsilon(0.01); // 为终止条件设置最小转换差异
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize(0.1);   // 为More-Thuente线搜索设置最大步长
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution(1.0); // 设置NDT网格结构的分辨率 VoxelGridCovariance

这里设置一些尺度相关的参数,因为 NDT 算法使用一个体素化数据结构和More-Thuente 线搜索，因此需要缩放一些参数来适应数据集。以上参数看起来在我们使用的房间尺寸比例下运行地很好，但是它们如果需要处理例如一个咖啡杯的扫描之类更小物体，需要对参数进行很大程度的缩小。在变换中 Epsilon 参数分别从长度和弧度，定义了变换矢量[ x, y, z,roll,pitch, yaw]的最小许可的递增量，一旦递增量减小到这个临界值以下 ，那么配准算法就将终止。步长StepSize参数定义了 More-Thuente 线搜索允许的最大步长，这个线搜索算法确定了最大值以下的最佳步长，当靠近最优解时该算法会缩短迭代步长，在更大的最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。

//设置匹配迭代的最大次数
ndt.setMaximumIterations (35);

这个MaximumIterations参数控制了优化程序运行的最大迭代次数，一 般来说，在达到这个限制值之前优化程序就会在 epsilon 变换阈值下终止。添加此最大迭代次数限制能够增加程序鲁棒性,阻止了它在错误的方向运行太久。

// 设置过滤后的输入源点云（第二次扫描数据）
ndt.setInputSource (filtered_cloud);
// 设置目标点云（第一次扫描数据），作为对其的目标。
ndt.setInputTarget (target_cloud);

这里，我们把点云赋给 NDT 配准对象，目标点云的坐标系是被匹配的输入点云的参考坐标系，匹配完成后输入点云将被变换到与目标点云统一坐标系下，当加载目标点云后，NDT 算法的内部数据结构被初始化。

  ///设置使用机器人测距法得到的粗略初始变换矩阵
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ());
  Eigen::Translation3f init_translation (1.79387, 0, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

在这部分的代码块中我们创建了一个点云配准变换矩阵的初始估计，虽然算法运行并不需要这样的一个初始变换矩阵，但是有了它易于得到更好的结果，尤其是当参考坐标系之间有较大差异时（本例即是）， 在机器人应用程序（例如用于生成此数据集的应用程序）中，通常使用里程表数据生成初始转换。

// 计算所需的刚体变换，以使输入云与目标云对齐。
pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud);
ndt.align (*output_cloud, init_guess);

std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
    << " score: " << ndt.getFitnessScore() << std::endl;

最后，我们准备对齐点云。生成的转换后的输入云存储在输出云中。然后，我们显示对齐的结果以及欧几里得适合度得分FitnessScore，该分数计算为从输出云到目标云中最近点的距离的平方。

// 使用找到的变换矩阵，来对未过滤的输入云进行变换。
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation());

// 保存变换后的输入点云
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

在对齐之后，输出云output_cloud将立即包含过滤后的输入云的转换版本，因为我们向算法传递了过滤后的点云，而不是原始输入云。为了获得原始云的对齐版本，我们从NDT算法中提取最终转换矩阵并转换原始输入云。现在，我们可以将该云保存到文件room_scan2_transformed.pcd中，以备将来使用。

配准收敛过程轨迹 RegistrationVisualizer ，pcl_registration_visualizer

刚性物体的鲁棒姿态估计¶

Robust pose estimation of rigid objects

在本教程中，我们将展示如何在具有杂波和遮挡的场景中找到刚体的对齐姿势。

#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP FeatureEstimationT;
typedef pcl::PointCloud FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom ColorHandlerT;

// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudT::Ptr object_aligned (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);
  FeatureCloudT::Ptr object_features (new FeatureCloudT);
  FeatureCloudT::Ptr scene_features (new FeatureCloudT);
  
  // Get input object and scene
  if (argc != 3)
  {
    pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
    return (1);
  }
  
  // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");
  if (pcl::io::loadPCDFile (argv[1], *object) < 0 ||
      pcl::io::loadPCDFile (argv[2], *scene) < 0)
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (1);
  }
  
  // Downsample
  pcl::console::print_highlight ("Downsampling...\n");
  pcl::VoxelGrid grid;
  const float leaf = 0.005f;
  grid.setLeafSize (leaf, leaf, leaf);
  grid.setInputCloud (object);
  grid.filter (*object);
  grid.setInputCloud (scene);
  grid.filter (*scene);
  
  // Estimate normals for scene
  pcl::console::print_highlight ("Estimating scene normals...\n");
  pcl::NormalEstimationOMP nest;
  nest.setRadiusSearch (0.01);
  nest.setInputCloud (scene);
  nest.compute (*scene);
  
  // Estimate features
  pcl::console::print_highlight ("Estimating features...\n");
  FeatureEstimationT fest;
  fest.setRadiusSearch (0.025);
  fest.setInputCloud (object);
  fest.setInputNormals (object);
  fest.compute (*object_features);
  fest.setInputCloud (scene);
  fest.setInputNormals (scene);
  fest.compute (*scene_features);
  
  // Perform alignment
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective align;
  align.setInputSource (object);
  align.setSourceFeatures (object_features);
  align.setInputTarget (scene);
  align.setTargetFeatures (scene_features);
  align.setMaximumIterations (50000); // Number of RANSAC iterations
  align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (5); // Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
  align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
  {
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned);
  }
  
  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
    
    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment");
    visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
    return (1);
  }
  
  return (0);
}

代码详解¶

对齐配准对象创建与配置

 // Perform alignment
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective align;
  align.setInputSource (object);
  align.setSourceFeatures (object_features);
  align.setInputTarget (scene);
  align.setTargetFeatures (scene_features);
  align.setMaximumIterations (50000); // Number of RANSAC iterations
  align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (5); // Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
  align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis

除了常用的输入点云和功能以外，此类还包含一些其他运行时参数，这些参数对对齐算法的性能有很大影响。 前两个与对齐类pcl::SampleConsensusInitialAlignment作用相同

• 样本数-setNumberOfSamples()

  在对象和场景之间进行采样的点对应数。 至少需要3个点才能计算姿势。

• 对应随机性-setCorrespondenceRandomness()

  我们可以在N个最佳匹配之间随机选择，而不是将每个对象FPFH描述符匹配到场景中最接近的匹配特征。 这增加了必要的迭代，但也使算法对异常匹配具有鲁棒性。

• 多边形相似度阈值-setSimilarityThreshold()

  对齐类使用pcl::registration::CorrespondenceRejectorPoly类，根据采样点之间距离的位置不变的几何一致性，尽早消除不良姿势。

  在物体和场景上， 将该值设置为越接近1，则贪婪程度越高，从而使算法变得更快。 但是，这也会增加存在噪音时消除好姿势的风险。

• 内在阈值-setMaxCorrespondenceDistance()

  这是欧几里德距离阈值，用于确定变换后的对象点是否正确对齐到最近的场景点。

  在此示例中，我们使用的启发式值为点云分辨率的1.5倍。

• Inlier分数-setInlierFraction()

  在许多实际情况下，由于混乱，遮挡或两者兼而有之，场景中观察到的对象的大部分都不可见。

  在这种情况下，我们需要考虑不会将所有对象点都用于对准场景的姿势假设（猜想）。

  正确对齐的点的绝对数量是使用inlier阈值确定的，并且如果该数量与对象中总点数的比大于指定的inlier分数，则我们接受姿势假设（猜想）为有效。

执行配准并输出结果

最后，我们执行对齐配准过程：

{
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned);
}

对齐的对象存储在点云object_aligned中。 如果找到了一个具有足够inliers的姿势（占对象点总数的25％以上），则该算法会收敛，并且我们可以打印并可视化结果。

    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());

    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment");
    visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
    visu.spin ();