SLAM中的非线性优-3D图优化之相对位姿g2o::EdgeSE3Expmap（十）

本讲总结下g2o中实现的相对位姿约束，本节主要还是参考了**"白巧克力亦唯心"**的那篇博客，尊重下原创，当然个人对g2o的代码风格不太喜欢，并且时间有限，因此这里只是展示下，不做代码验证，依然沿用之前的图进行说明

一. 基础知识

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

上面这两个推导都利用了伴随矩阵（adjoint）的性质。这部分内容在Ethan Eade的文档中有阐述，也可参考Strasdat博士《Local Accuracy and Global Consistency for Efficient Visual SLAM》论文2.4.7节的定义：

以及

利用该性质可以交换二者的乘法顺序（左乘变为右乘），这在求导时非常有用。同理可得其变体形式：

二. EdgeSE3Expmap边分析

新版貌似不在那个文件下了，不过问题不大

复制代码

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

复制代码

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

这里的barfoot的状态估计应该是这本书《state estimation for robotics》公式（7.80）如下

显然不是这个公式，实际上就是BCH公式的变形，这里推导也可以参考我之前章节的方法，直接构造对数的形式，而不是指数的形式

总结

本节博客主要总结，收集相关理论，并非个人原创，因为本博客系列是以后端优化为背景介绍的，虽然个人不太愿意完全参考别人的博客，但出于完整性考虑，此处还是搬运下，也作为以后的工具可查询。