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

本文主要是介绍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}}$

其中 $\phi^{\wedge}A \phi^{\wedge}$ 是二阶，是相对于一阶的无穷小，这里只讨论一阶的展开且 $\phi$ 本身就是小量，所以直接约掉

$=(A\phi^{\wedge}-\phi^{\wedge}A)·P^{c_{i}}$
$=-AP^{\wedge}\phi+(AP)^{\wedge}\phi$
约掉 $\phi$
$=-AP^{\wedge}+(AP)^{\wedge}$

对逆深度 $\lambda$ 求导

$\frac{\partial p_{j}}{\partial P^{c_{i}}}\frac{\partial P^{c_{i}}}{\partial \lambda}$

前面的 $\frac{\partial p_{j}}{\partial P^{c_{i}}}=R^{T}_{bc}R^{T}_{wb_{j}}R_{wb_{i}}R_{bc}$

$P^{c_{i}}=\frac{1}{\lambda} ·p$ ， $p$ 是归一化相机系下的3D点

$\frac{\partial P^{c_{i}}}{\partial \lambda}=-\frac{1}{\lambda^{2}}·p$

这个 $-\frac{1}{\lambda^{2}}$ 是个系数，移到哪里都可以

零空间漂移处理

优化的时候会固定滑窗中的第一帧的xyz和yaw角，因为IMU约束的是相对位姿，且IMU的4个不可观自由度就是 $y a w 、 x 、 y 、 z$ ，绝对位姿是没有约束的，所以可能会产生在 4自由度的 0 空间漂移的情况，fusion中的GPS就是约束绝对位姿的。

VINS中的固定是先计算第一帧的yaw和xyz的偏移量，然后把后面的帧都减去这个偏移量，偏移回之前的位置，这样的做法就类似ORB中的固定第一帧的位姿，不过这里是减去第一帧的偏移量，其实就是滑窗中的整条轨迹调整回偏移前的位置，这样就保证不受 0 空间的影响。

和 yaw相关的量都会受影响，就是和旋转向量的量都会受影响， $P, V$ 受影响，零偏 $B a, B g$ ，外参 $T b c$ 不受影响

前面会把旋转矩阵变成rpy，然后把yaw的角度差取出来构成新的偏移旋转矩阵，因为只是yaw发生漂移

这篇关于VINS-Mono-后端优化（三：视觉雅可比推导）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

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

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