VLP-16第四课: 3D点云的配准方法

额外拓展记录: IMU的滤波手段: AHRS + Mahony

IMU+Visual融合: https://github.com/ygling2008/direct_edge_imu

1. 点云开源匹配算法

• 其他众多点云配准算法

    “常用的点云配准算法”
    1. 正态分布变换方法  NDT  正态分布变换进行配准（normal Distributions Transform） 
    2. 和著名的 迭代最近点 Iterative Closest Point （ICP） 点云配准
    
    “其他的配准算法”
    
    1.ICP：稳健ICP、point to plane ICP、point to line ICP、MBICP、GICP 
    2.NDT: NDT 3D、Multil-Layer NDT
    3.FPCS、KFPSC、SAC-IA
    4.Line Segment Matching、ICL
  

2. 点云配准ICP算法–原理简介

• 点云配准的基本流程

1.识别最能代表两个数据集中场景中的兴趣点（关键点）；

2.在每个关键点处，计算特征描述符（如FPFH）；

关键点 keypoints(NARF, SIFT 、FAST、均匀采样 UniformSampling)、
       特征描述符　descriptions，NARF、 FPFH、BRIEF 、SIFT、ORB )

3.从特征描述符集合以及他们在两个数据集中的x,y,z位置，基于特征和位置之间的相似性来估计对应关系；

4.假设数据是包含噪声的，即不是所有的对应关系都是有效的，所以舍弃对配准过程产生负面影响的那些对应关系；

可以采用随机采样一致性估计，或者其他方法剔除错误的对应关系

5.从剩下的一组好的对应关系中，估计一个变换行为。

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

• 点云配准的核心术语

**特征描述: ** 点云基于发现的关键点，提取特征组合信息并生成向量来进行比较（PCL有专门的模块讲述点云特征）

Feature based registration 配准
    1. SIFT 关键点 (pcl::SIFT…something)
    2. FPFH 特征描述符  (pcl::FPFHEstimation)  
    3. 估计对应关系 (pcl::CorrespondenceEstimation)
    4. 错误对应关系的去除( pcl::CorrespondenceRejectionXXX )  
    5. 坐标变换的求解

一致性估计： 给定两组数据获取的特征描述向量，找到相应的特征以在数据中找到重叠的部分；

• ICP的迭代过程

1.对于目标点云中的每个点，匹配参考点云（或选定集合）中的最近点。

2.求得使上述对应点对计算均方根（root mean square，RMS）最小的刚体变换，求得平移参数和旋转参数。

3.使用获得的转换矩阵来转换目标点云。

4.迭代（重新关联点），直到满足终止迭代的条件（迭代次数或误差小于阈值）。这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。

使用点匹配（XYZ）时， ICP配准，针对有序点云和无序点云具有不同的策略：

  1. 穷举配准（brute force matching）;
  2. kd树最近邻查询（FLANN）;
  3. 在有序点云数据的图像空间中查找;
  4. 在无序点云数据的索引空间中查找.

特征描述符匹配（如FPFH）：

  1. 穷举配准（brute force matching）;
  2. kd树最近邻查询（FLANN）。

	ICP迭代最近点算法 pcl::IterativeClosestPoint icp;

	  pcl::IterativeClosestPoint icp;
	     //创建IterativeClosestPoint的对象
	  icp.setInputCloud(cloud_in);                 //cloud_in设置为点云的源点
	  icp.setInputTarget(cloud_out);               //cloud_out设置为与cloud_in对应的匹配目标
	  pcl::PointCloud Final;        //存储经过配准变换点云后的点云
	  icp.align(Final);  
	
	结果：
	  icp.hasConverged()
	  icp.getFitnessScore()
	  icp.getFinalTransformation()

• 点云配准种类

手动配准： 依赖仪器的配准；

自动配准： 通过一定的算法或者统计学规律计算两块点云之间的错位，从而达到两块点云自动配准的效果。

问题的关键： 如何让得到的变换参数R+T，使得两个不同时刻视角下测得的三维数据经过坐标变换后的距离最小。

配准算法可分为： 整体配准和局部配准；PCL库中有单独的配准模块，可以使用模块实现ICP算法，如(PCL中ICP实现) 。

• ICP迭代最近点算法

• 非线性ICP 配准对象 逐步匹配多幅点云

• 交互式ICP可视化

• 正态分布变换进行配准

参考链接：

[1]. ICP原理介绍

[2]. pcl整体框架阐述

3. 几个点云匹配和拼接的例子

粗配准+高精度配准

SAC-IA+ICP算法，计算FPFH特征

多副点云配准