从零开始学习Slam|ICP原理与应用
ICP全称Iterative Closest Point，翻译过来就是迭代最近点。ICP在点云配准（registration）领域应用的非常广泛.
ICP算法流程
-
首先对于一幅点云中的每个点，在另一幅点云中计算匹配点（最近点）
-
极小化匹配点间的匹配误差，计算位姿
-
然后将计算的位姿作用于点云
-
再重新计算匹配点
-
如此迭代，直到迭代次数达到阈值，或者极小化的能量函数变化量小于设定阈值
求解ICP的三个步骤
- 计算两组点的质心位置 (平均位置)p,p’，然后计算每个点的去质心坐标： q i = p i − p ; q i ′ = p i ′ − p ′ q_i= p_i - p; q_i' = p_i'-p' qi=pi−p;qi′=pi′−p′
- 根据以下优化问题计算旋转矩阵：
这一步的目标是找到一个旋转矩阵 R ∗ 这一步的目标是找到一个旋转矩阵R^* 这一步的目标是找到一个旋转矩阵R∗ ，使得 旋转后的点集 R q i ′ 与点集 q i 之间的距离最小。 旋转后的点集{Rq_i'}与点集 {q_i }之间的距离最小。 旋转后的点集Rqi′与点集qi之间的距离最小。这里的距离是通过计算每个点对之间的欧氏距离的平方和来衡量的：
R ∗ = a r g m i n R 1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2 R^* = argmin_R \frac{1}{2}\sum_{i=1}^{n}\left\| q_i - Rq_i' \right\|^2 R∗=argminR21i=1∑n∥qi−Rqi′∥2,n是点的数量。 - 根据第二步的 R，计算 t：
一旦找到了最优的旋转矩阵 R* ，就可以计算平移向量 t* 。这个向量表示在旋转和平移之后，两个点集的质心之间的相对位置。
平移向量 t* 的计算公式是： t ∗ = p − R ∗ p ′ t^*= p−R^* p' t∗=p−R∗p′ 。这里，p 和 p’ 是原始点集的质心，R* 是找到的最优旋转矩阵。
缺陷
上面介绍是最简单的点和点匹配的ICP算法，实际情况中由于噪声，我们仍需其他方法去使算法更加鲁棒。例如，极小化的误差项包括对应点的点到点的欧式距离，和对应点的点到平面距离,以及极小化对应点的颜色值误差等。2003年的时候，pottman 和Hofer两位大牛的论文中证明了当两幅点云比较接近时，极小化对应点的点到平面距离比点到点距离更接近两个平面之间的真实距离，也就是说计算点到平面的距离更靠谱！
题目
1.
证明：
- 展开等式左边， q i T R q i ′ q_i^T Rq_i' qiTRqi′是一个标量（向量乘以矩阵再乘以向量的结果是标量）
- 展开等式右边 ：
由于 R q i ′ q i T Rq_i'q_i^T Rqi′qiT是一个矩阵，其第i个对角线元素可以表示为： ∑ j = 1 n R i j ( q i ′ q i T ) j i \sum_{j=1}^{n}R_{ij}(q_{i}'q_{i}^T)_{ji} j=1∑nRij(qi′qiT)ji
由于 q i ′ q i T q_i'q_i^T qi′qiT是一个外积,其元素 ( q i ′ q i T ) j i (q_i'q_i^T)_{ji} (qi′qiT)ji
可以表示为：
( q i ′ q i T ) j i = q i ′ j q i , j (q_i'q_i^T)_{ji}=q_{i'j}q_{i,j} (qi′qiT)ji=qi′jqi,j
因此我们可以将 ( R q i ′ q i T ) i i (Rq_i'q_i^T)_{ii} (Rqi′qiT)ii重写为 ( R q i ′ q i T ) i i = ∑ j = 1 n R i j q i ′ , j q i , j (Rq_i'q_i^T)_{ii} = \sum_{j=1}^n R_{ij} q_{i',j} q_{i,j} (Rqi′qiT)ii=j=1∑nRijqi′,jqi,j将这个表达式代入迹的计算中，我们得到：
tr ( R q i ′ q i T ) = ∑ i = 1 n ∑ j = 1 n R i j q i ′ j q i , j \text{tr}(R q_i' q_i^T) = \sum_{i=1}^n \sum_{j=1}^n R_{ij} q_{i'j}q_{i,j} tr(Rqi′qiT)=i=1∑nj=1∑nRijqi′jqi,j这实际上是 q i T R q i ′ q_i^TRq_i' qiTRqi′的展开形式，因为：
q i T R q i ′ = ∑ i = 1 n ∑ j = 1 n q i , i R i j q i ′ j q_i^T R q_i' = \sum_{i=1}^n \sum_{j=1}^n q_{i,i} R_{ij} q_{i'j} qiTRqi′=i=1∑nj=1∑nqi,iRijqi′j
由于矩阵乘法的交换律，我们可以将 q i ′ i 和 q i ′ j q _{i'i}和 q_{i'j } qi′i和qi′j的顺序交换，得到：
q i T R q i ′ = ∑ i = 1 n ∑ j = 1 n R i j q i ′ j q i , j q_i^T R q_i' = \sum_{i=1}^n \sum_{j=1}^n R_{ij} q_{i'j} q_{i,j} qiTRqi′=i=1∑nj=1∑nRijqi′jqi,j
2.
2、 精心设计的ICP编程实践
给定一个轨迹１，数据格式：timestamp tx ty tz qx qy qz qw， 自定义一个任意的旋转矩阵和平移向量（可以尝试不同的值，甚至加一些噪声看看结果有什么变化），对轨迹１进行变换，得到一个新的轨迹２， 使用ICP算法（提示：取平移作为三维空间点）估计轨迹１，２之间的位姿，然后将该位姿作用在轨迹２。
验证：ICP算法估计的旋转矩阵和平移向量是否准确；轨迹１，２是否重合。
如下是我加了一个旋转平移量后的两个轨迹，经过ICP计算好位姿后再反作用在变换后的轨迹，最终两个轨迹是重合滴！
/****************************
* 题目：给定一个轨迹１，数据格式：timestamp tx ty tz qx qy qz qw
* 自定义一个任意的旋转矩阵和平移向量（可以尝试不同的值看看结果有什么变化），对轨迹１进行变换，得到一个新的轨迹２
* 使用ICP算法（提示：取平移作为三维空间点）估计轨迹１，２之间的位姿，然后将该位姿作用在轨迹２
* 验证：ICP算法估计的旋转矩阵和平移向量是否准确；轨迹１，２是否重合
*
* 本程序学习目标：
* 熟悉ICP算法的原理及应用。
*
* 公众号：计算机视觉life。发布于公众号旗下知识星球：从零开始学习SLAM，参考答案请到星球内查看。
* 时间：2019.05
* 作者：小六
****************************/
#include