1.视觉惯性ORB-SLAM基本知识

视觉惯性：Visual-Inertial (VI)VI ORB-SLAM：视觉惯性ORB-SLAMVI 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 dimensionsSE(3)：The group of rigid body motions (comprising rotations and translations) in 3 dimensions

2. 常用符号

常用符号：

IMU：BIMU坐标系：B惯性坐标系：I摄像机坐标系：C世界坐标系：W加速度：aB角速度：ωB时间间隔：△t加速度(Accelerometer)偏差：ba陀螺仪(Gyroscope)偏差：bg重力加速度：gW重力方向：g^W=g∗W/||g∗W||重力大小：GIMU方向：RWB∈SO(3)IMU位置：PWBIMU速度：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)+PCBxk：图像中关键点（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)||2N：关键帧数量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+RWCPCBPWB：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 VisionMultiple View Geometry in Computer Vision Second EditionVisual-Inertial Monocular SLAM with Map Reuse