《视觉SLAM十四讲》学习笔记-3D-3D位姿估计-ICP

ICP问题

问题描述：假设匹配好的3D点为：

P=p⃗ 1,⋯,p⃗ n,  P′=p⃗ ′1,⋯,p⃗ ′n P = p → 1 , ⋯ , p → n ,     P ′ = p → 1 ′ , ⋯ , p → n ′

想要找一个变换 R,t⃗  R , t → ， s.t.:

∀i,p⃗ i=Rp⃗ ′i+t⃗  ∀ i , p → i = R p → i ′ + t →

此问题可用 迭代最近点来求解(Iterative Closest Point, ICP). 为描述方便， ICP统指匹配好的两组点间运动估计问题。

ICP的求解主要有线性代数的求解(SVD)和非线性优化的求解(BA).

线性的方法：SVD

首先定义第 i i 对点的误差项为:

e⃗ i=p⃗ i−(R) e → i = p → i − ( R )

首先定义第 i i 对的误差项：

e⃗ i=p⃗ i−Rp⃗ ′i+t⃗  e → i = p → i − R p → i ′ + t →

则其最小二乘问题为：

minR,t⃗  J=12∑i=1n∥p⃗ i−(Rp⃗ ′i+t⃗ )∥22 min R , t →   J = 1 2 ∑ i = 1 n ‖ p → i − ( R p → i ′ + t → ) ‖ 2 2

为解上式，定义两组点的质心为：

p⃗ =1n∑i=1np⃗ i,  p⃗ ′=1n∑i=1np⃗ ′i p → = 1 n ∑ i = 1 n p → i ,     p → ′ = 1 n ∑ i = 1 n p → i ′

展开误差函数：

12∑i=1n∥p⃗ i−(Rp⃗ ′i+t⃗ )∥22=12∑i=1n∥p⃗ i−Rp⃗ ′i−t⃗ −p⃗ +Rp⃗ ′+p⃗ −Rp⃗ ′∥22=12∑i=1n∥(p⃗ i−p⃗ −R(p⃗ ′i−p⃗ ′))+(p⃗ −Rp⃗ ′−t⃗ )∥22=12∑i=1n∥(p⃗ i−p⃗ −R(p⃗ ′i−p⃗ ′))∥22+∥p⃗ −Rp⃗ ′−t⃗ ∥22  +2(p⃗ i−p⃗ −R(p⃗ ′i−p⃗ ′))⊤(p⃗ −Rp⃗ ′−t⃗ ) 1 2 ∑ i = 1 n ‖ p → i − ( R p → i ′ + t → ) ‖ 2 2 = 1 2 ∑ i = 1 n ‖ p → i − R p → i ′ − t → − p → + R p → ′ + p → − R p → ′ ‖ 2 2 = 1 2 ∑ i = 1 n ‖ ( p → i − p → − R ( p → i ′ − p → ′ ) ) + ( p → − R p → ′ − t → ) ‖ 2 2 = 1 2 ∑ i = 1 n ‖ ( p → i − p → − R ( p → i ′ − p → ′ ) ) ‖ 2 2 + ‖ p → − R p → ′ − t → ‖ 2 2     + 2 ( p → i − p → − R ( p → i ′ − p → ′ ) ) ⊤ ( p → − R p → ′ − t → )

上式中， (p⃗ i−p⃗ −R(p⃗ ′i−p⃗ ′)) ( p → i − p → − R ( p → i ′ − p → ′ ) ) 在求和后为0，可以上式可简化为：

minR,t⃗  J=12∑i=1n∥(p⃗ i−p⃗ −R(p⃗ ′i−p⃗ ′))∥22+∥p⃗ −Rp⃗ ′−t⃗ ∥22 min R , t →   J = 1 2 ∑ i = 1 n ‖ ( p → i − p → − R ( p → i ′ − p → ′ ) ) ‖ 2 2 + ‖ p → − R p → ′ − t → ‖ 2 2

可以看到，左边只和旋转 R R 有关，右边有 R R 和 t⃗  t → 。所以ICP的求解可拆分为三个步骤：

计算两组点的质心 p⃗  p → 和 p⃗ ′ p → ′ ,再计算去质心坐标：

q⃗ i=p⃗ i−p⃗ ,q⃗ ′i=p⃗ ′i−p⃗ ′ q → i = p → i − p → , q → i ′ = p → i ′ − p → ′

(实际上是上式引入中间变量)
求解优化问题：

R∗=argminR J=12∑i=1n∥q⃗ i−Rq⃗ ′i∥22 R ∗ = arg ⁡ min R   J = 1 2 ∑ i = 1 n ‖ q → i − R q → i ′ ‖ 2 2

再根据第二步的 R R , 计算 t⃗  t → :

t⃗ ∗=p⃗ −Rp⃗ ′ t → ∗ = p → − R p → ′

展开关于 R R 的误差项，得到：

12∑i=1n∥q⃗ i−Rq⃗ ′i∥22=12∑i=1nq⃗ ⊤iq⃗ i+q⃗ ′⊤iR⊤Rq⃗ ′i−2q⃗ ′⊤iRq⃗ ′i 1 2 ∑ i = 1 n ‖ q → i − R q → i ′ ‖ 2 2 = 1 2 ∑ i = 1 n q → i ⊤ q → i + q → i ′ ⊤ R ⊤ R q → i ′ − 2 q → i ′ ⊤ R q → i ′

显然第一项与 R R 无关， R⊤R=I R ⊤ R = I , 第二项亦与 R R 无关，所以目标可简化为：

∑i=1n−q⃗ ′⊤iRq⃗ ′i=∑i=1n−tr(Rq⃗ ′iq⃗ ⊤i)=−tr(R∑i=1nq⃗ ′iq⃗ ⊤i) ∑ i = 1 n − q → i ′ ⊤ R q → i ′ = ∑ i = 1 n − t r ( R q → i ′ q → i ⊤ ) = − t r ( R ∑ i = 1 n q → i ′ q → i ⊤ )

定义矩阵:

W=∑i=1nq⃗ iq⃗ ′⊤i W = ∑ i = 1 n q → i q → ′ i ⊤

所以 W W 是一个 3×3 3 × 3 的矩阵，对其进行SVD分解得到：

W=UΣV⊤ W = U Σ V ⊤

当 W W 为满秩时, R R 为：

R=UV⊤ R = U V ⊤

解出 R R 后，代入即可求出 t⃗  t → .

非线性的优化方法

非线性优化的思想是用迭代的方式去寻找最优值。当以李代数表达位姿时，目标函数改写为：

minξ12∑i=1n∥(p⃗ i−exp(ξ∧)p⃗ ′i)∥22 min ξ 1 2 ∑ i = 1 n ‖ ( p → i − exp ⁡ ( ξ ∧ ) p → i ′ ) ‖ 2 2

它关于位姿导数已推导过，使用李代数扰动模型：

∂e⃗ ∂δξ=−(exp(ξ∧)p⃗ ′i)⊕ ∂ e → ∂ δ ξ = − ( exp ⁡ ( ξ ∧ ) p → i ′ ) ⊕

• ICP问题存在唯一解和无穷多解的情况
• 若存在唯一解，极小值点即为全局最优解
• 在匹配已知的情况下，最小二乘实际上具有解析解，没有必要用迭代优化
• 可以混合使用PnP和ICP优化
  
  • 深度值已知的点，用建模它们的3D-3D误差
  • 深度值未知的点，用建模3D-2D的重投影误差