PCL中点云配准（非常详细，建议收藏）
PCL中点云配准
- 1 PCL中实现的配准算法及相关概念
- 1.1 一对点云配准
- 1.2 对应估计
- 1.3 对应关系去除
- 1.4 变换矩阵估算
- 1.5 迭代最近点算法
- 1.6 采样一致性初始对齐算法
- 1.7 PCL中 Registration 模块及类
- 2 点云配准入门级实例解析
- 2.1 如何使用迭代最近点ICP算法
- 2.2 如何逐步匹配多幅点云
- 2.3 如何使用正态分布变换进行配准
- 2.4 交互式迭代最近点配准
- 2.5 刚性物体的位姿估计
随着计算机辅助设计技术的发展，通过实物模型产生数字模型的逆向工程技术获得了越来越广泛的应用，与此同时硬件设备的日趋完善也为数字模型操作提供了足够的技术支持。在逆向工程、计算机视觉、文物数字化等领域中，由于点云的不完整、旋转错位、平移错位等，使得要得到完整点云就需对局部点云进行配准。为了得到被测物体的完整数据模型，需要确定一个合适的坐标变换，将从各个视角得到的点集合并到一个统一的坐标系下，形成一个完整的数据点云，然后就可以方便地进行可视化等操作，这就是点云数据的配准。点云配准有手动配准、依赖仪器的配准和自动配准。通常我们所说的点云配准技术即是指最后一种自动配准。点云自动配准技术是通过一定的算法或者统计学规律，利用计算机计算两块点云之间的错位，从而达到把两片点云自动配准的效果。其实质是把在不同的坐标系中测量得到的数据点云进行坐标变换，以得到整体的数据模型。问题的关键是如何求得坐标变换参数 R（旋转矩阵）和 T（平移向量），使得两视角下测得的三维数据经坐标变换后的距离最小。目前，配准算法按照实现过程可以分为整体配准和局部配准。PCL中有单独的配准模块，实现了配准相关的基础数据结构与经典配准算法如ICP等，以及配准过程中的对应点估计、错误对应点去除等流程。由于配准的关键技术有关键点选取、特征描述与提取两部分，这两部分在前面教程 PCL中点云关键点提取、 PCL中3D点云特征描述与提取（一）、 PCL中3D点云特征描述与提取（二）和 PCL中3D点云特征描述与提取（三）中已详细介绍，所以读者在阅读本篇文章时可以先回顾和参考前面的内容。
本文首先对PCL中实现的配准算法以及相关概念进行简介，其次对PCL的配准相关模块及类进行简单介绍，最后通过应用实例来展示如何对PCL中配准模块进行灵活运用。
1 PCL中实现的配准算法及相关概念
1.1 一对点云配准
我们称一对点云数据集的配准问题为两两配准（pairwise registration 或 pair-wise registration）。通常通过应用一个估计得到的表示平移和旋转的4×4刚体变换矩阵来使一个点云数据集精
确地与另一个点云数据集（目标数据集）进行完美配准。
具体实现步骤如下。
（1）首先从两个数据集中按照同样的关键点选取标准，提取关键点。
（2）对选择的所有关键点分别计算其特征描述子。
（3）结合特征描述子在两个数据集中的坐标的位置，以两者之间特征和位置的相似度为基础，来估算它们的对应关系，初步估计对应点对。
（4）假定数据是有噪声的，除去对配准有影响的错误的对应点对。
（5）利用剩余的正确对应关系来估算刚体变换，完成配准。
PCL中两两配准步骤如下图所示，整个配准过程，最重要的是关键点的提取以及关键点的特征描述，以确保对应估计的准确性和效率，这样才能保证后续流程中的刚体变换矩阵估计的无误性，所以PCL对于关键点和特征描述提取两个重要技术也有单独的模块，可以参看博主之前的教程。
1.2 对应估计
假设我们已经得到由两次扫描的点云数据获得的两组特征向量，在此基础上，我们必须找到相似特征再确定数据的重叠部分，然后才能进行配准。根据特征的类型，PCL使用不同方法来搜索特征之间的对应关系。
进行点匹配时（使用点的xyz三维坐标作为特征值），针对有序点云数据和无序点云数据有如下不同的处理策略。
∙\bullet∙ 穷举配准（brute force matching）
∙\bullet∙ kd-树最近邻查询（FLANN）
∙\bullet∙ 在有序点云数据的图像空间中查找
∙\bullet∙ 在无序点云数据的索引空间中查找
进行特征匹配时（不使用点的坐标，而是某些由查询点邻域确定的特征，如法向量、局部或全局形状直方图等），有以下几种方法。
∙\bullet∙ 穷举配准
∙\bullet∙ kd-树最近邻查询（FLANN）
除了查询之外，对应估计也区分了两种类型。
∙\bullet∙ 直接对应估计（默认）：为点云A中的每一个点搜索点云B中的对应点，确认最终对应点对。
∙\bullet∙ “相互”（Reciprocal）对应估计：首先为点云A中的点到点云B搜索对应点，然后又从点云B到点云A搜索对应点，最后只取交集作为对应点对。
所有这些在PCL类设计和实现中都以函数的形式让用户可以自由设定和使用。
1.3 对应关系去除
由于噪声的影响，通常并不是所有估计的对应关系都是正确的。由于错误的对应关系对于最终的刚体变换矩阵的估算会产生负面的影响，所以必须去除它们，可以使用随机采样一致性（RANSAC，Random Sample Consensus）估计或者其他方法剔除错误对应关系，最终使用的对应关系数量只使用一定比例的对应关系，这样即可以提高变换矩阵的估计精度，也可以提高配准速度。
遇到有一对多对应关系的特例情况，即目标模型中的一个点与源中的若干个点对应。可以通过只取与其距离最近的对应点，或者检查附近的其他匹配的滤波方法过滤掉其他伪对应关系。同样的针对对应关系的去除，PCL有单独设计类与之对应。
1.4 变换矩阵估算
估算变换矩阵步骤如下。
（1）在对应关系的基础上评估一些错误的度量标准。
（2）在摄像机位姿（运动估算）和最小化错误度量标准下估算一个（刚体）变换。
（3）优化点的结构。
（4）使用刚体变换把源旋转/平移到于目标所在的同一坐标系下，用所有点、点的一个子集或者关键点运行一个内部ICP循环。
（5）进行迭代，直到符合收敛性判断标准为止。
1.5 迭代最近点算法
ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度。ICP处理流程分为4个主要步骤：（1）对原始点云数据进行采样；（2）确定初始对应点集；（3）去除错误对应点对；（4）坐标变换的求解。
1.6 采样一致性初始对齐算法
配准算法从精度上分两类，一种是初始的变换矩阵的粗略估计，另一种是像ICP一样的精确的变换矩阵估计。对于初始的变换矩阵粗略估计，贪婪的初始配准方法工作量很大，它使用了点云数据旋转不变的特性。此外，因为这是一个贪婪算法，所以有可能只能得到局部最优解。因此我们采用采样一致性方法，试图保持相同的对应关系的几何关系而不必尝试了解有限个对应关系的所有组合。相反，我们从候选对应关系中进行大量的采样并通过以下的步骤对它们中的每一个进行排名。
（1）从P中选择s个样本点，同时确定他们的配对距离大于用户设定的最小值dmin。
（2）对于每个样本点，在Q中找到满足直方图和样本点直方图相似的点存入一个列表中。从这些点中随机选择一些代表采样点的对应关系。
（3）计算通过采样点定义的刚体变换和其对应变换，计算点云的度量错误来评价转换的质量。
我们计划通过查看非常大量的对应关系，快速找到一个好的变换。
重复这三个步骤，直至取得储存了最佳度量错误，并使用暴力配准部分数据。最后使用一个 Levenberg-Marquardt算法进行非线性局部优化。
1.7 PCL中 Registration 模块及类
PCL中 Registration 模块实现了点云配准相关的确定初始对应点集，去除错误对应点对，坐标变换的求解，利用 Registration 对点云数据进行不同的配准以达到合并点云的目的，其依赖于pcl::common，pcl::kdtree，pcl::sample_consensus，pcl::features 模块。类和函数的详细接口说明可以查看官网文档。
2 点云配准入门级实例解析
2.1 如何使用迭代最近点ICP算法
本小节演示了在代码中使用ICP迭代最近点算法，程序随机生成一个点云作为源点云，并将其沿X轴平移后作为目标点云，然后利用ICP估计源到目标的刚体变换矩阵，中间对所有的信息都打印到终端中。
首先创建一个工作空间iterative_closest_point，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p iterative_closest_point/src
接着，在iterative_closest_point/src路径下，创建一个文件并命名为iterative_closest_point.cpp，拷贝如下代码：
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> // 点类型定义头文件
#include <pcl/registration/icp.h> // ICP配准类相关的头文件int main (int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);/* 填入点云数据 */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;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;}pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputCloud(cloud_in);icp.setInputTarget(cloud_out);pcl::PointCloud<pcl::PointXYZ> Final; // 存储配准变换源点云后的点云icp.align(Final);std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;std::cout << icp.getFinalTransformation() << std::endl;return (0);
}
【解释说明】
接下来分析上述源代码中的关键语句。
std::cout << "size:" << cloud_out->points.size() << std::endl;
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;
以上代码实现一个简单的点云刚体变换，以构造目标点云，然后打印出数据值。
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in); //设置源点云
icp.setInputTarget(cloud_out); //设置目标点云
这部分代码创建了一个 IterativeClosestPoint 的对象，并设置了对应的目标点云和源点云。
pcl::PointCloud<pcl::PointXYZ> Final; //存储配准变换源点云后的点云
icp.align(Final); //执行配准，存储变换后的点云到Finalstd::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl; //输出最终估计的变换矩阵
创建一个 pcl::PointCloud<pcl::PointXYZ>实例Final对象，存储配准变换后的点云，应用ICP算法后，IterativeClosestPoint能够保存结果点云集，如果这两个点云匹配正确的话（也就是说仅仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云)，那么icp.hasConverged() = 1（true），然后会输出最终变换矩阵的适合性评估和一些相关的信息。
【编译和运行程序】
在工作空间根目录iterative_closest_point下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(iterative_closest_point)find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (${PROJECT_NAME}_node src/iterative_closest_point.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})
在工作空间根目录iterative_closest_point下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件iterative_closest_point_node，运行该可执行文件：
./iterative_closest_point_node
运行上述命令后，将看到类似于下图所示的输出信息，即打印出利用ICP算法估计的变换矩阵等信息。
2.2 如何逐步匹配多幅点云
本例演示了迭代最近点算法的使用，以便逐步地对一系列点云进行两两匹配。它的思想是对所有的点云进行变换，使得都与第一个点云在统一坐标系中。在每个连贯的有重叠的点云之间找到最佳的变换，并累积这些变换到全部的点云。能够进行ICP算法的点云需要粗略的预匹配（如：在一个机器人的量距内或在地图框架内），并且一个点云与另一个点云需要有重叠部分。
首先创建一个工作空间pairwise_incremental_registration，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p pairwise_incremental_registration/src
接着，在pairwise_incremental_registration/src路径下，创建一个文件并命名为pairwise_incremental_registration.cpp，拷贝如下代码：
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#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);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);/* 配准 */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);/* 在一个循环中运行相同的最优化并且使结果可视化 */Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;PointCloudWithNormals::Ptr reg_result = points_with_normals_src;reg.setMaximumIterations (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); //可视化当前状态}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;
}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);p->createViewPort (0.5, 0, 1.0, 1.0, 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 ());pairAlign (source, target, temp, pairTransform, true);pcl::transformPointCloud (*temp, *result, GlobalTransform); //把当前的两两配对转换到全局变换GlobalTransform = pairTransform * GlobalTransform; //更新全局变换/* 保存配准对，转换到第一个点云框架中 */std::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);}
}
【解释说明】
接下来分析上述源代码中的关键语句。
首先快速看一下相关类和数据结构的头文件声明，声明下面的语句包含了所有我们使用
的类的定义的头文件。
#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> //可视化类相关头文件
为了可视化目的，方便用户直观地观察到配准前后结果以及配准过程，创建全局可视化对象变量，并定义左右视点分别显示配准前和配准后的结果点云。
pcl::visualization::PCLVisualizer *p; //创建可视化工具
int vp_1, vp_2; //定义左右视点ID
声明一个结构体，方便对点云以文件名和点云对象进行成对处理管理，在配准过程中，可以同时接受多个点云文件输入，程序从第一个文件开始，连续的两两配准处理，然后存储配准后的点云文件。
/* 处理点云的方便的结构定义 */
struct PCD
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;std::string f_name;PCD() : cloud (new pcl::PointCloud<pcl::PointXYZ>) {};
};
定义一个新的点表示。
/* 以< x, y, z, curvature >形式定义一个新的点 */
class MyPointRepresentation : 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 <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");cout << endl;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.0, 0, 0.5, 1.0, vp_1); //用左半窗口创建视口vp_1p->createViewPort (0.5, 0, 1.0, 1.0, vp_2); //用右半窗口创建视口vp_2PointCloud::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); //把当前的两两配对后的点云temp转换到全局坐标系下（第一个输入点云的坐标系），返回resultGlobalTransform = pairTransform * GlobalTransform; //用当前的两组点云之间的变换矩阵更新全局变换/* 保存转换到第一个点云坐标系下的当前配准后的两组点云result到文件i.pcd */std::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);}
}
整体的程序框架了解后，我们来分别对不同的子函数实现进行分析。加载数据非常简单，我们迭代其他程序的参数，检查每一个参数是否指向一个pcd文件，如果是，则创建一个添加到点云矢量data中的pcd对象。
/** 加载一组我们想要匹配在一起的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 ()) //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);std::vector<int> indices;pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices); //从点云中移除NAN点models.push_back (m);}}
}
现在我们开始实际匹配，由子函数pairAlign具体实现，其中参数有输入一组需要配准的点云，以及是否进行下采样的设置项，其他参数输出配准后的点云以及变换矩阵。
/* 下采样 */
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);
一切都准备好了，可以进行配准了，创建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); //设置目标点云
ICP在内部进行计算时，限制其最大的迭代次数为2，即每迭代两次就认为收敛，停止内部迭代。
reg.setMaximumIterations (2);
因为这是一个指导实例，我们希望显示配准的迭代过程，为达该目的，我们实现手动迭代（本例中是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);
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;
【编译和运行程序】
在工作空间根目录pairwise_incremental_registration下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)project(pairwise_incremental_registration)find_package(PCL 1.4 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (${PROJECT_NAME}_node src/pairwise_incremental_registration.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})
在工作空间根目录pairwise_incremental_registration下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件pairwise_incremental_registration_node，如果我们只需要配准capture0001.pcd和capture0002.pcd，只需执行：
../pairwise_incremental_registration_node capture0001.pcd capture0002.pcd
此时将看到类似如下图所示的结果，此图为未进行配准前只在窗口左边显示源和目标点云，并使用不同的颜色显示，其中红色为源点云，绿色为目标点云。
此时，命令行提示需要继续执行配准则按键q，用户按键q后，会在窗口右边看到不断调整的点云，其实是配准过程中的迭代中间结果输出，如下图所示。在迭代次数小于设定的次数之前，右边会不断刷新最新的配准结果，直到收敛。
配准迭代30次后完成整个配准过程，最终显示如下图所示。图中左边和右边有明显的不同，右边显示配准后的结果，大体上观察其红色部分和绿色部分距离小于左边未配准的点云红绿对应之间的距离。用户再次按键q则会退出程序，并在同一目录下会看到存储的1.pcd文件，此文件为第一和第二个输入点云配准后与第一个输入点云在同一坐标系下的点云。
另一个执行例子是，如果用户需要配准capture0001.pcd、capture0002.pcd、capture0003.pcd等等，只需执行如下代码。
../pairwise_incremental_registration_node capture0001.pcd capture0002.pcd capture0003.pcd ...
整个过程与上面的两个输入是同样的，程序对相邻的两个（第一个与第二个、第二个与第三个…）进行配准，并将最终的点云都变换到与第一个输入点云同一坐标系下，同时存储所有的输出点云（1.pcd、2.pcd…），这里需要注意的是所有的点云都经过变换最终与第一个输入点云到了同一坐标系下，即完成了对所有点云的配准。
2.3 如何使用正态分布变换进行配准
本节我们将介绍如何使用正态分布变换算法来确定两个大型点云（都超过100,000个点）之间的刚体变换。正态分布变换算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，更多关于正态分布变换算法的详细的信息，可以参考Martin Magnusson博士的博土毕业论文"The Three-Dimensional Normal Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection."
首先创建一个工作空间normal_distributions_transform，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p normal_distributions_transform/src
接着，在normal_distributions_transform/src路径下，创建一个文件并命名为normal_distributions_transform.cpp，拷贝如下代码：
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h> //NDT配准类对应头文件
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>int main (int argc, char** argv)
{/* 加载房间的第一次扫描点云数据作为目标 */pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1){PCL_ERROR ("Couldn't read file room_scan1.pcd \n");return (-1);}std::vector<int> indices1;pcl::removeNaNFromPointCloud(*target_cloud,*target_cloud, indices1); //从点云中移除NAN点std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;/* 加载从新视角得到的房间的第二次扫描点云数据作为源点云 */pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1){PCL_ERROR ("Couldn't read file room_scan2.pcd \n");return (-1);}std::vector<int> indices2;pcl::removeNaNFromPointCloud(*input_cloud,*input_cloud, indices2); //从点云中移除NAN点std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;/* 将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度 */pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::ApproximateVoxelGrid<pcl::PointXYZ> 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;pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; //初始化正态分布变换（NDT）ndt.setTransformationEpsilon (0.01); //为终止条件设置最小转换差异ndt.setStepSize (0.1); //为More-Thuente线搜索设置最大步长ndt.setResolution (1.0); //设置NDT网格结构的分辨率（VoxelGridCovariance）ndt.setMaximumIterations (35); //设置匹配迭代的最大次数ndt.setInputCloud (filtered_cloud); //设置源点云ndt.setInputTarget (target_cloud); //设置目标点云/* 设置使用机器人测距法得到的初始对准估计结果 */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 ();/* 计算需要的刚体变换以便将输入的点云匹配到目标点云 */pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);ndt.align (*output_cloud, init_guess);std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()<< " score: " << ndt.getFitnessScore () << std::endl;pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ()); //使用创建的变换对未过滤的输入点云进行变换pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud); //保存转换的输入点云boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer")); // 初始化点云可视化界面viewer_final->setBackgroundColor (0, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> //对目标点云着色（红色）并可视化target_color (target_cloud, 255, 0, 0);viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "target cloud");/* 对转换后的目标点云着色（绿色）并可视化 */pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>output_color (output_cloud, 0, 255, 0);viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "output cloud");/* 启动可视化 */viewer_final->addCoordinateSystem (1.0);viewer_final->initCameraParameters ();/* 等待直到可视化窗口关闭。 */while (!viewer_final->wasStopped ()){viewer_final->spinOnce (100);boost::this_thread::sleep (boost::posix_time::microseconds (100000));}return (0);
}
【解释说明】
接下来分析上述源代码中的关键语句。
#include <pcl/registration/ndt.h> //NDT配准类对应头文件
#include <pcl/filters/approximate_voxel_grid.h>
这些是使用正态分布变换算法和用来过滤数据的过滤器对应的头文件，这个过滤器可以用其他过滤器来替换，但是使用体素网格过滤器（approximate voxel filter）处理结果较好。
/* 加载房间的第一次扫描点云数据作为目标 */
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
{PCL_ERROR ("Couldn't read file room_scan1.pcd \n");return (-1);
}
std::vector<int> indices1;
pcl::removeNaNFromPointCloud(*target_cloud,*target_cloud, indices1); //从点云中移除NAN点
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;/* 加载从新视角得到的房间的第二次扫描点云数据作为源点云 */
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
{PCL_ERROR ("Couldn't read file room_scan2.pcd \n");return (-1);
}
std::vector<int> indices2;
pcl::removeNaNFromPointCloud(*input_cloud,*input_cloud, indices2); //从点云中移除NAN点
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
以上代码加载了两个pcd文件到共享指针，后续配准是完成对源点云到目标点云的参考坐标系变换矩阵的估计，即得到这里的第二组点云变换到第一组点云坐标系下的变换矩阵。
/* 将输入的扫描过滤到原始尺寸的大概10%以提高匹配的速度 */
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> 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%左右，而目标点云不需要滤波处理，因为NDT算法中，对目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据。
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; //初始化正态分布变换（NDT）
这里创建默认参数的NDT，NDT对象内部的数据结构等在后面再一一设置。
ndt.setTransformationEpsilon (0.01); //为终止条件设置最小转换差异
ndt.setStepSize (0.1); //为More-Thuente线搜索设置最大步长
ndt.setResolution (1.0); //设置NDT网格结构的分辨率（VoxelGridCovariance）
这里我们设置一些尺度相关的参数，因为NDT算法使用一个体素化数据结构和More-Thuente线搜索，所以需要缩放一些参数来适应数据集。以上参数看起来似乎在我们使用的房间尺寸比例下运行得很好，但是它们如果需要处理例如一个咖啡杯的扫描之类更小物体，需要对参数进行很大程度的缩小。
在变换中 Epsilon参数分别从长度和弧度，定义了变换矢量[x, y, z, roll, pitch, yaw]的最小许可的递增量，一旦递增量减小到这个临界值以下，那么配准算法就将终止。步长参数定义了More-Thuente线搜索允许的最大步长，这个线搜索算法确定了最大值以下的最佳步长，当靠近最优解时该算法会缩短迭代步长，在更大的最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。最后，分辨率参数定义了内部NDT网格结构的体素分辨率。这个结构非常方便搜索，并且每个体素包含与点有关的统计数据，平均值，协方差等，统计数据作为一组多元高斯分布用来模拟点云，并且允许我们计算和优化体素内任意位置点的存在概率。该参数是与尺度最相关的，对每个体素它需要足够大来容纳至少6个点，但是又要足够小到能够代表唯一的场景，即具有辨别性。
ndt.setMaximumIterations (35); //设置匹配迭代的最大次数
这个参数控制了优化程序运行的最大迭代次数，一般来说，在达到这个限制值之前优化程序就会在 epsilon 变换阈值下终止。添加此最大迭代次数限制能够增加程序鲁棒性，阻止了它在错误的方向运行过长时间。
ndt.setInputCloud (filtered_cloud); //设置源点云
ndt.setInputTarget (target_cloud); //设置目标点云
这里，我们把点云赋给NDT配准对象，目标点云的坐标系是被匹配的输入点云的参考坐标系，匹配完成后输入点云将被变换到与目标点云统一坐标系下，当加载目标点云后，NDT算法的内部数据结构被初始化。
/* 设置使用机器人测距法得到的粗略初始变换矩阵结果 */
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 ();
在这部分的代码块中，我们创建了一个点云配准变换矩阵的初始估计，虽然算法运行并不需要这样的一个初始变换矩阵，但是有了它，易于得到更好的结果，尤其是当参考坐标系之间有较大差异时，其实此处用的实例测试点云数据之间差距较大。在机器人应用中，如用于生成数据集，通常使用测程法数据生成初始转换。
/* 计算需要的刚体变换以便将输入的点云匹配到目标点云 */
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged () << " score: " << ndt.getFitnessScore () << std::endl;
最后，我们准备好进行配准点云，生成变换后的源点云保存在输出点云里，此处是将第二组打开的点云变换后保存。然后我们打印出配准结果和欧式适合度评分，其中欧式适合度评分是计算输出点云到最近目标点云对应点对的距离平方和。
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ()); //使用创建的变换对未过滤的输入点云进行变换
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud); //保存转换的输入点云
配准程序完成之后，输出点云将包含一个过滤的输入点云的变换版本，因为我们传递给算法的输入是滤波后的输入点云，而非原始的输入点云，为了获得原始点云的配准版本，我们从NDT算法的结果中提取最终变换矩阵，并变换我们的原始输入点云，现在保存这个点云到文件room_scan2_transformed.pcd中以便将来使用。
接下来的这部分不是必需的，但是若想看到最终程序的可视化结果，使用点云库的可视化类，可以轻松地完成此部分，首先我们用黑色背景生成一个可视化对象，并加载需要显示的点云到对象中。最后，启动可视化对象，等待直到可视化对象的窗口关闭。
【编译和运行程序】
在工作空间根目录normal_distributions_transform下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(normal_distributions_transform)FIND_PACKAGE(PCL 1.5 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable(${PROJECT_NAME}_node src/normal_distributions_transform.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})
在工作空间根目录normal_distributions_transform下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件normal_distributions_transform_node，运行如下命令：
../normal_distributions_transform_node
在这里，并没有键入要打开的点云文件名作为参数，因为程序是默认从当前工作目录中打开点云文件。配准之前，如下面第一幅图所示，源点云与目标点云之间变换比较大，完成配准之后，将会在终端中看到下面第二幅图所示类似的结果，打印出加载的配准源和目标文件名以及最终配准后的详细信息，最后匹配点云的可视化结果如下面第三幅图所示。很明显，源点云和目标点云之间基本重合，并且使用不同的颜色显示。
2.4 交互式迭代最近点配准
在完成初始配准的情况下，一般采用ICP算法进行精细配准，标准的ICP配准算法是对源点云中的每个点，在目标点云中采用穷尽式搜索方法搜索距离最近的点作为对应点。然后求出配准对齐所有对应点对的变换矩阵，最后将源点云数据通过该矩阵进行变换。算法一般设计一种或多种收敛规则，重复迭代地进行上述操作直到设计的收敛规则条件满足。如果不考虑测量误差，ICP算法的精度受到测量采样密度的影响，误差值正比于平均采样距离，即采样密度越高，拼合的精度也就越高。
本小节将学习如何编写交互式ICP配准，并可视化其配准过程。该程序将输入初始点云，并对其进行初始刚性变换。接下来我们将配准初始转换点云到原始点云。每次当用户按下space按键时，ICP配准将迭代用户设置的次数，可视化窗口也随之更新最新的配准结果。
首先创建一个工作空间interactive_icp，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p interactive_icp/src
接着，在interactive_icp/src路径下，创建一个文件并命名为interactive_icp.cpp，拷贝如下代码：
#include <iostream>
#include <string>#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h> // TicToctypedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;bool next_iteration = false;void print4x4Matrix (const Eigen::Matrix4d & matrix)
{printf ("Rotation matrix :\n");printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));printf ("Translation vector :\n");printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, void* nothing)
{if (event.getKeySym () == "space" && event.keyDown ()){next_iteration = true;}
}int main (int argc, char* argv[])
{PointCloudT::Ptr cloud_in (new PointCloudT); // 初始点云PointCloudT::Ptr cloud_tr (new PointCloudT); // 转换点云PointCloudT::Ptr cloud_icp (new PointCloudT); // 输出点云/* 检查输入参数 */if (argc < 2){printf ("Usage :\n");printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);PCL_ERROR ("Provide one ply file.\n");return (-1);}int iterations = 1; // 默认ICP配准的迭代次数if (argc > 2){iterations = atoi (argv[2]);//将字符串变量转换为整数变量if (iterations < 1){PCL_ERROR ("Number of initial iterations must be >= 1\n");return (-1);}}pcl::console::TicToc time;time.tic ();if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0){PCL_ERROR ("Error loading cloud %s.\n", argv[1]);return (-1);}std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity (); // 定义旋转矩阵和平移矩阵/* 旋转矩阵的具体定义 */double theta = M_PI / 20; // 设置旋转弧度的角度transformation_matrix (0, 0) = cos (theta);transformation_matrix (0, 1) = -sin (theta);transformation_matrix (1, 0) = sin (theta);transformation_matrix (1, 1) = cos (theta);/* 设置平移矩阵 */transformation_matrix (0, 3) = 0.0;transformation_matrix (1, 3) = 0.0;transformation_matrix (2, 3) = 0.0;/* 在终端输出转换矩阵 */std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;print4x4Matrix (transformation_matrix);// 执行初始变换pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);*cloud_tr = *cloud_icp; // 将cloud_icp变量备份/* 设置ICP配准算法输入参数 */time.tic ();pcl::IterativeClosestPoint<PointT, PointT> icp;icp.setMaximumIterations (iterations); //设置迭代次数icp.setInputSource (cloud_icp);icp.setInputTarget (cloud_in);icp.align (*cloud_icp);icp.setMaximumIterations (1); // 当再次调用.align ()函数时，我们设置该变量为1。std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;if (icp.hasConverged ()){std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix = icp.getFinalTransformation ().cast<double>();print4x4Matrix (transformation_matrix);}else{PCL_ERROR ("\nICP has not converged.\n");return (-1);}pcl::visualization::PCLVisualizer viewer ("ICP demo"); // 初始可视化对象int v1 (0); // 创建视口v1int v2 (1); // 创建视口v2viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);float bckgr_gray_level = 0.0; // 黑色float txt_gray_lvl = 1.0 - bckgr_gray_level;/* 设置初始点云为白色，并在左右视口都显示 */pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl);viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);/* 设置用构造的变换矩阵变换后的点云为绿色 */pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 0, 250, 0);viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);/* 设置ICP配准后的点云为红色 */pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 250, 0, 0);viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);/* 在两个视口，分别添加文字描述 */viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);std::stringstream ss;ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str ();viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);/* 设置背景颜色 */viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);/* 设置相机位置和方向 */viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);viewer.setSize (1280, 1024); // 设置可视化窗口的尺寸/* 通过键盘，调用回调函数 */viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);/* 设置显示器 */while (!viewer.wasStopped ()){viewer.spinOnce ();if (next_iteration) // 用户按下空格键{// 配准算法开始迭代time.tic ();icp.align (*cloud_icp);std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;if (icp.hasConverged ()){printf ("\033[11A"); printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix *= icp.getFinalTransformation ().cast<double>();print4x4Matrix (transformation_matrix); // 输出初始矩阵和当前矩阵的转换矩阵ss.str ("");ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str ();viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");}else{PCL_ERROR ("\nICP has not converged.\n");return (-1);}}next_iteration = false;}return (0);
}
【解释说明】
接下来分析上述源代码中的关键语句。为了测试，首先创建一变换矩阵，对原始模型进行变换，然后再将变换后的模型与原始模型进行配准，以测试ICP。该变换矩阵，实现绕Z轴旋转M_PI / 20，同时沿Z轴平移4.0。
/* 旋转矩阵的具体定义 */
double theta = M_PI / 20; // 设置旋转弧度的角度
transformation_matrix (0, 0) = cos (theta);
transformation_matrix (0, 1) = -sin (theta);
transformation_matrix (1, 0) = sin (theta);
transformation_matrix (1, 1) = cos (theta);/* 设置平移矩阵 */
transformation_matrix (0, 3) = 0.0;
transformation_matrix (1, 3) = 0.0;
transformation_matrix (2, 3) = 4.0;/* 在终端输出转换矩阵 */
std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
print4x4Matrix (transformation_matrix);// 执行初始变换
pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);
*cloud_tr = *cloud_icp; // 将cloud_icp变量备份
利用刚体变换对初始点云进行转换， cloud_in 为初始模型，cloud_icp 为变换后的模型，cloud_tr 时变换后数据的备份用于后期显示。
/* 设置ICP配准算法输入参数 */
time.tic ();
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setMaximumIterations (iterations); //设置迭代次数
icp.setInputSource (cloud_icp);
icp.setInputTarget (cloud_in);
icp.align (*cloud_icp);
创建ICP配准对象，设置ICP算法的参数， setMaximumIterations 函数设置ICP每次迭代最大次数为1，主要是为了后续对每一步迭代的效果进行可视化，读者可以调整设置每次迭代更多。然后，变换模型在初次配准进行后，当后续ICP配准再被调用时（当用户按下space 按键时），每次调用迭代次数都与这里设置的参数一致。
if (icp.hasConverged ())
{std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix = icp.getFinalTransformation ().cast<double>();print4x4Matrix (transformation_matrix);
}
检查ICP算法是否收敛，如果不收敛，退出程序。如果成功配准，打印输出变换矩阵。这里的成功配准并不是指配准误差足够小，而是满足设置的迭代次数后有配准矩阵估计输出。getFitnessScore ()函数返回的是ICP迭代结束后所有对应点间的距离平方和，配准效果越好，该数值越小。
pcl::visualization::PCLVisualizer viewer ("ICP demo"); // 初始可视化对象
int v1 (0); // 创建视口v1
int v2 (1); // 创建视口v2
viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
为了可视化，在绘制窗口中创建两个视口。
float bckgr_gray_level = 0.0; // 黑色
float txt_gray_lvl = 1.0 - bckgr_gray_level;/* 设置初始点云为白色，并在左右视口都显示 */
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl);
viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);/* 设置用构造的变换矩阵变换后的点云为绿色 */
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 0, 250, 0);
viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);/* 设置ICP配准后的点云为红色 */
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 250, 0, 0);
viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
我们添加初始点云到所述两个视口，并用相同的变量 txt_gray_lvl 赋予初始点云颜色。初始转换后的点云被设置为绿色，添加到左视口，ICP配准后的点云被设置成红色，添加到右视口。
/* 在两个视口，分别添加文字描述 */
viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);std::stringstream ss;
ss << iterations;
std::string iterations_cnt = "ICP iterations = " + ss.str ();
viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
对每个视口，添加文字描述，以便用户了解该视口相关信息。需要注意的是，因为输入的迭代次数变量 iterations 为整型，需要转换其格式为字符串。
/* 设置背景颜色 */
viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);/* 设置相机位置和方向 */
viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
viewer.setSize (1280, 1024); // 设置可视化窗口的尺寸/* 通过键盘，调用回调函数 */
viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
根据变量 bckgr_gray_level，设置两个视口的背景颜色，用户只需要按下C按键，就可以去获取相机参数。然后我们向这个函数复制参数以保存相机位置、朝向和焦点。当查看显示器窗口时，任何时候只要按下按键，通过 registerKeyboardCallback 函数，我们来回调相关函数，以实现用户交互式的进行配准。
当用户按下键盘的任何键， keyboardEventOccurred 将会被调用，这个函数将会检查这个按键是否是空格键。如果是空格键，全局布尔变量next_iteration 被设置为真，允许可视化显示循环进入下面部分代码，ICP配准对象被调用来配准点云。前面我们已经对该ICP配准对象设置了源和目标点云，且设置循环时的最大迭代次数为1，但是要注意的是这里用函数 setlnputSource 设置源点云模型为 cloud_icp，配准输出也是cloud_icp。也就是说每次迭代配准的源点云模型其实就是上次迭代配准的结果，读者不要误认为ICP算法的输入点云模型没有改变。
if (next_iteration) // 用户按下空格键
{// 配准算法开始迭代time.tic ();icp.align (*cloud_icp);std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;if (icp.hasConverged ()){printf ("\033[11A"); printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix *= icp.getFinalTransformation ().cast<double>();print4x4Matrix (transformation_matrix); // 输出初始矩阵和当前矩阵的转换矩阵ss.str ("");ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str ();viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");}else{PCL_ERROR ("\nICP has not converged.\n");return (-1);}
}
next_iteration = false;
为了得到从初始点云到当前配准好的点云的刚体变换，函数 getFinalTransformation()返回的只是调用一次 align()函数时ICP所得到的配准矩阵，即该函数返回的是每次迭代前后cloud_icp的位姿变化。这并不是我们想要的从初始点云到当前配准好的点云的刚体变换，如果我们把每次的位姿变化用矩阵相乘来累计，结果获取的转换矩阵是从开始到当前的迭代矩阵。
但这只是从数学角度上说是正确的，由于计算中舍入的关系，我们会发现在实际程序中，这并不成立。这就是我们为什么要引入最大代次数作为输入参数（用 setMaximumlterations 设置）的原因，读者试着设置该参数为40，并存储矩阵。设置该参数为1或者2，然后通过按空格键实现交互式迭代配准，直到迭代次数达到40次。通过对比，会发现略微的不同，初始迭代参数设置为40获取的配准矩阵和真值完全相等，而通过交互式得到的变换矩阵就与真值不同，但是这并不能说明，最终的配准效果不好，可以看一下getFitnessScore ()返回的数值，可以发现最终都能够达到3的-11次方误差。这里注意对于设置该参数为40的，运行后还需要迭代两三次，否则两者的 getFitnessScore 也差较大，因为getFitnessScore 的返回值与当前的迭代使用的对应点有关。
【编译和运行程序】
在工作空间根目录interactive_icp下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)project(pcl-interactive_icp)find_package(PCL 1.5 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (${PROJECT_NAME}_node src/interactive_icp.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})
在工作空间根目录interactive_icp下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件pcl-interactive_icp_node，运行如下命令：
../pcl-interactive_icp_node monkey.ply 1
运行之后，在可视化窗口中可以看到参数设置为1，迭代1次后的可视化结果，其中左视口为原始的源点云模型与目标模型，白色为原始点云模型，绿色为经过变换矩阵转换之后的点云模型，左视口用于对比配准结果。右视口为配准的源点云模型与目标模型，白色为原始点云模型，红色为ICP迭代之后的点云模型。
随着用户按 space 键多次后，可以看到每次迭代的点云模型的位姿变化，经过60次迭代后，配准结果基本已经没有偏差了，如下图所示。
2.5 刚性物体的位姿估计
位姿估计是许多点云处理问题的必要步骤，在本小节，我们将学习如何在存在遮挡情况的场景中估计出某个刚性物体的位姿。
首先创建一个工作空间Robust_pose_estimation_of_rigid_objects，然后再在工作空间创建一个文件夹src用于存放源代码：
mkdir -p Robust_pose_estimation_of_rigid_objects/src
接着，在Robust_pose_estimation_of_rigid_objects/src路径下，创建一个文件并命名为alignment_prerejective.cpp，拷贝如下代码：
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT; //FPFH特征描述子
typedef pcl::FPFHEstimation<PointNT,PointNT,FeatureT> FeatureEstimationT; //FPFH特征估计类
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT; //自定义颜色句柄int main (int argc, char **argv)
{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);if (argc != 3){pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);return (1);}/* 加载目标物体和场景点云 */pcl::console::print_highlight ("Loading point clouds...\n");if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0){pcl::console::print_error ("Error loading object/scene file!\n");return (1);}/* 下采样 */pcl::console::print_highlight ("Downsampling...\n");pcl::VoxelGrid<PointNT> grid;const float leaf = 0.005f; //下采样时使用的空间分辨率 grid.setLeafSize (leaf, leaf, leaf);grid.setInputCloud (object);grid.filter (*object);grid.setInputCloud (scene);grid.filter (*scene);/* 估计场景法线 */pcl::console::print_highlight ("Estimating scene normals...\n");pcl::NormalEstimation<PointNT,PointNT> nest;nest.setRadiusSearch (0.01);nest.setInputCloud (scene);nest.compute (*scene);// 特征估计pcl::console::print_highlight ("Estimating features...\n");FeatureEstimationT fest;fest.setRadiusSearch (0.025); //该搜索半径决定FPFH特征描述的范围，一般设置为分辨率的10倍以上fest.setInputCloud (object);fest.setInputNormals (object);fest.compute (*object_features);fest.setInputCloud (scene);fest.setInputNormals (scene);fest.compute (*scene_features);/* 实施配准 */pcl::console::print_highlight ("Starting alignment...\n");pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;align.setInputSource (object);align.setSourceFeatures (object_features);align.setInputTarget (scene);align.setTargetFeatures (scene_features);align.setMaximumIterations (50000); // 采样一致性迭代次数align.setNumberOfSamples (3); // 创建假设所需的样本数align.setCorrespondenceRandomness (5); // 使用的临近特征点的数目align.setSimilarityThreshold (0.9f); // 多边形边长度相似度阈值align.setMaxCorrespondenceDistance (2.5f * 0.005); // 判断是否为内点的距离阈值align.setInlierFraction (0.25f); // 接受位姿假设所需的内点比例{pcl::ScopeTime t("Alignment");align.align (*object_aligned);}if (align.hasConverged ()){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 ());pcl::visualization::PCLVisualizer visu("Robust_pose_estimation_of_rigid_objects");int v1(0),v2(0);visu.createViewPort(0,0,0.5,1,v1);visu.createViewPort(0.5,0,1,1,v2);visu.setBackgroundColor(0,0,0,v1);visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene",v1);visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned",v1);visu.addPointCloud(object,ColorHandlerT (object, 0.0, 0.0, 255.0), "object_before_aligned",v2);visu.addPointCloud(scene,ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene_v2",v2);visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene");visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_aligned");visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_before_aligned");visu.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene_v2");visu.spin ();}else{pcl::console::print_error ("Alignment failed!\n");return (1);}return (0);
}
【解释说明】
接下来分析上述源代码中的关键语句。我们首先声明定义来避免后续定义复杂的数据类型。
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT; //FPFH特征描述子
typedef pcl::FPFHEstimation<PointNT,PointNT,FeatureT> FeatureEstimationT; //FPFH特征估计类
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT; //自定义颜色句柄
然后，我们实例化本例所需的数据容器，并解析输入参数、加载目标对象和场景的点云。为了加速点云处理进程，使用0.1m体素分辨率对目标物体和场景点云进行空间均匀下采样。
/* 下采样 */
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
const float leaf = 0.005f; //下采样时使用的空间分辨率
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
利用类 NormalEstimationOMP 对场景表面缺失的法向量进行估计，所计算的表面法向量用于计算特征值，该特征值用于后续匹配工作，该类利用了多线程机制的法线估计，编译安装PCL时依赖OpenMP，对于没用使用 OpenMP 的用户，直接将 NormalEstimationOMP 中的OMP删除，使用NormalEstimation 直接替代即可。
/* 估计场景法线 */
pcl::console::print_highlight ("Estimating scene normals...\n");
pcl::NormalEstimation<PointNT,PointNT> nest;
nest.setRadiusSearch (0.01);
nest.setInputCloud (scene);
nest.compute (*scene);
对于下采样后的点云的每一个点，我们使用 FPFHEstimationOMP() 类计算其快速点云特征直方图（FPFH）描述子，这些描述子用于在配准过程中的初步匹配，该类也使用 OpenMP 多线程提高计算性能的机制，与上述法线估计类似处理。
// 特征估计
pcl::console::print_highlight ("Estimating features...\n");
FeatureEstimationT fest;
fest.setRadiusSearch (0.025); //该搜索半径决定FPFH特征描述的范围，一般设置为分辨率的10倍以上
fest.setInputCloud (object);
fest.setInputNormals (object);
fest.compute (*object_features);
fest.setInputCloud (scene);
fest.setInputNormals (scene);
fest.compute (*scene_features);
以下代码用于启动配准进程，我们使用类 SampleConsensusPrerejective，该类实现了一种有效的基于采样一致性的位姿估计，其高效之处是利用类 CorrespondenceRejectorPoly 提前去除了错误位姿的假设。
/* 实施配准 */
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (50000); // 采样一致性迭代次数
align.setNumberOfSamples (3); // 创建假设所需的样本数
align.setCorrespondenceRandomness (5); // 使用的临近特征点的数目
align.setSimilarityThreshold (0.9f); // 多边形边长度相似度阈值
align.setMaxCorrespondenceDistance (2.5f * 0.005); // 判断是否为内点的距离阈值
align.setInlierFraction (0.25f); // 接受位姿假设所需的内点比例
注意：与常规的基于随机采样一致性的变换矩阵估计类 SampleConsensusInitialAlignment不同，该类输入除了常规的输入点云和特征，该类还需要额外的参数，具体如下。
∙\bullet∙ 假设需要的采样数目—setNumberOfSamples ()：在目标物体和场景之间，对应点对的样本的数目，为了正常估计位姿至少需要3点。
∙\bullet∙ 选择对应点随机度（Correspondence randomness）—setCorrespondenceRandomness ()：构造对应点时，并非直接选择特征匹配距离最小的，而是在N个最佳匹配点之间进行随机选择，这增加了迭代次数，但使该算法对包含离群点的数据更具鲁棒性。
∙\bullet∙ 多边形相似阈值（Polygonal similarity threshold）—setSimilarityThreshold ()：对于目标物体和场景上的对应点，根据它们在各自的空间里之间的欧氏距离是不变的这一几何一致性，使用类 CorrespondenceRejectorPoly 去消除错误对应点。这个值越接近1，该算法通过减少迭代次数而变得很快速。但是，当噪声存在时，这也增加了排除正确位置的风险。
∙\bullet∙ 判断内点阈值（Inlier threshold）—setMaxCorrespondenceDistance ()：此处的阈值用来判断用假设的变换矩阵变换 setInputSource 输入源点云后，与 setlnputTarget 输入的目标点云最近点的距离小于阈值，则认为该对应点对为内点。在该例中，我们使用一个1.5倍于点云分辨率的启发式数值。
∙\bullet∙ 内点比例（Inlier fraction）—setInlierFraction ()：在许多实际的应用中，观察到的目标对象的大部分在其所在的场景中是不可见的。可能因为噪声太大，也可能因为遮挡，还可能二者兼有，在这样的情况中，对于场景我们不需要位姿估计的结果使得 setInputSource 的点云数据中的所有点都配准。如果在 setInputSource 输入的点云中，内点数目占所有点云数目的比例比指定的内点比例高，那算法认为该变换假设是有效的。
配准后的 setlnputSource 点云数据存储在对象 object_aligned中，如果在该变换下，有足够的内点可以找到（大于25%的setInputSource点数据大小）。该算法被认为是收敛的，输出并可视化结果。
【编译和运行程序】
在工作空间根目录Robust_pose_estimation_of_rigid_objects下，编写CMakeLists.txt文件如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(alignment_prerejective)find_package(PCL 1.7 REQUIRED REQUIRED COMPONENTS io registration segmentation visualization)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (${PROJECT_NAME}_node src/alignment_prerejective.cpp)
target_link_libraries (${PROJECT_NAME}_node ${PCL_LIBRARIES})
在工作空间根目录Robust_pose_estimation_of_rigid_objects下创建一个build文件夹，用于存放编译过程中产生的文件，然后执行编译：
mkdir build
cd build
cmake ..
make
此时，会在build文件夹下生成一个可执行文件alignment_prerejective_node，运行如下命令，其中chef.pcd为目标点云文件，rs1.pcd为含有遮挡的场景点云文件：
./alignment_prerejective_node chef.pcd rs1.pcd
运行之后，在可视化窗口中可以看到目标物体的位姿估计结果，如下图所示，其右视口为原始点云，左视口为配准结果。
读者在终端也将看到类似于下面所示的输出，打印出利用该算法估计的变换矩阵和执行时间等。
> Downsampling...
> Estimating scene normals...
> Estimating features...
> Starting alignment...
Alignment took 1008ms.| 0.005 -0.700 -0.714 |
R = | -0.991 -0.101 0.092 | | -0.136 0.707 -0.694 | t = < -0.499, 0.108, 0.290 >Inliers: 1384/3432
PCL中点云配准（非常详细，建议收藏）相关推荐
- 一分钟详解PCL中点云配准技术
原文首发于微信公众号「3D视觉工坊」: 一分钟详解PCL中点云配准技术 本文是对前两篇文章:
- 用pcl读ply文件_一分钟详解PCL中点云配准技术
原文首发于微信公众号「3D视觉工坊」:一分钟详解PCL中点云配准技术 本文是对前两篇文章:点云配准(一 两两配准)以及3D点云(二 多福点云配准)的补充,希望可以在一定程度上帮助大家对点云配准理解地更 ...
- PCL中点云特征描述与提取精通级实例解析
PCL中点云特征描述与提取精通级实例解析 1 3D对象识别的假设验证 2 隐式形状模型方法 2.1 隐式形状模型识别方法概述 2.2 实例解析 1 3D对象识别的假设验证 本教程将学习在高噪声和严 ...
- 呕心沥血 JavaScript知识点梳理大全，超详细 建议收藏！！！
呕心沥血 JavaScript知识点梳理大全,超详细 建议收藏!!! ✴️大家好,我是王同学,爆肝三天三夜王同学把JavaScript 知识点梳理了一遍,文章没有一点套路,只有满满的干货 ✴️如果对你 ...
- PCL中点云关键点提取
PCL中点云关键点提取 1 关键点概念及相关算法 1.1 NARF关键点 1.2 Harris关键点 1.3 PCL中keypoints模块及类介绍 2 关键点入门级实例解析 2.1 如何从深度图像中 ...
- python点云快速配准_分享一个V-SLAM中点云配准算法改进的方法
近年来,随着自主导航的广泛应用,视觉同时定位与地图构建(Visual simultaneous localization and mapping, V-SLAM)通过自身携带的视觉传感器对周围环境进行 ...
- PCL中点云的超体素(SuperVoxel)
各位小伙伴们,有没有发现PCL库中已经集成了太多我们想实现的算法或者功能呢?所以这里组织一下学习小组针对PCL库中实现的算法进行剖析与论文解读,所以希望更多的小伙伴们参与进来,我们一起吃透PCL,欢迎 ...
- PCL点云配准（1）
在逆向工程,计算机视觉,文物数字化等领域中,由于点云的不完整,旋转错位,平移错位等,使得要得到的完整的点云就需要对局部点云进行配准,为了得到被测物体的完整数据模型,需要确定一个合适的坐标系,将从各个视 ...
- PCL 点云配准衡量指标
0. 简介 PCL作为目前最为强大的点云库,内部存在有大量集成好的算法.而对于数据量大.非同源.含大量噪声且部分重叠的激光点云与影像重建点云,其稀疏程度.噪声程度等不同,非重叠区域的面积很大.真实场景 ...
- PCL点云配准（ICP）
/*********************************** 点云配准 *****************************************/ #include <pc ...
最新文章
- bzoj1227: [SDOI2009]虔诚的墓主人（树状数组，组合数）
- 相机与激光雷达融合的3D目标检测方法MVAF-Net
- python使用numpy中的flatten函数将2D numpy数组拉平为1Dnumpy数组、使用np.linalg.matrix_rank函数计算2D numpy数组的秩(rank)
- 总结：SpringMVC 中 GET 和 POST 方式请求中的中文乱码问题
- Ajax/JavaScript脚本大全,JS脚本大全
- group by两个条件
- 【报告分享】2020年抖音美妆直播报告.pdf（附下载链接）
- SQL Server 本地数据库登录不上 解决方法
- 【数据结构笔记30】拓扑排序、AOV网络、AOE网络、关键工序
- 实现算法2.15、2.16的程序（一个数组只生成一个静态链表）
- 外观(门面)模式-结构型
- 3D LUT调色预设如何导入并应用？(fcpx/PR/AE/PS/LR/达芬奇)
- 下载DirectX 9 SDK开发包
- ubuntu Nvidia dkms 驱动恢复
- 自定义input单选框样式
- xp每天定时关机命令
- 超全面的前端切图技巧，读这篇就够了
- 2022支付宝集五福攻略,一天20张不是事
- Java面试宝典（2019版）
- Cookies vs. Sessions