[数据结构与算法]VIO(notes) —— （3）VIO残差构建与IMU预积分

一、VIO残差函数的构建

1. 系统所需的状态变量

为了节约计算量采用滑动窗口形式的 Bundle Adjustment, 在 $i$ 时刻， 滑动窗口内待优化的系统状态量定义如下:
$$\begin{array}{rl}& \mathcal{X}=\left[{\mathbf{x}}_n,{\mathbf{x}}_{n+1},\dots ,{\mathbf{x}}_{n+N},{\lambda }_m,{\lambda }_{m+1},\dots ,{\lambda }_{m+M}\right]\\ & {\mathbf{x}}_i={\left[{\mathbf{p}}_{w{b}_i},{\mathbf{q}}_{w{b}_i},{\mathbf{v}}_i^w,{\mathbf{b}}_a^{b_i},{\mathbf{b}}_g^{b_i}\right]}^{?},i\in [n,n+N]\end{array}$$
其中：

• ${\mathbf{x}}_i$ 包含 $i$ 时刻 IMU 机体的在惯性坐标系中的位置，速度，姿态， 以及 IMU 机体坐标系中的加速度和角速度的偏置量估计。
• $n,m$ 分别是机体状态量, 路标在滑动窗口里的起始时刻。
• $N$ 滑动窗口中关键帧数量。
• $M$ 是被滑动窗口内所有关键帧观测到的路标数量。

2. 视觉重投影误差

2.1 视觉重投影误差

定义: 一个特征点在归一化相机坐标系下的估计值与观测值的差。
$${\mathbf{r}}_c=\left[\begin{array}{l}\frac{x}{z}?u\\ \frac{y}{z}?v\end{array}\right]$$
其中，待估计的状态量为特征点的三维空间坐标 $(x,y,z{)}^{?}$, 观测值 $(u,v{)}^{?}$ 为特征在相机归一化平面的坐标。

2.2 逆深度参数化

特征点在归一化相机坐标系与在相机坐标系下的坐标关系为：
$$?\begin{array}{l}x\\ y\\ z\end{array}?=\frac{1}{\lambda }?\begin{array}{l}u\\ v\\ 1\end{array}?$$
其中 $\lambda =1/z$ 称为逆深度。

• 为什么使用逆深度表示而不是深度值呢？

1. 使用逆深度的缘故是相较于深度值其更符合高斯分布的特性;
2. 深度值一般很大，会影响优化的节奏，使用倒数更能保证数值的稳定性。

2.3 VIO 中基于逆深度的重投影误差

特征点逆深度在第 $i$ 帧中初始化得
到，在第 $j$ 帧又被观测到，预测其 在第 $j$ 中的坐标为:
$$?\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}?={\mathbf{T}}_{bc}^{?1}{\mathbf{T}}_{w{b}_j}^{?1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}?\begin{array}{c}\frac{1}{\lambda }{u}_{c_i}\\ \frac{1}{\lambda }{v}_{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}?$$
其中：
$$\begin{array}{r}T=\left[\begin{array}{ll}R& t\\ 0^T& 1\end{array}\right]\end{array}$$

1. 将i帧中观测到的数据变换到相机坐标系，将相机坐标系变换到body坐标系；
2. 将第i个body坐标系变换到世界坐标系；
3. 将世界坐标系变换到第j个body坐标系；
4. 将body坐标系变换到相机坐标系，得到第j帧的预测值。

这期间相对于纯视觉多了相机坐标系变换到body坐标系，然后再由body坐标系变换回相机坐标系的过程。

视觉重投影误差为:
$$\begin{array}{rl}& {\mathbf{r}}_c=?\begin{array}{l}\frac{x_{c_j}}{z_{c_j}}?u_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}?v_{c_j}\end{array}?\end{array}$$

3. 预积分模型由来及意义

3.1 为什么需要预积分？

IMU的测量值为 $\omega ,b$ ，则有
$$\begin{array}{c}{\tilde{\omega }}^b={\omega }^b+b^g+n^g\\ {\tilde{a}}^b=q_{bw}\left(a^w+g^w\right)+b^a+n^a\end{array}$$
PVQ对时间的导数可写成
$$\begin{array}{c}{\dot{p}}_{w{b}_t}=v_t^w\\ {\dot{v}}_t^w=a_t^w\\ {\dot{q}}_{w{b}_t}=q_{w{b}_t}?\left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right]\end{array}$$
根据上面的导数关系，可以从第 $\mathrm{i}$ 时刻的 $\mathrm{P}\mathrm{V}\mathrm{Q}$ ，通过对 $\mathrm{I}\mathrm{M}\mathrm{U}$ 的测量值进行积分，得到第时刻的 PVQ:
$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t+{?}_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}?{\mathbf{g}}^w\right)\delta t^2\\ & {\mathbf{v}}_j^w={\mathbf{v}}_i^w+{\int }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}?{\mathbf{g}}^w\right)\delta t\\ & {\mathbf{q}}_{w{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}?\left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$

• 问题: 为什么需要预积分？
  每次 $q_{\omega b_t}$ 更新后，都需要重新进行积分，运算量大；我们知道在紧耦合中待优化的状态变量是$p$,$q$,$v$在优化的一次次迭代中这些变量是在不停变化的。由公式可以看到当$b_i$时刻的状态发生改变的时候，我们需要一次次的积分来求取$b_j$时刻的状态，这个是很耗费计算资源的，为了节省计算资源和时间，我们希望能够避免积分部分的重复计算。

3.2 怎么预积分？

一个很简单的公式转换，就可以将积分模型转为预积分模型:
$${\mathbf{q}}_{w{b}_t}={\mathbf{q}}_{w{b}_i}?{\mathbf{q}}_{b_i{b}_t}$$
那么，PVQ 积分公式中的积分项则变成相对于第i 时刻的姿态，而不是相对于世界坐标系的姿态（注意，积分的变化）：
$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t?\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{?}_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t^2\\ & {\mathbf{v}}_j^w={\mathbf{v}}_i^w?{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_{?}}{\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_z{b}_t}{\mathbf{a}}^{b_t}\right)\delta t\\ & {\mathbf{q}}_{w{b}_j}={\mathbf{q}}_{w{b}_i}{\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}?\left[\begin{array}{c}0\\ \frac{1}{2}{\omega }^{b_t}\end{array}\right]\delta t\end{array}$$
这里的预积分量仅仅跟IMU 测量值有关，它将一段时间内的IMU 数据直接积分起来就得到了预积分量:
$$\begin{array}{rl}{\mathbf{\alpha }}_{b_i{b}_j}& ={?}_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t^2\\ {\mathbf{\beta }}_{b_i{b}_j}& ={\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t\\ {\mathbf{q}}_{b_i{b}_j}& ={\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}?\left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$
重新整理下PVQ 的积分公式，有:
$$?\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}?=?\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t?\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\alpha }_{b_i{b}_j}\\ {\mathbf{v}}_i^w?{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_{\mathbf{q}}}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}?$$

3.3 预积分是什么？

预积分其实是加速度和角速度引起的$b_j$时刻的状态量关于$b_i$时刻的增量。
所谓的预积分，就是预先积分好，之后需要的时候拿来用。我们知道IMU测量的是线性加速度和角速度。状态量中速度是关于加速度的积分，位置是关于加速度的二次积分，姿态是关于角速度的积分。因此，在当前坐标系下（因为IMU的测量值就是在当前坐标系下的）把IMU测量值积分好，当需要的是，只需要根据情况进行坐标系转换和加减了。
可以看到预积分与待优化的状态量（忽略零偏）无关，它是一个固定的值。

4. 预积分量方差的计算

预积分误差定义：一段时间内IMU 构建的预积分量作为测量值，对两时刻之间的状态量进行约束
$${?\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}?}_{15\times 1}=?\begin{array}{c}{\mathbf{q}}_{b_{i}w}\left({\mathbf{p}}_{w{b}_j}?{\mathbf{p}}_{w{b}_i}?{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2\right)?{\alpha }_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_j{b}_i}?\left({\mathbf{q}}_{b_{i}w}?{\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{q}}_{b_{i}w}\left({\mathbf{v}}_j^w?{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)?{\beta }_{b_i{b}_j}\\ {\mathbf{b}}_j^a?{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g?{\mathbf{b}}_i^g\end{array}?$$
上面误差中位移,速度,偏置都是直接相减得到。第二顶是关于四元数的旋转误差,其中 [.] xyz 表示只取四元数的虚部 $(x,y,z)$ 组成的三维向量。

5. 预积分离散方法

关于预积分的计算，前面提到过有欧拉法与中值法。这里使用mid-point 方法，即两个相邻时刻k 到 $k+1$ 的位姿是用两个时刻的测量值 $a,w$ 的平均值来计算:
$$\begin{array}{rl}\mathbf{\omega }& =\frac{1}{2}\left(\left({\mathbf{\omega }}^{b_k}?{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}?{\mathbf{b}}_k^g\right)\right)\\ {\mathbf{q}}_{b_i{b}_{k+1}}& ={\mathbf{q}}_{b_i{b}_k}?\left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]\\ \mathbf{a}& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}?{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}?{\mathbf{b}}_k^a\right)\right)\\ {\alpha }_{b_i{b}_{k+1}}& ={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}\mathbf{a}\delta t^2\\ {\beta }_{b_i{b}_{k+1}}& ={\beta }_{b_i{b}_k}+\mathbf{a}\delta t\\ {\mathbf{b}}_{k+1}^a& ={\mathbf{b}}_k^a+{\mathbf{n}}_{{\mathbf{b}}_k^a\delta t}\\ {\mathbf{b}}_{k+1}^g& ={\mathbf{b}}_k^g+{\mathbf{n}}_{{\mathbf{b}}_k^g\delta t}\end{array}$$

6. 预积分量的方差

疑问？：
一个 IMU 数据作为测量值的噪声方差我们能够标定。现在,一段时间内多个 IMU 数据积分形成的预积分量的方差呢?
Covariance Propagation(协方差传播)
举例说明：
已知一个变量 $y=Ax,x\in N\left(0,{\Sigma }_x\right)$, 则有 ${\Sigma }_y=A{\Sigma }_x{A}^{?}$
$$\begin{array}{rl}{\Sigma }_y=& E\left((Ax)(Ax{)}^{?}\right)\\ =& E\left(Ax{x}^{?}{A}^{?}\right)\\ & =A\Sigma x{A}^{?}\end{array}$$
所以,要推导预积分量的协方差,我们需要知道 imu 噪声和预积分量之间的线性递推关系。
假设已知了相邻时刻误差的线性传递方程:
$${\eta }_{ik}=F_{k?1}{\eta }_{ik?1}+G_{k?1}{n}_{k?1}$$
比如: 状态量误差为 ${\eta }_{ik}=\left[\delta {\theta }_{ik},\delta v_{ik},\delta p_{ik}\right]$, 测量唱声为 $n_k=\left[n_k^g,n_k^a\right]$ 。
误差的传递由两部分组成当前时刻的误差 传递给下一时刻,当前时刻测量噪声传递给下一时刻。
一个有趣的例子：
综艺节目中常有传递信息的节目,前一个人根据上一个人的信息 + 自己的理解(测量)传递给下一个人,导致这个信息越传越错。 协方差矩阵可以通过递推计算得到:
$${\Sigma }_{ik}=F_{k?1}{\Sigma }_{ik?1}{F}_{k?1}^{?}+G_{k?1}{\Sigma }_n{G}_{k?1}^T$$
其中, ${\Sigma }_n$ 是测量噪声的协方差矩阵, 方差从 i 时刻开始进行递推, ${\Sigma }_{ii}=0$ 。

7. 状态误差线性递推公式的推导

通常对于状态量之间的递推关系是非线性的方程如 $x_k=f\left(x_{k?1},u_{k?1}\right)$, 其中状态量为 $x,u$ 为系统的输入量。

• 我们可以用两种方法来推导状态误差传递的线性递推关系:

1. 一种是基于一阶泰勒展开的误差递推方程。
2. 一种是基于误差随时间变化的递推方程。

7. 1基于泰勒展开的误差传送(应用于 EKF 的协方差预测)

令状态量为 $x=\hat{x}+\delta x$, 其中, 真值为 $\hat{x}$, 误差为 $\delta x$ 。另外, 输入量 $u$ 的噪声为 $n$ 。
非线性系统 $x_k=f\left(x_{k?1},u_{k?1}\right)$ 的状态误差的线性递推关系如下:
$$\delta x_k=F\delta x_{k?1}+G{n}_{k?1}$$
其中, $\mathrm{F}$ 是状态量 $x_k$ 对状态量 $x_{k?1}$ 的雅克比矩阵, $\mathrm{G}$ 是状态量 $x_k$ 对输入量 $u_{k?1}$ 的雅克比矩阵。
证明:对非线性状态方程进行一阶泰勒展开有:
$$\begin{array}{c}{x}_k=f\left(x_{k?1},u_{k?1}\right)\\ {\hat{x}}_k+\delta x_k=f\left({\hat{x}}_{k?1}+\delta x_{k?1},{\hat{u}}_{k?1}+n_{k?1}\right)\\ {\hat{x}}_k+\delta x_k=f\left({\hat{x}}_{k?1},{\hat{u}}_{k?1}\right)+F\delta x_{k?1}+G{n}_{k?1}\end{array}$$

7.2 基于误差随时间变化的递推方程

如果我们能够推导状态误差随时间变化的导数关系, 比如：
$$\delta x^{\prime }=A\delta x+Bn$$
则误差状态的传递方程为：
$$\begin{array}{c}\delta x_k=\delta x_{k?1}+\delta x_{k?1}^{\prime }\Delta t\\ \to \delta x_k=(I+A\Delta t)\delta x_{k?1}+B\Delta t{n}_{k?1}\end{array}$$
这两种推导方式的可以看出有：
$$F=I+A\Delta t,G=B\Delta t$$

• 第一种方法不是很好么,为什么会随着去弄误差随时间的变化呢？

这是因为 VIO 系统中已经知道了状态的导数和状态之间的转移矩阵。如：我们已经知道速度和状态量之间的关系:
$$\dot{v}=R{a}^b+g$$
那我们就可以推导速度的误差和状态误差之间的关系,再每一项上都加上各自的误差 就有:
$$\begin{array}{c}\dot{v}+\delta \dot{v}=R\left(I+[\delta \theta {]}_{\times }\right)\left(a^b+\delta a^b\right)+(g+\delta g)\\ \delta \dot{v}=R\delta a^b+R[\delta \theta {]}_{\times }\left(a^b+\delta a^b\right)+\delta g\\ \delta \dot{v}=R\delta a^b?R{\left[a^b\right]}_{\times }\delta \theta +\delta g\end{array}$$
由此就能依次类推,轻易写出整个 $A$ 和 $B$ 其他方程了。

8. 预积分的误差递推公式推导

首先回顾预积分的误差递推公式, 将测量噪声也考虑进模型:
$$\begin{array}{rl}\mathbf{\omega }& =\frac{1}{2}\left(\left({\mathbf{\omega }}^{b_k}+{\mathbf{n}}_k^g?{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}+{\mathbf{n}}_{k+1}^g?{\mathbf{b}}_k^g\right)\right)\\ {\mathbf{q}}_{b_i{b}_{k+1}}& ={\mathbf{q}}_{b_i{b}_k}?\left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]\\ \mathbf{a}& =\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}+{\mathbf{n}}_k^a?{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}+{\mathbf{n}}_{k+1}^a?{\mathbf{b}}_k^a\right)\right)\\ {\mathbf{\alpha }}_{b_i{b}_{k+1}}& ={\mathbf{\alpha }}_{b_i{b}_k}+{\mathbf{\beta }}_{b_i{b}_k}\delta t+\frac{1}{2}\mathbf{a}\delta t^2\\ {\mathbf{\beta }}_{b_i{b}_{k+1}}& ={\mathbf{\beta }}_{b_i{b}_k}+\mathbf{a}\delta t\\ {\mathbf{b}}_{k+1}^a& ={\mathbf{b}}_k^a+{\mathbf{n}}_{{\mathbf{b}}_k^a}\delta t\\ {\mathbf{b}}_{k+1}^g& ={\mathbf{b}}_k^g+{\mathbf{n}}_{{\mathbf{b}}_k^g}\delta t\end{array}$$
确定误差传递的状态量,噪声量,然后开始构建传递方程。
用前面一阶泰勒展开的推导方式 $\delta x_k=F\delta x_{k?1}+G{n}_{k?1}$, 我们?望能推导出如下的形式:
$$?\begin{array}{c}\delta {\mathbf{\alpha }}_{b_{k+1}{b}_{k+1}^{\prime }}\\ \delta {\mathbf{\theta }}_{b_{k+1}{b}_{k+1}^{\prime }}\\ \delta {\mathbf{\beta }}_{b_{k+1}{b}_{k+1}^{\prime }}\\ \delta {\mathbf{b}}_{k+1}^a\\ \delta {\mathbf{b}}_{k+1}^g\end{array}?=\mathbf{F}?\begin{array}{c}\delta {\mathbf{\alpha }}_{b_k{b}_k^{\prime }}\\ \delta {\mathbf{\theta }}_{b_k{b}_k^{\prime }}\\ \delta {\mathbf{\beta }}_{b_k{b}_k^{\prime }}\\ \delta {\mathbf{b}}_k^a\\ \delta {\mathbf{b}}_k^g\end{array}?+\mathbf{G}?\begin{array}{c}{\mathbf{n}}_k^a\\ {\mathbf{n}}_k^g\\ {\mathbf{n}}_{k+1}^a\\ {\mathbf{n}}_{k+1}^g\\ {\mathbf{n}}_{{\mathbf{b}}_k^a}\\ {\mathbf{n}}_{{\mathbf{b}}_k^g}\end{array}?$$
$\mathrm{F},\mathrm{G}$ 为两个时刻间的协方差传递矩阵。
这里我们直接给出 $F,G$ 的最终形式,后面会对部分项进行详细推导:
$$\mathbf{F}=?\begin{array}{ccccc}\mathbf{I}& {\mathbf{f}}_{12}& \mathbf{I}\delta t& ?\frac{1}{4}\left({\mathbf{q}}_{b_i{b}_k}+{\mathbf{q}}_{b_i{b}_{k+1}}\right)\delta t^2& {\mathbf{f}}_{15}\\ 0& \mathbf{I}?[\mathbf{\omega }{]}_{\times }& 0& 0& ?\mathbf{I}\delta t\\ 0& {\mathbf{f}}_{32}& \mathbf{I}& ?\frac{1}{2}\left({\mathbf{q}}_{b_i{b}_k}+{\mathbf{q}}_{b_i{b}_{k+1}}\right)\delta t& {\mathbf{f}}_{35}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}?$$
$$\mathbf{G}=?\begin{array}{cccccc}\frac{1}{4}{\mathbf{q}}_{b_i{b}_k}\delta t^2& {\mathbf{g}}_{12}& \frac{1}{4}{\mathbf{q}}_{b_i{b}_{k+1}}\delta t^2& {\mathbf{g}}_{14}& 0& 0\\ 0& \frac{1}{2}\mathbf{I}\delta t& 0& \frac{1}{2}\mathbf{I}\delta t& 0& 0\\ \frac{1}{2}{\mathbf{q}}_{b_i{b}_k}\delta t& {\mathbf{g}}_{32}& \frac{1}{2}{\mathbf{q}}_{b_i{b}_{k+1}}\delta t& {\mathbf{g}}_{34}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}\delta t& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\delta t\end{array}?$$

二、残差Jacobian的推导

1. 视觉重投影残差的Jacobian

视觉残差为：
对于第 $\mathrm{i}$ 帧中的特征点, 它投影到第 $\mathrm{j}$ 帧相机坐标系下的值为：
$$?\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}?={\mathbf{T}}_{bc}^{?1}{\mathbf{T}}_{w{b}_j}^{?1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}?\begin{array}{c}\frac{1}{\lambda }{u}_{c_i}\\ \frac{1}{\lambda }{v}_{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}?$$
拆成三维坐标形式为：
$$\begin{array}{rl}{\mathbf{f}}_{c_j}=?\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\end{array}?& ={\mathbf{R}}_{bc}^{?}{\mathbf{R}}_{w{b}_j}^{?}{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}\frac{1}{\lambda }?\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}?\\ & +{\mathbf{R}}_{bc}^{?}\left({\mathbf{R}}_{w{b}_j}^{?}\left(\left({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i}\right)?{\mathbf{p}}_{w{b}_j}\right)?{\mathbf{p}}_{bc}\right)\end{array}$$
再推导各类 Jacobian 之前, 为了简化公式, 先定义如下变量：
$$\begin{array}{rl}{\mathbf{f}}_{b_i}& ={\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{p}}_{bc}\\ {\mathbf{f}}_w& ={\mathbf{R}}_{w{b}_i}{\mathbf{f}}_{b_i}+{\mathbf{p}}_{w{b}_i}\\ {\mathbf{f}}_{b_j}& ={\mathbf{R}}_{w{b}_j}^{?}\left({\mathbf{f}}_w?{\mathbf{p}}_{w{b}_j}\right)\end{array}$$
Jacobian 为视觉误差对两个时刻的状态量，外参，以及逆深度求导：
$$\mathbf{J}=\left[\begin{array}{l}\frac{?{\mathbf{r}}_c}{?\left[\begin{array}{l}\delta {\mathbf{p}}_{b_i{b}_i^{\prime }}\\ \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]}\frac{?{\mathbf{r}}_c}{?\left[\begin{array}{l}{\mathbf{p}}_{b_j{b}_j^{\prime }}\\ \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}\end{array}\right]}\frac{?{\mathbf{r}}_c}{?\left[\begin{array}{l}\delta {\mathbf{p}}_{c{c}^{\prime }}\\ \delta {\mathbf{\theta }}_{c{c}^{\prime }}\end{array}\right]}\frac{?{\mathbf{r}}_c}{?\delta \lambda }\end{array}\right]$$

参考文献

VIO 中残差雅可比的推导

阅读世界，共赴山海

423全民读书节，邀你共读