前言
PCL库中有很多配准的方式，主要都是基于ICP
ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P = {Pi, i = 0,1, 2,…,k}及U = {Ui,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k ≥ n。配准过程就是求取 2 个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：
(1)计算最近点，即对于集合U中的每一个点，在集合P中都找出距该点最近的对应点，设集合P中由这些对应点组成的新点集为Q = {qi,i = 0,1,2,…,n}。
(2)采用最小均方根法，计算点集 U 与 Q 之间的配准，使 得到配准变换矩阵 R，T，其中R是 3×3 的旋转矩阵，T 是 3×1 的平移矩阵。
(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U1，即U1 = RU + T
(4)计算U1与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U1替换U，重复上述步骤。
SLAM中点云的地图构建，本质就是把点云拼在一起，形成的点云模型。
那么如何把点云拼在一起呢，方法有很多，例如ICP、NDT
点云拼接，点云配准其实是一个意思，都是把不同位置的点云通过重叠部分的信息，变换到同一个位置。
本次使用PCL中的ICPNL（IterativeClosestPointNonLinear）算法 来实现 多幅点云的拼接
功能的实现思想就是对所有点云进行变换，使得都与第一个点云在统一的坐标系中，在每个连贯的有重叠的点云之间找到最佳的变换，并积累这些变换到全部的点云。
Code
///////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <boost/make_shared.hpp>//boost 指针相关头文件
#include <pcl/point_types.h>//点云类型定义
#include <pcl/point_cloud.h>//点云类
#include <pcl/point_representation.h>//点相关表示
#include <pcl/io/pcd_io.h>//pcd文件打开存储类
#include <pcl/filters/voxel_grid.h>//体素滤波
#include <pcl/filters/filter.h>//滤波
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/registration/icp.h>//icp类
#include <pcl/registration/icp_nl.h>//非线性icp类
#include <pcl/registration/transforms.h>//变换矩阵
#include <pcl/visualization/pcl_visualizer.h>//可视化相关头文件 功能已注释
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//定义点、点云 的类型
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;设置点云类型
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//创建可视化工具
pcl::visualization::PCLVisualizer *p;
//定义左右视点
int vp_1, vp_2;创建可视化工具 分成左右两个 视点 左边显示 匹配的两帧原始数据直接拼一起的效果 右边显示拼接过程与最终效果
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//声明一个结构体 里面可以存放文件名和对应的点云
struct PCD
{
PointCloud::Ptr cloud;//存储对应的点云
std::string f_name;//存储对应的文件名
PCD():cloud(new PointCloud) {};//结构体构造函数
};声明一个结构体 里面可以存放文件名和对应的点云
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//以< x, y, z, curvature >形式定义一个新的点
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
MyPointRepresentation ()
{
//定义尺寸值
nr_dimensions_ = 4;
}
//覆盖copyToFloatArray方法来定义我们的特征矢量
virtual void copyToFloatArray (const PointNormalT &p, float * out) const
{
// < x, y, z, curvature >
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.curvature;
}
};PCL的点的表示相关 以< x, y, z, curvature >形式定义一个新的点
class pcl::PointRepresentation< PointT > 这个给主要提供 将一个点转换成n维向量的 方法
这是一个抽象类， 子类必须 在构造函数中 设置 nr_dimensions_为合适的值 ， 并且提供纯虚函数 copyToFloatArray（）方法的实现
定义的类可以调用的 父类（ pcl::PointRepresentation） 的函数有
getNumberOfDimensions() 返回点的向量表示中的维数。 也就是 nr_dimensions_ 的 值
setRescaleValues(const float * rescale_array) 设置点的缩放值 这个在后面会用到
///////////////////////////////////////////////////////////////////////////////////////////////////////////
void loadData(int argc, char **argv,std::vector<PCD, Eigen::aligned_allocator<PCD> > &models )
{
std::string extension (".pcd");//声明一个字符串 要读取文件的后缀名
//循环读指令 指定的文件 第1个参数是执行程序，第二个参数是pcd文件 所有i从1开始
for(int i=1;i<argc;i++)
{
std::string fname = std::string (argv[i]);//提取第i个pcd文件的名字
//对文件的名字的长度判断大小
if(fname.size ()<= extension.size ()){ continue;}//跳出本次循环
//将文件中的大写字母转成小写 如果有的话
std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);
//检查文件的名字 后缀 是否 为 pcd
if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0)
{
/*加载点云并保存在总体的模型列表中*/
PCD m;//声明一个PCD结构体
m.f_name = argv[i];//赋值文件名
if(pcl::io::loadPCDFile(argv[i], *m.cloud)==-1)//取点云
{
//没有读到 输出信息
std::cout<<"路径不对，没读到pcd文件： "<<argv[i]<<std::endl;
continue;//跳出本次循环
}
//从点云中移除NAN点
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
models.push_back (m);//保存在总体的模型列表中
}
}
}从PCD文件中读取 点云
过程在代码里面基本每行都注释了
主要就是判断设置的文件名字是否正确，然后读取，保存在总体的模型列表中
///////////////////////////////////////////////////////////////////////////////////////////////////////////
/** 在可视化窗口的第一视点显示源点云和目标点云
*
*/
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
p->removePointCloud ("vp1_target");
p->removePointCloud ("vp1_source");
pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0);
p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1);
p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);
PCL_INFO ("Press q to begin the registration.\n");
p-> spin();
}
/**在可视化窗口的第二视点显示源点云和目标点云
*
*/
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
p->removePointCloud ("source");
p->removePointCloud ("target");
pcl::visualization::PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");
if (!tgt_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!");
pcl::visualization::PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature");
if (!src_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!");
p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);
p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);
p->spinOnce();
}左右的可视化窗口 显示程序
///////////////////////////////////////////////////////////////////////////////////////////////////////////
下面是ICP的两两校准函数
void pairAlign(const PointCloud::Ptr cloud_src,const PointCloud::Ptr cloud_tgt,PointCloud::Ptr output,Eigen::Matrix4f &final_transform,bool downsample = false)匹配一对点云数据集并且返还结果
参数 cloud_src 是源点云
参数 cloud_src 是目标点云
参数output输出的配准结果的源点云
参数final_transform是在来源和目标之间的转换
/*下采样*/
//为了一致性和高速的下采样
//注意：为了大数据集需要允许这项
PointCloud::Ptr src (new PointCloud);//下采样后的源点云
PointCloud::Ptr tgt (new PointCloud);//下采样后的目标点云
pcl::VoxelGrid<PointT> grid;//体素滤波
if(downsample)//根据是否进行下采样的参数
{
grid.setLeafSize (0.05, 0.05, 0.05);//设置体素滤波参数
grid.setInputCloud (cloud_src);//体素滤波点云输入
grid.filter (*src);//源点云下采样
grid.setInputCloud (cloud_tgt);//体素滤波点云输入
grid.filter (*tgt); //目标点云下采样
}
else
{
src = cloud_src;//不下采样直接赋值
tgt = cloud_tgt;//不下采样直接赋值
}为了一致性和高速的下采样 就是体素滤波的内容，之前写过
///////////////////////////////////////////////////////////////////////////////////////////////////////////
/*计算曲面法线和曲率*/
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);//有法向量的源点云
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);//有法向量的目标点云
pcl::NormalEstimation<PointT, PointNormalT> norm_est;//法向量估计
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());//kdtree
norm_est.setSearchMethod (tree);
norm_est.setKSearch (30);
//估计源点云的各点法向量
norm_est.setInputCloud (src);
norm_est.compute (*points_with_normals_src);
//src中的xyz覆盖掉points_with_normals_src中的xyz值，然后把xyz+normal的信息给points_with_normals_src
pcl::copyPointCloud (*src, *points_with_normals_src);
//估计目标点云的各点法向量
norm_est.setInputCloud (tgt);
norm_est.compute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);计算曲面法线和曲率 为ICP提供点匹配的特征
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//我们自定义点的表示（以上定义）
MyPointRepresentation point_representation;
//调整'curvature'尺寸权重以便使它和x, y, z平衡
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);设置点的表示向量，并设置缩放
///////////////////////////////////////////////////////////////////////////////////////////////////////////
/*配准*/
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;//icpnl对象
// 设置 两次变换矩阵之间的停止差值 (默认 2)
reg.setTransformationEpsilon (1e-6);
//将两个对应关系之间的(src<->tgt)最大距离设置为10厘米
// 设置最大的对应关系距离为 多少 单位m， （对应距离大于此距离将被忽略）
reg.setMaxCorrespondenceDistance (15);
//设置点表示 继承与 Registration 类的 setPointRepresentation 的方法 功能 提供指向 PointRepresentation 的 boost 共享指针，以便在比较点时使用。
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputCloud (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
reg.setMaximumIterations (2);//设置最大迭代次数 因为以手动方式设置的很小
进行配准的相关设置
- x容差
- 对应距离
- 点的表示
- 输入、目标点云
- 迭代次数
//Ti 本次迭代的转换矩阵 prev上次迭代的转换矩阵 targetToSource 目标点云到源点云的变换
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;//本次迭代结合的点云
//设置一个初始的估计 位姿矩阵
Eigen::Matrix4f T_init = Eigen::Matrix4f::Identity ();
T_init<<1,0,0, 0,
0,1,0,0,
0,0,1,0,
0,0,0,1;设置相关的矩阵
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//手动循环30次
for (int i = 0; i < 30; ++i)
{
//打印迭代的第几次
PCL_INFO ("Iteration Nr. %d.\n", i);
//为了可视化的目的保存点云
points_with_normals_src = reg_result;//更新上一次结合的点云
//估计
reg.setInputCloud (points_with_normals_src);
reg.align (*reg_result,T_init);//得到结合的点云
//在每一个迭代之间累积转换
Ti = reg.getFinalTransformation () * Ti;
//如果这次转换和之前转换之间的差异小于阈值
//则通过减小最大对应距离来改善程序
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
{
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
prev = reg.getLastIncrementalTransformation ();
std::cout<<"reduce max correspond distance"<<std::endl;
}
//可视化当前状态
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
// 得到目标点云到源点云的变换
targetToSource = Ti.inverse();
//把目标点云转换回源点云坐标系
pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
//可视化最终结果状态
p->removePointCloud ("source");
p->removePointCloud ("target");
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);
p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");
*output += *cloud_src;//将源点云加的结果上 即 返回拼接的点云
final_transform = targetToSource;//返回的 目标点云转换回源点云的坐标变换
}手动循环30次
进行估计
在每一个迭代之间累积转换
得到目标点云到源点云的变换
将源点云加的结果上 即 返回拼接的点云
///////////////////////////////////////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)下面是main函数
//加载数据
std::vector<PCD, Eigen::aligned_allocator<PCD> > data;//data是PCD的数据类型，第二个参数是动态分配内存
loadData (argc, argv, data);//根据用户指令 读取数据 存在data中加载数据
//创建一个PCL可视化对象
p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
p->createViewPort (0.0, 0, 0.5, 1.0, vp_1);
p->createViewPort (0.5, 0, 1.0, 1.0, vp_2);创建PCL可视化对象
//result是 统一转换到第一个pcd坐标系系的 点云 source 是源点云 上一帧 target是目标点云 当前帧
PointCloud::Ptr result (new PointCloud), source, target;
PointCloud::Ptr all_result (new PointCloud);
//GlobalTransform 是保存的两个pcd结合转到 第一帧的坐标变换
//pairTransform 是两帧pcd之间的坐标变换
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
声明相关变量
///////////////////////////////////////////////////////////////////////////////////////////////////////////
//循环处理每个pcd
for (size_t i = 1; i < data.size (); ++i)
{
//从data中分别提取 点云
source = data[i-1].cloud;
target = data[i].cloud;
//添加可视化数据
showCloudsLeft(source, target);
//结合的点云
PointCloud::Ptr temp (new PointCloud);
//打印正在转换的 文件和对应点的个数
PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
//执行对准
pairAlign (source, target, temp, pairTransform, true);
//把当前的两两配对转换到全局变换
pcl::transformPointCloud (*temp, *result, GlobalTransform);
*all_result += *result;
//update the global transform更新全局变换
GlobalTransform = pairTransform * GlobalTransform;
//保存配准对，转换到第一个点云框架中
std::stringstream ss;
ss << i << ".pcd";
pcl::io::savePCDFile (ss.str (), *all_result, true);
}循环 两两 处理每个pcd
把当前的两两配对转换到全局变换
保存配准对
///////////////////////////////////////////////////////////////////////////////////////////////////////////
Result
