g2o_EdgeSE3Expmap_pose_graph代码分析和雅克比推导.md

主要代码:
g2o/types/sba/types_six_dof_expmap.h
g2o/types/sba/types_six_dof_expmap.cpp

基本结构

SE3Quat包括四元数和vector3

相机位姿顶点

1
2
VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {
}

路标3D点顶点

1
2
VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3>(){
}

位姿–位姿 边

1
2
3
EdgeSE3Expmap::EdgeSE3Expmap() :
BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>() {
}

线性化:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
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();
}

误差模型:

1
2
3
4
5
6
7
8
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();
}

推导:

注:1.Twc camera to world,但是g2o里面setEstimate用的是Tcw,setMeasurement用的是Tji。ORB中的例子:

1
2
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));

2.markdown 里\^ 打不出skew符号 用s代替,有时也可以直接用$exp(\xi)$代表指数映射。
3.lie伴随性质$exp((Adj(T)\xi)^s ) = Texp(\xi^s)T^{-1} $其中

1
2
3
4
Adj(T) =   \begin{bmatrix}
R & t^SR \\
0 & R
\end{bmatrix}

变体:
$ exp( (\xi^s) )T = T exp(( Adj(T^{-1})\xi)^s ) $这个公式在求导中很有用,可以用来替换左乘右乘的位置
在Lie Groups for 2D and 3D Transformations》中示例两个SE3乘积的求导:

1
2
3
4
C = C_1C_0  

\frac{\partial C}{\partial C_O}=\frac{\partial C}{\partial \delta}[C_1 exp(\delta) C_0]
=\frac{\partial C}{\partial \delta}[exp(Adj(C_1) \delta) C_1 C_0]=Adj(C_1)

开始求雅克比:
在g2o的边EdgeSE3Expmap中,两个顶点如上所述setEstimate()分别是Tiw, Tjw,根据代码中的误差模型可以看到误差:

1
2

T_{err} = T_{jw}^{-1} T_{ji} T_{iw}

其中$T_{ji}$是测量值,通过setMeasurement赋值。ORB中Sim3边的使用方式如下(在Optimizer::OptimizeEssentialGraph中Set Loop edges)

1
2
3
4
5
6
7
const g2o::Sim3 Sjw = vScw[nIDj];
const g2o::Sim3 Sji = Sjw * Swi;

g2o::EdgeSim3* e = new g2o::EdgeSim3();
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
e->setMeasurement(Sji);

再看线性化模型,里面的中间变量Tij应该是反了?是Tji的话才能推误差模型。

优化变量有两个:$\xi_i \xi_j$,所以$e_{ij}$要分别对两个变量求导,分别对应代码中的_jacobianOplusXi和_jacobianOplusXj

以对位姿j推导为例,对Tj左乘一个小扰动:

1
2
3
4
5
6
7
8
9
10
T_{err} = T_{jw}^{-1} T_{ji} T_{iw} 
T_{err}(\delta\xi_{j}) = ( exp(\delta\xi_{j})^s T_{jw} )^{-1} T_{ji} T_{iw} = T_{jw}^{-1} exp(-\delta\xi_{j})T_{ji} T_{iw}

= T_{jw}^{-1} T_{ji} T_{iw} exp( (-adj_{(T_{ji} T_{iw})^{-1}} * \delta\xi_j) ^s)

= exp((\xi_{err})^s ) exp( (-adj_{(T_{ji} T_{iw})^{-1}} * \delta\xi_j) ^s)

= exp( (\xi_{err} - J^{-1}adj_{(T_{ji} T_{iw})^{-1}} * \delta\xi_j) ^s)

J^{-1}\approx I

因此误差对位姿j的雅克比为:

1
\frac{\partial \xi_{err}}{\partial {\delta \xi_{j}} } =-adj_{(T_{ji} T_{iw})^{-1}}

对应代码里的:

1
_jacobianOplusXj = -infTi_invTij.adj();