1.视觉惯性ORB-SLAM基本知识
- 视觉惯性:Visual-Inertial (VI)
- VI ORB-SLAM:视觉惯性ORB-SLAM
- VI ORB-SLAM输入:
- IMU数据(用B表示,加速度: aB;角速度:ωB ;时间间隔: △t )
- 单目图像
- 在图像中提取KeyPoints时,对像素坐标进行畸变校正,即此KeyPoint坐标可与投影点坐标进行匹配
- IMU数据(加速度和角速度)的测量除了被传感器噪声影响之外,还被缓慢变化的加速度(Accelerometer)偏差( ba )和陀螺仪(Gyroscope)偏差( bg )
- 加速度:受重力加速度( gw )影响,所在在计算运动时,需要减去重力加速度的作用。
- SO(3) :The group of rotations about the origin in 3 dimensions
- SE(3) :The group of rigid body motions (comprising rotations and translations) in 3 dimensions
2. 常用符号
常用符号:
- IMU:B
- IMU坐标系:B
- 惯性坐标系:I
- 摄像机坐标系:C
- 世界坐标系:W
- 加速度: aB
- 角速度: ωB
- 时间间隔: △t
- 加速度(Accelerometer)偏差: ba
- 陀螺仪(Gyroscope)偏差: bg
- 重力加速度: gW
- 重力方向: g^W=g∗W/||g∗W||
- 重力大小: G
- IMU方向: RWB∈SO(3)
- IMU位置: PWB
- IMU速度: VWB
- 加速度雅可比矩阵(Jacobians): Ja(.) (偏差变化的一阶近似,而不需要重新计算预积分)
- 陀螺仪雅可比矩阵(Jacobians): Jg(.) (偏差变化的一阶近似,而不需要重新计算预积分)
- TCB=[RCB|PCB]
3. 方程
3.1 方程1:针孔摄像机模型
- 针孔相机模型:
- 投影函数 π:R3→Ω
- 投影函数功能:把摄像机坐标系中的3D点 (Xc∈R3) 投影到像素坐标系中的2D点 (xc∈Ω⊂R2))
-
π(Xc)=⎡⎣⎢⎢⎢fuXcZc+cufvYcZc+cv⎤⎦⎥⎥⎥,Xc=[XcYcZc]T(方程1)
- [fufv]T 是焦距长度;而 [cucv]T 是主点位置,二者均以像素为单位
3.2 方程2:IMU参数更新方程
- 更新IMU参数:
- 更新IMU方向( R )
- 更新IMU速度(
V ) - 更新IMU位置( P )
(1)
(2) Vk+1WB=VkWB+gW△t+RkWB(akB−bka)△t
⇒Vk+1WB=VkWB+(gW+RkWB(akB−bka))△t
设:a=gW+RkWB(akB−bka)
则:Vk+1WB=VkWB+a△t
(3) Pk+1WB=PkWB+VkWB△t+12gW△t2+12RkWB(akB−bka)△t2
⇒Pk+1WB=PkWB+VkWB△t+12a△t2
-
RWB
:IMU在世界坐标系(W)中的方向
-
VWB
:IMU在世界坐标系(W)中的速度
-
PWB
:IMU在世界坐标系(W)中的位置
-
RWC
:Camera在世界坐标系(W)中的方向
-
PCB
:IMU在摄像机坐标系(C)中的位置
-
PWC
:Camera在世界坐标系(W)中的位置
3.3 方程3:预积分(preintegration)
- 两个连续的关键帧间的运动可以通过预积分( △R,△V,△P )进行定义。
(1) Ri+1WB=RiWB△Ri,i+1Exp((Jg△Rbig))
(2) Vi+1WB=ViWB+gW△ti,i+1+RiWB(△Vi,i+1+Jg△Vbig+Ja△Vbia)
(3) Pi+1WB=PiWB+ViWB△ti,i+1+12gW△t2i,i+1+RiWB(△Pi,i+1+Jg△Pbig+Ja△Pbia)
- 雅可比矩阵和预积分:当IMU测量数据到达时, 通过迭代的方式进行计算。
4. VI ORB-SLAM主要变化
4.1 跟踪(Tracking)
- VI跟踪以帧率速度跟踪:
- 传感器位姿(Sensor Pose)
- 传感器速度(Sensor Velocity)
- IMU偏差(IMU biases)
- VI跟踪可以非常可靠地预测摄像机位姿(Camera Pose)
- 摄像机位姿观测=>
- Local Map中的Map Points投影到Frame J,并与Frame J中KeyPoints进行匹配=>
- 通过最小化重投影误差的方式优化Frame J的Pose=>
- 并生成IMU误差项(IMU error)
4.1.1 图a): 地图更新之后执行Tracking
- 在地图更新之后执行Tracking,IMU误差项连接当前帧
j
和最后一个关键帧
i 方程4:求最优解θ
θ={RjWB,PjWB,VjWB,bjg,bja}
θ∗=argminθ(∑kEproj(k,j)+EIMU(i,j))Eproj:匹配的MapPoint( K )的重投影误差
方程5:(重投影误差)
Eproj(k,j)=ρ((xk−π(XkC))T∑k(xk−π(XkC)))
XkC=RCBRjBW(XkW−PjWB)+PCB- xk :图像中关键点(Keypoint)的位置
- XkW :MapPoint在世界坐标系中的位置
- ∑k :与Keypoint Scale相关的信息矩阵
方程6:(IMU误差项)
-
- ∑I :预积分信息矩阵
- ∑R :bias random walk
-
ρ
:胡贝尔鲁棒成本函数(Huber robust cost function)
胡贝尔成本函数:是凸的,可微的,对离群值有好的鲁棒性(是二次和线性的组合)。
- 求 θ∗ 的优化方法:Gauss-Newton算法(在g2o中被实现)
4.1.2 图b): a)优化的结果作为下一次优化的先验
- 图a)优化之后的估计结果和 海森矩阵(Hessian矩阵) 作为下一次优化的先验知识。
- 海森矩阵用途:被应用于牛顿法解决的大规模优化问题。
4.1.3 图c): 无地图更新并使用先验
- 帧( j+1 )优化:有一个与帧( j )的连接,且使用以前的优化算出的先验知识(图b))
方程7:(无地图更新且使用先验知识)
θ={RjWB,PjWB,VjWB,bjg,bja,Rj+1WB,Pj+1WB,Vj+1WB,bj+1g,bj+1a}
θ∗=argminθ(∑kEproj(k,j+1)+EIMU(i,j+1)+Eprior(j))-
方程8:Eprior(是一个先验项)
Eprior(j)=ρ⎛⎝[eTReTVeTPeTb]∑p[eTReTVeTPeTb]T⎞⎠
eR=Log(R¯jBWRjWB)
eV=V¯jWB−VjWB
eP=P¯jWB−PjWB
eb=b¯j−bj
- R¯jWB,V¯jWB,P¯jWB,b¯j:在图b)中估计的状态
- ∑p :图b)中计算出海森矩阵
4.1.4 图d): Frame
j
被边缘化
- 只要地图没有更新,e-f重复c-d过程,且e-f一直重复下去,一直重复到地图变化或
prior 无效。
4.2 局部建图(Local Mapping)
- 功能:在新的关键帧插入之后,Local Mapping线程执行 BA
- 目标:它优化最后的N个关键帧(Local Window)和这N关键帧可看到的所有MapPoints
- 比较:ORB-SLAM Local BA与VI ORB-SLAM Local BA的比较(P:位姿 v:速度 b:偏差)
4.3 闭环检测(Loop Closing)
- 目标:闭环检测线程旨在减少累积漂移
- 位置识别(Place recognition):把最近的关键帧与过去的关键帧进行匹配
- 位姿图优化(pose-graph):与ORB-SLAM相比,VI ORB-SLAM在pose-graph上执行6DoF优化,而不是7DoF优化,因为尺度(scale)是可观察的
- 位姿图优化:忽略了IMU信息,不优化速度、IMU偏差
- Full BA:优化所有状态,包括:速度和偏差
5. IMU初始化
- 陀螺仪偏差估计
- 尺度和重力近似(未考虑加速度偏差)
- 加速度偏差估计,且校准尺度和重力方向
- 速度估计
5.1 陀螺仪偏差估计
- 陀螺仪偏差估计:通过连续两个关键帧的已经方向进行估计
方程9:估计陀螺仪偏差
argminbg∑i=1N−1||Log((△Ri,i+1Exp(Jg△Rbg))TRi+1BWRiWB)||2- N:关键帧数量
- R(⋅)WB=R(⋅)WCRCB:R(⋅)WC 由ORB-SLAM计算, RCB 由标定而得
- △Ri,i+1 :是连续两个关键帧间的陀螺仪积分
- 使用Gauss-Newton方法求解 bg ,其初始值为0
5.2 尺度和重力近似(未考虑加速度偏差)
- 当估计完陀螺仪偏差之后,就可以预积分:速度、位置
- 由ORB-SLAM计算的摄像机轨迹的尺度是任意的,在摄像机坐标系C与IMU B坐标系间变换需要加入一个尺度因子 s
方程10:计算IMU在世界坐标系中的位置
PWB=sPWC+RWCPCB- PWB :IMU在世界坐标系(W)中的位置
- PWC :Camera在世界坐标系(W)中的位置
- RWC :Camera在世界坐标系(W)中的方向
- PCB :IMU在摄像机坐标系(C)中的位置,是一个常量
方程11:把方程10代入方程3的位置方程,且忽略加速度偏差
sPi+1WC=sPiWC+ViWB△ti,i+1+12gw△t2i,i+1+RiWB△Pi,i+1+(RiWC−Ri+1WC)PCB- 目标:估计 s和gw(gw:3×1列向量∗∗重点内容∗∗)
- 求解方法:通过求解线性方程组
- 为了避免求解速度,使用三个连续的关键帧,且使用方程3中的速度关系
-
方程12:只有R,P,时间间隔的方程
[λ(i)β(i)][sgW]=γ(i) - 为了书写方便,把关键帧 i,i+1,i+2分别记为1,2,3 ,则 λ(i)、β(i)和γ(i)表示为:
方程13:矩阵的各项表达式
λ(i)=(P2WC−P1WC)△t23−(P3WC−P2WC)△t12
β(i)=12I3×3(△t212△t23+△t12△t223)
γ(i)=(R1WC−R2WC)PCB△t23−(R2WC−R3WC)PCB△t12+R1WB△P12△t23−R2WB△P23△t12−R1WB△V12△t12△t23方程12的形式为:
A3(N−2)×4x4×1=B3(N−2)×1- 方程12可通过SVD方法求解
- 由于有4个未知量(即 n=4 ),为了求 Ax=b ,需 m⩾4,=>N⩾4(至少需要4个连续关键帧)
- 通过此方程求解 s∗和g∗w
5.3 加速度偏差估计,且优化尺度和重力方向
- 重力方向(在惯性坐标系中)
g^I={0,0,−1} - 已经计算出的重力方向(在世界坐标系中)
g^W=g∗W/||g∗W|| -
方程14:惯性系统在世界坐标系中的方向
:
RWI=Exp(v^θ)v^=g^I×g^W||g^I×g^W||,θ=atan2(||g^I×g^W||,g^I⋅g^W)
注:( double atan2(double y,double x) 返回的是原点至点(x,y)的方位角,即与 x 轴的夹角。也可以理解为复数 x+yi 的辐角。返回值的单位为弧度,取值范围为(-\pi, \pi] $) -
方程15:重力向量(在世界坐标系中)
:
gW=RWIg^IG -
方程16:使用扰动优化旋转
gW=RWIExp(δθ)g^IGδθ=[δθTxy0]T,δθxy=[δθxδθy]T -
方程17:gW的一阶近似
gW≈RWIg^IG−RWI(g^I)×Gδθ -
方程18:有加速度偏差的效果(把方程17代入方程11)
sPi+1WC=sPiWC+ViWB△ti,i+1−12RWI(g^I)×G△t2i,i+1+RiWB(△Pi,i+1+Ja△Pba)+(RiWC−Ri+1WC)PCB+12RWIg^IG△t2i,i+1 -
方程19:三个连续关键帧方程
[λ(i)ϕ(i)ξ(i)]⎡⎣⎢sδθxyba⎤⎦⎥=ψ(i) -
方程20:方程19中的各元素
λ(i)=(P2WC−P1WC)△t23−(P3WC−P2WC)△t12(与方程12中的一致)
ϕ(i)=[12RWI(g^I)×G(△t212△t23+△t223△t12)]:,1:2注:[]:,1:2表示矩阵的前面2列
ξ(i)=R2WBJa△P23△t12+R1WBJa△V23△t12△t23−R1WBJa△P12△t23
- 方程19可以写为:
A3(N−2)×6x6×1=B3(N−2)×1 - 可通过SVD求解此以下未知量:
- s∗ :尺度因子
- δθ∗xy :重力方向校正
- b∗a : 加速度计偏差
- 有 3(N−2) 个方程和6个未知量,为求解此方程,至少需要 4 个关键帧
- 计算条件数(condition number):如:最大奇异值/最小奇异值
- 条件数:可用于检查此问题是否条件良好(如:IMU执行的运动使所有变量均可观察)
5.4 速度估计
- 在连续三个关键帧的关系方程(12)和(19)中,这样所产生的线性方程组中没有另外3N个未知量(这些未知量与速度相对应)
- 所有关键帧的速度可以通过方程(18)进行计算,此时
scale,gravityandbias 已知。 - 使用方程(3)中的 速度方程 计算最近关键帧的速度
- 重新初始化 bg :当长时间运行之后,若使用位置识别进行系统重定位,则使用方程(9)重新初始化 bg
- 求解 ba :通过求解方程(19)估计 ba ,此时只有 ba 未知, scale和gravity 已知
参考文献:
- Lie groups, Lie algebras, projective geometry and optimization for 3D Geometry, Engineering and Computer Vision
- Multiple View Geometry in Computer Vision Second Edition
- Visual-Inertial Monocular SLAM with Map Reuse