VINS ---视觉约束的雅克比推导

1 注

高博SLAM 十四讲 P186 中介绍了点的视觉重投影误差相对各个优化变量的雅克比
这儿做一个大概的公式介绍

1.1 投影误差公式

1. 我们选择定义：（与高博书上相反，VINS确是这样定义的，为了与代码对应，我们也这样）
   误差 = 理论预测值 - 观测值
2. 十四讲 是使用像素平面下的像素坐标做差，我们也为了与VINS对应，使用归一化坐标系的点做差

我们记世界坐标系下一点$P_w=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$ 通过 $T_{wc}$转到相机坐标 $P_c=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$ ，再转到归一化坐标系$p$，$p$为理论预测值:

$$P_c=R_{cw}(P_w+t_{cw})=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$
$$p=\left[\begin{array}{c}x/z\\ y/z\\ 1\end{array}\right]$$
且在相机归一化坐标系下有 $p$ 对应的观测值$p^{{}^{\prime }}=\left[\begin{array}{c}{x}^{{}^{\prime }}/z^{{}^{\prime }}\\ y^{{}^{\prime }}/z^{{}^{\prime }}\\ 1\end{array}\right]$

投影误差即为：
$$e=\left[\begin{array}{c}x/z\\ y/z\end{array}\right]-\left[\begin{array}{c}{x}^{{}^{\prime }}/z^{{}^{\prime }}\\ y^{{}^{\prime }}/z^{{}^{\prime }}\end{array}\right]$$
这样的观测点$p$与路标点$P_w$点对，就能构成一项投影残差，实际情况下，就是有很多对这样的点对，组成多项投影误差，然后我们通过调整相机位姿$T_{wc}$使得整体的误差最小化，这个时候的位姿也就是最优化的位姿，这就是构建最小二乘问题去优化变量。

1.2 投影误差相对于待优化变量的雅克比

待优化的变量即为 相机的位姿$T_{wc}$ 由 旋转与平移构成，可以用李代数 $\xi$ 6维向量表示，也可以用四元数与平移量表示 $\left[\begin{array}{cc}t& Q\end{array}\right]$

1. 十四讲使用的是 李代数表示，误差相对于李代数的雅克比为：
   $$\frac{\partial e}{\partial \delta \xi }=\frac{\partial e}{\partial P_c}\frac{\partial P_c}{\partial \delta \xi }$$
   $$\frac{\partial e}{\partial P_c}={\left[\begin{array}{ccc}\frac{\partial (x/z)}{\partial x}& \frac{\partial (x/z)}{\partial y}& \frac{\partial (x/z)}{\partial z}\\ \frac{\partial (y/z)}{\partial x}& \frac{\partial (y/z)}{\partial y}& \frac{\partial (y/z)}{\partial z}\end{array}\right]}_{2\times 3}={\left[\begin{array}{ccc}1/z& 0& -x/z^2\\ 0& 1/z& -y/z^2\end{array}\right]}_{2\times 3}$$
   $$\frac{\partial P_c}{\partial \delta \xi }={\left[\begin{array}{cc}{I}_{3\times 3}& -\widehat{P_c}\end{array}\right]}_{3\times 6}$$

   注：$\frac{\partial P_c}{\partial \delta \xi }$ 投影点（投影到相机坐标系）关于位姿的雅克比，见书本推导 ；误差函数中 $\left[\begin{array}{c}{x}^{{}^{\prime }}/z^{{}^{\prime }}\\ y^{{}^{\prime }}/z^{{}^{\prime }}\end{array}\right]$ 为观测值，是常数。

2. VINS中的位姿使用的是四元数与平移量表示，误差相对于四元数与平移量的雅克比为：
   $$\frac{\partial e}{\partial T}={\left(\begin{array}{c}\frac{\partial e}{\partial P_c}\frac{\partial P_c}{\partial T}\end{array}\right)}_{2\times 7}$$
   维度变成 $2\times 7$,$\frac{\partial e}{\partial P_c}$ 在上面已经算出
   $${\frac{\partial P_c}{\partial T}}_{3\times 7}=\left[\begin{array}{cc}\frac{\partial P_c}{\partial t}& \frac{\partial P_c}{\partial Q}\end{array}\right]={\left[\begin{array}{cc}{R}_{cw}& -{\widehat{P_c}}_{3\times 3}\end{array}\right]}_{3\times 7}$$
   ${\frac{\partial P_c}{\partial T}}_{3\times 7}$ 的雅克比矩阵维度为 $3\times 7$，但是最后一列全为0 。
   这样就与VINS 对应上了，只不过VINS 中包含IMU 坐标系，还要注意的是这儿讲的是投影误差，并不是重投影误差。

1.3重投影误差公式

通过对上面的投影误差介绍，我们接下来介绍 VINS 中的重投影误差，投影的起点不是从世界坐标系开始，而是从相机i 的归一化平面一点$P_i$ 开始，重投影到相机j 的归一化平面下为$P_i^{{}^{\prime }}$ ，与相机j下的观测值$P_j$ 做差。

1.4重投影误差相对各个待优化变量的雅克比

整个过程中涉及到的变量有：
相机i的位姿，相机j的位姿，相机与IMU的外参，$P_i$ 对应的逆深度
雅克比计算过程和上面的类似，VINs在代码中，预先计算了$\frac{\partial e}{\partial P_c}$,所有的雅克比都是通过 $P_c$作为桥梁，进行链式求导。

并且在对重投影误差乘上了一个协方差

（具体推导后续有空补上）