ORB-SLAM3 自学笔记

---

前言

提示：自学ORB-SLAM3记下的笔记，无参考价值

---

ORB图像特征

ORB特征有关键点和描述子两部分组成。它的关键点称为“Oriented FAST”，是一种改进的FAST角点。它的描述子称为“BRIEF”。

1. ORB关键点

ORB关键点是一种改进的FAST角点，这里先讨论FAST关键点。

FAST是一种角点，主要检测局部像素灰度变化明显的地方。FAST角点的思路：如果一个像素与相邻的像素差别很大(过亮或过暗)，那么它更可能是角点。

FAST角点的检测过程如下：

1. 在图像中选取像素p，假设他的亮度为 I_p
2. 设置一个阈值T（比如I_p的20%）
3. 以像素p为中心，选取半径为3的圆边上的16个像素点
4. 假如选取的圆的边上有连续N个点的亮度大于(I_p+T)或小于(I_p-T)，那么像素p可以被选来作FAST角点（N通常选12，称为FAST-12，常用的还有FAST-9和FAST-11）

为了该算法更高效，可以添加一项预测试操作：对于每一个像素，先检测圆边上的第1，5，9，13个像素点的亮度。只有当这4个像素中有3个同时大于(I_p+T)或小于(I_p-T)时，当前这个像素才有可能是一个FAST角点，否则应该直接排除。

此外，原始的FAST角点经常出现“扎堆”的现象。所以才第一次检测之后，还需要用非极大值抑制，在一定区域内仅保留响应极大值的角点，避免角点集中的问题

FAST角点的缺点：不具方向性，并且存在尺度问题，远处看是角点，近处看可能不是角点。针对FAST角点的缺点，ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法实现。

• 金字塔的底层是原始的图像。每往上一层，就对图像进行一次固定的倍率缩放，这样我们就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中，我们可以匹配不同层上的图像，从而实现尺度不变性。
• 在旋转方面，我们计算特征点附近区域图像的的灰度质心(指以图像块灰度值作为权重的中心)。步骤如下：

  1. 在一个小的图像块B中，定义图像块的矩为：
     

  2. 通过矩可以找到图像的质心：
     

  3. 连接图像的几何中心O与质心Q，得到一个方向向量OQ，于是特征点的方向可以定义为：a=arctan(m₀₁/m₁₀)

通过以上方法，FAST角点便具有尺度和旋转的描述，从而大大提升了其在不同图像之间的表述的鲁棒性。所以在ORB中，把这种改进后的FAST角点称为 Oriented FAST。

2. ORB描述子

BRIEF是一种二进制描述子，编码了关键点附近两个像素(p,q)的大小关系:若p>q,则取1， 反之则取0。如果我们取了128对这样的p和q，最后可以得到一组有128个0和1组成的向量。

BRIEF算法的具体操作过程如下：

1. 以关键点p为圆心，以d为半径作圆O

2. 在圆O内随机选取N个点对(每一帧在该圆上选取的方式相同)。设N=4，如下图：
   
   假设当前选的点对为p₁(A,B),p₂(A,B),p₃(A,B),p₄(A,B)。

因为ORB在FAST角点提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转之后的**“Steer BRIEF**”特征，使ORB的描述子具有较好的旋转不变性。

**由于考虑到了旋转和缩放，ORB在平移和旋转的变换下仍有良好的表现。**ORB描述子采用二进制数来表达，不仅节约了存储空间和缩短了匹配时间。

例如假设ORB特征点A，B的描述子如下：
A：10101011
B：10101010
首先，设置一个阈值，比如85%，当A和B的描述子的相似度大于85%时，我们判断A和B是相同的特征点，即两点匹配成功。在本例子中，A和B的前七位都相同，则相似度为87.5%，所以A和B是匹配的。

---

相关概念

1. 关键帧 与 共视关键帧

关键帧指图像帧中具有代表性的帧。图像插入的频率过高会导致信息冗余度快速增加，而这些冗余的信息对提升系统的精度却十分有限，甚至没有提升，反而消耗了更多计算资源。

关键帧的目的在于，适当地降低信息冗余度，减少计算资源的损耗，保证系统的平稳运行。

关键帧的选取原则：

• 图像质量：画面清晰，特征点多，特征点分布均匀
• 连接关系：与其他关键帧有一定的共视关系，同时不能重复度太高，既存在约束，又要减少冗余信息。

共视关键帧：任取一关键帧，与该关键帧能观测到相同地图点的关键帧（也称为一级关键帧）
该关键帧的一级关键帧的一级关键帧称为该关键帧的二级关键帧。
注：能观测到相同关键帧的数目由共视程度来表征。

2. 共视图 生成树 本质图

共视图是无向加权图，每个节点是关键帧，如果两个关键帧之间满足一定的共视关系（至少15个共同观测地图点）他们就连成一条边，边的权重就是共视地图点数目。
通常，只会使用一级相邻层级的共视关系及信息，而局部地图优化时用两级相邻共视关系进行优化。

本质图的节点表示所有关键帧，但连接的边只保留联系紧密关键帧之间的边（至少有100个共视地图点），使得结果更加精确。包含生成树的连接关系，形成闭环的连接关系。

生成树，即本质图的生成树，节点表示所有关键帧，但连接的边只保留联系最紧密关键帧之间的边
三者关系

• 相同点 ： 都是以关键帧作为节点，是帧与帧之间通过边来连接的图
• 不同点 ： 共视图最稠密，本质图次之，生成树最稀疏

3. IMU

IMU(Inertial measurement unit)惯性测量单元，主要包括加速度计和陀螺仪。加速度计用于测量物体三个单轴的加速度，陀螺仪用于测量物体的三轴角速度。可以通过加速度和角速度计算物体的位姿。

IMU的优势和纯视觉SLAM的优势相互补：
1）运动过快的场景下，相机会出现图像模糊，或者两帧图像之间的重叠区域太少甚至没有，造成无法基于两帧图像中相同点和不同点测算运动；这种情况下，IMU可以补充发挥作用，继续提供可靠的(R,t)估算。
2）纯视觉难以处理动态场景，环境变化时，会误认为自己在运动，而IMU则能够感受到自己运动，避免运动误判。
3）视觉在纹理丰富的场景中可以正常工作，然而遇到玻璃、白墙等特征稀少的场景时就无法正常工作。另外视觉还受光线条件影响和限制。

总之：
IMU可以为纯视觉slam提供：尺度信息，重力方向，速度
纯视觉slam可以得到IMU的bais

【IMU的偏置(bias)】
IMU（惯性测量单元）的偏置（bias）指的是IMU中的加速度计和陀螺仪测量中的常数误差。这些常数误差是由于传感器本身的制造和环境因素引起的，它们在短时间内可能不会发生显著变化，因此需要在数据处理中进行校准和补偿。

具体来说，IMU的偏置包括：

1. 加速度计偏置：这是加速度计测量的恒定误差，通常表示为加速度的零点漂移。即使IMU处于静止状态，加速度计测量可能不是零，这是由于加速度计本身的误差所致。
2. 陀螺仪偏置：这是陀螺仪测量的恒定误差，通常表示为角速度的零点漂移。陀螺仪在没有旋转的情况下可能会产生角速度测量，这是由于陀螺仪内部的误差引起的。

为了准确地使用IMU数据，通常需要在数据处理中校准和补偿这些偏置。校准过程通常包括将IMU置于已知条件下（如静止）以确定偏置值，然后将这些值用于修正测量数据。这样可以提高IMU的精度，使其更适合导航、姿态估计和其他应用。注意，随着时间的推移，IMU的偏置可能会发生变化，因此需要定期重新校准。

IMU得到的加速度和角速度都是在IMU坐标系下的，可以通过积分得到速度和位置。其中对加速度积分以获取速度，然后对速度积分以获取位置。

IMU预积分是一种用于估计速度和位置的技术，它使用IMU的输出数据（包括加速度计和陀螺仪的测量值）进行数值积分，以估计运动状态，同时减少传感器噪声和误差的影响。预积分通常用于姿态估计、导航和定位应用。
预积分的基本思想是在一个小时间步长内对IMU的输出进行积分，然后将这些积分结果用于估计速度和位置。这可以减小误差的累积，因为在小时间步长内，IMU的输出通常可以被认为是准确的。

4. VIO

VIO 是 Visual-Inertial Odometry（视觉惯性里程计）的缩写。它是一种用于估计相机和IMU（Inertial Measurement Unit，惯性测量单元）之间的相对运动和相机位置的技术。VIO 结合了视觉传感器（相机）和惯性传感器（IMU）的数据，以提供更准确和稳定的运动估计。

VIO视觉惯性里程计包括四个坐标系：

1. 世界坐标系：世界坐标系是一个固定的全局坐标系，通常用于表示物体的绝对位置和方向。这是一个不随时间而变化的坐标系，用于地图建模和全局导航。(通常为第一帧图像的相机坐标系)
2. 相机坐标系
3. IMU坐标系：IMU坐标系是与IMU传感器相关的坐标系，通常与IMU的主轴对齐。这个坐标系用于表示IMU测量的加速度和角速度。
4. 惯性坐标系：IMU坐标系和惯性坐标系通常是同一坐标系，或者可以视为在相同坐标系下运行。重力只在z轴上。

在VIO中，IMU的加速度和角速度测量通常与相机的视觉数据相结合，以估计相机或物体的运动和位姿。

视觉与IMU融合两类方案：

1. 松耦合：通过图像数据计算相机位姿，然后与IMU融合，计算出物体的位姿
   

2. 紧耦合：将图像数据和IMU数据一起作为输入，计算物体的位姿
   

5. VO 与 VIO 的区别

视觉里程计VO通过最小化相机帧中的重投影误差，计算得到相机的位姿和地标的位置。

视觉惯性里程计VIO中IMU对相邻两帧的位姿之间添加约束，而且对每一帧添加了状态量：陀螺仪和加速度的偏差及速度。对于这样一个新的结构，建立一个包含重投影误差和IMU误差的统一损失函数进行联合优化：
其中，i,j,k分别表示计算机，特征点和关键帧的索引，W_r ^i,j,k表示特征的信息矩阵，W_s^k表示IMU误差的信息矩阵，而e_r^i,j,k为视觉重投影误差，e_s^k为IMU误差项。

---

相机模型

单目相机模型

针孔模型，如下图

O-x-y-z为相机坐标系，O-x’-y’-z为物理成相平面。空间P通过小孔O投影到物理成像平面，成像点为P’
在相机坐标系下，设P的坐标为[X, Y, Z]，P’的坐标为[X’, Y’, Z’]，物理成像平面到小孔的距离为f

根据三角形的相似关系，存在
可以将成像平面对称到相机前面
则有：
在相机中获得的像素，需要对物理成像平面上对成像进行采样和量化。因此，物理成像平面存在一个像素平面 O-u-v，设空间点P在像素平面的投影为P‘，坐标为[u, v]。像素坐标与成像平面之间，相差一个缩放和原点的平移。则
转化为矩阵形式，过程如下：
将Z移到左边：
中间的矩阵为相机内参矩阵K，相机的R，t又称为相机的内参矩阵。内参在视觉slam中，通常是被认定为不变的，外参会随着相机的运动而变化，同时也是slam中待估计的目标，代表相机的运动轨迹。

可以将一个世界坐标系下的点，先转换到相机坐标系下，再去掉它最后一维的数值(归一化)，得到点P在相机归一化平面上的投影：

归一化坐标，可以看成是成相机前方Z=1处平面上一个点，Z=1平面又称为归一化平面。归一化坐标左乘相机内参K，就得到像素坐标。因此，也可以把像素左边[u, v]^T看成是对归一化平面上的点进行量化测量的结果。

双目相机模型

双目相机通过同步采集左右相机的图像，计算图像间视差，估计每个像素的的深度

双目相机通常由两个水平放置的相机组成，两相机的光圈中心分别记为 O L 、 O R O_L、O_R OL​、OR​，两者之间的距离称为双目相机的基线 b b b，相机焦距为 f

若对于三维空间内一点 P P P进行捕捉，此时两相机将各自成像，记为 P L 、 P R P_L、P_R PL​、PR​。理想情况下，相机尽在X轴进行位移，对应像素仅在 u u u轴上存在差异，则记物体在两图像上的坐标为 u L u_L uL​及 − u R -u_R −uR​：
则可由相似三角形得到：
从而得到深度信息：

其中，参数 d称为视差 （disparity），用于描述物体在两相机上形成像素的横坐标之差：
由视差可估计得到物体的深度信息，视差越大，距离越近。由于视差最小为一个像素，所以双目能够测量的深度存在一个理论上的最大值。
双目相机的基线越大，测量范围就越大。

---

多视图几何算法

1. 2D-2D: 对极几何

已知一组匹配好的图像特征点对，通过这些二位图像点的对应关系，恢复出两帧之间摄像机的运动。

(1) 对极约束

以上图为例，我们希望求取两帧图像I₁,I₂之间的运动，设第一帧到第二帧的运动为R，t。两个相机的中心为O₁、O₂。I₁中的一个特征点p₁与I₂中的特征点p₂相匹配。

在单目相机模型中知道: P_uv=KP_归
世界坐标系下的一固定点P_w，在相机下的坐标为P_c(X,Y,Z)，把它投影到Z=1的归一化平面上，得P_w得归一化坐标为P_归=(X/Z, Y/Z, 1)

• 设在第一帧的相机坐标系下，P点的坐标为[X,Y,Z]^T，已知两个像素点p₁p₂的位置，那么有：
  s₁p₁ = KP， s₂p₂ = K(RP+t)
  K为相机的内参矩阵，R，t为两个坐标系的相机运动，这里计算的是R₂₁和t₁₂。

• 把K提到左边，得：s₁K^-1p₁=P， s₂K^-1p₂=RP+t

• 令 x₁=K^-1p₁ ， x₂=K^-1p~2，代进上面两式
  则有 s₁x₁=P ， x₂=RP+t

• 把s₁x₁=P代入x₂=RP+t，得 s₂x₂=R(s₁x₁)+t

• 左右两边同时叉乘t得到：txs₂x₂=txR(s₁x₁)+txt（其中txt=0）

• 左右两边同时左乘x₂^T，得到： x₂^T · (txs₂x₂) = x₂^T · [txR(s₁x₁)]
  (txs₂x₂)是一个既与t垂直又与x₂垂直的向量，左式为0故有 x₂^T · [txR(s₁x₁)] = 0

• 又s₁是标量，所以有 x₂^T txRx₁= 0

• 即 x₂^T t^ R x₁ = 0，把x₁=K^-1p₁ ， x₂=K^-1p~2代入，得到对极约束： p₂^T K^-T t^ R K^-1p₁ = 0

(2) 本质矩阵E估计R,t

已知对极约束：p₂^T K^-T t^ R K^-1p₁ = 0，设E= t^ R，F= K^-Tt^R K^-1 = K^-TEK^-1。则称E为本质矩阵，F为基础矩阵。

• 于是相机位置估计问题变为
  1.根据配对点的像素位置求出E或者F
  2.根据E或者F求出R，t

• 本质矩阵的性质
  1.E乘以任非零常数后，对极约束仍然满足。称E在不同尺度下是等价的（E的尺度等价性）
  2.一个3x3矩阵是本质矩阵的充要条件是它的奇异值中有两个相等且第三个为0
  3.由于平移和旋转各有三个自由度，所以t^R有6个自由度。但由于尺度的等价性，故E实际上有5个自由度

使用八点法来估计E——即使用8对点来估计E


如果8对匹配点组成的矩阵满足秩为8的条件，那么E的各个元素就可以由上述方程解出。接下来的问题就是根据E恢复相机的运动R，t。这个过程由奇异值分解(SVD)得到。设E的SVD为 E=U∑V^T

其中U，V为正交矩阵，∑为奇异值矩阵。根据E的性质，我们知道∑=(a,a,0)。在SVD分解中，对任一一个E，存在两个可能的t和R与它对应：

其中，R_Z(π/2)表示沿Z轴旋转90度得到的旋转矩阵。因此从E分解R，t时，会得出四个可能的解。不过幸运的是，把任一一点P代入这四个解中，只有一种解中P的深度为正值，也就可以确定哪一个解是正确的了。

【讨论】
因为E=t^R，又E具有尺度等价性，所以它分解得到的t，R也有一个尺度等价性。而R∈SO(3)自身具有约束，所以t具有一个尺度。换而言之，在分解的过程中，对t乘以任意非零常数，分解都是成立的。因此，我们通常把t进行归一化，让它的长度等于1，即|t|=1。

【尺度不确定性】
对两张图像的t归一化，相当于固定了尺度。虽然我们不知道它的实际长度，当我们可以以这时的t为单位1，计算相机运动和特征点的3D位置。这就是单目SLAM的初始化。初始化之后，就可以用3D-2D计算相机运动了。

初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。因此，单目SLAM有一步不可避免的初始化。初始化的两张图像必须有一定的平移，而后的轨迹和地图都将以此步的平移为单位

【初始化的纯旋转问题】
如果相机的运动时纯旋转，那么t=0，也就没有几何约束了，导致我们无法求解R。因此，单目SLAM的初始化不能只有旋转，必须要有一定程度的平移。如果相机时左右移动而不是旋转，就很容易实现单目SLAM的初始化。

(3)三角测量

前面我们用对极几何约束估计了相机运动，现在我们需要用相机的运动估计特征点的位置。

在单目SLAM中，仅通过单张图像是无法获得像素的深度信息的，我们需要通过三角测量的方法估计地图点的深度。如下图所示。

三角测量指，通过不同位置对同一个路标点进行测量，从观察到的位置推断路标点的距离。理论上，直线O₁p₁与直线O₂p₂在场景中回相交于一点P，P点即两个特征点所对应的三维场景中地图点的位置。由于噪声的影响，这两条直线往往无法相交。因此，可以通过最小二乘法求解。

前面我们已知：s₁k^-1p₁ = P， s₂K^-1p₂ = RP+t ，所以有
\\\\\\\\\\\ s₂x₂ = s₂K^-1p₂
\\\\\\\\\\\\\\\\\\ = RP + t
\\\\\\\\\\\\\\\\\\ = Rs₁K^-1p₁ + t
\\\\\\\\\\\\\\\\\\ =s₁Rx₁ + t

前面，用对极几何约束估计了相机运动，已知道R，t，现在求两个特征点的深度s₁,s₂ 。对等式 s₂x₂ = s₁Rx₁ + t，两边左乘一个x^₂,得 s₂x^₂x₂ = 0 = s₁x^₂Rx₁ + x^₂t

上式左侧为0，右侧可以看成s₁得方程，因为x^₂，R，x₁，t这些变量得值已知，可以求解出s₁。有了s₁，s₂也非常容易求解出来。

【讨论】
三角测量是由位移得到的，相机有了位置上的变化，才会有对极几何中的三角形，才会有三角测量。因此，纯旋转是无法使用三角测量的，因为在平移为0时，对极约束一直为0。

【三角测量的矛盾】

当平移很小时，像素上的不确定性将导致较大的深度不确定性。也就是说，如果特征点匹配误差为一个像素δx，使得视线角度偏差了δθ度，那么测量到的深度值有了δd的变化。从上图可以发现，t越大时，深度误差δd会越小，这说明，平移较大时，同样的相机分辨率下，三角化测量将更精准。

要提高三角化的精度，一种方式是提高特征点的提取精度，也就是提高图像的分辨率，但这会导致图像变大，增加计算成本。另一种方式是使平移量增大，但是这会导致两帧图像的外观发生明显的变化，使得特征的提取与匹配变得更难。

总而言之，增大平移，可能导致匹配点失效；而平移太小，则三角化精度不够。这就是三角化的矛盾。

(4) 单应矩阵H

单应矩阵H描述处于共同平面上的一些点在在两张图像之间的变换关系。
设图像I₁和I₂有一对匹配好的特征点，这个特征点对应的3D点落在平面P上，设平面满足方程：n^TP+d=0。整理得： -(n^TP/d)=1。

由s₂p₂ = K(RP+t)得以下式子：

把中间部分记为H，于是有 p₂≈Hp₁。H的定义与旋转，平移及平面的参数有关。H是一个3x3的矩阵，求解思路和E相似，可以先估计匹配点计算H，然后将他分解以计算旋转和平移。p₂≈Hp₁展开得：

得到了两项约束，于是自由度为8的单应矩阵可以通过4对匹配的特征点算出。即求以下线性方程组：

这种做法把H看成向量，通过求解改向量的线性方程来得出H，这种方法称为直接线性变换法DLT。

对求出的H进行分解得到R和t。分解的方法包括数值法和解析法。同样，单应矩阵的分解会返回四组旋转矩阵和平移向量组合的解。如果已知成像的地图点的深度全为正值，则又可以排除两组解，最后剩两组解，需要通过更多的先验信息进行判断。通过我们可以通过假设已知场景平面的法向量来求解，如场景平面与相机平面平行，那么法向量n的理论值为1^T。

2. 3D-2D: PnP

特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此：
(1) 双目或RGB-D的视觉里程计，可直接使用PnP估计相机运动。
(2) 单目视觉里程计，必须先进行初始化，然后才能使用PnP。

PnP问题又很多种求解方法，例如：用3对点估计位姿得P3P、直接线性变换(DLT)、EPnP、UPnP等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的光束平差法BA。

(1) 理解3D-2D中的3D

• 3D有两种情况：上一帧相机坐标系或者世界坐标系下特征点的3D坐标
• 2D：当前帧的像素坐标

---

相机成像模型中的公式：

因为3D有两种情况，所以位姿求解也就有以下两种场景：

【场景一】世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机相对于世界坐标系的位姿


【场景二】在上一帧的相机坐标系下的3D和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换


本质上还是对相机成像模型的理解。

P_i是上一帧相机坐标系下的点，则ξ是当前帧相对上一帧的位姿变换。Pi是世界坐标系下的点，则ξ就是当前帧相机相对世界坐标系的位姿。

(2) 直接线性变换DLT

空间中一点P（有两种场景），齐次坐标为 P = ( X , Y , Z , 1 ) ^T ，在图像中的投影坐标为 x ₁ = ( u ₁ , v ₁ , 1 ) ^T (以归一化平面齐次坐标表示) ，相机的位姿R,t未知。定义增光矩阵[R|t]为3*4的矩阵,包含旋转和平移。

① sp_uv=KP_c ； 把K移到左边，得：sK^-1p_uv=P_c
② x₁为归一化坐标，所以 x₁=K^-1p_uv
③ 又 P_c=RP+t （这里P为已知的3D点，世界/上一帧相机坐标下坐标点），写成P_c=[R|t]P
由以上可得公式：sx₁=[R|t]P

所以有

化简得：

用最后一行把s消去，得到两个约束：

然后有：u₁(t₉X+t₁₀Y+t₁₁Z+t₁₂) = t₁X+t₂Y+t₃Z+t₄；v₁(t₉X+t₁₀Y+t₁₁Z+t₁₂) = t₅X+t₆Y+t₇Z+t₈

定义[R|t]的行向量为：t₁=(t₁,t₂,t₃,t₄)^T，t₂=(t₅,t₆,t₇,t₈)^T，t₃=(t₉,t₁₀,t₁₁,t₁₂)^T

于是两个约束可以转换为：
u₁ t₃^T P= t₁^TP 即： t₁^TP - t₃^T P u₁ = 0
u₁ t₃^T P= t₂^TP 即： t₂^TP - t₃^T P v₁ = 0

综上，可以得知每个3D-2D的特征点提供了两个关于t_i的约束(t_i为待求量，如t₁t₂…t₁₂)。假设一共有N个这样的点,则可以列出如下的线性方程组：

t一共有12维，最少通过6对3D-2D的点即12个约束，才可以可线性求解矩阵T，这种方法称为“直接线性变换DLT”

用DLT求解出来的R不一定满足R∈SO(3)的约束，所以我们必须寻找一个最好的旋转矩阵对其进行近似。

(3) 最小重投影误差求解PnP

重投影误差法，也叫Bundle Adjustment(BA法)，顾名思义这个问题的误差项是3D点的投影位置与实际位置作差，如图1所示，p₁和p₂是同一个空间点P在不同相机姿态下的投影，但由于我们不知道相机姿态。在初始值中。P的投影为与实际的p₂有一定的距离。于是我们通过优化的方式调整位姿，使得这个距离变小。不过，由于这个调整需要考虑多个点，所以最后的效果是整体误差的缩小，而每个点的误差通常不会精确为零。

考虑n个三维空间点$P$及其投影$p$，计算相机的姿态$R,t$ ，它的李群表示为$T$。假设某空间点的坐标为$P_i=[X_i,Y_i,Z_i]$ ，其投影的像素坐标为$u_i=[u_i,v_i{]}^T$ 。根据相机模型，建立空间点位置与像素位置关系如公式(1):

$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]\dots \dots (1)$$

写成矩阵形式是：
$$s_i{u}_i=KT{P}_i$$

该式隐含了一次从齐次坐标到非齐次的转换，否则按矩阵的乘法来说维度是不对的，也就是$T{P}_i$ 是4x1的坐标，取出前三维变成非齐次坐标后再与内参相乘。

现在由于相机位姿位置以及观测点存在噪声(非slam问题可能没有就无需优化),该等式存在一个误差。因此，我们把误差求和，构建最小二乘法问题，然后寻找最优的相机位姿，使公式(3)最小化:

$$T^{\ast }=argmi{n}_T\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_{i}KT{P}_i}|{|}_2^2\dots \dots \dots (3)$$

该问题的误差项，是将3D点的投影位置与实际相机观测位置作差，所以称为重投影误差

对公式(3)进行优化的方法有很多，比如一阶梯度法、二阶梯度法、高斯牛顿法和列文伯格-马夸尔特方法等，这里使用高斯牛顿法，我们首先简单介绍下高斯牛顿法。

【高斯牛顿法】
高斯牛顿法是优化算法中最简单的方法。他的思想是将f(x)进行一阶的泰勒展开，也可以说是一阶线性化，注意这里不是目标函数F(x)，否则就变成牛顿法了。
$$f(x+\Delta x)\approx f(x)+J(x{)}^T\Delta x$$
这里J(x)^T 为f (x)关于x的导数,也是一个雅可比矩阵。当前的目标是寻找增量Δx，使得||f(x+Δx)||²达到最小。为了求Δx，我们需要解一个线性的最小二乘问题：
$$\Delta x^{\ast }=argmi{n}_{\Delta x}\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2$$
要使得||f(x+Δx)||²（||f(x+Δx)||²是与F(x)的二阶泰勒近似的式子）的值最小，根据极值条件，目标函数要对Δx求导，并令导数为0。为此，我们先展开目标函数的平方项：
$$\begin{gathered} \frac{1}{2}||f(x+\Delta x)|{|}^2=\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2 \\ =\frac{1}{2}(f(x)+J(x{)}^T\Delta x{)}^T(f(x)+J(x{)}^T\Delta x) \\ =\frac{1}{2}(||f(x)|{|}_2^2+2f(x)J(x{)}^T\Delta x+\Delta x^{T}J(x)J(x{)}^T\Delta x) \end{gathered}$$
求上式关于Δx的导数，并令其等于0：
$$J(x)f(x)+J(x)J^T(x)\Delta x=0$$
可以得到下面方程组：
$$\underset{H(x)}{\underbrace{J(x)J^T(x)}}\Delta x=\underset{g(x)}{\underbrace{-J(x)f(x)}}$$
上面这个方程式关于变量Δx的线性方程组 $H\Delta x=g$，称它为增量方程。求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出增量方程，那么高斯牛顿法的算法步骤可以写成：
(1).给定初始值x₀
(2).对于第k次迭代，求出当前的雅可比矩阵J (x) 和误差f ( x )
(3).求解增量方程 $H\Delta x=g$
(4).若Δ x 足够小，则停止。否则，令x~k + 1~ = x _k + Δ x _k ，返回第二步。

根据公式(3)设误差项如公式(6)所示：
$$e_i=u_i-\frac{1}{s_i}Kexp(\xi \widehat{)}{P}_i\dots \dots \dots (6)$$
e使用齐次坐标形式是三维，但由于最后一维始终为1，所以实际上是2维，由于要进行求导所以进行了一次T的指数映射，也就是T = exp(ξ^)，这里的$\xi$是T对应的李代数，维度为6x1。

根据高斯牛顿法对误差项进行一阶泰勒展开，也就是线性化的过程化：
$$e(x+△x)\approx e(x)+J^T△x\dots \dots \dots (7)$$
由于在高斯牛顿法中，关键是解出增量方程，而增量方程中含有J，所以J^T 是关键。我们固然可以使用数值导数，但如果能够推导出解析形式，则优先考虑解析导数。

现在，由于e是像素坐标的误差(2维)，x为相机的姿态，由于是李代数的形式，所以是6维，根据矩阵乘法，J^T是一个2 × 6 的矩阵。

根据公式(14)我们便可以求出J^T，，然后根据增量方程求解$△x$,那么$△x=\delta \xi$，然后优化T，使用李代数$T=exp(\delta \xi \widehat{)}\cdot T$，具体步骤在高斯牛顿法处介绍过。

如果需要优化特征点的空间位置，需要讨论e关于空间点P的导数，这个导数矩阵相对容易。仍利用链式法则，有：
$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial P}\dots \dots \dots (15)$$

第一项在前面已经做了推导，关于第二项，按照定义：
$$P^{\prime }=(TP{)}_{1:3}=RP+t\dots \dots \dots (16)$$

P’对P求导只剩下R，于是
$$\frac{\partial e}{\partial P}=\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]R$$

于是，我们推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化迭代。

(4) P3P

输入数据为3对3D-2D的点。记3D点位A,B,C，2D点位a,b,c。此外，P3P还需要使用一对验证点，用于从可能的解中选出正确的那一个，记为D和d。如下图所示

根据高斯牛顿法对误差项进行一阶泰勒展开，也就是线性化的过程化：

A,B,C是世界坐标系下的点，而不是相机坐标系下的点。一旦这一帧的3D点在相机坐标系下的坐标能够得出，那另一帧的3D点在相机坐标系下的坐标也能用同样的方法算出来，我们就能得到3D-3D的对应点，就可以把PnP问题转化为ICP问题了。

因此，P3P原理为利用三角形相似，求解投影点a,b,c在相机坐标系下的3D坐标，最后把问题转化为一个3D到3D的位姿估计问题。

由上图可知相似关系：△Oab-△OAB,△Obc-△OBC,△Oac-△OACA相似

利用余弦定理，可得：
$$\begin{gathered} O{A}^2+O{B}^2-2OA\cdot OB\cdot cos(a,b)=A{B}^2 \\ O{B}^2+O{C}^2-2OB\cdot OC\cdot cos(b,c)=B{C}^2 \\ O{A}^2+O{C}^2-2OA\cdot OC\cdot cos(a,c)=A{C}^2 \end{gathered}$$
对以上三式子全体除以OC²，并记x=OA/OC，y=OB/OC，得
$$\begin{gathered} x^2+y^2-2xycos(a,b)=A{B}^2/O{C}^2 \\ {y}^2+1^2-2ycos(b,c)=B{C}^2/O{C}^2 \\ {x}^2+1^2-2xcos(a,c)=A{C}^2/O{C}^2 \end{gathered}$$
记v=AB²/OC²，uv=BC²/OC²，wv=AC²/OC²，有
$$\begin{gathered} x^2+y^2-2xycos(a,b)-v=0 \\ {y}^2+1^2-2ycos(b,c)-uv=0 \\ {x}^2+1^2-2xcos(a,c)-wv=0 \end{gathered}$$

把上面第一个式子的v放在右边，并代入其后的两个式子，得
$$\begin{gathered} (1-u)y^2-u{x}^2-2cos(b,c)y+2uxycos(a,b)+1=0 \\ (1-w)x^2-w{y}^2-2cos(a,c)x+2wxycos(a,b)+1=0 \end{gathered}$$
由于我们知道2D点得图像位置，3个余弦角cao(a,b)，cos(b,c)，cao(a,c)是已知的。同时 u=BC²/AB²，w=AC²/AB²，可以通过ABC在世界坐标系下的坐标算出。上式中，x,y是未知的，随着相机移动会发生变化。

该方程组是关于x,y的一个二元二次方程组，求解是一个复杂的过程，需要用吴消元法。该方程最多可能得到4个解，要用验证点来计算嘴可能的解，得到A,B,C在相机坐标系下的3D坐标，然后根据3D-3D的点对计算相机运动的R和t。

P3P存在问题

1. 只利用三个点的信息，多于三组点时，难以利用。
2. 存在误匹配或者3D点或2D点受噪声影响，则算法失效。

3. 3D-3D: ICP

如果是在slam前端中，则是上一帧相机坐标系下的3D点与当前帧相机坐标系下的3D点，求出来的位姿也是当前帧相对上一帧的位姿。如果是在VR中，则是世界坐标系下的3D点与当前帧相机坐标系下的3D点，求出来的是当前帧相机相对世界的位姿。

假设我们有一组匹配号的3D点，(例如我们对两幅RGB-D图像进行匹配)：
P = { p 1 , … p n } , P ′ = { p 1 ′ , … p n ′ } P=\{p_1,…p_n\},P'=\{p_1',…p_n'\} P={p1​,…pn​},P′={p1′​,…pn′​}
现在想找一个欧式变换R,t，使得
$$\forall i,p_i=R{p}_i^{\prime }+t$$

(1) 线性代数SVD方法

我们先定义第i对点的误差项：
$$e_i=p_i-(R{p}_i^{\prime }+t)$$
然后构建最小二乘问题，使得误差平方达到最小，求得R和t

$$mi{n}_{R,t}\frac{1}{2}\sum _{i=1}^n||(p_i-(R{p}_i^{\prime }+t))|{|}_2^2$$

下面来推导它的求解方法，首先先定义两组点的质心：
$$p=\frac{1}{n}\sum _{i=1}^n(p_i)，p^{\prime }=\frac{1}{n}\sum _{i=1}^n(p_i^{\prime })$$
注意，质心是没有下标的。随后，在误差函数做如下处理：
$$\begin{gathered} \frac{1}{2}\sum _{i=1}^n||p_i-(R{p}_i^{\prime }+t)|{|}^2=\frac{1}{2}\sum _{i=1}^n||p_i-R{p}_i^{\prime }-t-p+R{p}^{\prime }+p-R{p}^{\prime }|{|}^2 \\ =\frac{1}{2}\sum _{i=1}^n(||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2+2(p_i-p-R(p_i^{\prime }-p^{\prime }){)}^T(p-R{p}^{\prime }-t)). \end{gathered}$$

注意到其中$(p_i-p-R(p_i^{\prime }-p^{\prime }))$在求和之后为0，因此优化目标函数可以简化为：
$$mi{n}_{R,t}J=\frac{1}{2}\sum _{i=1}^n(||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2)$$
仔细观察左右两项，我们发现左边只有R相关，而右边既有R也有t，但只和质心相关。只要我们获得R，令第二项为零就能得到t。于是ICP可分为以下三个步骤：

1. 计算两组质心的位置p,p’，然后计算每个点的去质心坐标
   $$q_i=p_i-p，q_i^{\prime }=p_i^{\prime }-p^{\prime }$$
2. 先用左边求R，根据以下优化问题计算旋转矩阵
   $$R^{\ast }=argmi{n}_r\frac{1}{2}\sum _{i=1}^n||q_i-R{q}_i^{\prime }||$$
3. 根据第二步的R计算t
   $$t^{\ast }=p-R{p}^{\prime }$$

先看第二步，展开关于R的误差项，得
$$\frac{1}{2}\sum _{i=1}^n||q_i-R{q}_i^{\prime }||=\frac{1}{2}\sum _{i=1}^n||q_{i}T{q}_i+q_i^{\prime T}{R}^{T}R{q}_i^T-2q_{i}TR{q}_i^{\prime }||$$

注意到第一项和R无关，第二项由于R^TR=I，业与R无关。因此，实际优化目标函数变为：
$$\sum _{i=t}^n-q_i^{T}R{q}_i^{\prime }=\sum _{i=t}^n-tr(R{q}_i^{\prime }{q}_i^T)=-tr(R\sum _{i=t}^n{q}_i^{\prime }{q}_i^T)$$

接下来通过SVD方法解出上述问题的最有R。先定义矩阵
$$W=\sum _{i=1}^n{q}_i{q}_i^{\prime T}$$
W是一个3x3的矩阵，对W进行SVD分解，得
$$W=U\sum V^T$$

其中，∑为奇异值组成的对角矩阵，对线元素从大到小排列，而U和V为对角矩阵，当W满秩时，R为
$$R=U{V}^T$$
解得R后，接着求解求解t。如果此时R的行列式为负值，则取-R为最优值。

(2) 非线性优化BA方法

以迭代的方式找最优值，与前面讲述的PnP非常类似。以李代数表示位姿时，目标函数可以写成
$$mi{n}_{\xi }=\frac{1}{2}\sum _{i=1}^n||(p_i-exp(\xi \stackrel{ˆ}{})p_i^{\prime }|{|}_2^2$$

单个误差项关于位姿的导数在前面已推导，使用李代数扰动模型即可：
$$\frac{\partial e}{\partial \delta \xi }=-(exp(\xi \stackrel{ˆ}{})p_i^{\prime })$$
于是，在非线性优化中，只需不断迭代，就能找到极小值。

ICP问题存在唯一值和无穷多解的情况。在唯一值情况，只能找到极小值解，这个极小值就是全局最优值。这也意味着ICP求解可以任意选定初始值。

视觉里程计2

特征点法的缺点：
①关键点的提取和描述子的匹配非常耗时。
②忽略了特征点以外的所有信息。一张图像的像素有几十万，特征点只要几百个。
③相机有时会运动到特征缺失的地方，比如白墙，或者空荡的走廊。这些场景特征点数量明显减少。
④由于只提取特征点，因此通过特征点法只能重构出稀疏地图。

解决这些缺点的思路：
①保留特征点，但只计算关键点（必须是角点），不计算描述子。使用光流法跟踪特征点的运动，这样可以减少计算和匹配描述子带来的时间，光流本身的计算时间小于描述子的计算与匹配。
这种方法仍然使用特征点，只是把匹配描述子换成了光流跟踪，估计相机的运动仍使用对极几何、PnP、ICP。这依然要求提取到的关键点要具有可区别性，即需要提取角点。

②只计算关键点（不要求提取到的必须是角点。甚至可以是随机的选点），不计算描述子。同时使用直接法计算特征点在下一时刻图像中的位置。这样跳过了描述子的计算过程，也省去了光流的计算时间。根据图像灰度信息同时估计相机运动和点的投影，不要求提取到的点必须为角点。

特征点法和直接点法的区别：
①特征点法：$最小化重投影误差$优化相机运动，需要精确地知道空间点在两个相机中投影后的像素位置。
直接法：$最小化光度误差$优化相机运动,不需要知道点和点之间的对应关系.

②直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子，避免了特征匹配的时间，也避免了特征缺失的情况

1. 2D光流

直接法从光流法发展而来。光流是一种描述像素随时间在图像之间的运动的方法。如下图所示，随着时间的流逝，同一个像素会在图像中运动，而我们希望追踪它的运动过程。

计算部分像素运动的称位稀疏光流，计算所有像素的称为稠密光流。其中，稀疏光流以Lucas-Kanade光流为代表。

(1) Lucas-Kanade光流

在LK光流中，我们认为图像是随时间变化的。图像可以看作时间函数：$I(t)$，那么，一个在t时刻，位于(x,y)的像素的灰度可以写成：$$I(x,y,t)$$

这样就把图像看成是一个关于位置与时间的函数，它的值域是图像中像素的灰度。

引入$灰度不变假设$：同一空间点的像素灰度值，在各个图像中是固定不变的。

对于t时刻位于(x,y)处的像素，我们设t+dt时刻它运动到(x+dx,y+dy)处，由于灰度不变，我们有：
$$I(x+dx,y+dy,t+dt)=I(x,y,t)$$

对左边进行泰勒展开，保留一阶项：
$$I(x+dx,y+dy,t+dt)\approx I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt$$
因为我们假设了灰度不变，于是下一时刻的灰度等于之前的灰度，从而：
$$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$$
两边除以dt得
$$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}$$
其中，dx/dt为像素在x轴上的运动速度，dy/dt为y轴上的速度，把它们记为u，v。$\partial I/\partial x$为图像在该点处x方向的梯度，$\partial I/\partial y$则为图像在该点处y方向的梯度，记为I_x,I_y。把图像灰度对时间的变化量记为I_t，写成矩阵形式，有
$$\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-I_t$$

像计算u,v，但该式是带有两个变量的一次方程，仅凭它无法计算出u,v。因此，必须引入额外的约束：在LK光流中，$假设某一窗口内的像素具有相同的运动$。

考虑一个大小为 $w\ast w$ 的窗口，它含有w²个像素。像素都具有同样的运动，因此有w²个方程：
$${\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]}_k\left[\begin{array}{c}u\\ v\end{array}\right]=-I_{tk},k=1,2,\dots ,w^2$$

记：

$$A=\left[\begin{array}{cc}[I_x& I_y{]}_1\\ \dots & \\ [I_x& I_y{]}_k\end{array}\right],b=\left[\begin{array}{c}{I}_{t1}\\ \dots \\ I_{tk}\end{array}\right]$$

于是整个方程为：
$$A\left[\begin{array}{c}u\\ v\end{array}\right]=-b$$

这是一个关于u，v的超定线性方程，传统的解法是求最小二乘解。最小二乘经常被用到：
$${\left[\begin{array}{c}u\\ v\end{array}\right]}^{\ast }=-(A^{T}A{)}^{-1}{A}^{T}b$$

这样就得到像素在图像间的运动速度u,v。当t取离散的时间而不是连续的时间，我们可以估计某块像素在若干图像中出现的位置。

(2) 多层光流

如果相机运动较快，两张图像差异较明显，那么单层图像光流容易达到一个局部极小值。这种情况可以通过引入图像金字塔来改善。

图像金字塔指对同一个图像进行缩放，得到不同分辨率下的图像。做法：以原始图像作为金字塔塔底，没往上一层，就对下层图像进行一定倍率缩放，就得到一个金字塔。然后计算光流时，先从塔顶层的图像开始计算，然后把上一层的追踪结果，作为下一层光流的初始值。由于上层图像相对粗糙，所以这个过程也称为由粗至精的光流，也是实用光流法的通常流程。

好处在于，当原始图像的像素较大时，在金字塔的顶层看来，运动仍然在一个很小的范围。

2. 直接法

回忆特征法由于我们通过匹配描述子，知道了p₁，p₂的像素位置。所以可以计算重投影误差的位置。但在直接法中，由于没有特征匹配，我们无从知道哪一个p₂与p₁对应着同一个点。

直接法的思路：根据当前相机的位姿估计值寻找p₂的位置，但若相机的位姿估计不够好，找到的p₂的外观会与p₁有明显的差别。于是，为了减小这个明显的差别，我们优化相机的位姿，来寻找与p₁更相似的p₂。

这同样可以解一个优化问题来完成，$但此时最小化的不是重投影误差，而是光度误差$。也就是P的两个像素的光度的误差
$$e=I_1(p_1)-I_2(p_2)$$

这里的e是一个标量，优化目标为该误差的二范数，暂时取不加权的形式
$$mi{n}_{T}J(T)=||e|{|}^2$$

能够做这种优化的理由，仍然是基于$灰度不变性$。我们假设一个看见点在各个视角下成像的灰度是不变的。我们有许多个(N个)空间点P_i，那么整个相机位姿估计问题变为：
$$mi{n}_{T}J(T)=\sum _{i=1}^N{e}_i^T{e}_i,e_i=I_1(p_{1,i})-I_2(p_{2,i})$$

这里优化的是相机位姿T，而光流法优化的是各个特征的运动。为了解这个优化问题，我们关心误差e是如何跟着相机位姿T变化的。需要分析它们的导数关系，因此定义两个中间变量：

$$\begin{array}{rl}\mathbf{q}& =\mathbf{T}\mathbf{P}\\ \mathbf{u}& =\frac{1}{Z_2}\mathbf{K}\mathbf{q}.\end{array}$$
这里的q为P在第二相机坐标系下的坐标，而u是它的像素坐标。显然，q是T的函数，而u是q的函数，从而u是T的函数。考虑李代数的做扰动模型，利用一阶泰勒展开，因为：
$$e(T)=I_1(p_1)-I_2(u)$$

所以：
$$\begin{array}{r}\end{array}\frac{\partial e}{\partial \mathbf{T}}=\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}\delta \mathbf{\xi },$$
其中，$\partial \xi$为T的左扰动。上面一阶导数由链式法则分成了3项，而这三项容易求算：

1. $\partial I_2/\partial u$为 $u$ 处的像素梯度
2. $\partial u/\partial q$为投影方程关于相机坐标系下的三维点的导数。记$q=[X,Y,Z]$，导数为：
   $$\partial u/\partial q=\left[\begin{array}{ccc}\frac{\partial u}{\partial X}& \frac{\partial u}{\partial Y}& \frac{\partial u}{\partial Z}\\ \frac{\partial v}{\partial X}& \frac{\partial v}{\partial Y}& \frac{\partial v}{\partial Z}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\end{array}\right]$$
3. $\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}$为变换后的三维点对变换的导数，由于后两项只与三维点q有关，而与图像无关，我们把它们合并在一起：
   $$\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}=\left[\begin{array}{cccccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}& -\frac{f_{x}XY}{Z^2}& f_x+\frac{f_{x}X}{Z^2}& -\frac{f_{x}Y}{Z}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}& -f_y-\frac{f_y{Y}^2}{Z^2}& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z}\end{array}\right]$$
   于是我们推导出误差相对于李代数的雅可比矩阵：$J=-\frac{\partial {\mathbf{I}}_2}{\partial u}\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}$

对于N个点的问题，我们可以用这种方法计算优化问题的雅可比矩阵，然后使用高斯牛顿法或列文伯格-马夸尔特方法计算增量。迭代求解

---

---

后端1

---

---

后端2

---

---

ORB-SlAM历代版本基本原理与系统架构

1. ORB-SLAM系列算法演进

(1) PTM（Parallel Tracking and Mapping）

PTM算法是基于关键帧的视觉SLAM算法，该算法由牛津大学 Georg Klein 和 David Murray 于 2007年提出。

PTM是第一个将跟踪和地图构建分成两个线程的SLAM算法：
1.跟踪线程负责跟踪相机的位姿，同时绘制虚拟的模型
2.地图构建线程负责建立场景的模型和绘制场景的地图

(2) ORB-SLAM

Raul Mur-Artal等人于2015年在PTM的基础上，提出了基于特征点的单目视觉SLAM算法ORB-SLAM

1. 跟踪线程
   提取ORB特征，根据上一帧进行初始位姿估计，或者通过全局重定位初始化相机位姿，然后跟踪已减重建的局部地图来优化位姿，最后根据一些规则输出关键帧
2. 局部建图线程
   包括插入关键帧，验证最近生成的地图点并进行筛选，同时生成新的地图点，使用局部BA，最后对插入关键帧进行筛选，去除多余的关键帧
3. 闭环检测线程
   分为闭环探测和闭环校正。首先通过BOW加速闭环匹配帧的选择，然后通过Sim3计算相似变换。最后通过闭环融合和本质图优化，实现闭环检测功能

(3) ORB-SLAM-VI

Raul Mur-Artal 等人于2017年在ORB-SLAM的基础上，加入惯性测量单元(Inertial Measurement Unit, IMU)，提出了新的紧耦合视觉-惯性SLAM系统。

针对于单目SLAM缺少的尺度信息，提出了新颖的IMU初始化的方法，以高精度快速的计算尺度，重力方向，速度以及陀螺仪和加速度计偏差，并重用地图，在已建图区域实现零漂移定位。

(4) ORB-SLAM-2

Raul Mur-Artal 等人于2017年发布了ORB-SLAM的改进版本ORB-SLAM2。
ORB-SLAM2从单目相机扩展到双目和RGB-D相机。

ORB-SLAM2是第一个适用于单目，双目，RGB-D相机的开元SLAM系统
ORB-SLAM2利用远近双目点和单目观察，使得在双目相机的情况下，算法的准确率高于直接法视觉SLAM

(5) ORB-SLAM-ATLAS

ORB-SLAM-ATLAS于2019年被提出，是一个能够处理无限数量的非连接的子图系统，包括一个鲁棒的地图合并算法，使得相机跟踪丢失时不会停止更新地图，立即构建一个新的字图。

1. 利用Atlas来解决不限数量字图融合问题。Atlas有不限数量的字图关键帧的词袋数据库，保证地图场景重识别效率
2. 多地图的操作算法：新地图生成，在混合地图中重定位和地图融合
3. 可以避免在闭环的过程中由于高度不确定的位姿导致的位姿图优化误差过大
   

(6) ORB-SLAM3

ORB-SLAM3是一个支持视觉，视觉+惯性，混合地图的SLAM系统，可以在单目，双目和RGB-D相机上利用针孔或鱼眼模型运行。

在大/小场景，室内/外，ORB-SLAM3都能鲁棒地实时运行。

---

2. ORB-SLAM3基本原理

ORB-SLAM3主要包括3个线程：跟踪，建图和闭环检测与地图重建

• 跟踪线程
  Tracking于Atlas部分相配合，决定当前帧是否为关键帧，为Atlas输送新的关键帧，并且对该帧计算最小优化重投影误差。VI模式中，通过IMU残差计算相机的速度与IMU偏差
• 建图线程
  添加新的关键帧于MapPoint到活动的地图中，删除冗余，利用滑动窗口通过BA更新地图。VI模式中，利用最大后验估计对IMU的参数进行初始化与更新
• 闭环检测与地图重建
  每添加一个关键帧，就检测活动地图与其他地图是否有重合：如果有重合，执行回环校正；如果重合区域不属于同一个地图，则将它们融合成一个。在校正后另开一个线程进行整体的BA进一步更新地图且不影响实时性

---

3. ORB-SLAM3子进程

Tracking子进程

Tracking线程用来处理传感器信息(图像和IMU)，决定何时将当前帧判定为关键帧。

• 计算当前帧相对于Active map的位姿以及最小化匹配到的地图点的重投影误差
• 在VI模式下，相机的位姿以及IMU的bias被通过优化惯性残差来估计
• 当系统跟踪丢失后，会触发重定位模式，即当前帧在所有的Atlas进行重定位
  若定位成功，当前帧恢复追踪状态
  若定位失败，经过一段时间(超过5s)，当前的Active map会被存储到Non-active map，同时开启新的建图线程

Local Mapping子线程

Local Mapping线程主要；用来构建地图点云

• 向Active map中，新增/删减/优化关键帧以及地图点，上述的操作是通过维护一个靠近当前帧的局部窗口的关键帧进行BA实现
• VI模式中，利用最大后验估计对IMU的参数进行初始化与更新

Loop & Map Merging子线程

• 在SLAM系统运行的过程中，若发现此时的相机位姿发生漂移时，将创建关键帧并送入Atlas地图册中，与Active map进行匹配分析
• 若有闭环检测中发现共同区域，则进行闭环校正和地图融合。Atlas将地图册分为两部分：Active map 和 Inactive map
• 其中Active map相当于地图册的工作区，用于和关键帧进行匹配和融合。 Inactive map则不进行匹配等相关工作以降低计算量

---

4. ORB-SLAM3运行效果演示

ORB-SLAM3使用的数据集 EuRoC 和 TUM-VI

EuRoC 利用微型飞行器(MAV)收集的视觉-惯性数据集
TUM-VI 是由实验人员手持视觉-惯性传感器收集的数据集

EuRoC

EuRoC包含11个双目序列，这些序列是由小型无人机在两个不同的房间和一个大型工业环境中飞行时记录下来的。

提供了两种类型的数据集：图片文件的ASL格式和适合ros的rosbag格式

TUM-VI

TUM-VI是由TUM在2018年发布的数据集，由实验人员手持设备进行拍摄，用于视觉惯性里程计的估计。
提供了标定序列和数据集序列，而且同一序列提供了不同的分辨率，分布为512x512和1024x1024，有图片文件夹tar压缩格式和适合ros的bag格式。

---

下载数据集Machine Hall 01。在项目ORBSLAM3目录下，创建dataset目录，把下载好的数据集放在dataset目录下。

在Example目录下，运行此指令

 ./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml ../dataset/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt

5. ORBSLAM3项目目录介绍

4. Tracking线程子模块详解

ORB-SLAM3初始化

运行ORB-SLAM3的第一步就是初始化。初始化ORB-SLAM3是通过System.cc开始。其对应的头文件为System.h。

• System.cc文件在src目录下
• System.h文件在include目录下

System.h

System.h文件中的头文件如下：

#include "Tracking.h"			//是Tracking.cc的头文件，即跟踪线程的代码
#include "FrameDrawer.h"	
#include "MapDrawer.h"
#include "Atlas.h"
#include "LocalMapping.h"		//是LocalMapping.cc的头文件，即局部建图线程的代码
#include "LoopClosing.h"		//是LoopClosing.cc的头文件，即闭环与地图合并线程的代码
#include "KeyFrameDatabase.h"
#include "ORBVocabulary.h"
#include "Viewer.h"
#include "ImuTypes.h"
#include "Settings.h"

System.cc

构造函数System::System()的参数

//ORB-SLAM3初始化是通过函数System开始的：
System::System(const string &strVocFile,    //磁带文件所在的路径
               const string &strSettingsFile, //配置文件所在的路径
               const eSensor sensor,    //传感器类型
               const bool bUseViewer,   //是否使用可视化界面
               const int initFr,        //表示初始化帧的id，开始设置为0
               const string &strSequence    //序列名，在跟踪线程和局部建图线程会用到
               ):

构造函数System::System()的功能

1. 检测Slam系统传感器初始化类型，需要用到枚举类型的变量eSensor
   该变量定义在System类中，是类System的公共成员变量

   enum eSensor{
           MONOCULAR=0,
           STEREO=1,
           RGBD=2,
           IMU_MONOCULAR=3,
           IMU_STEREO=4,
           IMU_RGBD=5,
       };
   

2. 检查传感器配置文件，也就是相机标定参数等，需要用到strSettingsFile

3. 实例化ORB词典对象，并加载ORB词典

4. 根据ORB词典实例对象，创建关键帧Database

5. 创建Altas，也就是创建多地图系统

6. IMU初始化设置，如果需要

7. 利用Altas创建Drawer，包括FrameDrawer和MapDrawer

8. 初始化Tracking线程

9. 初始化局部地图LocalMapping线程，并加载

10. 初始化闭环LocalClosing线程，并加载

11. 初始化用户可视化线程，并加载

12. 将上述实例化的对象指针，传入需要用到的线程，方便进行数据共享

传感器输入模块

Tracking线程子模块

Tracking线程代码详解