SLAM技术入门：视觉里程计原理与实践

1 特征点法

从图像中选取比较有代表性的点，这些点在相机视角发生少量变化后会保持不变，可以在各个图像中找到相同的点。在经典 SLAM 模型中，这些点称为路标（ Landmark）。在视觉 SLAM 中，路标则是指图像特征（ Feature）。特征点由关键点（key-point）和描述子（descriptor）两部分组成。

• 关键点：该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。
• 描述子：通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量空间上的距离相近，就可以认为它们是同样的特征点。
• 特征点常提取的是角点，提取算法有Harris 角点、FAST 角点、GFTT 角点等。但有时候从远处看上去是角点的地方，当相机走近之后，可能就不显示为角点了。或者，当旋转相机时，角点的外观会发生变化，我们也就不容易辨认出那是同一个角点。现在已有的稳定的局部图像特征有：SIFT、SURF、ORB等。SIFT（尺度不变特征变换）性能强但计算量太大，很难实时计算。也有的适当降低精度和鲁棒性，以提升计算的速度，如FAST关键点（无描述子），但其不具有方向性。ORB特征是对其改进后的具有代表性的实时图像特征。

ORB 特征

ORB：Oriented FAST and Rotated BRIEF

1. 关键点：Oriented FAST，一种改进的 FAST 角点。FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。针对 FAST 角点不具有方向性和尺度的弱点， ORB 添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法（ Intensity Centroid）实现的。
2. 描述子：ORB 使用改进的 BRIEF 特征描述，BRIEF 是一种二进制描述子，其描述向量由许多个 0 和 1 组成，这里的 0 和 1 编码了关键点附近两个随机像素（比如 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就取 0。如果我们取了 128个这样的 p，q，最后就得到 128 维由 0、 1 组成的向量。BRIEF 使用了随机选点的比较，速度非常快，而且由于使用了二进制表达，存储起来也十分方便，适用于实时的图像匹配。原始的 BRIEF 描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而 ORB 在FAST 特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征使 ORB 的描述子具有较好的旋转不变性。

FAST 检测过程 在图像中选取像素 p p p，假设它的亮度为 I p I_p Ip​。 设置一个阈值 T T T（比如， I p I_p Ip​ 的 20%）。 以像素 p p p 为中心，选取半径为 3 的圆上的 16 个像素点。 假如选取的圆上有连续的 N N N 个点的亮度大于 I p + T I_p + T Ip​+T 或小于 I p − T I_p − T Ip​−T，那么像素 p p p 可以被认为是特征点（ N 通常取 12，即为 FAST-12)。 循环以上四步，对每一个像素执行相同的操作。

• 金字塔：指对图像进行不同层次的降采样，以获得不同分辨率的图像
  
      每往上一层，就对图像进行一个固定倍率的缩放，这样就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中，可以匹配不同层上的图像，从而实现尺度不变性。例如，如果相机在后退，那么应该能够在上一个图像金字塔的上层和下一个图像的下层中找到匹配。
• 灰度质心：质心是指以图像块灰度值作为权重的中心，在一个小的图像块 B 中，定义图像块的矩，通过矩找到图像块的质心，连接图像块的几何中心 O 与质心 C，得到一个方向向量  O C ⃗ \vec {OC} OC   ，于是可以定义特征点的方向。

特征匹配

最简单的特征匹配方法就是暴力匹配（ BruteForce Matcher）。即对每一个特征点  x t m x_t^m xtm​ 与所有的  x t + 1 n x_{t+1}^n xt+1n​ 测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示了两个特征之间的相似程度。而对于二进制的描述子（如BRIEF），使用汉明距离作为度量——两个二进制串之间的汉明距离，指的是其不同位数的个数。

   快速近似最近邻（ FLANN） 算法更加适合于匹配点数量极多的情况。

匹配好点对后根据点对来估计相机的运动。

• 单目：根据两组 2D 点估计运动，用对极几何来解决
• 双目、 RGB-D：根据两组 3D点估计运动，用 ICP 来解决
• 一组为 3D，一组为 2D：即一些 3D 点和它们在相机的投影位置，通过 PnP 求解。

2 对极几何（2D−2D）

2.1对极约束

• 几何关系
  

O 1 ， O 2 ， P O_1，O_2，P O1​，O2​，P 三点确定一个平面称为极平面  e 1 ， e 2 e_1，e_2 e1​，e2​ 称为极点（ Epipoles）， O 1 O 2 O_1O_2 O1​O2​ 被称为基线，极平面与两个像平面  I 1 , I 2 I_1,I_2 I1​,I2​ 之间的相交线  l 1 , l 2 l_1,l_2 l1​,l2​ 为极线（ Epipolar line）。

   特征点 p 1 p_1 p1​是 O 1 P O_1P O1​P所在直线上所有点的投影，特征点 p 2 p_2 p2​是 O 2 P O_2P O2​P所在直线上所有点的投影，又应为 p 1 p_1 p1​和 p 2 p_2 p2​匹配，所以就能推断出 P 的空间位置，以及相机的运动

• 代数关系
     

对极约束简洁地给出了两个匹配点的空间位置关系。相机位姿估计问题变为以下两步

1. 根据配对点的像素位置求出  E \pmb{E} EEE 或者  F \pmb{F} FFF。（实践中常用 E \pmb{E} EEE）
2. 根据  E \pmb{E} EEE 或者  F \pmb{F} FFF 求出  R ， t \pmb{R，t} R，tR，tR，t。

2.2 本质矩阵

E = t \pmb{E} = \pmb{t} EEE=ttt^ R \pmb{R} RRR

• 这是一个是一个 3 × 3 的矩阵，内有 9 个未知数。采用八点法估计  E \pmb{E} EEE，即使用8对点。
• 估得的本质矩阵  E \pmb{E} EEE 后 奇异值分解（ SVD） 得到相机的运动  R ， t \pmb{R，t} R，tR，tR，t。

从  E \pmb{E} EEE 分解到  R ， t \pmb{R，t} R，tR，tR，t 时，一共存在 4 个可能的解，只有第一种解中 P 在两个相机中都具有正的深度。因此，只要把任意一点代入 4 种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

2.3 单应矩阵

单应矩阵(Homography)  H \pmb{H} HHH，它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙、地面等），则可以通过单应性来进行运动估计

• 单应性在 SLAM 中具有重要意义。当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化（ degenerate）。现实中的数据总包含一些噪声，这时候如果继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵  F \pmb{F} FFF 和单应矩阵  H \pmb{H} HHH，选择重投影误差比较小的那个作为最终的运动估计矩阵。

尺度不确定性

E \pmb{E} EEE 具有尺度等价性，所以分解的  R , t \pmb{R,t} R,t​R,t​​R,t 也具有尺度等价性，但因为 R ∈ S O ( 3 ) \pmb{R} \in SO(3) RRR∈SO(3) 自身具有约束，所以  t \pmb{t} ttt 具有一个尺度，分解过程中对  t \pmb{t} ttt 乘以任意非零常数分解都成立，因此常对t进行归一化，使其长度为1。这直接导致了单目视觉的尺度不确定性。例如，程序中输出的  t \pmb{t} ttt 第一维约为 0.822。这个 0.822 究竟是指 0.822 米还是 0.822 厘米，我们是没法确定的。

• 初始化 ： 对两张图像的  t \pmb{t} ttt 归一化相当于固定了尺度。虽然不知道它的实际长度是多少，但以这时的  t \pmb{t} ttt 为单位 1，计算相机运动和特征点的 3D 位置。这被称为单目 SLAM 的初始化
• 另一种方法是令初始化时所有的特征点平均深度为 1，也可以固定一个尺度。相比于令 t 长度为 1 的做法，把特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定些。

初始化的纯旋转问题

• 单目初始化不能只有纯旋转，必须要有一定程度的平移。从  E \pmb{E} EEE 分解到  R , t \pmb{R,t} R,t​R,t​​R,t 的过程中，如果相机发生的是纯旋转，导致  t \pmb{t} ttt 为零，那么，得到的 E \pmb{E} EEE 也将为零，这将导致我们无从求解  R \pmb{R} RRR。让相机进行左右平移以顺利地进行初始化。

3 三角测量 (三角化 Triangulation)

• 对极几何约束估计了相机运动后需要用相机的运动估计特征点的空间位置

三角测量是指，通过在两处观察同一个点的夹角，从而确定该点的距离。理论上直线  O 1 p 1 O_1p_1 O1​p1​ 与  O 2 p 2 O_2p_2 O2​p2​ 在场景中会相交于一点  P P P，该点即两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。因此，可以通过最二小乘法求解。

• 设  x 1 ， x 2 x_1，x_2 x1​，x2​ 为两个特征点的归一化坐标，已知  R , t \pmb{R,t} R,t​R,t​​R,t ，求解两特征点深度 s 1 ， s 2 s_1，s_2 s1​，s2​
• 右侧可看成  s 2 s_2 s2​ 的一个方程，可以根据它直接求得  s 2 s_2 s2​，然后求出  s 1 s_1 s1​,得到了两帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，我们估得的  R , t \pmb{R,t} R,t​R,t​​R,t 不一定精确使上式为零，所以更常见的做法是求最小二乘解而不是零解。

三角测量的矛盾：增大平移，会导致匹配失效；而平移太小，则三角化精度不够

4 PnP（3D-2D)

• 描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。
• PnP 问题解法用三对点估计位姿的 P3P，直接线性变换（DLT），EPnP（Efficient PnP）等。还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment 。

1. 直接线性变换 设某个空间点 P P P 的齐次坐标为 P = ( X ， Y ， Z ， 1 ) T P = (X，Y，Z，1)^T P=(X，Y，Z，1)T。在图像 I 1 I_1 I1​ 中，投影到特征点 x 1 = ( u 1 , v 1 , 1 ) T x_1 = (u_1,v_1,1)^T x1​=(u1​,v1​,1)T（以归一化平面齐次坐标表示）。此时相机的位姿 R , t \pmb{R,t} R,t​R,t​​R,t 是未知的。与单应矩阵的求解类似，定义增广矩阵 [ R ∣ t ] [\pmb{R|t}] [R∣t​R∣t​​R∣t] 为一个 3 × 4 的矩阵，包含了旋转与平移信息。(和 SE(3) 中的变换矩阵 T \pmb{T} TTT 是不同的) 定义这个矩阵的行向量分别为 t 1 = ( t 1 , t 2 , t 3 , t 4 ) T , t 2 = ( t 5 , t 6 , t 7 , t 8 ) T , t 3 = ( t 9 , t 10 , t 11 , t 12 ) T t_1 = (t_1,t_2,t_3,t_4)^T , t_2 = (t_5,t_6,t_7,t_8)^T ,t_3 = (t_9,t_{10},t_{11},t_{12})^T t1​=(t1​,t2​,t3​,t4​)T,t2​=(t5​,t6​,t7​,t8​)T,t3​=(t9​,t10​,t11​,t12​)T t \pmb{t} ttt 是待求的变量，一共12维，最少六对匹配点可求解出该矩阵。 ( P 1 T 0 − u 1 P 1 T 0 P 1 T − v 1 P 1 T . . . . . . P N T 0 − u N P N T 0 P N T − v N P N T ) ( t 1 t 2 t 3 ) = 0 \left( PPT10..PPTN00PPT1..0PPTN−u1PPT1−v1PPT1..−uNPPTN−vNPPTN P P 1 T 0 − u 1 P P 1 T 0 P P 1 T − v 1 P P 1 T . . . . . . P P N T 0 − u N P P N T 0 P P N T − v N P P N T \begin{matrix} \pmb{P}_1^T &0 &-u_1\pmb{P}_1^T \\0 &\pmb{P}_1^T &-v_1\pmb{P}_1^T \\.&.&.\\ .&.&.\\\pmb{P}_N^T &0 &-u_N\pmb{P}_N^T \\0 &\pmb{P}_N^T &-v_N\pmb{P}_N^T \\ \end{matrix} \right)\left( tt1tt2tt3 t t 1 t t 2 t t 3 \begin{matrix} \pmb{t}_1\\\pmb{t}_2\\\pmb{t}_3\end{matrix} \right) =0 ⎝⎜⎜⎜⎜⎜⎜⎛​PPP1T​0..PPPNT​0​0PPP1T​..0PPPNT​​−u1​PPP1T​−v1​PPP1T​..−uN​PPPNT​−vN​PPPNT​​⎠⎟⎟⎟⎟⎟⎟⎞​⎝⎛​ttt1​ttt2​ttt3​​⎠⎞​=0 在 DLT 求解中，我们直接将 T \pmb{T} TTT 矩阵看成了 12 个未知数，忽略了它们之间的联系。因为旋转矩阵 R ∈ S O ( 3 ) \pmb{R} \in SO(3) RRR∈SO(3)，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。平移向量比较好办，它属于向量空间。对于旋转矩阵 R \pmb{R} RRR，我们必须针对 DLT 估计的 T \pmb{T} TTT 的左边3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成 [3, 48]，相当于把结果从矩阵空间重新投影到 S E ( 3 ) SE(3) SE(3) 流形上，转换成旋转和平移两部分。
2. P3P 仅使用三对匹配点，对数据要求较少。它的输入数据为三对 3D-2D 匹配点。记 3D点为 A，B，C， 2D 点为 a， b， c，其中小写字母代表的点为大写字母在相机成像平面上的投影。此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 D − d，相机光心为 O。求解思路是利用三角形相似性质，求解投影点 a，b，c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。 ( 1 − u ) y 2 − u x 2 − c o s ⟨ b , c ⟩ y + 2 u x y c o s ⟨ a , b ⟩ + 1 = 0 (1-u)y^2-ux^2-cos⟨b,c⟩y+2uxycos⟨a,b⟩+1 = 0 (1−u)y2−ux2−cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0 ( 1 − w ) x 2 − w y 2 − c o s ⟨ a , c ⟩ x + 2 w x y c o s ⟨ a , b ⟩ + 1 = 0 (1-w)x^2-wy^2-cos⟨a,c⟩x+2wxycos⟨a,b⟩+1 = 0 (1−w)x2−wy2−cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0 由于我们知道 2D 点的图像位置，三个余弦角cos ⟨a,b⟩ ，cos ⟨b,c⟩ ，cos ⟨a,c⟩ 是已知的。同时， u = B C 2 A B 2 u = \frac{BC^2}{AB^2} u=AB2BC2​， w = A C 2 A B 2 w = \frac{AC^2}{AB^2} w=AB2AC2​ 可以通过A，B，C 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的 x， y 是未知的，随着相机移动会发生变化。因此，该方程组是关于 x，y 的一个二元二次方程（多项式方程）。求解这个解析解比较麻烦，需要用吴消元法。该方程最多得到4个解，需要用验证点来验证最有可能的解。 P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。 在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。
3. Bundle Adjustment 我们可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题,是一个最小化重投影误差（Reprojection error）

• \right] = \pmb{K} exp(\pmb{ξ} si​⎣⎡​ui​vi​1​⎦⎤​=KKKexp(ξ​ξ​​ξ^ ) [ X i Y i Z i 1 ] )\left[  XiYiZi1      X i       Y i       Z i      1      \begin{matrix} X_i\\Y_i\\Z_i\\1 \end{matrix}\right] )⎣⎢⎢⎡​Xi​Yi​Zi​1​⎦⎥⎥⎤​ ——> s i u i = K e x p ( ξ s_i\pmb{u}_i = \pmb{K} exp(\pmb{ξ} si​uuui​=KKKexp(ξ​ξ​​ξ^ ) P i )\pmb{P}_i )PPPi​
  
      中间隐含着的齐次坐标到非齐次的转换。由于相机位姿未知以及观测点的噪声，该等式存在一个误差,是将像素坐标与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差就是重投影误差
  ξ ∗ = arg ⁡ min ⁡ ξ 1 2 ∑ i = 1 n ∣ ∣ u i − 1 s i K e x p ( ξ > ) P i ∣ ∣ 2 \pmb{ξ}^* = \underset{ξ}{\arg\min}\frac{1}{2}\sum_{i=1}^n ||\pmb{u}_i-\frac{1}{s_i}\pmb{K} exp(\pmb{ξ}^>)\pmb{P}_i||^2 ξ​ξ​​ξ∗=ξargmin​21​i=1∑n​∣∣uuui​−si​1​KKKexp(ξ​ξ​​ξ>)PPPi​∣∣2
  
      可以构建无约束的优化问题，通过 G-N, L-M 等优化算法进行求解
     
• ∂δξ​ξ​​ξ∂eee​​=δξ→0lim​δξ​ξ​​ξeee(δξ​ξ​​ξ⨁ξ​ξ​​ξ)​=∂PPP′∂eee​∂δξ​ξ​​ξ∂PPP′​=−[Z′fx​​0​0Z′fy​​​Z′2fx​X′​−Z′2fy​Y′​​−Z′2fx​X′Y′​−fy​−Z′2fy​Y′2​​fx​+Z′2fx​X2​Z′2fy​X′Y′​​−Z′fx​Y′​Z′fy​X′​​]​
• ∂PPP∂eee​​=∂PPP′∂eee​∂PPP∂PPP′​=−[Z′fx​​0​0Z′fy​​​−Z′2fx​X′​Z′2fy​Y′​​]RRR​
  BA优化
  
      节点：第二个相机的位姿节点  ξ ∈ S E ( 3 ) \pmb{ξ} \in SE(3) ξ​ξ​​ξ∈SE(3)，以及所有特征点的空间位置  P ∈ R 3 \bf{P} \in \R^3 P∈R3
  
      边：每个 3D 点在第二个相机中的投影，以观测方程来描述： z j = h ( ξ , P j ) z_j=h(\pmb\xi,\pmb{P}_j) zj​=h(ξ​ξ​​ξ,PPPj​)
  

5 ICP(3D-3D)

假设有一组配对好的 3D 点 ： P = \pmb{P} = PPP={ p 1 , . . . , p n p_1,...,p_n p1​,...,pn​}， P ′ = \pmb{P}' = PPP′={ p 1 ′ , . . . , p n ′ p_1',...,p_n' p1′​,...,pn′​}

   现在，想要找一个欧氏变换  R , t \pmb{R,t} R,t​R,t​​R,t，使得： ∀ i , p i = R p i ′ + t \forall i,\pmb{p}_i = \pmb{Rp}_i'+\pmb{t} ∀i,p​p​​pi​=Rp​Rp​​Rpi′​+ttt

   这个问题可以用迭代最近点（Iterative Closest Point, ICP）求解。

1. SVD方法（线性代数的求解） 首先，定义两组点的质心： p = 1 n ∑ i = 1 n ( p i ) \pmb{p} =\frac{1}{n} \sum_{i=1}^n(\pmb{p}_i) p​p​​p=n1​∑i=1n​(p​p​​pi​) ， p ′ = 1 n ∑ i = 1 n ( p i ′ ) \pmb{p}' =\frac{1}{n} \sum_{i=1}^n(\pmb{p}_i') p​p​​p′=n1​∑i=1n​(p​p​​pi′​) 然后计算每个点的去质心坐标： q i = p i − p \pmb{q}_i = \pmb{p}_i-\pmb{p} q​q​​qi​=p​p​​pi​−p​p​​p， q i ′ = p i ′ − p ′ \pmb{q}_i' = \pmb{p}_i'-\pmb{p}' q​q​​qi′​=p​p​​pi′​−p​p​​p′ 根据优化问题求解旋转矩阵 R ∗ = arg ⁡ min ⁡ R 1 2 ∑ i = 1 n ∣ ∣ q i − R q i ′ ∣ ∣ 2 \pmb{R}^* = \underset{\bf{R}}{\arg\min}\frac{1}{2}\sum_{i=1}^n ||\pmb{q}_i-\pmb{Rq}_i'||^2 RRR∗=Rargmin​21​i=1∑n​∣∣q​q​​qi​−Rq​Rq​​Rqi′​∣∣2 先定义3×3矩阵 W \bf W W： W = ∑ i = 1 n q i q i ′ T \pmb{W} = \sum_{i=1}^n\pmb{q}_i\pmb{q}_i'^T WWW=∑i=1n​q​q​​qi​q​q​​qi′T​ 对 W \bf W W 进行SVD分解: W = U ∑ V T \pmb{W} = \pmb{U\sum V}^T WWW=U∑V​U∑V​​U∑VT 其中， ∑ \pmb{\sum} ∑​∑​​∑为奇异值组成的对角矩阵，对角线元素从大到小排列，而 U \pmb{U} UUU 和 V \pmb{V} VVV 为正交矩阵。当 W \pmb{W} WWW 满秩时， R = U V T \pmb{R} = \pmb{UV}^T RRR=UVUVUVT 根据 R \pmb{R} RRR计算 t \pmb{t} ttt： t ∗ = p − R p ′ \pmb{t}^* = \pmb{p}-\pmb{Rp}' ttt∗=p​p​​p−Rp​Rp​​Rp′
2. 非线性优化方法 目标函数为： min ⁡ ξ = 1 2 ∑ i = 1 n ∣ ∣ p i − exp ⁡ ( ξ \underset{\pmb{\xi}}{\min} = \frac{1}{2}\sum_{i=1}^n ||\pmb{p}_i-\exp(\xi ξ​ξ​​ξmin​=21​∑i=1n​∣∣p​p​​pi​−exp(ξ^ ) p i ′ ∣ ∣ 2 2 )\pmb{p}_i'||_2^2 )p​p​​pi′​∣∣22​ 单个误差项关于位姿导数： ∂ e ∂ δ ξ = − ( exp ⁡ ( ξ \frac{\partial\pmb{e}}{\partial \delta\pmb{\xi}} = -(\exp(\xi ∂δξ​ξ​​ξ∂eee​=−(exp(ξ^ ) p i ′ ) ⨀ )\pmb{p}_i')^{\bigodot} )p​p​​pi′​)⨀

• 在RGB-D SLAM 中，由于一个像素的深度数据可能测量不到，所以我们可以混合着使用 PnP和 ICP 优化：对于深度已知的特征点，用建模它们的 3D-3D 误差；对于深度未知的特征点，则建模 3D-2D 的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。

直接法

• 特征点法的缺点

1. 关键点的提取与描述子的计算非常耗时
2. 使用特征点时，忽略了除特征点以外的所有信息
3. 相机有时会运动到特征缺失的地方，往往这些地方没有明显的纹理信息

• 解决以上问题的思路：

1. 保留特征点，但只计算关键点，不计算描述子。同时，使用光流法来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算需要一定时间；
2. 只计算关键点，不计算描述子。同时，使用直接法来计算特征点在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加简单。
3. 既不计算关键点、也不计算描述子，而是根据像素灰度的差异， 直接计算相机运动。

• 特征点法：最小化重投影误差——需要精确地知道空间点在两个相机中投影后的像素位置
• 直接法：最小化光度误差——根据像素的亮度信息，估计相机的运动，可以完全不用计算关键点和描述子，既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像梯度）。
• 根据使用像素的数量，直接法分为稀疏、稠密和半稠密三种。相比于特征点法只能重构稀疏特征点（稀疏地图），直接法还具有恢复稠密或半稠密结构的能力。

1 光流 ( Optical Flow )

光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较慢（或采集频率较高）

• 分类：

本节主要讲LK光流

• 一个在 t 时刻，位于 (x， y) 处的像素的灰度可以写成： I ( x , y , t ) \pmb{I}(x,y,t) III(x,y,t)
• 灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。
• 光流推导：
     
• 角点具有更好的辨识度，边缘次之，区块最少
• LK 光流跟踪能够直接得到特征点的对应关系。这个对应关系就像是描述子的匹配，但实际上大多数时候只会碰到特征点跟丢的情况，而不太会遇到误匹配，这应该是光流相对于描述子的一点优势。但是，匹配描述子的方法在相机运动较大时仍能成功，而光流必须要求相机运动是微小的。从这方面来说，光流的鲁棒性比描述子差一些。
• 通过光流跟踪的特征点，用 PnP、 ICP 或对极几何来估计相机运动。

2 直接法（Direct Methods）

已知某个空间点 P 的世界坐标为 [X，Y，Z]和它在两个相机上成像的非齐次像素坐标 p 1 ， p 2 \pmb{p}_1，\pmb{p}_2 p​p​​p1​，p​p​​p2​。我们的目标是求第一个相机到第二个相机的相对位姿变换。

特征点法中，通过匹配描述子，知道 p 1 ， p 2 \pmb{p}_1，\pmb{p}_2 p​p​​p1​，p​p​​p2​的像素位置，就可以定位空间点然后计算重投影的位置。

   直接法的思路是根据当前相机的位姿估计值，来寻找  p 2 \pmb{p}_2 p​p​​p2​ 的位置。若相机位姿不够好，  p 2 \pmb{p}_2 p​p​​p2​ 的外观和  p 1 \pmb{p}_1 p​p​​p1​ 会有明显差别（没有用投影，而是用的亮度）。为了减小这个差别，我们优化相机的位姿，来寻找与  p 1 \pmb{p}_1 p​p​​p1​ 更相似的  p 2 \pmb{p}_2 p​p​​p2​ 。

• 推导 以第一个相机为参照系，设第二个相机旋转和平移为 R , t \pmb{R,t} R,t​R,t​​R,t（对应李代数为 ξ \pmb{\xi} ξ​ξ​​ξ）。相机的内参相同为 K \pmb{K} KKK， Z 1 Z_1 Z1​ 是 P 的深度， Z 2 Z_2 Z2​ 是 P 在第二个相机坐标系下的深度。投影方程： p 1 = [ u v 1 ] 1 = 1 Z 1 K P \pmb{p}_1 = \left[ uv1 u v 1 \begin{matrix} u \\v\\1 \end{matrix}\right]_1 = \dfrac{1}{Z}_1\pmb{KP} p​p​​p1​=⎣⎡​uv1​⎦⎤​1​=Z1​1​KPKPKP p 2 = [ u v 1 ] 2 = 1 Z 2 K ( R P + t ) = 1 Z 2 K ( exp ⁡ ( ξ \pmb{p}_2 = \left[ uv1 u v 1 \begin{matrix} u \\v\\1 \end{matrix}\right]_2 = \dfrac{1}{Z}_2\pmb{K}(\pmb{RP+t}) = \dfrac{1}{Z}_2\pmb{K}(\exp(\pmb{\xi} p​p​​p2​=⎣⎡​uv1​⎦⎤​2​=Z1​2​KKK(RP+t​RP+t​​RP+t)=Z1​2​KKK(exp(ξ​ξ​​ξ^ ) P ) 1 : 3 )\pmb{P})_{1:3} )PPP)1:3​ 解一个优化问题———最小化光度误差，也就是 P 的两个像的亮度误差： e = I 1 ( p 1 ) − I 2 ( p 2 ) e = \pmb{I}_1(\pmb{p}_1)-\pmb{I}_2(\pmb{p}_2) e=III1​(p​p​​p1​)−III2​(p​p​​p2​)——> min ⁡ ξ J ( ξ ) = ∣ ∣ e ∣ ∣ 2 \underset{\pmb{\xi}}{\min} \pmb{J}(\pmb{\xi})=||e||^2 ξ​ξ​​ξmin​JJJ(ξ​ξ​​ξ)=∣∣e∣∣2 假设一个空间点在各个视角下，成像的灰度是不变的。N 个空间点 P i P_i Pi​的条件下整个相机位姿估计问题变为： min ⁡ ξ J ( ξ ) = ∑ i = 1 N e i T e i ， e i = I 1 ( p 1 , i ) − I 2 ( p 2 , i ) \underset{\pmb{\xi}}{\min} \pmb{J}(\pmb{\xi})=\sum_{i=1}^Ne_i^Te_i，e_i = \pmb{I}_1(\pmb{p}_1,i)-\pmb{I}_2(\pmb{p}_2,i) ξ​ξ​​ξmin​JJJ(ξ​ξ​​ξ)=i=1∑N​eiT​ei​，ei​=III1​(p​p​​p1​,i)−III2​(p​p​​p2​,i) 优化变量是相机位姿 ξ \pmb{\xi} ξ​ξ​​ξ ∂ e ∂ δ ξ = lim ⁡ δ ξ → 0 e ( δ ξ ⨁ ξ ) δ ξ \dfrac{\partial\pmb{e}}{\partial \delta\pmb{\xi}} = \lim_{\delta\xi \to 0} \dfrac{\pmb{e}(\delta\pmb{\xi} \bigoplus\pmb{\xi})}{\delta\pmb{\xi}} ∂δξ​ξ​​ξ∂eee​=limδξ→0​δξ​ξ​​ξeee(δξ​ξ​​ξ⨁ξ​ξ​​ξ)​ e ( δ ξ ⨁ ξ ) = I 1 ( 1 Z 1 K P ) − I 2 ( 1 Z 2 K exp ⁡ ( δ ξ ∧ ) exp ⁡ ( ξ ∧ ) P ) ≈ I 1 ( 1 Z 1 K P ) − I 2 ( 1 Z 2 K ( 1 + δ ξ ∧ ) exp ⁡ ( ξ ∧ ) P = I 1 ( 1 Z 1 K P ) − I 2 ( 1 Z 2 K exp ⁡ ( ξ ∧ ) P + 1 Z 2 K δ ξ ∧ exp ⁡ ( ξ ∧ ) P ) ee(δξξ⨁ξξ)=II1(1Z1KPKP)−II2(1Z2KKexp(δξ∧)exp(ξ∧)PP)≈II1(1Z1KPKP)−II2(1Z2KK(1+δξ∧)exp(ξ∧)PP=II1(1Z1KPKP)−II2(1Z2KKexp(ξ∧)PP+1Z2KKδξ∧exp(ξ∧)PP) e e ( δ ξ ξ ⨁ ξ ξ ) = I I 1 ( 1 Z 1 K P K P ) − I I 2 ( 1 Z 2 K K exp ⁡ ( δ ξ∧ ) exp ⁡ ( ξ∧ ) P P ) ≈ I I 1 ( 1 Z 1 K P K P ) − I I 2 ( 1 Z 2 K K ( 1 + δ ξ∧ ) exp ⁡ ( ξ∧ ) P P = I I 1 ( 1 Z 1 K P K P ) − I I 2 ( 1 Z 2 K K exp ⁡ ( ξ∧ ) P P + 1 Z 2 K K δ ξ∧ exp ⁡ ( ξ∧ ) P P ) \begin{aligned}\pmb{e}(\delta\pmb{\xi} \bigoplus\pmb{\xi}) &= \pmb{I}_1(\dfrac{1}{Z_1}\pmb{KP})-\pmb{I}_2(\dfrac{1}{Z_2}\pmb{K}\exp(\delta\xi^\wedge)\exp(\xi^\wedge)\pmb{P})\\ & \approx \pmb{I}_1(\dfrac{1}{Z_1}\pmb{KP})-\pmb{I}_2(\dfrac{1}{Z_2}\pmb{K}(1+\delta\xi^\wedge)\exp(\xi^\wedge)\pmb{P}\\ &=\pmb{I}_1(\dfrac{1}{Z_1}\pmb{KP})-\pmb{I}_2(\dfrac{1}{Z_2}\pmb{K}\exp(\xi^\wedge)\pmb{P}+\dfrac{1}{Z_2}\pmb{K}\delta\xi^\wedge\exp(\xi^\wedge)\pmb{P}) \end{aligned}eee(δξ​ξ​​ξ⨁ξ​ξ​​ξ)​=III1​(Z1​1​KPKPKP)−III2​(Z2​1​KKKexp(δξ∧)exp(ξ∧)PPP)≈III1​(Z1​1​KPKPKP)−III2​(Z2​1​KKK(1+δξ∧)exp(ξ∧)PPP=III1​(Z1​1​KPKPKP)−III2​(Z2​1​KKKexp(ξ∧)PPP+Z2​1​KKKδξ∧exp(ξ∧)PPP)​ 设 q \pmb{q} q​q​​q 为 P \pmb{P} PPP 在扰动之后，位于第二个相机坐标系下的坐标，而 u \pmb{u} uuu 为它的像素坐标 q = δ ξ \pmb{q} = \delta\pmb{\xi} q​q​​q=δξ​ξ​​ξ^ exp ⁡ ( ξ \exp(\pmb{\xi} exp(ξ​ξ​​ξ ^ ) P )\pmb{P} )PPP， u = 1 Z 2 K q \pmb{u} = \dfrac{1}{Z_2}\pmb{Kq} uuu=Z2​1​Kq​Kq​​Kq 一阶泰勒展开得 e ( δ ξ ⨁ ξ ) = I 1 ( 1 Z 1 K P ) − I 2 ( 1 Z 2 K exp ⁡ ( ξ ∧ ) P + u ) ≈ I 1 ( 1 Z 1 K P ) − I 2 ( 1 Z 2 K exp ⁡ ( ξ ∧ ) P ) − ∂ I 2 ∂ u ∂ u ∂ q ∂ q ∂ δ ξ δ ξ = e ( ξ ) − ∂ I 2 ∂ u ∂ u ∂ q ∂ q ∂ δ ξ δ ξ ee(δξξ⨁ξξ)=II1(1Z1KPKP)−II2(1Z2KKexp(ξ∧)PP+uu)≈II1(1Z1KPKP)−II2(1Z2KKexp(ξ∧)PP)−∂II2∂uu∂uu∂qq∂qq∂δξξδξξ=e(ξξ)−∂II2∂uu∂uu∂qq∂qq∂δξξδξξ e e ( δ ξ ξ ⨁ ξ ξ ) = I I 1 ( 1 Z 1 K P K P ) − I I 2 ( 1 Z 2 K K exp ⁡ ( ξ∧ ) P P + u u ) ≈ I I 1 ( 1 Z 1 K P K P ) − I I 2 ( 1 Z 2 K K exp ⁡ ( ξ∧ ) P P ) − ∂ I I 2 ∂ u u ∂ u u ∂ q q ∂ q q ∂ δ ξ ξ δ ξ ξ = e ( ξ ξ ) − ∂ I I 2 ∂ u u ∂ u u ∂ q q ∂ q q ∂ δ ξ ξ δ ξ ξ \begin{aligned}\pmb{e}(\delta\pmb{\xi} \bigoplus\pmb{\xi}) &= \pmb{I}_1(\dfrac{1}{Z_1}\pmb{KP})-\pmb{I}_2(\dfrac{1}{Z_2}\pmb{K}\exp(\xi^\wedge)\pmb{P}+\pmb{u}) \\ & \approx \pmb{I}_1(\dfrac{1}{Z_1}\pmb{KP})-\pmb{I}_2(\dfrac{1}{Z_2}\pmb{K}\exp(\xi^\wedge)\pmb{P})-\dfrac{\partial\pmb{I}_2}{\partial \pmb{u}}\dfrac{\partial\pmb{u}}{\partial \pmb{q}}\dfrac{\partial\pmb{q}}{\partial \delta\pmb{\xi}}\delta\pmb{\xi}\\ & = e(\pmb{\xi})-\dfrac{\partial\pmb{I}_2}{\partial \pmb{u}}\dfrac{\partial\pmb{u}}{\partial \pmb{q}}\dfrac{\partial\pmb{q}}{\partial \delta\pmb{\xi}}\delta\pmb{\xi} \end{aligned}eee(δξ​ξ​​ξ⨁ξ​ξ​​ξ)​=III1​(Z1​1​KPKPKP)−III2​(Z2​1​KKKexp(ξ∧)PPP+uuu)≈III1​(Z1​1​KPKPKP)−III2​(Z2​1​KKKexp(ξ∧)PPP)−∂uuu∂III2​​∂q​q​​q∂uuu​∂δξ​ξ​​ξ∂q​q​​q​δξ​ξ​​ξ=e(ξ​ξ​​ξ)−∂uuu∂III2​​∂q​q​​q∂uuu​∂δξ​ξ​​ξ∂q​q​​q​δξ​ξ​​ξ​ ∂ I 2 ∂ u \dfrac{\partial\pmb{I}_2}{\partial \pmb{u}} ∂uuu∂III2​​为 u \pmb{u} uuu 处的像素梯度； ∂ u ∂ q \dfrac{\partial\pmb{u}}{\partial \pmb{q}} ∂q​q​​q∂uuu​为投影方程关于相机坐标系下的三维点的导数; ∂ q ∂ δ ξ \dfrac{\partial\pmb{q}}{\partial \delta\pmb{\xi}} ∂δξ​ξ​​ξ∂q​q​​q​为变换后的三维点对变换的导数; 后两项只与三维点 ± q \pm{q} ±q 有关，而与图像无关，把它合并在一起： ∂ u ∂ δ ξ = [ f x Z ′ 0 f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ] \dfrac{\partial\pmb{u}}{\partial \delta\pmb{\xi}} = \left[ fxZ′00fyZ′fxX′Z′2−fyY′Z′2−fxX′Y′Z′2−fy−fyY′2Z′2fx+fxX2Z′2fyX′Y′Z′2−fxY′Z′fyX′Z′ f x Z′ 0 f x X′ Z ′ 2 − f x X′ Y′ Z ′ 2 f x + f x X2 Z ′ 2 − f x Y′ Z′ 0 f y Z′ − f y Y′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X′ Y′ Z ′ 2 f y X′ Z′ \begin{matrix} \frac{f_x}{Z'}&0 &\frac{f_xX'}{Z'^2} & -\frac{f_xX'Y'}{Z'^2}&f_x+\frac{f_xX^2}{Z'^2}&-\frac{f_xY'}{Z'}\\0 &\frac{f_y}{Z'} & -\frac{f_yY'}{Z'^2} &-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'} \end{matrix}\right] ∂δξ​ξ​​ξ∂uuu​=[Z′fx​​0​0Z′fy​​​Z′2fx​X′​−Z′2fy​Y′​​−Z′2fx​X′Y′​−fy​−Z′2fy​Y′2​​fx​+Z′2fx​X2​Z′2fy​X′Y′​​−Z′fx​Y′​Z′fy​X′​​] -所以误差相对于李代数的雅可比矩阵： J = − ∂ I 2 ∂ u ∂ u ∂ δ ξ \pmb{J} = - \dfrac{\partial\pmb{I}_2}{\partial \pmb{u}}\dfrac{\partial\pmb{u}}{\partial \delta\pmb{\xi}} JJJ=−∂uuu∂III2​​∂δξ​ξ​​ξ∂uuu​ 对于 N 个点的问题，可以用这种方法计算优化问题的雅可比，然后使用 G-N 或L-M 计算增量，迭代求解
• 分类 P 是一个已知位置的空间点，它是怎么来的呢？

1. P 来自于稀疏关键点，我们称之为稀疏直接法。通常我们使用数百个至上千个关键点，并且像 L-K 光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度最快，但只能计算稀疏的重构。
2. P 来自部分像素。如果像素梯度为零，整一项雅可比就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。这称之为半稠密（Semi-Dense）的直接法，可以重构一个半稠密结构。
3. P 为所有像素，称为稠密直接法。稠密重构需要计算所有像素（一般几十万至几百万个），因此多数不能在现有的 CPU 上实时计算，需要 GPU 的加速。但是，如前面所讨论的，梯度不明显的点，在运动估计中不会有太大贡献，在重构时也会难以估计位置

3 直接法的讨论

• 相比于特征点法，直接法完全依靠优化来求解相机位姿。像素梯度引导着优化的方向。如果我们想要得到正确的优化结果，就必须保证大部分像素梯度能够把优化引导到正确的方向。 在 I 1 I_1 I1​中测的已知深度的 P P P灰度值为229，又得到了一张新的图像，需要估计它的相机位姿。这个位姿是由一个初值不断地优化迭代得到的。假设我们的初值比较差，在这个初值下，空间点 P 投影后的像素灰度值是 126。于是，这个像素的误差为 229 − 126 = 103。为了减小这个误差，我们希望微调相机的位姿，使像素更亮一些。 沿 u u u 轴往前走一步，该处的灰度值变成了 123，即减去了 3。同样地，沿 v v v 轴往前走一步，灰度值减 18，变成 108。在这个像素周围，我们看到梯度是 [−3，−18]，为了提高亮度，我们会建议优化算法微调相机，使 P 的像往左上方移动。这个过程中，我们用像素的局部梯度近似了它附近的灰度分布，不过请注意真实图像并不是光滑的，所以这个梯度在远处就不成立了。 但是，优化算法不能只听这个像素的一面之词，还需要听取其他像素的建议。综合听取了许多像素的意见之后，优化算法选择了一个和我们建议的方向偏离不远的地方，计算出一个更新量 exp(ξ^)。加上更新量后，图像从 I 2 I_2 I2​ 移动到了 I 2 ′ I_2^{'} I2′​，像素的投影位置也变到了一个更亮的地方。通过这次更新，误差变小了。 直接法的梯度是直接由图像梯度确定的，因此我们必须保证沿着图像梯度走时，灰度误差会不断下降。然而，图像通常是一个很强烈的非凸函数，很容易由于图像本身的非凸性（或噪声）落进一个局部极小值中，无法继续优化。只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接法才能成立。 在例程中，我们只计算了单个像素的差异，并且这个差异是由灰度直接相减得到的。然而，单个像素没有什么区分性，周围很可能有好多像素和它的亮度差不多。所以，我们有时会使用小的图像块（patch），并且使用更复杂的差异度量方式，例如归一化相关性（ NCC）等。 8.4 直接法的优缺点 优点： 可以省去计算特征点、描述子的时间 只要求有像素梯度即可，无须特征点。可以在特征缺失的场合下使用，比较极端的例子是只有渐变的一张图像。它可能无法提取角点类特征，但可以用直接法估计它的运动 可以构建半稠密乃至稠密的地图 缺点： 非凸性——直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。 单个像素没有区分度. 找一个和他像的实在太多了！——于是我们要么计算图像块，要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致。只能少数服从多数，以数量代替质量 灰度值不变是很强的假设. 如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时亦会出现这种情况。特征点法对光照具有一定的容忍性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败。针对这一点，目前的直接法开始使用更细致的光度模型标定相机，以便在曝光时间变化时也能让直接法工作

免责声明：本文系网络转载或改编，未找到原创作者，版权归原作者所有。如涉及版权，请联系删