g2o中 EdgeSE3Expmap类型Jacobian的计算

位姿优化的时候,两个顶点的类型是SE3,涉及到的误差雅克比是pose error对pose的求导,里面有些知识值得注意,故记录下来。

前期准备

重新翻看Ethan Eade的《Lie Groups for 2D and 3D Transformations》,发现他的文档早已有相关推导。比如针对两个SO3乘积对其中一个求导:

这里写图片描述

比如同理两个SE3乘积, 对其中一个求导:

这里写图片描述

上面这两个推导都利用了adjoint的性质。这部分Ethan Eade的文档中有,也可参看strasdat phd thesis 2.4.7部分的定义:

exp([adjTξ]×)=Texp(ξ^)T1

由上面的公式可以简单得到如下性质, 这个性质要牢记,预积分里反复用到了:
Texp(ξ^)=Texp(ξ^)T1T=exp([adjTξ]×)Texp(ξ^)T1=T1exp([adjTξ]×)

很容易看出,当 T exp()相乘时,利用这个性质可以调换它们的乘法位置,左乘变右乘,这在求导的时候十分有用。
稍加改变有:
exp(ξ^)T=Texp([adjT1ξ]×)

同时可以计算出 adjT 的计算公式(Ethan Eade文档):
这里写图片描述

EdgeSE3Expmap中雅克比推导

有了Ethan文档的雅克比推导,EdgeSE3Expmap中雅克比的计算依葫芦画瓢就行了。首先通过代码看图优化里位姿边的误差定义,types_six_dof_expmap.h 文件第118行:

    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();
    }

写成公式形式如下:

Terr=T1jwT^jiTiw

其中 T^ji 表示位姿变换的测量。

上面误差公式是多个位姿相乘的形式,所以借鉴前面的推导过程,下面开始对误差的雅克比进行推导,先看下g2o代码的实现,types_six_dof_expmap.cpp 文件第274行,可以看到求得的导数也是某个位姿的adj()形式,所以验证了推导思路没错:

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());

//注意这里把测量标记为Tij应该是标记错误了,应该是Tji,不然整个误差公式说不通了
//这个可以看orbslam EdgeSim3里添加测量时就是用的Sji
  const SE3Quat & Tij = _measurement;   // shoulb be Tji
  SE3Quat invTij = Tij.inverse();

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

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

误差雅克比中误差对变量 Xj (位姿j)的推导,也就是微小增量作用在位姿j上:

Terr=T1jwT^jiTiwξerr=log(Terr)Terr(δξjj)=(exp(δξjjˆ)Tjw)1T^jiTiw=T1jwexp(δξ^jj)T^jiTiw=T1jwT^jiTiwexp([adj(T^jiTiw)1δξjj]×)=exp(ξ^err)exp([adj(T^jiTiw)1δξjj]×)()
(*)式要是连个幂指数是矩阵的相乘,这时候要用barfoot 状态估计一书的公式7.80展开
Terr(δξjj)=exp([ξerrJr(ξerr)1adj(T^jiTiw)1δξjj]×)log(Terr(δξjj))=ξerrJr(ξerr)1adj(T^jiTiw)1δξjj

注意误差 ξerr0 ,所以 Jr(ξerr)1I ,因此误差对位姿j的雅克比为:
Δlog(Terr(δξjj))Δδξjj=adj(Tji^Tiw)1

和代码 jacobianOplusXj=infTi_invTji.adj() 对应, 注意g2o那个EdgeSE3雅克比代码里写的是 Tij 应该为 Tji .
Tiw 的更新雅克比有两个思路,一个是完全仿照上面的过程,如下
Terr=T1jwT^jiTiwξerr=log(Terr)Terr(δξii)=T1jwT^jiexp(δξiiˆ)Tiw=T1jwT^jiTiwexp([adj(Tiw)1δξii]×)

得到
Jxi=adj((Tiw)1)

这个雅克比猛一看和代码中的不一样,注意上面推导过程中是把 exp(δξiiˆ) 这一项往右边移动的,同理,我们可以把他往左边移动。
Terr(δξii)=T1jwT^jiexp(δξiiˆ)Tiw=exp([adj(T1jwT^ji)δξii]×)T1jwT^jiTiw

这时候使用BCH公式的另一个,并近似 JlI , 就能得到
Jxi=adj(T1jwT^ji)

这个和代码里是符合的,猛一看前一个推导不一样,但其实他俩近似。因为,在忽略误差的情况下有 T1jwT^ji=Twi ,所以可以认为上面两个 Jxi 的推导方式都是正确的。但是搞不清为啥g2o中要这么弄.

©️2020 CSDN 皮肤主题: 游动-白 设计师:上身试试 返回首页