1 基本资料
https://blog.csdn.net/heyijia0327/article/details/51773578
2推导公式
2-1 结论
2-2 slam14讲解推导
伴随性质利用
伴随性质利用
其中用到了近似
https://github.com/b51/CeresSim3Optimize
Jacobian Calculation of sim(3)
Derivation of Jacobiani
Same to Jacobianj
3代码
https://github.com/b51/CeresSim3Optimize
G2O代码
void EdgeSE3Expmap::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()); 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(); }