当前位置：首页 > news >正文

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

news 2026/7/16 23:27:47

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

一. 基础知识

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

上面这两个推导都利用了伴随矩阵（adjoint）的性质。这部分内容在Ethan Eade的文档中有阐述，也可参考Strasdat博士论文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(); }