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)=\left[\begin{array}{cc}R& t^{S}R\\ 0& R\end{array}\right]$$

变体:
$$exp(({\xi }^s))T=Texp((Adj(T^{-1})\xi {)}^s)$$这个公式在求导中很有用，可以用来替换左乘右乘的位置
在Lie Groups for 2D and 3D Transformations》中示例两个SE3乘积的求导：
$$C=C_1{C}_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((-ad{j}_{(T_{ji}{T}_{iw}{)}^{-1}}\ast \delta {\xi }_j{)}^s)=exp(({\xi }_{err}{)}^s)exp((-ad{j}_{(T_{ji}{T}_{iw}{)}^{-1}}\ast \delta {\xi }_j{)}^s)$$
$$T_{err}=exp(({\xi }_{err}-J^{-1}ad{j}_{(T_{ji}{T}_{iw}{)}^{-1}}\ast \delta {\xi }_j{)}^s)J^{-1}\approx I$$
因此误差对位姿j的雅克比为：
$$\frac{\partial {\xi }_{err}}{\partial \delta {\xi }_j}=-ad{j}_{(T_{ji}{T}_{iw}{)}^{-1}}$$
对应代码里的：

  _jacobianOplusXj = -infTi_invTij.adj();