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

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

   在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]$$
转到相机坐标系下的点:
Pc=[cxcycz] P_c = [c_x \quad c_y \quad c_z]