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

【约定符号】：
特征点在相机坐标系下的坐标为$[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}}& =\left[\begin{array}{ccc}\frac{\partial r_{c1}}{\partial x_{c_j}}& \frac{\partial r_{c1}}{\partial y_{c_j}}& \frac{\partial r_{c1}}{\partial z_{c_j}}\\ \frac{\partial r_{c2}}{\partial x_{c_j}}& \frac{\partial r_{c2}}{\partial y_{c_j}}& \frac{\partial r_{c2}}{\partial z_{c_j}}\end{array}\right]\\ & =\left[\begin{array}{ccc}\frac{1}{z_{c_j}}& 0& -\frac{x_{c_j}}{z_{c_j}^2}\\ 0& \frac{1}{z_{c_j}}& -\frac{y_{c_j}}{z_{c_j}^2}\end{array}\right]\end{array}$$

第二步：求$\frac{\partial f_{c_j}}{\partial state}$。

在开始第二部分的求导之前，对$f_{c_j}$做一些等价变形。
公式（1-1）的等价形式：公式（1-2） 将四维齐次形式改写，拆成三维形式，并做一些符号简化：
$$\begin{array}{rl}{f}_{c_j}& \triangleq \left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\end{array}\right]\\ & =R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}\frac{1}{{\lambda }_{c_i}}\left[\begin{array}{c}{\mu }_{c_j}\\ {\nu }_{c_i}\\ 1\end{array}\right]\\ & +R_{bc}^T(R_{w{b}_j}^T((R_{w{b}_i}{p}_{bc}+p_{w{b}_i})-p_{w{b}_j})-p_{bc})\end{array}\text{\hspace{1em}(1-2)}$$

$$\begin{array}{rl}{f}_{b_i}& \triangleq R_{bc}{f}_{c_i}+p_{bc}\\ f_w& \triangleq R_{w{b}_i}{f}_{b_i}+p_{w{b}_i}\\ f_{b_j}& \triangleq R_{w{b}_j}^T(f_w-p_{w{b}_j})\\ f_{c_j}& \triangleq R_{bc}^T(f_{b_j}-p_{bc})\end{array}$$
不难看出，上面四个式子依次给出了特征点在$c_i,b_i,w,b_j,c_j$坐标系下的坐标。将四个式子依次从上到下代入，展开即可得到公式（1-2）的结果。

问： $p_{w{c}_j}$与$f_{c_j}$含义相同吗？
答：不相同，$f_{c_j}$表示特征点在$c_j$相机坐标系下的坐标；
$p_{w{c}_j}$表示相机$c_j$在世界坐标系下的坐标！

已知公式（1-2）：
$$\begin{array}{rl}{f}_{c_j}& =R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}\\ & +R_{bc}^T(R_{w{b}_j}^T((R_{w{b}_i}{p}_{bc}+p_{w{b}_i})-p_{w{b}_j})-p_{bc})\end{array}$$
1.1 $i$时刻的位移：
即$p_{w{b}_i}:=p_{w{b}_i}+\delta p_{b_i{b}_i^{\prime }}$，不难写出：
$$\frac{\partial f_{c_j}}{\partial \delta p_{b_i{b}_i^{\prime }}}=R_{bc}^T{R}_{w{b}_j}^T$$

1.2 $i$时刻的姿态：
即$R_{w{b}_i}:=R_{w{b}_i}(I+[\delta {\theta }_{b_i{b}_i^{\prime }}{]}_{\times })$
$f_{c_j}$中与$R_{w{b}_i}$有关的项有两部分，可合成简化为：
$$\begin{array}{rl}{f}_{c_j}& =R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}+R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{p}_{bc}+(...)\\ & =R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{f}_{b_i}+(...)\end{array}$$
则：
$$\begin{array}{rl}\frac{\partial f_{c_j}}{\partial \delta {\theta }_{b_i{b}_i^{\prime }}}& =\frac{R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}(I+[\delta {\theta }_{b_i{b}_i^{\prime }}{]}_{\times })f_{b_i}}{\delta {\theta }_{b_i{b}_i^{\prime }}}\\ & =-R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}[f_{b_i}{]}_{\times }\end{array}$$
【注】这里有一个写法上的简化。

2.1 $j$时刻的位移：
即$p_{w{b}_j}:=p_{w{b}_j}+\delta p_{b_j{b}_j^{\prime }}$，不难写出：
$$\frac{\partial f_{c_j}}{\partial \delta p_{b_j{b}_j^{\prime }}}=-R_{bc}^T{R}_{w{b}_j}^T$$

2.2 $j$时刻的姿态：
即$R_{w{b}_j}:=R_{w{b}_j}(I+[\delta {\theta }_{b_j{b}_j^{\prime }}{]}_{\times })$
$f_{c_j}$中与$R_{w{b}_j}$有关的项有两部分，可合成简化为：
$$\begin{array}{rl}{f}_{c_j}& =R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}\\ & +R_{bc}^T(R_{w{b}_j}^T((R_{w{b}_i}{p}_{bc}+p_{w{b}_i})-p_{w{b}_j})-p_{bc})\\ & =R_{bc}^T{R}_{w{b}_j}^T(R_{w{b}_i}(R_{bc}{f}_{c_i}+p_{bc})+p_{w{b}_i}-p_{w{b}_j})+(...)\\ & =R_{bc}^T{R}_{w{b}_j}^T(f_w-p_{w{b}_j})+(...)\end{array}$$
则：
$$\begin{array}{rl}\frac{\partial f_{c_j}}{\partial \delta {\theta }_{b_j{b}_j^{\prime }}}& =\frac{R_{bc}^T[R_{w{b}_j}(I+[\delta {\theta }_{b_j{b}_j^{\prime }}{]}_{\times }){]}^T(f_w-p_{w{b}_j})}{\delta {\theta }_{b_j{b}_j^{\prime }}}\\ & =\frac{R_{bc}^T(I-[\delta {\theta }_{b_j{b}_j^{\prime }}{]}_{\times })R_{w{b}_j}^T(f_w-p_{w{b}_j})}{\delta {\theta }_{b_j{b}_j^{\prime }}}\\ & =\frac{R_{bc}^T(I-[\delta {\theta }_{b_j{b}_j^{\prime }}{]}_{\times })f_{b_j}}{\delta {\theta }_{b_j{b}_j^{\prime }}}\\ & =R_{bc}^T[f_{b_j}{]}_{\times }\end{array}$$

3.1 imu和相机之间外参中的位移：
即$p_{bc}:=p_{bc}+\delta p_{c{c}^{\prime }}$，不难写出：
$$\frac{\partial f_{c_j}}{\partial \delta p_{c{c}^{\prime }}}=R_{bc}^T(R_{w{b}_j}^T{R}_{w{b}_j}^T-I_{3\times 3})$$
3.2 imu和相机之间外参中的姿态：
即$R_{bc}:=R_{bc}(I+[\delta {\theta }_{c{c}^{\prime }}{]}_{\times })$
$f_{c_j}$中与$R_{c{c}^{\prime }}$有关的项有两部分，且不容易简化，故分为两部分求解：
第一部分：
$$f_{c_j}^{[1]}\triangleq R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}$$
则：
$$\begin{array}{rl}\frac{\partial f_{c_j}^{[1]}}{\partial \delta {\theta }_{c{c}^{\prime }}}& =\frac{(I-[\delta {\theta }_{c{c}^{\prime }}{]}_{\times })R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}(I+[\delta {\theta }_{c{c}^{\prime }}{]}_{\times })f_{c_i}}{\delta {\theta }_{c{c}^{\prime }}}\\ & \approx \frac{-[\delta {\theta }_{c{c}^{\prime }}{]}_{\times }{R}_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}+R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}[\delta {\theta }_{c{c}^{\prime }}{]}_{\times }{f}_{c_i}}{\delta {\theta }_{c{c}^{\prime }}}\\ & =[R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}{]}_{\times }-R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}[f_{c_i}{]}_{\times }\end{array}$$
第二部分：
$$f_{c_j}^{[2]}=R_{bc}^T(R_{w{b}_j}^T((R_{w{b}_i}{p}_{bc}+p_{w{b}_i})-p_{w{b}_j})-p_{bc})$$
则：
$$\begin{array}{rl}\frac{\partial f_{c_j}^{[2]}}{\partial \delta {\theta }_{c{c}^{\prime }}}& =\frac{(I-[\delta {\theta }_{c{c}^{\prime }}{]}_{\times })R_{bc}^T(R_{w{b}_j}^T((R_{w{b}_i}{p}_{bc}+p_{w{b}_i})-p_{w{b}_j})-p_{bc})}{\delta {\theta }_{c{c}^{\prime }}}\\ & =[R_{bc}^T(R_{w{b}_j}^T((R_{w{b}_i}{p}_{bc}+p_{w{b}_i})-p_{w{b}_j})-p_{bc}){]}_{\times }\end{array}$$
两部分相加，即$\frac{\partial f_{c_j}}{\partial \delta {\theta }_{c{c}^{\prime }}}=\frac{\partial f_{c_j}^{[1]}}{\partial \delta {\theta }_{c{c}^{\prime }}}+\frac{\partial f_{c_j}^{[2]}}{\partial \delta {\theta }_{c{c}^{\prime }}}$

4.逆深度：
即${\lambda }_{c_i}:={\lambda }_{c_i}+\delta {\lambda }_{c_i}$，$f_{c_j}$中仅$f_{c_i}$与${\lambda }_{c_i}$有关，链式法则$\frac{\partial f_{c_j}}{\partial \delta {\lambda }_{c_i}}=\frac{\partial f_{c_j}}{\partial \delta f_{c_i}}\cdot \frac{\partial f_{c_i}}{\partial \delta {\lambda }_{c_i}}$：
其中，
$$f_{c_j}=R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}{f}_{c_i}$$
则：
$$\frac{\partial f_{c_j}}{\partial \delta f_{c_i}}=R_{bc}^T{R}_{w{b}_j}^T{R}_{w{b}_i}{R}_{bc}$$
又有，
$$f_{c_i}=\frac{1}{{\lambda }_{c_i}}\left[\begin{array}{c}{\mu }_{c_j}\\ {\nu }_{c_i}\\ 1\end{array}\right]$$
则：
$$\frac{\partial f_{c_i}}{\partial \delta {\lambda }_{c_i}}=-\frac{1}{{\lambda }_{c_i}^2}\left[\begin{array}{c}{\mu }_{c_j}\\ {\nu }_{c_i}\\ 1\end{array}\right]=-\frac{1}{{\lambda }_{c_i}}{f}_{c_i}$$
至此，推导完成！

最后

以上就是可靠小熊猫最近收集整理的关于视觉残差函数及雅可比公式推导的全部内容，更多相关视觉残差函数及雅可比公式推导内容请搜索靠谱客的其他文章。