视觉SLAM位姿优化时误差函数雅克比矩阵的计算

概述

欢迎访问 持续更新：https://cgabc.xyz/posts/6a2809e2/

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

• 记 相机内参矩阵 $\mathbf{K}$，相机位姿 $\mathbf{T}=[\mathbf{R}\mathbf{t}]$ (or $\mathbf{\xi }$)
• 记 $I_1$ 的图像坐标系下，一像素点 $\mathbf{p}(u,v)$；在 $O_1$ 相机坐标系下，其对应的 三维点 $\mathbf{P}(X,Y,Z)$
• 记 $I_2$ 的图像坐标系下，一像素点 ${\mathbf{p}}^{\prime }(u^{\prime },v^{\prime })$；在 $O_2$ 相机坐标系下，其对应的 三维点 ${\mathbf{P}}^{\prime }(X^{\prime },Y^{\prime },Z^{\prime })$，归一化坐标为${{\mathbf{p}}^{\prime }}_{norm}$

$${\mathbf{P}}^{\prime }=\mathbf{R}\cdot \mathbf{P}+\mathbf{t}$$

$${\widetilde{{\mathbf{p}}^{\prime }}}_{norm}=({X^{\prime }}_{norm},{Y^{\prime }}_{norm},1)=\frac{{\mathbf{P}}^{\prime }}{Z^{\prime }}=(\frac{X^{\prime }}{Z^{\prime }},\frac{Y^{\prime }}{Z^{\prime }},1)$$

$$\widetilde{{\mathbf{p}}^{\prime }}=\mathbf{K}\cdot {\widetilde{{\mathbf{p}}^{\prime }}}_{norm}$$

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

$$\underset{\mathbf{\xi }}{\min }\frac{1}{2}{\Vert r(\mathbf{\xi })\Vert }^2$$

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

$$\begin{array}{rl}r(\mathbf{\xi })& ={\mathbf{I}}_2({\mathbf{p}}^{\prime })-{\mathbf{I}}_1(\mathbf{p})\\ & ={\mathbf{I}}_2(u^{\prime },v^{\prime })-{\mathbf{I}}_1(u,v)\end{array}$$

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

$$\begin{array}{rl}r(\mathbf{\xi })& ={\mathbf{p}}^{\prime }-\mathbf{p}\\ & =(u^{\prime },v^{\prime })-(u,v)\end{array}$$

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

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

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

$$\begin{array}{rl}\mathbf{J}(\mathbf{\xi })& =\frac{\partial r(\mathbf{\xi })}{\partial \mathbf{\xi }}\\ & =\frac{\partial r(\mathbf{\xi })}{\partial {\mathbf{p}}^{\prime }}\cdot \frac{\partial {\mathbf{p}}^{\prime }}{\partial {\mathbf{p}}_{norm}^{\prime }}\cdot \frac{\partial {\mathbf{p}}_{norm}^{\prime }}{\partial {\mathbf{P}}^{\prime }}\cdot \frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{\xi }}\\ & ={\mathbf{J}}_0\cdot {\mathbf{J}}_1\cdot {\mathbf{J}}_2\cdot {\mathbf{J}}_3\end{array}$$

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

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

雅克比矩阵 推导

Jacobian 0

$${\mathbf{J}}_0=\frac{\partial r(\mathbf{\xi })}{\partial {\mathbf{p}}^{\prime }}$$

直接法

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

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

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

$$\begin{array}{rl}{\mathbf{J}}_0& =\frac{\partial {\mathbf{I}}_2({\mathbf{p}}^{\prime })}{\partial {\mathbf{p}}^{\prime }}\\ & =[\frac{{\mathbf{I}}_2(u^{\prime }+1,v^{\prime })-{\mathbf{I}}_2(u^{\prime }-1,v^{\prime })}{2},\frac{{\mathbf{I}}_2(u^{\prime },v^{\prime }+1)-{\mathbf{I}}_2(u^{\prime },v^{\prime }-1)}{2}]\end{array}$$

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

特征点法

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

$$r(\mathbf{\xi })={\mathbf{p}}^{\prime }-\mathbf{p}$$

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

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

Jacobian 1

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

由于

$$\widetilde{{\mathbf{p}}^{\prime }}=\mathbf{K}\cdot {\widetilde{{\mathbf{p}}^{\prime }}}_{norm}$$

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

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

$$\mathbf{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$

所以

$${\mathbf{J}}_1=\left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]\in {\mathbb{R}}^{2\times 2}$$

Jacobian 2

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

根据

$${\widetilde{{\mathbf{p}}^{\prime }}}_{norm}=\frac{{\mathbf{P}}^{\prime }}{Z^{\prime }}$$

计算得

$$\begin{array}{rl}{\mathbf{J}}_2& =\left[\begin{array}{ccc}\frac{1}{Z^{\prime }}& 0& -\frac{X^{\prime }}{Z^{\prime 2}}\\ 0& \frac{1}{Z^{\prime }}& -\frac{Y^{\prime }}{Z^{\prime 2}}\end{array}\right]\\ & =\left[\begin{array}{ccc}1& 0& -\frac{X^{\prime }}{Z^{\prime }}\\ 0& 1& -\frac{Y^{\prime }}{Z^{\prime }}\end{array}\right]\cdot \frac{1}{Z^{\prime }}\in {\mathbb{R}}^{2\times 3}\end{array}$$

Jacobian 3

$$\begin{array}{rl}{\mathbf{J}}_3& =\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{\xi }}\\ & =\frac{\partial (\mathbf{T}\cdot \mathbf{P})}{\partial \mathbf{\xi }}\\ & =\frac{\partial (\exp ({\mathbf{\xi }}^{\wedge })\mathbf{P})}{\partial \mathbf{\xi }}\end{array}$$

其中

$$\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\in {\mathbb{R}}^6$$

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

$$\frac{df}{dx}=\underset{\Delta x\to 0}{\lim }\frac{f(x+\Delta x)-f(x)}{\Delta x}$$

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

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

$$\begin{array}{rl}\frac{\partial \widetilde{{\mathbf{P}}^{\prime }}}{\partial \mathbf{\xi }}& =\frac{\partial (\mathbf{T}\cdot \tilde{\mathbf{P}})}{\partial \mathbf{\xi }}\\ & =\frac{\partial (\exp ({\mathbf{\xi }}^{\wedge })\tilde{\mathbf{P}})}{\partial \delta \mathbf{\xi }}\text{(左扰动模型)}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp (\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\tilde{\mathbf{P}}-\exp ({\mathbf{\xi }}^{\wedge })\tilde{\mathbf{P}}}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\tilde{\mathbf{P}}-\exp ({\mathbf{\xi }}^{\wedge })\tilde{\mathbf{P}}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })\tilde{\mathbf{P}}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\cdot \mathbf{P}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}\cdot \mathbf{P}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\cdot \mathbf{P}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]\end{array}$$

所以

$$\begin{array}{rl}{\mathbf{J}}_3& =\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\end{array}\right]\\ & =\left[\begin{array}{cccccc}1& 0& 0& 0& Z^{\prime }& -Y^{\prime }\\ 0& 1& 0& -Z^{\prime }& 0& X^{\prime }\\ 0& 0& 1& Y^{\prime }& -X^{\prime }& 0\end{array}\right]\in {\mathbb{R}}^{3\times 6}\end{array}$$

注意：

• 上面的 $\mathbf{\xi }$ 中 平移 $\mathbf{\rho }$ 在前， 旋转 $\mathbf{\varphi }$ 在后；如果 旋转在前，平移在后，则 ${\mathbf{J}}_3$ 的前三列与后三列须对调。
• ch4 为什么能用左扰动模型来求导啊？[gaoxiang12/slambook Issues #183]

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

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

总结

直接法

$$\begin{array}{rl}\mathbf{J}(\mathbf{\xi })& ={\mathbf{J}}_0\cdot {\mathbf{J}}_1\cdot {\mathbf{J}}_2\cdot {\mathbf{J}}_3\\ & =\frac{\partial {\mathbf{I}}_2({\mathbf{p}}^{\prime })}{\partial {\mathbf{p}}^{\prime }}\cdot \left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]\cdot \left[\begin{array}{ccc}1& 0& -\frac{X^{\prime }}{Z^{\prime }}\\ 0& 1& -\frac{Y^{\prime }}{Z^{\prime }}\end{array}\right]\cdot \left[\begin{array}{cccccc}1& 0& 0& 0& Z^{\prime }& -Y^{\prime }\\ 0& 1& 0& -Z^{\prime }& 0& X^{\prime }\\ 0& 0& 1& Y^{\prime }& -X^{\prime }& 0\end{array}\right]\cdot \frac{1}{Z^{\prime }}\end{array}$$

特征点法

$$\begin{array}{rl}\mathbf{J}(\mathbf{\xi })& ={\mathbf{J}}_0\cdot {\mathbf{J}}_1\cdot {\mathbf{J}}_2\cdot {\mathbf{J}}_3\\ & =\mathbf{I}\cdot \left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]\cdot \left[\begin{array}{ccc}1& 0& -\frac{X^{\prime }}{Z^{\prime }}\\ 0& 1& -\frac{Y^{\prime }}{Z^{\prime }}\end{array}\right]\cdot \left[\begin{array}{cccccc}1& 0& 0& 0& Z^{\prime }& -Y^{\prime }\\ 0& 1& 0& -Z^{\prime }& 0& X^{\prime }\\ 0& 0& 1& Y^{\prime }& -X^{\prime }& 0\end{array}\right]\cdot \frac{1}{Z^{\prime }}\\ & =\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{X^{\prime }{f}_x}{Z^{\prime 2}}& -\frac{X^{\prime }{Y}^{\prime }{f}_x}{Z^{\prime 2}}& f_x+\frac{X^{\prime 2}{f}_x}{Z^{\prime 2}}& -\frac{Y^{\prime }{f}_x}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{Y^{\prime }{f}_y}{Z^{\prime 2}}& -f_y-\frac{Y^{\prime 2}{f}_y}{Z^{\prime 2}}& \frac{X^{\prime }{Y}^{\prime }{f}_y}{Z^{\prime 2}}& \frac{X^{\prime }{f}_y}{Z^{\prime }}\end{array}\right]\end{array}$$

注意事项

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

参考文献

• SLAM优化位姿时，误差函数的雅可比矩阵的推导
• 《视觉SLAM十四讲》