一、问题背景
- 个人理解觉得这是一个Bundle Adjustment的过程
- 引用《视觉SLAM十四讲》的一句话“在SLAM中,通常的做法是先使用P3P/EPnP等方法估计相机位姿,然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment)”。
- 这里“对估计值进行调整”可以用g2o,也可以用ceres进行图优化。
二、二维图优化
- 二维状态包含位置 p p (2维)和 角度 (1维 弧度制)
- 残差:a时刻
→
→
b时刻之间预测值与测量值(
p̂ ab,ψ̂ ab
p
^
a
b
,
ψ
^
a
b
)的误差
- rab=[RTa(pb−pa)−p̂ abNormalize(ψb−ψa−ψab^)] r a b = [ R a T ( p b − p a ) − p ^ a b N o r m a l i z e ( ψ b − ψ a − ψ a b ^ ) ]
- 【NOTE】
- Normalize()将角度限制在[- π π , π π )之间
- 需通过测量的不确定性对残差进行加权,即乘以一个协方差矩阵。
三、三维图优化
- 三维状态包含位置 p p (3维向量),方向 (4维向量 四元数)
- 残差:a时刻 → → b时刻之间测量值( p̂ ab,q̂ ab p ^ a b , q ^ a b )与预测值的误差。
- rab=[R(qa)T(pb−pa)−p̂ ab2.0vec((q−1aqb)q̂ −1ab)] r a b = [ R ( q a ) T ( p b − p a ) − p ^ a b 2.0 v e c ( ( q a − 1 q b ) q ^ a b − 1 ) ]
- 【NOTE】
- 同样需要乘以协方差矩阵对残差进行加权。
- 有一点不能理解,就是方向q的残差部分,还是需要补充一下四元数的知识:-)
【补充】四元数(quaternion)
用四个数表示旋转
★ ★ 一维情况:+1表示向右走一个单元,-4表示向左走4个单元。
★ ★ 二维情况:3+4i表示先向左走3个单元,再向上走4个单元,i可以理解为逆时针转90度。如果你想旋转90度,可以乘以i。
★ ★ 三维情况:
1. 规则: i2=j2=k2=ijk=−1 i 2 = j 2 = k 2 = i j k = − 1
2. h=a+bi+cj+dk h = a + b i + c j + d k 与 h∗=a−bi−cj−dk h ∗ = a − b i − c j − d k 共轭
3. 如果想绕 v(v1,v2,v3) v ( v 1 , v 2 , v 3 ) 轴旋转 θ θ ,则 h=(cos(θ2),v1∗sin(θ2),v2∗sin(θ2),v3∗sin(θ2)) h = ( c o s ( θ 2 ) , v 1 ∗ s i n ( θ 2 ) , v 2 ∗ s i n ( θ 2 ) , v 3 ∗ s i n ( θ 2 ) )
4. 假设想旋转空间点 p(p1,p2,p3) p ( p 1 , p 2 , p 3 ) ,四元数为(0,x,y,z),则 h·p·h∗ h · p · h ∗
5. 逆表示反方向旋转。