VINS-Mono-后端优化（三：视觉雅可比推导）

用逆深度是因为这样可以在优化中从优化3个变量降低到1个，降低优化的维度加快求解速度
用逆深度是因为当距离很远的时候， $\frac{1}{x}$ $x$ 就会无穷大，而3D点很近的情况也一般不会有，这也是为了数值稳定性

用逆深度的话就要和其中一帧进行绑定，这个就是和观测到该点的第一帧进行绑定，这样才能表示一个3D点信息

划窗中维护的全部都是IMU下的位姿，所以相机要通过外参变换到IMU坐标系下

在这里插入图片描述
这里就构成了视觉误差，需要求关于优化变量的雅可比矩阵，这里约束了第 $i$ 帧和第 $j$ 帧的 IMU 的姿态，同时还会优化相机和IMU的外参，这个也是紧耦合的特点之一（上一节同时优化 IMU预积分自身的零偏Ba也是紧耦合特点之一），3D点（逆深度）也是要优化，总共就是4个参数

转换公式如下：
第 $i$ 帧归一化坐标系 -> 第 $j$ 帧相机系， $\frac{1}{\lambda}$ ，就是深度， $\lambda$ 是逆深度
在这里插入图片描述
将旋转和平移分开后如下：

将刚刚第 $i$ 帧相机系下的3D点进行归一化，然后和光流追踪到的匹配点进行残差计算，这就获得了视觉重投影误差
在这里插入图片描述

计算残差对优化量的雅可比

有带时间延时估计的雅可比计算和不带时间估计的雅可比计算
这里先讲不带时间延时的雅可比计算

这里的误差项是2维的，坐标点是3维的
要求误差对旋转的雅可比只能通过链式求导的方式
$\frac{\partial r}{\partial p_{j}}·\frac{\partial p_{j}}{\partial x}$

$\frac{\partial r}{\partial p_{j}}$ 是2×3维的，对平移 $x, y, z$ 进行求导
$=\begin{bmatrix}\frac{1}{z}&0&-\frac{x}{z^{2}} \\ 0&\frac{1}{z}&-\frac{y}{z^{2}} \end{bmatrix}$

这里的误差也有协方差矩阵，提点的置信度是设定为1.5个像素不变
残差也得乘上置信度

计算 $p_{j}$ 对 $T$ 的雅可比

平移 $x, y, z$ 的公式为
在这里插入图片描述

$T$ 包含旋转 $R$ 和平移 $t$

对 $i$ 时刻求导

$i$ 时刻的变量也是要优化的量，所以当然也要求导

对 $p_{wb_{i}}$ 求导

$\frac{\partial p_{j}}{\partial p_{wb_{i}}}=R^{T}_{bc}·R^{T}_{wb_{j}}$

对 $R_{wb_{i}}$ 求导

$\frac{\partial p_{j}}{\partial R_{wb_{i}}}$
先把公式中有 $R_{wb_{i}}$ 的项提取出来

$=R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}(R_{bc}P^{c_{i}}+p_{bc})$
$AR_{wb_{i}}b$

后面那一串 $b$ 乘完后是向量，所以可以对李代数进行扰动求导（纯旋转矩阵是不能对李代数求导的，因为矩阵无法对向量求导，这里是乘完后是个向量，所以可以用向量来表示旋转的扰动量，然后用导数的定义来进行求导）

$\frac{\partial AR_{wb_{i}}b}{\partial \phi}=\frac{AR_{wb_{i}}exp(\phi^{\wedge})b-AR_{wb_{i}}b}{\phi}$
$=\frac{AR_{wb_{i}}(I+\phi^{\wedge})b-AR_{wb_{i}}b}{\phi}$
$=\frac{AR_{wb_{i}}\phi^{\wedge}b}{\phi}$
根据反对成矩阵的性质
$=\frac{-AR_{wb_{i}}b^{\wedge}\phi}{\phi}$
$=-AR_{wb_{i}}b^{\wedge}$

那个信息矩阵乘完第一步也得乘进来这里这个第2步的雅可比矩阵

对 $j$ 时刻进行求导

对 $p_{wb_{j}}$ 求导

$\frac{\partial p_{j}}{\partial p_{wb_{j}}}=-R^{T}_{bc}·R^{T}_{wb_{j}}$

对 $R_{wb_{j}}$ 求导

把和 $R_{wb_{j}}$ 有关的项提取出来
$=R^{T}_{bc}R^{T}_{wb_{j}}(R_{wb_{i}}R_{bc}P^{c_{i}}+R_{wb_{i}}p_{bc}+p_{wb_{i}}-p_{wb_{j}})$
$=AR^{T}_{wb_{j}}b$

对 $R^{T}_{wb_{j}}$ 是只能左乘的，但是我们现在要算他右乘的扰动方向，因为方向会不同，用左乘的求导结果加个负号就是右乘的结果，这里推导直接用右乘，所以要加个逆把这个转置消掉来进行推导
$=A(R_{wb_{j}}exp(\phi^{\wedge}))^{-1}b-A(R_{wb_{j}})^{-1}b$
$=A(I-\phi^{\wedge})R^{T}_{wb_{j}}b-A(R_{wb_{j}})^{-1}b$
$=-A\phi^{\wedge}R^{T}_{wb_{j}}b$
$=A(R^{T}_{wb_{j}}b)^{\wedge}\phi$
消去 $\phi$
$=A(R^{T}_{wb_{j}}b)^{\wedge}$

后面的 $R^{T}_{wb_{j}}b$ 实际就是 3D 点在第 $j$ 帧 IMU系下的位姿，按照展开前的刚体变换来理解一下就好了

对 IMU-相机的外参求导$

对 $p_{bc}$ 求导

$=R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}-R^{T}_{bc}$

对 $R_{bc}$ 求导

代码中的 $ric=R_{bc},tic=t_{bc}$ ， $Q=R_{wb}$

导数是符合加法的 $f(x)+g(x))^{'}=f^{'}(x)+g^{'}(x)$

加法后面的求导结果 $=(R^{T}_{bc}b)$ ，这个推导和上面类似，就不详细写了

加法前面的求导稍微复杂一点
$=(R_{bc}exp(\phi^{\wedge}))^{-1}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}exp(\phi^{\wedge})P^{c_{i}}-R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}P^{c_{i}}$

下面暂时省略写后面它自身

$=(I-\phi^{\wedge})R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R^{T}_{bc}(I+\phi^{\wedge})P^{c_{i}}$
$=(I-\phi^{\wedge})A(I+\phi^{\wedge})P^{c_{i}}$
$=(A-\phi^{\wedge}A)(I+\phi^{\wedge})P^{c_{i}}$
$=(A+A\phi^{\wedge}-\phi^{\wedge}A-\phi^{\wedge}A \phi^{\wedge})P^{c_{i}}-AP^{c_{i}}$