VINS后端非线性优化目标函数
1. 状态变量
vins在后端优化中，使用了滑动窗口，其状态向量包含窗口内的n+1个相机的状态(位置，旋转，速度，加速度计bias及陀螺仪bias)、相机到imu的外参、m+1个路标点的逆深度：
X=\left[x_{0}, x_{1}, \cdots x_{n}, x_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right]
x_{k}=\left[p_{b_{k}}^{w}, v_{b_{k}}^{w} q_{b_{k}}^{w}, b_{a}, b_{g}\right]
x_{c}^{b}=\left[p_{c}^{b}, q_{c}^{b}\right]
2.代价函数
我们建立后端需要优化的代价函数：
\min _{X}\left\{\left\|r_{p}-H_{p} X\right\|^{2}+\sum_{k \in B}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)\right\|_{P_{k+1}^{b_{k}}}^{2}+\sum_{(l, j) \in C}\left\|r_{C}\left(\hat{z}_{l}^{C_{j}}, X\right)\right\|_{P_{l}}^{2} c_{j}\right\}
代价函数中的3个残差项分别对应边缘化先验信息，IMU残差，视觉重投影残差，需要注意的是，三种残差都是使用马氏距离进行表示的(相比欧式距离，多了协方差矩阵)。
我们将IMU残差使用最小二乘法进行求解，当优化变量产生增量后，则有以下公式：
\min _{\delta X}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X+\delta \mathrm{X}\right)\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}=\min _{\delta X}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)+H_{b_{k+1}}^{b_{k}} \delta \mathrm{X}\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}
其中
H_{b_{k+1}}^{b_{k}}为相应Jacobian表达式，我们使用马氏距离公式对上式进行展开，并且求导，令导数为0，则可以得到增量的表达式为：
H_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}}{ }^{-1} H_{b_{k+1}}^{b_{k}} \delta \mathrm{X}=-H_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}}{ }^{-1} r_{B}
由此递推，我们可以得到代价函数的展开式：
\begin{aligned}
\left(\Lambda_{p}\right.&\left.+\sum H_{b_{k+1}}^{b_{k}}{ }^{T} p_{b_{k+1}}^{b_{k}}{ }^{-1} H_{b_{k+1}}^{b_{k}}+\sum H_{l}^{c_{j} T} P_{l}^{c_{j}-1} H_{l}^{c_{j}}\right) \delta \mathrm{X} \\
&=b_{p}+\sum H_{b_{k+1}}^{b_{k}} P_{b_{k+1}}^{b_{k}}{ }^{-1} r_{B}+\sum{ }_{l}^{c_{j}^{T}} P_{l}^{c_{j}^{-1}} r_{C}
\end{aligned}
其中
P_{b_{k+1}}^{b_{k}}为IMU预积分的协方差矩阵，
P_{l}^{c_{j}}为视觉观测的协方差矩阵。当IMU的协方差矩阵越大时，其逆越小，说明此时IMU的数据越来越不可靠，我们应该相信视觉的数据。
我们将上市简化，可以得到后端优化的增量方程：
\left(\Lambda_{p}+\Lambda_{B}+\Lambda_{C}\right) \delta \mathrm{X}=b_{p}+b_{B}+b_{C}
其中，左侧全部为Hessian矩阵。
2.1 IMU残差
使用估计值减去测量值，我们可以得到IMU的残差表达式：
r_{B}^{15 \times 1}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)=\left[\begin{array}{c}
\delta \alpha_{b_{k+1}}^{b_{k}} \\
\delta \theta_{b_{k+1}}^{b_{k}} \\
\delta \beta_{b_{k+1}}^{b_{k}} \\
\delta b_{a} \\
\delta b_{g}
\end{array}\right]=
\left[\begin{array}{c}
R_{w}^{b_{k}}\left(p_{b_{k+1}}^{w}-p_{b_{k}}^{w}-v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)-\alpha_{b_{k+1}}^{b_{k}} \\
2\left[{\gamma_{b_{k+1}}^{b_{k}}}^{-1} \otimes {q_{b_{k}}^{w}}^{-1} \otimes q_{b_{k+1}}^{w}\right]_{x y z} \\
R_{w}^{b_{k}}\left(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w} \Delta t_{k}\right)-\beta_{b_{k+1}}^{b_{k}} \\
b_{a_{b_{k+1}}}-b_{a_{b_{k}}} \\
b_{\omega_{b_{k+1}}}-b_{\omega_{b_{k}}} \\
\end{array}\right]
上述各增量对bias的Jacobian可从IMU预积分的大Jacobian公式相应位置获取。
我们对下面的优化变量开始求导：
\left[\delta p_{b_{k}^{\prime}}^{w} \delta \theta_{b_{k}}^{w}\right],\left[\delta v_{b_{k}^{\prime}}^{w} \delta b_{a_{k^{\prime}}}, \delta b_{\omega_{k}}\right],\left[\delta p_{b_{k+1}}^{w}, \delta \theta_{b_{k+1}}^{w}\right],\left[\delta v_{b_{k+1}}^{w}, \delta b_{a_{k+1}}, \delta b_{\omega_{k+1}}\right]
Jacobian具体推导过程比较简单，全部都是导数的基本性质，只有在对旋转求导时较为麻烦，需要通过导数的定义，四元数的定义，左右乘的性质进行推导：
J[0]^{15 \times 7}=\left[\frac{\partial r_{B}}{\partial \delta p_{b_{k}}^{w}}, \frac{\partial r_{B}}{\partial \delta \theta_{b_{k}}^{w}}\right]=
\left[\begin{array}{ccc}
-R_{w}^{b_{k}} & {\left[R_{w}^{b_{k}}\left(p_{b_{k+1}}^{w}-p_{b_{k}}^{w}-v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)\right]^{\wedge}} \\
0 & -\mathcal{L}\left[q_{b_{k+1}}^{w}-1 \otimes q_{b_{k}}^{w}\right] \mathcal{R}\left[\gamma_{b_{k+1}}^{b_{k}}\right] \\
0 & {\left[R_{w}^{b_{k}}\left(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w} \Delta t_{k}\right)\right]^{\wedge}} \\
0 & 0 \\
0 & 0
\end{array}\right]
J[1]^{15 \times 9}=\left[\frac{\partial r_{B}}{\partial \delta v_{b_{k}}^{W}}, \frac{\partial r_{B}}{\partial \delta b_{a_{k}}}, \frac{\partial r_{B}}{\partial \delta b_{w_{k}}}\right]=
\left[\begin{array}{ccc}
-R_{w}^{b_{k}} \Delta t & -J_{b_{a}}^{\alpha} & -J_{b_{\omega}}^{\alpha} \\
0 & 0 & -\mathcal{L}\left[q_{b_{k+1}}^{w}{ }^{-1} \otimes q_{b_{k}}^{w} \otimes \gamma_{b_{k+1}}^{b_{k}}]\right. J_{b_{\omega}}^{\gamma}\\
-R_{w}^{b_{k}} & -J_{b_{\omega}}^{\beta} & -J_{b_a}^{\beta}\\
0 & -I & 0 \\
0 & 0 & -I \\
\end{array}\right]
J[2]^{15 \times 7}=\left[\frac{\partial r_{B}}{\partial \delta p_{b_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial \delta \theta_{b_{k+1}}^{w}}\right]=\left[\begin{array}{ccc}
R_{w}^{b_{k}} & 0 \\
0 & \mathcal{L}\left[\gamma_{b_{k+1}}^{b_{k}} \otimes q_{b_{k}}^{w-1} \otimes q_{b_{k+1}}^{w}\right] \\
0 & 0 \\
0 & 0 \\
0 & 0
\end{array}\right]
J[3]^{15 \times 9}=\left[\frac{\partial r_{B}}{\partial \delta v_{b+1}^{W}}, \frac{\partial r_{B}}{\partial \delta b_{a_{k+1}}}, \frac{\partial r_{B}}{\partial \delta b_{w_{k+1}}}\right]=\left[\begin{array}{ccc}
0 & 0 & 0 \\
0 & 0 & 0 \\
R_{W}^{b_{k}} & 0 & 0 \\
0 & I & 0 \\
0 & 0 & I
\end{array}\right]
2.2 视觉残差
视觉主要是利用重投影，建立残差，我们直接给出视觉残差公式，推导公式比较简单，利用前后两帧的坐标系关系即可建立等式，不再赘述：
P_{l}^{c_{j}}=R_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{w}\left(R_{c}^{b} \frac{1}{\lambda_{l}} \bar{P}_{l}^{c_{i}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]-p_{c}^{b}\right\}
我们对以下变量进行求导：
\left[\delta p_{b_{i}}^{w}, \delta \theta_{b_{i}}^{w}\right],\left[\delta p_{b_{j}^{\prime}}^{w}, \delta \theta_{b_{j}}^{w}\right],\left[\delta p_{c}^{b}, \delta \theta_{c}^{b}\right], \lambda_{l}
Jacobian具体推导过程不再给出，主要利用了四元数的右乘扰动性质，并且忽略高阶导数。具体表达式如下：
J[0]^{3 \times 6}=\left[\frac{\partial r_{C}}{\partial \delta p_{b_{i}}^{w}}, \frac{\partial r_{C}}{\partial \delta \theta_{b_{i}}^{w}}\right]=\left[R_{b}^{c} R_{w}^{b_{j}} \quad-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w}\left(R_{c}^{b} \frac{1}{\lambda_{l}} \bar{P}_{l}^{c_{i}}+p_{c}^{b}\right)^{\wedge}\right]
J[1]^{3 \times 6}=\left[\frac{\partial r_{C}}{\partial \delta p_{b_{j}}^{w}}, \frac{\partial r_{C}}{\partial \delta \theta_{b_{j}}^{w}}\right]=\left[-R_{b}^{c} R_{w}^{b_{j}} \quad R_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{w}\left(R_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]\right\}^{\wedge}\right]
J[2]^{3 \times 6}=\left[\frac{\partial r_{C}}{\partial \delta p_{c}^{b}}, \frac{\partial r_{C}}{\partial \delta \theta_{c}^{b}}\right]==\left[\begin{array}{c}
R_{b}^{c}\left(R_{w}^{b_{j}} R_{b_{i}}^{w}-I_{3 \times 3}\right) \\
-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b}\left(\frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}}\right)^{\wedge}+\left(R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}}\right)^{\wedge}+\left\{R_{b}^{c}\left[R_{w}^{b_{j}}\left(R_{b_{i}}^{w} p_{c}^{b}+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right)-p_{c}^{b}\right]\right\}^{\wedge}
\end{array}\right]^{T}
J[3]^{3 \times 1}=\frac{\partial r_{C}}{\partial \lambda_{l}}=-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}^{2}}
视觉协方差公式：
\Omega_{\text {vis }}=\Sigma_{\text {vis }}^{-1}=\left(\frac{1.5}{f} I_{2 \times 2}\right)^{-1}=\frac{f}{1.5} I_{2 \times 2}
2.3 边缘化和舒尔补公式
VINS中使用的边缘化为传统的边缘化策略，当有新的帧进来的时候，我们希望删除最老的帧或者次新帧，不希望对这一帧的位姿及路标点再次进行计算，减少计算量，但我们不能直接删除，否则会破坏约束关系，导致求解崩溃，因此我们通过舒尔补公式，保留需要marg那一帧的约束关系。我们通过公式进行说明，将非线性优化公式
H \delta x=b改写为：
\left[\begin{array}{cc}
\Lambda_{a} & \Lambda_{b} \\
\Lambda_{b}^{T} & \Lambda_{c}
\end{array}\right]\left[\begin{array}{l}
\delta x_{a} \\
\delta x_{b}
\end{array}\right]=\left[\begin{array}{l}
g_{a} \\
g_{b}
\end{array}\right]
其中，
x_a与
x_b分别为我们需要merg掉的变量与需要保留的变量，使用舒尔补进行消元：
\left[\begin{array}{cc}
I & 0 \\
-\Lambda_{b}^{T} \Lambda_{a}^{-1} & \mathrm{I}
\end{array}\right]\left[\begin{array}{cc}
\Lambda_{a} & \Lambda_{b} \\
\Lambda_{b}^{T} & \Lambda_{c}
\end{array}\right]\left[\begin{array}{l}
\delta x_{a} \\
\delta x_{b}
\end{array}\right]=\left[\begin{array}{cc}
I & 0 \\
-\Lambda_{b}^{T} \Lambda_{a}^{-1} & \mathrm{I}
\end{array}\right]\left[\begin{array}{l}
g_{a} \\
g_{b}
\end{array}\right]
\Leftrightarrow\left[\begin{array}{cc}
\Lambda_{a} & \Lambda_{b} \\
0 & \Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}
\end{array}\right]\left[\begin{array}{c}
\delta x_{a} \\
\delta x_{b}
\end{array}\right]=\left[\begin{array}{c}
g_{a} \\
g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a}
\end{array}\right]
其中，
\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}就是
\Lambda_{a}在
\Lambda_{b}中的舒尔补项，我们将上式展开得：
\left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right) \delta x_{b}=g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a}
至此，后端非线性优化的代价函数就全部介绍完成，相应的求导，即Jacobian矩阵也全部求解完成，剩下的就需要合理的非线性优化算法根据求得的Jacobian对代价函数进行求解了。
3.边缘化例子
这里使用一个实例对边缘化进行说明，并且从图表中可以清楚的看到矩阵的变化情况，稠密或稀疏。我从网上找了一张示意图，读者请务必理解下图的变化过程。
假设有4个相机位姿，其看到了6个路标点，即特征点，相机与路标点之间表示观测，相机与相机之间表示IMU约束：
下图为上图的详细表示，每一个方块都代表对应的舒尔补项：
然后我们现在marg掉
x_{p_1}，则上图变化如下:
我们可以看到，与
x_{p_1}相关的都从矩阵中移除了，到了左上方，留下的矩阵即
\Lambda_c，我们计算
\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}(黄色)，然后得到
\left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right)的示意图：
我们将上图转成关系图后，如下，可以发现，约束关系变了。
在此基础上，我们merg掉
x_{m_1}后的矩阵如下：
对应的关系为：
从这里我们可以看出，如果不想让矩阵变得稠密，只需要merg掉那些不被观测的路标点即可。
