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公式的变形,这里推导也可以参考我之前章节的方法,直接构造对数的形式,而不是指数的形式

总结

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

相关推荐
TDengine (老段)7 分钟前
TDengine IDMP 预测、异常检测与数据插补
大数据·数据库·人工智能·时序数据库·tdengine·涛思数据·idmp
地平线开发者13 分钟前
让 AI Agent 和 征程6 台架高效、稳定、安全地协作
算法·安全·自动驾驶
甲维斯1 小时前
GPT6.2“买家秀”来了,吼吼呵呵!
人工智能
艾莉丝努力练剑1 小时前
【AI大模型接入SDK】C++的SessionManager 线程安全
java·开发语言·前端·c++·人工智能·学习·安全
kyle~1 小时前
SHA-256 哈希去重
算法·哈希算法
螺丝钉tina2 小时前
[Playwright] [20][AI] 失败分析:让AI大模型帮你debug
人工智能
MSTcheng.3 小时前
【AI应用实战】用 FastAPI + SSE 做一个自己的 AI 聊天网页:流式输出、多轮对话与完整源码
人工智能·python·大模型·fastapi·sse·ai聊天助手
世冠科技3 小时前
世冠科技GCEAgent重磅发布,开启复杂装备AI原生智能研发新范式
人工智能·科技·ai-native
轻松,带微笑3 小时前
AI竞争进入下半场:微软押注专业模型,通用大模型不再是唯一答案
人工智能·microsoft
哈尔滨张谦蛋3 小时前
兰州大学 · 甘肃省可穿戴装备重点实验室 · 2027年博士招生
人工智能·深度学习