Ceres_Pose Graph Example

一、问题背景

• 个人理解觉得这是一个Bundle Adjustment的过程
• 引用《视觉SLAM十四讲》的一句话“在SLAM中，通常的做法是先使用P3P/EPnP等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）”。
  
  • 这里“对估计值进行调整”可以用g2o，也可以用ceres进行图优化。

二、二维图优化

1. 二维状态包含位置 $p$ （2维）和 角度$\psi$ （1维 弧度制）
2. 残差：a时刻$\rightarrow$b时刻之间预测值与测量值($\hat{p}_{ab},\hat{\psi}_{ab}$)的误差
   
   • $r_{ab} = \left[\begin{matrix} R_a^{T}(p_b -p_a) - \hat{p}_{ab} \\ Normalize(\psi_{b} - \psi_{a} - \hat{\psi_{ab}})\end{matrix}\right]$
   • 【NOTE】
     
     • Normalize()将角度限制在[-$\pi$, $\pi$)之间
     • 需通过测量的不确定性对残差进行加权，即乘以一个协方差矩阵。

三、三维图优化

1. 三维状态包含位置 $p$ （3维向量），方向 $q$ （4维向量 四元数）
2. 残差：a时刻$\rightarrow$b时刻之间测量值（$\hat{p}_{ab},\hat{q}_{ab}$）与预测值的误差。
3. $r_{ab} = \left[\begin{matrix} R(q_{a})^{T}(p_{b} - p_{a}) - \hat{p}_{ab} \\ 2.0vec((q_{a}^{-1}q_{b})\hat{q}_{ab}^{-1}) \end{matrix}\right]$
4. 【NOTE】
   
   • 同样需要乘以协方差矩阵对残差进行加权。
   • 有一点不能理解，就是方向q的残差部分，还是需要补充一下四元数的知识:-)

【补充】四元数（quaternion）

用四个数表示旋转
$\bigstar$ 一维情况：+1表示向右走一个单元，-4表示向左走4个单元。
$\bigstar$二维情况：3+4i表示先向左走3个单元，再向上走4个单元，i可以理解为逆时针转90度。如果你想旋转90度，可以乘以i。
$\bigstar$三维情况：
1. 规则：$i^{2} = j^{2} = k^{2} = ijk = -1$
2. $h = a+bi+cj+dk$ 与 $h^{*} = a-bi-cj-dk$共轭
3. 如果想绕$v(v_{1}, v_{2}, v_{3})$轴旋转$\theta$，则$h = (cos(\frac{\theta}{2}), v_{1}*sin(\frac{\theta}{2}), v_{2}*sin(\frac{\theta}{2}), v_{3}*sin(\frac{\theta}{2}))$
4. 假设想旋转空间点$p(p_{1},p_{2},p_{3})$，四元数为（0,x,y,z），则$h·p·h^{*}$
5. 逆表示反方向旋转。