slam十四讲 _直接法_slam直接法

视觉里程计

直接法是视觉里程计另一主要分支，它与特征点法有很大不同。虽然它还没有成为现 在 VO 中的主流，但经过近几年的发展，直接法在一定程度上已经能和特征点法平分秋色。
特征点法的缺点：

1. 关键点的提取与描述子的计算非常耗时。
2. 使用特征点时，忽略了除特征点以外的所有信息。一张图像有几十万个像素，而特征 点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
3. 相机有时会运动到特征缺失的地方，往往这些地方没有明显的纹理信息。例如，有时 我们会面对一堵白墙，或者一个空荡荡的走廓。这些场景下特征点数量会明显减少， 我们可能找不到足够的匹配点来计算相机运动。

怎样克服这些缺点？
1.保留特征点，，但只计算关键点，不计算描述子。同时，使用光流法（Optical Flow）来 跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算 需要一定时间；

2只计算关键点，不计算描述子。同时，使用直接法（Direct Method）来计算特征点 在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加 简单。

3既不计算关键点、也不计算描述子，而是根据像素灰度的差异，直接计算相机运动。

第一种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时 仍使用对极几何、PnP 或 ICP 算法。而在后两个方法中，我们会根据图像的像素灰度信 息来计算相机运动，它们都称为直接法。
使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它 们在相机中的投影位置，通过最小化重投影误差（Reprojectionerror）来优化相机运动.。在 这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我 们为何要对特征进行匹配或跟踪的理由。。同时，我们也知道，计算、匹配特征需要付出大 量的计算量。相对的，在直接法中，我们并不需要知道点与点之间之间的对应关系，而是 通过最小化光度误差（Photometric error）来求得它们。

直接法根据 像素的亮度信息，估计相机的运动，可以完全不用计算关键点和描述子，于是，既避免了 特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不 形成局部的图像梯度），直接法就能工作。根据使用像素的数量，直接法分为稀疏、稠密和 半稠密三种。相比于特征点法只能重构稀疏特征点（稀疏地图），直接法还具有恢复稠密或 半稠密结构的能力。

使用 直接法的开源项目（如 SVO[56]、LSD-SLAM[57] 等）

光流

直接法是从光流演变而来的。它们非常相似，具有相同的假设条件。光流描述了像素 在图像中的运动，而直接法则附带着一个相机运动模型。
光流是一种描述像素随着时间，在图像之间运动的方法。随着时间的经过，同一个像素会在图像中运动，而我们希望追踪它的运动过程。计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。稀疏光流以 Lucas-Kanade 光流为代表，并 可以在 SLAM 中用于跟踪特征点位置。

LK光流

在 LK 光流中，我们认为来自相机的图像是随时间变化的。图像可以看作时间的函数： I(t)。。那么，一个在 t 时刻，位于 (x,y) 处的像素，它的灰度可以写成I(x,y,t).这种方式把图像看成了关于位置与时间的函数（三维函数？），它的值域就是图像中像素的灰度。某个固定的空间点，它在 t 时刻的像素坐标为 x,y。由于相机的运动，它的图像坐标将 发生变化。我们希望估计这个空间点在其他时刻里图像的位置。怎么估计呢？这里要引入 光流法的基本假设：
灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。
对于 t 时刻位于 (x,y) 处的像素，我们设 t +dt 时刻，它运动到 (x +dx,y +dy) 处。 由于灰度不变，我们有：I(x +dx,y +dy,t +dt) = I(x,y,t).
灰度不变假设是一个很强的假设，实际当中很可能不成立。事实上，由于物体的材质 不同，像素会出现高光和阴影部分；有时，相机会自动调整曝光参数，使得图像整体变亮 或变暗。这些时候灰度不变假设都是不成立的，因此光流的结果也不一定可靠。然而，从 另一方面来说，所有算法都是在一定假设下工作的。如果我们什么假设都不做，就没法设 计实用的算法。所以，暂且让我们认为该假设成立，看看如何计算像素的运动。
对左边进行泰勒展开，保留一阶项，得：
因为我们假设了灰度不变，于是下一个时刻的灰度等于之前的灰度，从而


其中 dx/dt 为像素在 x 轴上运动速度，，而 dy/dt 为 y 轴速度，把它们记为 u,v。同 时 ∂I/∂x 为图像在该点处 x 方向的梯度，另一项则是在 y 方向的梯度，记为 Ix,Iy。把图像灰度对时间的变化量记为 It，写成矩阵形式，有：

我们想计算的是像素的运动 u,v，但是该式是带有两个变量的一次方程，仅凭它无法 计算出 u,v。因此，必须引入额外的约束来计算 u,v。在 LK 光流中，我们假设某一个窗 口内的像素具有相同的运动。
考虑一个大小为 w×w 大小的窗口，它含有 w2 数量的像素。由于该窗口内像素具有 同样的运动，因此我们共有 w2 个方程：



这是一个关于 u,v 的超定线性方程，传统解法是求最小二乘解。最小二乘在很多时候都用到过：

这样就得到了像素在图像间的运动速度 u,v。当 t 取离散的时刻而不是连续时间时，我 们可以估计某块像素在若干个图像中出现的位置。由于像素梯度仅在局部有效，所以如果 一次迭代不够好的话，我们会多迭代几次这个方程。在 SLAM 中，LK 光流常被用来跟踪 角点的运动。

LK光流实践

TUM 数据集。

直接法

考虑某个空间点 P 和两个时刻的相机。P 的世界坐标为 [X,Y,Z]，它 在两个相机上成像，记非齐次像素坐标为 p1,p2。我们的目标是求第一个相机到第二个相 机的相对位姿变换。我们以第一个相机为参照系，设第二个相机旋转和平移为 R,t（对应李代数为 ξ）。同时，两相机的内参相同，记为 K。

完整的投影方程：

其中 Z1 是 P 的深度，Z2 是 P 在第二个相机坐标系下的深度，也就是 RP + t 的第三个 坐标值。。由于 exp(ξ∧) 只能和齐次坐标相乘，所以我们乘完之后要取出前三个元素。
回忆特征点法中，由于我们通过匹配描述子，知道了 p1,p2 的像素位置，所以可以计 算重投影的位置。但在直接法中，由于没有特征匹配，我们无从知道哪一个 p2 与 p1 对应 着同一个点。。直接法的思路是根据当前相机的位姿估计值，来寻找 p2 的位置。但若相机 位姿不够好，p2 的外观和 p1 会有明显差别。于是，为了减小这个差别，我们优化相机的 位姿，来寻找与 p1 更相似的 p2。这同样可以通过解一个优化问题，但此时最小化的不是 重投影误差，而是光度误差（Photometric Error），也就是 P 的两个像的亮度误差：

注意这里 e 是一个标量，所以没有加粗。同样的，优化目标为该误差的二范数，暂时 取不加权的形式，为：

能够做这种优化的理由，仍是基于灰度不变假设。在直接法中，我们假设一个空间点 在各个视角下，成像的灰度是不变的。我们有许多个（比如 N 个）空间点 Pi，那么，整 个相机位姿估计问题变为：

注意这里的优化变量是相机位姿 ξ。
为了求解这个优化问题，我们关心误差 e 是如何 随着相机位姿 ξ 变化的，需要分析它们的导数关系。因此，使用李代数上的扰动模型。

我 们给 exp(ξ) 左乘一个小扰动 exp(δξ)，得：

类似于上一章，记

这里的 q 为 P 在扰动之后，位于第二个相机坐标系下的坐标，而 u 为它的像素坐标。
利用一阶泰勒展开，有：

我们看到，一阶导数由于链式法则分成了三项，而这三项都是容易计算的：

1. ∂I2/∂u 为 u 处的像素梯度；
2. ∂u/∂q 为投影方程关于相机坐标系下的三维点的导数。记 q = [X,Y,Z]T，根据上一 节的推导，导数为：
   
3. ∂q/∂δξ 为变换后的三维点对变换的导数，这在李代数章节已经介绍过了：
   
   在实践中，由于后两项只与三维点 q 有关，而与图像无关，我们经常把它合并在一起：
   
   于是，我们推导了误差相对于李代数的雅可 比矩阵：
   
   对于 N 个点的问题，我们可以用这种方法计算优化问题的雅可比，然后使用 G-N 或 L-M 计算增量，迭代求解。

直接法的讨论

在我们上面的推导中，P 是一个已知位置的空间点，它是怎么来的呢？现在我们先来考虑简单的情况，即 P 深度已知的情况。
根据 P 的来源，我们可以把直接法进行分类:

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

可以看到，从稀疏到稠密重构，都可以用直接法来计算。它们的计算量是逐渐增长的。 稀疏方法可以快速地求解相机位姿，而稠密方法可以建立完整地图。具体使用哪种方法，需 要视机器人的应用环境而定。特别地，在低端的计算平台上，稀疏直接法可以做到非常快 速的效果，适用于实时性较高且计算资源有限的场合 。
相比于特征点法，直接法完全依靠优化来求解相机位姿。 像素梯度引导着优化的方向。如果我们想要得到正确的优化结果，就必须保证大部分像素梯 度能够把优化引导到正确的方向。这是什么意思呢？我们不妨设身处地地扮演一下优化算法。假设对于参考图像，我们 测量到一个灰度值为 229 的像素。并且，由于我们知道它的深度，可以推断出空间点 P 的 位置（图 8-6 中在 I1 中测量到的灰度）。

此时我们又得到了一张新的图像，需要估计它的相机位姿。这个位姿是由一个初值不 断地优化迭代得到的。假设我们的初值比较差，在这个初值下，空间点 P 投影后的像素灰 度值是 126。。于是，这个像素的误差为 229−126 = 103。为了减小这个误差，我们希望微 调相机的位姿，使像素更亮一些。 怎么知道往哪里微调，像素会更亮呢？
这就需要用到局部的像素梯度。我们在图像中 发现，沿 u 轴往前走一步，该处的灰度值变成了 123，即减去了 3。同样地，沿 v 轴往前 走一步，灰度值减 18，变成 108。在这个像素周围，我们看到梯度是 [−3,−18]，为了提高 亮度，我们会建议优化算法微调相机，使 P 的像往左上方移动。在这个过程中，我们用像 素的局部梯度近似了它附近的灰度分布，不过请注意真实图像并不是光滑的，所以这个梯 度在远处就不成立了。
但是，优化算法不能只听这个像素的一面之词，还需要听取其他像素的建议¬。综合听 取了许多像素的意见之后，优化算法选择了一个和我们建议的方向偏离不远的地方，计算 出一个更新量 exp(ξ∧)。加上更新量后，图像从 I2 移动到了 I′ 2，像素的投影位置也变到了 一个更亮的地方。我们看到，通过这次更新，误差变小了。在理想情况下，我们期望误差 会不断下降，最后收敛。
但是实际是不是这样呢？我们是否真的只要沿着梯度方向走，就能走到一个最优值？注 意到，直接法的梯度是直接由图像梯度确定的，因此我们必须保证沿着图像梯度走时，灰 度误差会不断下降。然而，图像通常是一个很强烈的非凸函数，如图 8-7 所示。实际当中， 如果我们沿着图像梯度前进，很容易由于图像本身的非凸性（或噪声）落进一个局部极小值中，无法继续优化。只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接 法才能成立。

在例程中，我们只计算了单个像素的差异，并且这个差异是由灰度直接相减得到的。 然而，单个像素没有什么区分性，周围很可能有好多像素和它的亮度差不多。所以，我们 有时会使用小的图像块（patch），并且使用更复杂的差异度量方式，例如归一化相关性 .。而例程为了简单起见，使用了误 差的平方和，以保持和推导的一致性。

直接法优缺点总结

优点：
· 可以省去计算特征点、描述子的时间
· 只要求有像素梯度即可，无须特征点。因此，直接法可以在特征缺失的场合下使用。 比较极端的例子是只有渐变的一张图像。它可能无法提取角点类特征，但可以用直接 法估计它的运动。
·可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。
缺点：
· 非凸性——直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中 需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小， 只在运动很小时直接法才能成功。
· 单个像素没有区分度。找一个和他像的实在太多了！——于是我们要么计算图像块， 要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致。只能少数 服从多数，以数量代替质量。
·灰度值不变是很强的假设。如果相机是自动曝光的，当它调整曝光参数时，会使得图 像整体变亮或变暗。光照变化时亦会出现这种情况。特征点法对光照具有一定的容忍 性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失 败。针对这一点，目前的直接法开始使用更细致的光度模型标定相机，以便在曝光时 间变化时也能让直接法工作。

以上全部摘自 slam十四讲