OKVIS论文公式推导笔记

OKVIS是早期（2015年）开源的基于关键帧的视觉-惯导SLAM方案，是众多SLAM研究者入门必须了解的VIO方案之一，放在今天OKVIS的数据集测试结果仍然非常优秀。这篇博文主要针对OKVIS论文[1]整理其中的理论推导。

定义 §

定义需要估计的机器人状态${\mathbf{x}}_R$：

$${\mathbf{x}}_R:=[{}_W{\mathbf{r}}_S^T,{\mathbf{q}}_{WS}^T,{}_S{\mathbf{v}}^T,{\mathbf{b}}_g^T,{\mathbf{b}}_a^T{]}^T$$

其中${}_W{\mathbf{r}}_S^T\in {\mathbb{R}}^3$是机器人在世界坐标系下的三维位置；单位四元数${\mathbf{q}}_{WS}^T\in S^3$是机器人在世界坐标系下的朝向，对应的旋转矩阵为${\mathbf{C}}_{WS}\in {\mathbb{R}}^{4\times 4}$。进一步将上述状态分为两个部分，位姿部分${\mathbf{x}}_T:=[{}_W{\mathbf{r}}_S^T,{\mathbf{q}}_{WS}^T]$和速度/零偏部分${\mathbf{x}}_{sb}:=[{}_S{\mathbf{v}}^T,{\mathbf{b}}_g^T,{\mathbf{b}}_a^T]$。

考虑相机外参的话，将其定义为${\mathbf{x}}_{C_i}:=[{}_S{\mathbf{r}}_{C_i}^T,{\mathbf{q}}_{S{C}_i}^T{]}^T$，其可以是通过事先校正得到的固定值，也可以当做一个一阶高斯变量来估计，因为外参只受到温度影响。

非线性优化 §

考虑代价函数$\mathbf{F}(\mathbf{x})$，$\mathbf{x}$是待估计的状态，$\mathbf{x}$的最优值可通过最小化代价函数求得：

$${\mathbf{x}}^{\ast }=\underset{x}{\mathrm{argmin}}\mathbf{F}(\mathbf{x})$$

由于代价函数一般是一个复杂的非线性函数，一般从一个初始值出发，不断地更新当前的状态估计值，使得状态估计值收敛。具体为：

1. 将$\mathbf{F}(\mathbf{x})$在状态估计值$\check{\mathbf{x}}$近似为一个容易处理的解析形式；
2. 求解增量$\Delta \mathbf{x}$;
3. 更新当前状态估计值$\check{\mathbf{x}}\leftarrow \check{\mathbf{x}}+\Delta \mathbf{x}$;
4. 迭代，直至状态估计量收敛。

在高斯分布的假设下，代价函数通过最大似然估计可以得到，是一个最小二乘问题。OKVIS中代价函数将视觉+惯导的定位和建图问题建模到一个函数中：

$$\mathbf{F}(\mathbf{x}):=\underset{视觉}{\underbrace{\sum _{i=1}^I\sum _{k=1}^K\sum _{j\in \mathcal{J}(i,k)}{\mathbf{e}}_r^{i,j,kT}{\mathbf{W}}_r^{i,j,k}{\mathbf{e}}_r^{i,j,k}}}+\underset{惯导}{\underbrace{\sum _{k=1}^{K-1}{\mathbf{e}}_s^{kT}{\mathbf{W}}_s^k{\mathbf{e}}_s^k}}$$

其中${\mathbf{e}}_r^{i,j,k}$为重投影误差，${\mathbf{e}}_s^k$为IMU误差项。$i$为系统中的相机下标，$k$为相机采集图像的下标，$j$为路标点的下标，$\mathcal{J}(i,k)$为相机$i$在第$k$帧图像观测到的路标点,$\mathbf{W}$为信息矩阵。谷歌提供了ceres库可以用来求解非线性优化问题。

在流形上进行优化 §

在求解优化问题时，一些状态量往往不在欧式空间中。比如单位四元数不在欧式空间中，其不满足加法交换律，单位四元数加法定义为:

$${\mathbf{q}}_1\oplus {\mathbf{q}}_2={\mathbf{q}}_1\otimes {\mathbf{q}}_2$$

是不可交换的。表达旋转只需要三个自由度，但是四元数有4个自由度，因此需要对四元数增加一个限制。当状态量不在欧式空间时，最小化代价函数求得的增量无法用加法去更新状态估计量[2]。

流形，是局部具有欧式空间性质的空间。在这种情况下，通常是在original space的限制下导出的流形上进行优化。也叫作”optimization in the error-space”, “optimization in the tangent space”, “optimization through local parametrization”。主旨是在minimal tangent space(Euclidean or error space)上进行更新状态量的步骤，从而保证在对应的original space状态量的更新是封闭的。

机器人对应的最小位姿误差状态量为$\delta {\chi }_T=[\delta {\mathbf{p}}^T,\delta {\alpha }^T{]}^T$，路标点误差$\delta \mathbf{l}:=[\delta {\beta }^T,0{]}^T$，考虑相机外参的话，最小误差状态量为$\delta {\chi }_C=[\delta {\mathbf{p}}^T,\delta {\alpha }^T{]}^T$.

重投影误差 §

重投影误差公式为：

$${\mathbf{e}}_{\mathbf{r}}^{j,k}={\mathbf{z}}^{j,k}-\mathbf{h}({\mathbf{T}}_{CS}^k{\mathbf{T}}_{SW}^k{}_W{\mathbf{l}}^j)$$

其中，${}_W{\mathbf{l}}^j\in {\mathbb{R}}^4$ 是第$j$个路标点在世界坐标系下的齐次坐标表示，第四维值为1。下标$k$表示时刻$k$。${\mathbf{z}}^{j,k}\in {\mathbb{R}}^3$表示路标点$j$在时刻$k$图像上的观测值，也是齐次坐标。${\mathbf{T}}_{SW}^k$为变换矩阵，将世界坐标系中的点变换到第$k$次测量时的sensor坐标系中。${\mathbf{T}}_{CS}^k$将第$k$次测量时sensor坐标系中的点变换到相机坐标系中。$\mathbf{h}()$为相机投影模型。以下在公式中省略时刻$k$。

为了进行非线性优化过程以及之后的边缘化步骤，需要求出重投影误差对最小机器人状态误差向量的雅克比矩阵。

变换矩阵的线性化 §

由[3]有旋转矩阵线性化表达为：

$$\mathbf{C}\approx (\mathbf{I}-\delta {\varphi }^{\times })\bar{\mathbf{C}}$$

其中，扰动$\delta \varphi \in {\mathbb{R}}^{3\times 1}$是$\delta \mathbf{q}\in {\mathbb{R}}^{4\times 1}$的最小表示。

变换矩阵

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{C}& \rho \\ 0^T& 1\end{array}\right]$$

其逆代表了逆向变换，

$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{C}}^T& -{\mathbf{C}}^T\rho \\ 0^T& 1\end{array}\right]$$

定义变量$\mathbf{x}$：

$$\mathbf{x}:={\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]}_{6\times 1}$$

对$\mathbf{x}$在$\bar{\mathbf{x}}$进行扰动，

$$\bar{\mathbf{x}}:={\left[\begin{array}{c}\bar{\rho }\\ \bar{\varphi }\end{array}\right]}_{6\times 1}$$

扰动量为：

$$\delta \mathbf{x}:={\left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right]}_{6\times 1}$$

那么有：

$$\begin{array}{rl}\mathbf{T}(\bar{\mathbf{x}}+\delta \mathbf{x})& \approx \left[\begin{array}{cc}\left(\mathbf{I}-\delta {\varphi }^{\times }\right)\bar{\mathbf{C}}& \bar{\rho }+\delta \rho \\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{I}-\delta {\varphi }^{\times }& \delta \rho +\delta {\varphi }^{\times }\bar{\rho }\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}\bar{\mathbf{C}}& \bar{\rho }\\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{I}-\delta {\varphi }^{\times }& \delta \rho +\delta {\varphi }^{\times }\bar{\rho }\\ 0^T& 1\end{array}\right]\bar{\mathbf{T}}\\ & =\left(\mathbf{I}-\left[\begin{array}{cc}\delta {\varphi }^{\times }& -\delta \varrho \\ 0^T& 0\end{array}\right]\right)\bar{\mathbf{T}}\end{array}$$

其中，

$$\delta \varrho :=\delta \rho +\delta {\varphi }^{\times }\bar{\rho }$$

令：

$$\delta \mathbf{t}:=\left[\begin{array}{c}\delta \varrho \\ \delta \varphi \end{array}\right]$$

对$3\times 1$列向量$\mathbf{r}$和$\mathbf{s}$定义运算符$(\cdot {)}^{⊞}$：

$${\left[\begin{array}{c}\mathbf{r}\\ \mathbf{s}\end{array}\right]}^{⊞}:=\left[\begin{array}{cc}{\mathbf{s}}^{\times }& -\mathbf{r}\\ 0^T& 0\end{array}\right]$$

我们有：

$$\mathbf{T}(\bar{\mathbf{x}}+\delta \mathbf{x})\approx (\mathbf{I}-\delta {\mathbf{t}}^{⊞})\bar{\mathbf{T}}$$

类似的，有

$$\begin{array}{rl}\mathbf{T}(\bar{\mathbf{x}}+\delta \mathbf{x}{)}^{-1}& \approx \left[\begin{array}{cc}{\left[(\mathbf{I}-\delta {\varphi }^{\times })\bar{\mathbf{C}}\right]}^T& -{\left[(\mathbf{I}-\delta {\varphi }^{\times })\bar{\mathbf{C}}\right]}^T(\bar{\rho }+\delta \rho )\\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{\bar{\mathbf{C}}}^T(\mathbf{I}+\delta {\varphi }^{\times })& -{\bar{\mathbf{C}}}^T(\mathbf{I}+\delta {\varphi }^{\times })\bar{\rho }-{\bar{\mathbf{C}}}^T\delta \rho \\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{\bar{\mathbf{C}}}^T& -{\bar{\mathbf{C}}}^T\bar{\rho }\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}\mathbf{I}+\delta {\varphi }^{\times }& -(\delta \rho +\delta {\varphi }^{\times }\bar{\rho })\\ 0^T& 1\end{array}\right]\end{array}$$

即：

$$\mathbf{T}(\bar{\mathbf{x}}+\delta \mathbf{x}{)}^{-1}={\bar{\mathbf{T}}}^{-1}(\mathbf{I}+\delta {\mathbf{t}}^{⊞})$$

设点$P$的齐次坐标为$\mathbf{p}$为：

$$\mathbf{p}:=\left[\begin{array}{c}\mathbf{u}\\ s\end{array}\right]$$

对变换矩阵${\mathbf{T}}^{-1}$进行扰动后，有：

$$\begin{array}{rl}{\mathbf{T}}^{-1}\mathbf{p}& \approx {\bar{\mathbf{T}}}^{-1}\mathbf{p}+{\bar{\mathbf{T}}}^{-1}\delta {\mathbf{t}}^{⊞}\mathbf{p}\\ & ={\bar{\mathbf{T}}}^{-1}\mathbf{p}+{\bar{\mathbf{T}}}^{-1}\left[\begin{array}{cc}\delta {\varphi }^{\times }& -\delta \varrho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{u}\\ s\end{array}\right]\\ & ={\bar{\mathbf{T}}}^{-1}\mathbf{p}+{\bar{\mathbf{T}}}^{-1}\left[\begin{array}{c}-s\delta \varrho +\delta {\varphi }^{\times }\mathbf{u}\\ 0\end{array}\right]\\ & ={\bar{\mathbf{T}}}^{-1}\mathbf{p}-{\bar{\mathbf{T}}}^{-1}\left[\begin{array}{cc}s\mathbf{I}& {\mathbf{u}}^{\times }\\ 0^T& 0^T\end{array}\right]\left[\begin{array}{c}\delta \varrho \\ \delta \varphi \end{array}\right]\end{array}$$

定义齐次坐标上的运算符$(\cdot {)}^{\boxminus }$为：

$${\left[\begin{array}{c}\mathbf{u}\\ s\end{array}\right]}^{\boxminus }=\left[\begin{array}{cc}s\mathbf{I}& {\mathbf{u}}^{\times }\\ 0^T& 0^T\end{array}\right]$$

那么有

$${\bar{\mathbf{T}}}^{-1}\delta {\mathbf{t}}^{⊞}\mathbf{p}=-{\bar{\mathbf{T}}}^{-1}{\mathbf{p}}^{\boxminus }\delta \mathbf{t}$$

同样的，有：

$$-\delta {\mathbf{t}}^{⊞}\bar{\mathbf{T}}\mathbf{p}=(\bar{\mathbf{T}}\mathbf{p}{)}^{\boxminus }\delta \mathbf{t}$$

雅克比矩阵 §

在这里需要注意的是，机器人位姿状态量为${\mathbf{x}}_T:=[{}_W{\mathbf{r}}_S^T,{\mathbf{q}}_{WS}^T]$ ，对应的变换矩阵为

$${\mathbf{T}}_{WS}=\left[\begin{array}{cc}{\mathbf{C}}_{WS}& {}_W{\mathbf{r}}_S^T\\ 0^T& 1\end{array}\right]$$

其中${\mathbf{C}}_{WS}$是四元数${\mathbf{q}}_{WS}$对应的旋转矩阵。有${\mathbf{T}}_{SW}={\mathbf{T}}_{WS}^{-1}$，因此重投影误差公式为：

$${\mathbf{e}}_{\mathbf{r}}^{j,k}={\mathbf{z}}^{j,k}-\mathbf{h}({\mathbf{T}}_{SC}^{k-1}{\mathbf{T}}_{WS}^{k-1}{}_W{\mathbf{l}}^j)$$

对其线性化,，设$\delta {\mathbf{t}}_T\in {\mathbb{R}}^{6\times 1}$是${\mathbf{T}}_{WS}$在值${\bar{\mathbf{T}}}_{WS}$的扰动，$\delta {\mathbf{t}}_C\in {\mathbb{R}}^{6\times 1}$是${\mathbf{T}}_{SC}$在值${\bar{\mathbf{T}}}_{SC}$的扰动，$\delta \mathbf{l}\in {\mathbb{R}}^{4\times 1}$是${}_W{\mathbf{l}}^j$在值${}_W{\bar{\mathbf{l}}}^j$的扰动值。对重投影误差进行扰动得到：

$${\mathbf{e}}_{\mathbf{r}}^{j,k}\approx {\mathbf{z}}^{j,k}-\mathbf{h}\left({\bar{\mathbf{T}}}_{CS}^{-1}(\mathbf{I}+\delta {\mathbf{t}}_C^{⊞}){\bar{\mathbf{T}}}_{WS}^{-1}(\mathbf{I}+\delta {\mathbf{t}}_T^{⊞})({}_W{\bar{\mathbf{l}}}^j+\delta \mathbf{l})\right)$$

展开，去除扰动项的积，得到：

$${\mathbf{e}}_{\mathbf{r}}^{j,k}\approx {\mathbf{z}}^{j,k}-\mathbf{h}\left({\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}{}_W{\bar{\mathbf{l}}}^j+{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}\delta \mathbf{l}+{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{WS}^{-1}\delta {\mathbf{t}}_T^{⊞}{}_W{\bar{\mathbf{l}}}^j+{\bar{\mathbf{T}}}_{SC}^{-1}\delta {\mathbf{t}}_C^{⊞}{}_S{\bar{l}}^j\right)$$

由之前推导有：

$${\mathbf{e}}_{\mathbf{r}}^{j,k}\approx {\mathbf{z}}^{j,k}-\mathbf{h}\left({\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}{}_W{\bar{\mathbf{l}}}^j+{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}\delta \mathbf{l}-{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{WS}^{-1}{}_W{\bar{\mathbf{l}}}^{j\boxminus }\delta {\mathbf{t}}_T-{\mathbf{T}}_{SC}^{-1}{}_S{\mathbf{l}}^{j\boxminus }\delta {\mathbf{t}}_C\right)$$

最后令

${\mathbf{J}}_r:=\frac{\delta \mathbf{h}(v)}{\delta v}{|}_{v={\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}{\bar{\mathbf{l}}}^j}$

得到：

$${\mathbf{e}}_{\mathbf{r}}^{j,k}\approx {\mathbf{z}}^{j,k}-\mathbf{h}({\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}{}_W{\bar{\mathbf{l}}}^j)-{\mathbf{J}}_r{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{SW}\delta \mathbf{l}+{\mathbf{J}}_r{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{WS}^{-1}{}_W{\bar{\mathbf{l}}}^{j\boxminus }\delta {\mathbf{t}}_T+{\mathbf{J}}_r{\mathbf{T}}_{SC}^{-1}{}_S{\mathbf{l}}^{j\boxminus }\delta {\mathbf{t}}_C$$

所以有：

$$\begin{array}{rl}\frac{\partial {\mathbf{e}}_{\mathbf{r}}^{j,k}}{\partial \delta {\chi }_T}& =\frac{\partial {\mathbf{e}}_{\mathbf{r}}^{j,k}}{\partial \delta {\mathbf{t}}_T}\frac{\partial \delta {\mathbf{t}}_T}{\partial \delta {\chi }_T}\\ & ={\mathbf{J}}_r{\bar{\mathbf{T}}}_{CS}{\bar{\mathbf{T}}}_{WS}^{-1}{}_W{\bar{\mathbf{l}}}^{j\boxminus }\frac{\partial \delta {\mathbf{t}}_T}{\partial \delta {\chi }_T}\end{array}$$

将上式展开得到：

$$\begin{array}{rl}\frac{\partial {\mathbf{e}}_{\mathbf{r}}^{j,k}}{\partial \delta {\chi }_T}& ={\mathbf{J}}_r{\bar{\mathbf{T}}}_{CS}\left[\begin{array}{cc}{\bar{\mathbf{C}}}_{SW}& -{\bar{\mathbf{C}}}_{SW}{}_W{\bar{\mathbf{r}}}_S\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{}_W{\bar{\mathbf{l}}}_4\mathbf{I}& {}_W{\bar{\mathbf{l}}}_{1:3}^{\times }\\ 0^T& 0^T\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& -{}_W{\bar{\mathbf{r}}}_S^{\times }\\ 0^T& 1^T\end{array}\right]\\ & ={\mathbf{J}}_r{\bar{\mathbf{T}}}_{CS}\left[\begin{array}{cc}{\bar{\mathbf{C}}}_{SW}{}_W{\bar{\mathbf{l}}}_4& {\bar{\mathbf{C}}}_{SW}[{}_W{\bar{\mathbf{l}}}_{1:3}-{}_W{\bar{\mathbf{r}}}_S{}_W{\bar{\mathbf{l}}}_4{]}^{\times }\\ 0^T& 0^T\end{array}\right]\end{array}$$

同理，有

$$\frac{\partial {\mathbf{e}}_{\mathbf{r}}^{j,k}}{\partial \delta {\chi }_C}={\mathbf{J}}_r\left[\begin{array}{cc}{\bar{\mathbf{C}}}_{CS}{}_S{\bar{\mathbf{l}}}_4& {\bar{\mathbf{C}}}_{CS}[{}_W{\bar{\mathbf{l}}}_{1:3}-{}_S{\bar{\mathbf{r}}}_C{}_S{\bar{\mathbf{l}}}_4{]}^{\times }\\ 0^T& 0^T\end{array}\right]$$

显然，

$$\frac{\partial {\mathbf{e}}_{\mathbf{r}}^{j,k}}{\partial \delta {\chi }_L}=-{\mathbf{J}}_r{\bar{\mathbf{T}}}_{CS}\left[\begin{array}{c}{\bar{\mathbf{C}}}_{SW}\\ 0^T\end{array}\right]$$

参考 §

[1] Leutenegger, Stefan, et al. “Keyframe-based visual–inertial odometry using nonlinear optimization.” The International Journal of Robotics Research 34.3 (2015): 314-334.

[2]Course on SLAM Institut de Robòtica i Informàtica industrial [PDF]

[3] Furgale, Paul Timothy. Extensions to the visual odometry pipeline for the exploration of planetary surfaces. Diss. University of Toronto, 2011.