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 }}=[\begin{array}{c}{x}^{{}^{\prime }}/z^{}\end{array}$