vSLAM位姿优化中雅克比矩阵的求解

[TOC]

概述

SLAM，即 同时定位与建图，视觉SLAM的 定位 即 求取相机位姿（旋转和平移 $[\mathbf{R} \quad \mathbf{t}]$）；在SLAM中，我们一般使用 李代数 $\boldsymbol{\xi}$ 来表示 旋转和平移。

• 记 相机内参矩阵 $\mathbf{K}$，相机位姿 $\mathbf{T} = [\mathbf{R} \quad \mathbf{t}]$ (or $\boldsymbol{\xi}$)

• 记 $I_1$ 的图像坐标系下，一像素点 $\mathbf{p}(u,v)$；在 $O_1$ 相机坐标系下，其对应的 三维点 $\mathbf{P}(X,Y,Z)$

• 记 $I2$ 的图像坐标系下，一像素点 $\mathbf{p’}(u’,v’)$；在 $O_2$ 相机坐标系下，其对应的 三维点 $\mathbf{P’}(X’,Y’,Z’)$，归一化坐标为$\mathbf{p’}{norm}$

$$\mathbf{P'} = \mathbf{R} \cdot \mathbf{P} + \mathbf{t}$$ $$\tilde{\mathbf{p'}}_{norm} = ({X'}_{norm},{Y'}_{norm},1) = \frac{\mathbf{P'}}{Z'} = (\frac{X'}{Z'}, \frac{Y'}{Z'}, 1)$$ $$\tilde{\mathbf{p'}} = \mathbf{K} \cdot \tilde{\mathbf{p'}}_{norm}$$

在 优化位姿 时，其思想是构造一个关于位姿变化的误差函数，当这个误差函数最小时，认为此时估计的位姿最优。视觉SLAM主要分为 直接法 和 特征点法，但无论是直接法还是特征点法，位姿的迭代优化都是求解一个 最小二乘问题。

$$\min_{\boldsymbol{\xi}} \frac{1}{2} \left\| r(\boldsymbol{\xi}) \right\|^2$$

• 直接法 最小化 光度误差，即 前后帧像素的灰度误差

$$\begin{aligned} r(\boldsymbol{\xi}) &= \mathbf{I}_2(\mathbf{p}') - \mathbf{I}_1(\mathbf{p}) \\ &= \mathbf{I}_2(u',v') - \mathbf{I}_1(u,v) \end{aligned}$$

• 特征点法 最小化 重投影误差，即地图点到当前图像投影点与匹配点的坐标误差

$$\begin{aligned} r(\boldsymbol{\xi}) &= \mathbf{p}' - \mathbf{p} \\ &= (u',v') - (u,v) \end{aligned}$$

误差函数对于位姿的 雅可比矩阵(Jacobian Matrix)，决定着下一步最优迭代估计时 位姿增量的方向。

$$\mathbf{J}(\boldsymbol{\xi}) = \frac{\partial{r(\boldsymbol{\xi})}}{\partial \boldsymbol{\xi}}$$

根据上面位姿变换的流程，我们可以用 链式法则 来表示 $\mathbf{J}$

$$\begin{aligned} \mathbf{J}(\boldsymbol{\xi}) &= \frac{\partial{r(\boldsymbol{\xi})}}{\partial \boldsymbol{\xi}} \\ &= \frac{\partial{r(\boldsymbol{\xi})}}{\partial \mathbf{p}'} \cdot \frac{\partial \mathbf{p}'}{\partial \mathbf{p}_{norm}'} \cdot \frac{\partial \mathbf{p}_{norm}'}{\partial \mathbf{P'}} \cdot \frac{\partial \mathbf{P'}}{\partial \boldsymbol{\xi}} \\ &= \mathbf{J}_0 \cdot \mathbf{J}_1 \cdot \mathbf{J}_2 \cdot \mathbf{J}_3 \end{aligned}$$

由此，直接法 与 特征点法 雅克比矩阵 只区别于 $\mathbf{J}_0$。

本文主要介绍 SLAM优化位姿时误差函数对位姿雅可比矩阵的推导。

雅克比矩阵 推导

$J_0$

$$\mathbf{J}_0 = \frac{\partial{r(\boldsymbol{\xi})}}{\partial \mathbf{p}'}$$

直接法

我们已知， 在直接法中，单像素点的误差函数是关于像素值的函数，即 光度误差

$$r(\boldsymbol{\xi}) = \mathbf{I}_2(u',v') - \mathbf{I}_1(u,v)$$

由于对于一个特定的像素点，$\mathbf{I}_1(\mathbf{p})$ 是关于 $\boldsymbol{\xi}$ 的常量，所以

$$\begin{aligned} \mathbf{J}_0 &= \frac{\partial \mathbf{I}_2(\mathbf{p}')}{\partial \mathbf{p}'} \\ &= \bigg[ \frac{\mathbf{I}_2(u'+1,v')-\mathbf{I}_2(u'-1,v')}{2}, \frac{\mathbf{I}_2(u',v'+1)-\mathbf{I}_2(u',v'-1)}{2} \bigg] \end{aligned}$$

为 图像 $\mathbf{I}_2$ 在 $\mathbf{p}’$ 点处的 像素梯度

特征点法

我们已知， 在直接法中，单像素点的误差函数是关于像素坐标的函数

$$r(\boldsymbol{\xi}) = \mathbf{p}' - \mathbf{p}$$

由于对于一个特定的像素点，$\mathbf{p}$ 是关于 $\boldsymbol{\xi}$ 的常量，所以

$$\mathbf{J}_0 = \frac{\partial \mathbf{p}'}{\partial \mathbf{p}'} = \mathbf{I} \in \mathbb{R}^{2 \times 2}$$

$J_1$

$$\mathbf{J}_1 = \frac{\partial \mathbf{p}'}{\partial \mathbf{p}_{norm}'} = \frac{\partial (u,v)}{\partial (X_{norm}',Y_{norm}')}$$

由于

$$\tilde{\mathbf{p'}} = \mathbf{K} \cdot \tilde{\mathbf{p'}}_{norm}$$

$\mathbf{J}_1$ 的计算跟 相机投影模型 有关，本文以 针孔相机模型 （不考虑畸变）为例 对其进行计算。

针孔相机模型（不考虑畸变） 的 数学模型 为

$$\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$$

所以

$$\mathbf{J}_1 = \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \in \mathbb{R}^{2 \times 2}$$

$J_2$

$$\begin{aligned} \mathbf{J}_2 &= \frac{\partial \mathbf{p}_{norm}'}{\partial \mathbf{P'}} \\ &= \frac{\partial (X_{norm}',Y_{norm}')}{\partial (X',Y',Z')} \end{aligned}$$

根据

$$\tilde{\mathbf{p'}}_{norm} = \frac {\mathbf{P'}} {Z'}$$

计算得

$$\begin{aligned} \mathbf{J}_2 &= \begin{bmatrix} \frac{1}{Z'} & 0 & -\frac{X'}{Z'^2} \\ 0 & \frac{1}{Z'} & -\frac{Y'}{Z'^2} \end{bmatrix} \\ &= \begin{bmatrix} 1 & 0 & -\frac{X'}{Z'} \\ 0 & 1 & -\frac{Y'}{Z'} \end{bmatrix} \cdot \frac{1}{Z'} \in \mathbb{R}^{2 \times 3} \end{aligned}$$

$J_3$

使用 李代数

$$\begin{aligned} \mathbf{J}_3 &= \frac{\partial \mathbf{P'}}{\partial \boldsymbol{\xi}} \\ &= \frac{\partial (\mathbf{T} \cdot \mathbf{P})}{\partial \boldsymbol{\xi}} \\ &= \frac{\partial ( \exp(\boldsymbol{\xi}^{\wedge}) \mathbf{P}) }{\partial \boldsymbol{\xi}} \end{aligned}$$

其中

$$\boldsymbol{\xi} = \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{bmatrix} \in \mathbb{R}^6$$

类似高数中，求取 $f(x)$ 的导数

$$\frac{df}{dx} = \lim_{\Delta x \rightarrow 0} \frac{f(x+\Delta x) - f(x)}{\Delta x}$$

我们可以 根据李代数加法来对李代数进行求导，计算雅克比矩阵。

一般更使用的，利用李群来左乘或者右乘微小扰动，在对这个扰动的李代数进行求导，利用 扰动模型 $\delta \boldsymbol{\xi} = [\delta \boldsymbol{\rho} \quad \delta \boldsymbol{\phi}]$，计算如下

$$\begin{aligned} \frac{\partial \tilde{\mathbf{P'}}}{\partial \boldsymbol{\xi}} &= \frac{\partial (\mathbf{T} \cdot \tilde{\mathbf{P}})}{\partial \boldsymbol{\xi}} \\ &= \frac{\partial ( \exp(\boldsymbol{\xi}^{\wedge}) \tilde{\mathbf{P}} ) }{\partial \delta \boldsymbol{\xi}} \quad \text{(左扰动模型)} \\ &= \lim_{\delta \boldsymbol{\xi} \rightarrow 0} \frac { \exp(\delta \boldsymbol{\xi}^{\wedge}) \exp(\boldsymbol{\xi}^{\wedge}) \tilde{\mathbf{P}} - \exp(\boldsymbol{\xi}^{\wedge}) \tilde{\mathbf{P}} } { \delta \boldsymbol{\xi} } \\ &\approx \lim_{\delta \boldsymbol{\xi} \rightarrow 0} \frac { (\mathbf{I}+ \delta \boldsymbol{\xi}^{\wedge}) \exp(\boldsymbol{\xi}^{\wedge}) \tilde{\mathbf{P}} - \exp(\boldsymbol{\xi}^{\wedge}) \tilde{\mathbf{P}} } { \delta \boldsymbol{\xi} } \\ &= \lim_{\delta \boldsymbol{\xi} \rightarrow 0} \frac { \delta \boldsymbol{\xi}^{\wedge} \exp(\boldsymbol{\xi}^{\wedge}) \tilde{\mathbf{P}} } { \delta \boldsymbol{\xi} } \\ &= \lim_{\delta \boldsymbol{\xi} \rightarrow 0} \frac { \begin{bmatrix} \delta \boldsymbol{\phi}^{\wedge} & \delta \boldsymbol{\rho} \\ \mathbf{0}^{T} & 0 \end{bmatrix} \begin{bmatrix} \mathbf{R} \cdot \mathbf{P} + \mathbf{t} \\ 1 \end{bmatrix} } { \delta \boldsymbol{\xi} } \\ &= \lim_{\delta \boldsymbol{\xi} \rightarrow 0} \frac { \begin{bmatrix} \delta \boldsymbol{\phi}^{\wedge} (\mathbf{R} \cdot \mathbf{P} + \mathbf{t}) + \delta \boldsymbol{\rho} \\ 0 \end{bmatrix} } { \delta \boldsymbol{\xi} } \\ &= \begin{bmatrix} \mathbf{I} & -(\mathbf{R} \cdot \mathbf{P} + \mathbf{t})^{\wedge} \\ \mathbf{0}^{T} & \mathbf{0}^{T} \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{I} & -\mathbf{P}'^{\wedge} \\ \mathbf{0}^{T} & \mathbf{0}^{T} \end{bmatrix} \end{aligned}$$

所以

$$\begin{aligned} \mathbf{J}_3 &= \begin{bmatrix} \mathbf{I} & -\mathbf{P}'^{\wedge} \end{bmatrix} \\ &= \begin{bmatrix} 1 & 0 & 0 & 0 & Z' & -Y' \\ 0 & 1 & 0 & -Z' & 0 & X' \\ 0 & 0 & 1 & Y' & -X' & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 6} \end{aligned}$$

注意：

• 上面的 $\boldsymbol{\xi}$ 中 平移 $\boldsymbol{\rho}$ 在前， 旋转 $\boldsymbol{\phi}$ 在后；如果 旋转在前，平移在后，则 $\mathbf{J}_3$ 的前三列与后三列须对调。

• ch4 为什么能用左扰动模型来求导啊？[gaoxiang12/slambook Issues #183]

  按照定义，左乘一个扰动，然后令扰动趋于零，求目标函数相对于扰动的变化率，作为导数来使用。同时，在优化过程中，用这种导数算出来的增量，以左乘形式更新在当前估计上，于是使估计值一直在SO(3)或SE(3)上。这种手段称为“流形上的优化”。

• 四元数矩阵与 so(3) 左右雅可比

使用 四元数

若旋转使用 四元数 表示，则更新小量为 $\begin{bmatrix} \delta \mathbf{t} \ \delta \boldsymbol{\theta} \end{bmatrix}$，则

$$\begin{aligned} \mathbf{J}_3 &= \frac{\partial \mathbf{P'}} {\partial \begin{bmatrix} \delta \mathbf{t} \\ \delta \boldsymbol{\theta} \end{bmatrix}} &= \frac{\partial (\mathbf{R} \cdot \mathbf{P} + \mathbf{t})} {\partial \begin{bmatrix} \delta \mathbf{t} \\ \delta \boldsymbol{\theta} \end{bmatrix}} &= \left[ \frac{\partial \mathbf{t}}{\partial \delta \mathbf{t}} \quad \frac{\partial (\mathbf{R} \cdot \mathbf{P})}{\partial \delta \boldsymbol{\theta}} \right] \end{aligned}$$

with

$$\begin{aligned} \frac{\partial (\mathbf{R} \cdot \mathbf{P})}{\partial \delta \boldsymbol{\theta}} &= \lim_{\delta \boldsymbol{\theta} \rightarrow 0} \frac {\exp(\delta \boldsymbol{\theta}^\wedge) \mathbf{R} \mathbf{P} - \mathbf{R} \mathbf{P}} {\delta \boldsymbol{\theta}} \\ &= \lim_{\delta \boldsymbol{\theta} \rightarrow 0} \frac {(\mathbf{I} + \delta \boldsymbol{\theta}^\wedge) \mathbf{R} \mathbf{P} - \mathbf{R} \mathbf{P}} {\delta \boldsymbol{\theta}} \\ &= \lim_{\delta \boldsymbol{\theta} \rightarrow 0} \frac {\delta \boldsymbol{\theta}^\wedge \mathbf{R} \mathbf{P}}{\delta \boldsymbol{\theta}} \\ &= -{(\mathbf{R} \cdot \mathbf{P})}^\wedge \end{aligned}$$

此时

$$\begin{aligned} \mathbf{J}_3 = \left[ \mathbf{I}_{3 \times 3} \quad -{(\mathbf{R} \cdot \mathbf{P})}^\wedge \right] \end{aligned}$$

总结

直接法

$$\begin{aligned} \mathbf{J}(\boldsymbol{\xi}) &= \mathbf{J}_0 \cdot \mathbf{J}_1 \cdot \mathbf{J}_2 \cdot \mathbf{J}_3 \\ &= \frac{\partial \mathbf{I}_2(\mathbf{p}')}{\partial \mathbf{p}'} \cdot \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & -\frac{X'}{Z'} \\ 0 & 1 & -\frac{Y'}{Z'} \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & 0 & 0 & Z' & -Y' \\ 0 & 1 & 0 & -Z' & 0 & X' \\ 0 & 0 & 1 & Y' & -X' & 0 \end{bmatrix} \cdot \frac{1}{Z'} \end{aligned}$$

特征点法

• 使用 李代数

$$\begin{aligned} \mathbf{J}(\boldsymbol{\xi}) &= \mathbf{J}_0 \cdot \mathbf{J}_1 \cdot \mathbf{J}_2 \cdot \mathbf{J}_3 \\ &= \mathbf{I} \cdot \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & -\frac{X'}{Z'} \\ 0 & 1 & -\frac{Y'}{Z'} \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & 0 & 0 & Z' & -Y' \\ 0 & 1 & 0 & -Z' & 0 & X' \\ 0 & 0 & 1 & Y' & -X' & 0 \end{bmatrix} \cdot \frac{1}{Z'} \\ &= \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{X'f_x}{Z'^2} \\ 0 & \frac{f_y}{Z'} & -\frac{Y'f_y}{Z'^2} \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & 0 & 0 & Z' & -Y' \\ 0 & 1 & 0 & -Z' & 0 & X' \\ 0 & 0 & 1 & Y' & -X' & 0 \end{bmatrix} \\ &= \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{X'f_x}{Z'^2} & -\frac{X'Y'f_x}{Z'^2} & f_x+\frac{X'^2f_x}{Z'^2} & -\frac{Y'f_x}{Z'} \\ 0 & \frac{f_y}{Z'} & -\frac{Y'f_y}{Z'^2} & -f_y-\frac{Y'^2f_y}{Z'^2} & \frac{X'Y'f_y}{Z'^2} & \frac{X'f_y}{Z'} \end{bmatrix} \end{aligned}$$

• 使用 四元数

$$\mathbf{J} = \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{X'f_x}{Z'^2} \\ 0 & \frac{f_y}{Z'} & -\frac{Y'f_y}{Z'^2} \end{bmatrix} \cdot \left[ \mathbf{I}_{3 \times 3} \quad -{(\mathbf{R} \cdot \mathbf{P})}^\wedge \right]$$

注意事项

• $\mathbf{J}_1$ 的计算是根据 针孔相机模型（不考虑畸变） 进行计算的
• 本文的 $\boldsymbol{\xi}$ 中 平移 $\boldsymbol{\rho}$ 在前， 旋转 $\boldsymbol{\phi}$ 在后；如果 旋转在前，平移在后，则 $\mathbf{J}_3$ 的前三列与后三列须对调
• 本文定义的 误差函数 $r(\boldsymbol{\xi})$ 为 预测值减观测值；如果定义成 观测值减预测值，本文计算的结果 $\mathbf{J}$ 前须加 负号

参考文献

• SLAM优化位姿时，误差函数的雅可比矩阵的推导

• 《视觉SLAM十四讲》