视觉残差函数及雅可比公式推导

【约定符号】：
特征点在相机坐标系下的坐标为$[x,y,z{]}^T$；
特征点在归一化相机坐标系下的坐标为$[\mu ,\nu ,1{]}^T$或$[\mu ,\nu {]}^T$
特征点的这两种坐标之间的关系：
$$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\frac{1}{\lambda }\left[\begin{array}{c}\mu \\ \nu \\ 1\end{array}\right]$$
其中，$\lambda =1/z$，称为逆深度。

【定义概念】视觉重投影误差
假设预测的（估计的） 特征点的坐标为$[x,y,z{]}^T$（相机坐标系），观测到的 特征点的坐标为$[\mu ,\nu {]}^T$（归一化相机坐标系），则视觉重投影误差定义为：
$$r_c=\left[\begin{array}{c}\frac{x}{z}-\mu \\ \frac{y}{z}-\nu \end{array}\right]$$
基于以上内容，开始推导。

---

已知第$i$帧中某特征点的坐标$[{\mu }_i,{\nu }_i{]}^T$（归一化相机坐标系）及逆深度${\lambda }_i$，可以预测该特征点在第$j$帧的相机坐标系下的坐标$[x_{c_j},y_{c_j},z_{c_j}{]}^T$为：
$$\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}\right]=T_{bc}^{-1}{T}_{w{b}_j}^{-1}{T}_{w{b}_i}{T}_{bc}\left[\begin{array}{c}\frac{1}{{\lambda }_{c_i}}\mu \\ \frac{1}{{\lambda }_{c_i}}\nu \\ \frac{1}{{\lambda }_{c_i}}\\ 1\end{array}\right]\text{\hspace{1em}(1-1)}$$
【注】关于$T_{w{b}_i}$和$T_{w{b}_j}$，此时我们有一个粗略的值。
同时，该特征点在第$j$帧确实被观测到了，坐标为$[{\mu }_{c_j},{\nu }_{c_j}{]}^T$，则不难构建重投影误差（抄过来）如下：
$$r_c=\left[\begin{array}{c}\frac{x_{c_j}}{z_{c_j}}-{\mu }_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-{\nu }_{c_j}\end{array}\right]\triangleq \left[\begin{array}{c}{r}_{c1}\\ r_{c2}\end{array}\right]$$
这就是残差函数。
残差函数构成损失函数，在使用LM算法优化过程中，需要使用残差函数的Jacobian矩阵（一阶泰勒展开）$\frac{\partial r_c}{\partial state}=\frac{\partial r_c}{\partial f_{c_j}}\cdot \frac{\partial f_{c_j}}{\partial state}$。【具体详见LM算法】

---

求残差函数的Jacobian矩阵
首先，明确$r_c$需要对哪些变量求偏导。
共四大部分：1. $i$时刻的位移和姿态，2. $j$时刻的位移和姿态，3. imu和相机的外参，4. 逆深度。

应用链式法则，$\frac{\partial r_c}{\partial state}=\frac{\partial r_c}{\partial f_{c_j}}\cdot \frac{\partial f_{c_j}}{\partial state}$

第一步，先求$\frac{\partial r_c}{\partial f_{c_j}}$得：
$$\begin{array}{rl}\frac{\partial r_c}{\partial f_{c_j}}& =[\begin{array}{ccc}\frac{\partial r_{c1}}{\partial x_{c_j}}& \frac{\partial r_{c1}}{\partial y_{c_j}}& \partial z_{c_j}\\ \frac{\partial r_{c2}}{\partial x_{c_j}}& \frac{\partial r_{c2}}{\partial y_{c_j}}& \end{array}\\ & \end{array}$$