第六篇 相机位姿优化问题一 ：雅克比矩阵推导

一 相机位姿优化问题背景概述

   在SLAM系统或者自动驾驶系统中有一个很重要的模块，就是定位模块。基于视觉的定位基本思路就是采用将地图中的特征重投影到相机的UV平面，使相同特征之间的重投影误差最小。
   例如，自动驾驶中，高精度地图车道线如下左图所示，车载相机看到的车道线如下右图所示，求解车辆位姿就是将下左图的车道线根据相机位姿和外参投影到相机的UV平面，是它和下游图的线完全重合。

二 相机位姿优化问题解决思路

高精度地图根据估计位姿可以投影到相机UV平面，然后求出每个相机UV观测的点到高精度地图的点线距离。那么每个点就可以观测到一个误差E。
$$\frac{dE}{dV}=J$$
其中 J 是 雅克比矩阵，V 是位姿误差矢量。
$$\Delta V=(J^{T}J{)}^{-1}\ast J^T\ast \Delta E$$
设定位姿初值，然后多次迭代，即可以使重投影误差最小。那么最困难的问题是求出E对V 的雅克比。

三 相机位姿优化雅克比矩阵分步求解

3.1 符号约定

位姿表达式:姿态的等效旋转矢量和位置矢量
$$\begin{gathered} \theta =[\theta 1\theta 2\theta 3] \\ {R}_{mr}=toRotate(\theta ) \\ {t}_{mr}=[xyz] \\ V=[\theta t_{mr}{]}^{\prime } \end{gathered}$$
地图上的点：
$$P_m=[m_x{m}_y{m}_z]$$
车辆载体系下的点：
$$P_r=[r_x{r}_y{r}_z]$$
转到相机坐标系下的点:
$$P_c=[c_x{c}_y{c}_z]$$
转到相机UV平面的点
$$P_{muv}=[u_m{v}_m]$$
相机观测到的UV平面的点
$$P_{cuv}=[u_c{v}_c]$$

3.2 分步求解

$$\begin{gathered} P_m=R_{mr}\ast P_r+t_{mr} \\ {P}_r=R_{mr}^{\prime }(P_m-t_{mr}) \\ {P}_c=R_{cr}\ast P_r+t_{cr} \\ {P}_{muv}=\begin{array}{c}[\frac{c_x}{c_z}\frac{c_y}{c_z}\end{array}{]}^{\prime } \\ E=P_{cuv}-P_{muv} \end{gathered}$$
于是方向逐级求雅克比：
$$J1=\frac{dE}{d{P}_{muv}}=-1$$
$$J2=\frac{d{P}_{muv}}{d{P}_c}=\left[\begin{array}{ccc}\frac{1}{c_z}& 0& -\frac{c_x}{c_z^2}\\ 0& \frac{1}{c_z}& -\frac{c_y}{c_z^2}\end{array}\right]$$
$$J3=\frac{d{P}_c}{d{P}_r}=R_{cr}$$
$$\begin{gathered} J4=\frac{d{P}_r}{dV}=[\frac{d{P}_r}{d\theta }\frac{d{P}_r}{d{t}_{mr}}{]}^{\prime } \\ \frac{d{P}_r}{d{t}_{mr}}=-R_{mr}^{\prime } \\ \frac{d{P}_r}{d\theta }=????? \end{gathered}$$
问号处求导见第六篇文章。
$$J=J1\ast J2\ast J3\ast J4=\frac{dE}{d{P}_{muv}}\ast \frac{d{P}_{muv}}{d{P}_c}\ast \frac{d{P}_c}{d{P}_r}\ast \frac{d{P}_r}{dV}$$

求出误差函数相对于状态量的雅克比后，运用本章最前面的方法求出状态量的误差量，并减去误差量后，重新计算。反复迭代N次收敛。此方法为高斯牛顿法。
还有一个问题没有解决，就是 J4 中求出相对于等效旋转矢量的雅克比如何求？待后续分解。