位姿图学习

基本概念

1. 何为位姿图？（Pose Graph）
   参考BA优化，对路标点与其对应点轨迹进行位姿和路标点优化（特征点、GPS、IMU获得位姿），去除路标只考虑相机之间位姿的联系，进行图优化，构建位姿图。

2. 位姿图的节点和边
   （1）：节点：相机位姿$T_1,...,T_n$
   （2）：边：两节点之间的相对运动（可以来自于GPS、IMU、特征点法、直接点法）

公式推导

1. 估计$T_i,与T_j$之间的运动$\Delta T_{ij}：(李群)$:
   $$\Delta {\xi }_{ij}={\xi }_i^{-1}。{\xi }_j=\ln (T_i^{-1}{T}_j{)}^{\vee }(用李群表示为：T_{ij}=T_i^{-1}{T}_j)$$
2. 构建最小二乘优化位姿
   ${\xi }_i,{\xi }_j:待优化变量$
   $${\hat{e}}_{ij}=\Delta {\xi }_{ij}\ln (T_{ij}^{-1}{T}_i^{-1}{T}_j{)}^{\vee }$$
   证明：
   已知$e^{Ad(T)\xi \times }=T{e}^{\xi \times }{T}^{-1}(注：Ad(T)=\left[\begin{array}{cc}R& t\times R\\ 0& R\end{array}\right])⟺e^{\xi \times }T=T{e}^{Ad(T^{-1})\xi \times }$
3. 求导：
   $\vee :SE(3)\to se(3)$
   $\times :se(3)\to SE(3)$
   $\frac{e_{ij}}{\partial \delta {\xi }_i}=J_r^{-1}(e_{ij})Ad(T_j^{-1})$
   $\frac{\partial e_{ij}}{\partial \delta {\xi }_j}=J_r^{-1}(e_{ij})Ad(T_j^{-1})$
   $J_r^{-1}(e_{ij})\approx I+\frac{1}{2}\left[\begin{array}{cc}{\varphi }_e\times & p_e\times \\ 0& {\varphi }_e\times \end{array}\right]({\varphi }_e=so(hat(SO)),p_e=t)$
   $$\begin{gathered} {\hat{e}}_{ij}=ln(T_{ij}^{-1}{T}_i^{-1}{e}^{-\delta {\xi }_i\times }{T}_j^{-1}{)}^{\vee } \\ =ln(T_{ij}^{-1}{T}_i^{-1}{T}_j(e^{Ad(T_i^{-1})\times {\xi }_i}{)}^{\vee }(e^{Ad(T_j^{-1}{\xi }_j)\times }){)}^{\vee } \\ =ln(T_{ij}^{-1}{T}_i^{-1}{T}_j[I-(Ad(T_i^{-1})\times {)}^{\vee }+(Ad(T_j^{-1})\times )]{)}^{\vee } \\ =e_{ij}+\frac{\partial e_{ij}}{\partial \delta {\xi }_i}\delta {\xi }_i+\frac{\partial e_{ij}}{\partial \delta {\xi }_j}\delta {\xi }_j \end{gathered}$$
4. 目标函数:
   $$min\frac{1}{2}\sum _{i,j\xi }{e}_{ij}^T\underset{ij}{\overset{-1}{\bigvee }}{e}_{ij}$$