g2o EdgeSE3Expmap pose graph优化代码分析和雅克比推导

主要代码：
g2o/types/sba/types_six_dof_expmap.h
g2o/types/sba/types_six_dof_expmap.cpp

基本结构

SE3Quat包括四元数和vector3

相机位姿顶点

VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {
}

路标3D点顶点

VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3>(){
}

位姿–位姿边

EdgeSE3Expmap::EdgeSE3Expmap() :
  BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>() {
}

线性化：

void EdgeSE3Expmap::linearizeOplus() {
  VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
  SE3Quat Ti(vi->estimate());

  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat Tj(vj->estimate());

  const SE3Quat & Tij = _measurement;
  SE3Quat invTij = Tij.inverse();

  SE3Quat invTj_Tij = Tj.inverse()*Tij;
  SE3Quat infTi_invTij = Ti.inverse()*invTij;

  _jacobianOplusXi = invTj_Tij.adj();
  _jacobianOplusXj = -infTi_invTij.adj();
}

误差模型：

    void computeError()  {
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
      const VertexSE3Expmap* v2 = static_cast<const VertexSE3Expmap*>(_vertices[1]);

      SE3Quat C(_measurement);
      SE3Quat error_= v2->estimate().inverse()*C*v1->estimate();
      _error = error_.log();
    }

推导：

注：1.Twc camera to world，但是g2o里面setEstimate用的是Tcw，setMeasurement用的是Tji。ORB中的例子：

    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));

2.markdown 里\^ 打不出skew符号用s代替，有时也可以直接用 $exp(\xi)$ 代表指数映射。
3.lie伴随性质 $exp((Adj(T)\xi)^s ) = Texp(\xi^s)T^{-1}$ 其中
$Adj(T) = \begin{bmatrix} R & t^SR \\ 0 & R \end{bmatrix}$

变体:
$exp( (\xi^ s) )T = T exp(( Adj(T ^ {-1})\xi)^s )$ 这个公式在求导中很有用，可以用来替换左乘右乘的位置
在Lie Groups for 2D and 3D Transformations》中示例两个SE3乘积的求导：
$C = C_1C_0$
$\frac{\partial C}{\partial C_O}=\frac{\partial C}{\partial \delta}[C_1 exp(\delta) C_0] =\frac{\partial C}{\partial \delta}[exp(Adj(C_1) \delta) C_1 C_0]=Adj(C_1)$

开始求雅克比：
在g2o的边EdgeSE3Expmap中，两个顶点如上所述setEstimate()分别是Tiw, Tjw，根据代码中的误差模型可以看到误差：
$T_{err} = T_{jw}^{-1} T_{ji} T_{iw}$
其中 $T_{ji}$ 是测量值，通过setMeasurement赋值。ORB中Sim3边的使用方式如下

            const g2o::Sim3 Sjw = vScw[nIDj];
            const g2o::Sim3 Sji = Sjw * Swi;

            g2o::EdgeSim3* e = new g2o::EdgeSim3();
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
            e->setMeasurement(Sji);

再看线性化模型，里面的中间变量Tij应该是反了？是Tji的话才能推误差模型。

优化变量有两个： $\xi_i \xi_j$ ，所以 $e_{ij}$ 要分别对两个变量求导，分别对应代码中的_jacobianOplusXi和_jacobianOplusXj

以对位姿j推导为例,对Tj左乘一个小扰动：
$T_{err} = T_{jw}^{-1} T_{ji} T_{iw} T_{err}(\delta\xi_{j}) = ( exp(\delta\xi_{j})^s T_{jw} )^{-1} T_{ji} T_{iw} = T_{jw}^{-1} exp(-\delta\xi_{j})T_{ji} T_{iw}$
$T_{err} = T_{jw}^{-1} T_{ji} T_{iw} exp( (-adj_{(T_{ji} T_{iw})^{-1}} * \delta\xi_j) ^s) = exp((\xi_{err})^s ) exp( (-adj_{(T_{ji} T_{iw})^{-1}} * \delta\xi_j) ^s)$
$T_{err} = exp( (\xi_{err} - J^{-1}adj_{(T_{ji} T_{iw})^{-1}} * \delta\xi_j) ^s) J^{-1}\approx I$
因此误差对位姿j的雅克比为：
$\frac{\partial \xi_{err}}{\partial {\delta \xi_{j}} } =-adj_{(T_{ji} T_{iw})^{-1}}$
对应代码里的：

  _jacobianOplusXj = -infTi_invTij.adj();