位姿优化的时候,两个顶点的类型是SE3,涉及到的误差雅克比是pose error对pose的求导,里面有些知识值得注意,故记录下来。
前期准备
重新翻看Ethan Eade的《Lie Groups for 2D and 3D Transformations》,发现他的文档早已有相关推导。比如针对两个SO3乘积对其中一个求导:
比如同理两个SE3乘积, 对其中一个求导:
上面这两个推导都利用了adjoint的性质。这部分Ethan Eade的文档中有,也可参看strasdat phd thesis 2.4.7部分的定义:
exp([adjT⋅ξ]×)=Texp(ξ^)T−1
由上面的公式可以简单得到如下性质, 这个性质要牢记,预积分里反复用到了:
Texp(ξ^)=Texp(ξ^)T−1T=exp([adjT⋅ξ]×)Texp(ξ^)T−1=T−1exp([adjT⋅ξ]×)
很容易看出,当 T 和
稍加改变有:
exp(ξ^)T=Texp([adjT−1⋅ξ]×)
同时可以计算出 adjT 的计算公式(Ethan Eade文档):
EdgeSE3Expmap中雅克比推导
有了Ethan文档的雅克比推导,EdgeSE3Expmap中雅克比的计算依葫芦画瓢就行了。首先通过代码看图优化里位姿边的误差定义,types_six_dof_expmap.h 文件第118行:
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();
}
写成公式形式如下:
Terr=T−1jwT^jiTiw
其中 T^ji 表示位姿变换的测量。
上面误差公式是多个位姿相乘的形式,所以借鉴前面的推导过程,下面开始对误差的雅克比进行推导,先看下g2o代码的实现,types_six_dof_expmap.cpp 文件第274行,可以看到求得的导数也是某个位姿的adj()形式,所以验证了推导思路没错:
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();
}
误差雅克比中误差对变量
Xj
(位姿j)的推导,也就是微小增量作用在位姿j上:
Terr=T−1jwT^jiTiwξerr=log(Terr)Terr(δξj′j)=(exp(δξj′jˆ)Tjw)−1T^jiTiw=T−1jwexp(−δξ^j′j)T^jiTiw=T−1jwT^jiTiwexp([−adj(T^jiTiw)−1⋅δξj′j]×)=exp(ξ^err)exp(−[adj(T^jiTiw)−1⋅δξj′j]×)(∗)
(*)式要是连个幂指数是矩阵的相乘,这时候要用barfoot 状态估计一书的公式7.80展开
Terr(δξj′j)=exp([ξerr−Jr(ξerr)−1adj(T^jiTiw)−1⋅δξj′j]×)log(Terr(δξj′j))=ξerr−Jr(ξerr)−1adj(T^jiTiw)−1⋅δξj′j
注意误差 ξerr≈0 ,所以 Jr(ξerr)−1≈I ,因此误差对位姿j的雅克比为:
Δlog(Terr(δξj′j))Δδξj′j=−adj(Tji^Tiw)−1
和代码 jacobianOplusXj=−infTi_invTji.adj() 对应, 注意g2o那个EdgeSE3雅克比代码里写的是 Tij 应该为 Tji .
对 Tiw 的更新雅克比有两个思路,一个是完全仿照上面的过程,如下
Terr=T−1jwT^jiTiwξerr=log(Terr)Terr(δξi′i)=T−1jwT^jiexp(δξi′iˆ)Tiw=T−1jwT^jiTiwexp([adj(Tiw)−1⋅δξi′i]×)
得到
Jxi=adj((Tiw)−1)
这个雅克比猛一看和代码中的不一样,注意上面推导过程中是把 exp(δξi′iˆ) 这一项往右边移动的,同理,我们可以把他往左边移动。
Terr(δξi′i)=T−1jwT^jiexp(δξi′iˆ)Tiw=exp([adj(T−1jwT^ji)⋅δξi′i]×)T−1jwT^jiTiw
这时候使用BCH公式的另一个,并近似 Jl≈I , 就能得到
Jxi=adj(T−1jwT^ji)
这个和代码里是符合的,猛一看前一个推导不一样,但其实他俩近似。因为,在忽略误差的情况下有 T−1jwT^ji=Twi ,所以可以认为上面两个 Jxi 的推导方式都是正确的。但是搞不清为啥g2o中要这么弄.