视觉惯性单目SLAM （一）算法描述

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) Rk+1WB=RkWBExp((ωkB−bkg)△t)

(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误差项）

EIMU(i,j)=ρ([eTReTVeTP]∑I[eTReTVeTP]T)+ρ(eTb∑Reb)

eR=Log((△RijExp(Jg△Rbjg))TRiBWRjWB)

eV=RiBW(VjWB−ViWB−gw△tij)−(△Vij+Jg△Vbjg+Ja△Vbja)

eP=RiBW(PjWB−PiWB−ViWB△tij−12gw△t2ij)−(△Pij+Jg△Pbjg+Ja△Pbja)

eb=bj−bi

• 
  
  • ∑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

ψ(i)=(R2WC−R1WC)PCB△t23−(R3WC−R2WC)PCB△t12+R2WB△P23△t12+R1WB△V12△t12△t23−R1WB△P12△t23+12RWIg^IG△t2ij

• 方程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