视觉SLAM理论入门——（9）视觉里程计之特征点法—ICP

假设有一组配对好的 3D 点

想要找一个欧氏变换 R,t，使得：

这个问题可以用迭代最近点（Iterative Closest Point, ICP）求解。ICP求解方法有两种方法：线性代数求解（SVD）、非线性优化求解

 

1、线性代数求解法

定义第 i 对点的误差为

定义两组点的质心：

构建最小二乘问题，求使误差平方和达到极小的 R,t：

 在求和后为 0，因此目标函数简化为：

上式第一项只与 R，有关，第二项与 R，t有关，因此可以从第一项估计 R，然后代入第二项，令其为 0，求得t

将第一项展开：

上式第一项与 R无关，第二项中  ，因此也与R无关，因此对上式的估计可以简化为对下式的估计：

定义 3x3 矩阵  （书上对 W 的定义是这里 W 的转置，最后求解 R 也是这里 R 的转置），为了求 R，对 W 进行SVD分解：

优化问题变为对下面的迹求最大值：

V、R、U都是正交阵，因此 M 也是正交阵，将矩阵展开：

当 W 满秩并且优化问题取最优解（迹最大），这时上式不等号取为等号，那么 ，由于 M 是正交阵，从而 M 只能是单位阵，因此：

求得 R 后，根据下式求解 t：

 

总结线性代数方法求解过程：

1、计算点对的去质心坐标

2、估计旋转矩阵 R

3、根据下式求解平移向量 t

 

 

2、非线性优化方法

以李代数表达位姿时，目标函数可以写成：

求解雅可比矩阵时需要计算 e 对李代数的导数：

 

ICP问题存在唯一解或无穷多解的情况。在唯一解情况下，只要找到极小值解，这个解也是全局最优解，因此求解问题时可以任意选取初值