视觉SLAM笔记（43） 直接法

---

1. 直接法的推导

考虑某个空间点 $P$ 和两个时刻的相机
$P$ 的世界坐标为 $[X,Y,Z]$，它在两个相机上成像，记非齐次像素坐标为 $p$₁，$p$₂
目标是求第一个相机到第二个相机的相对位姿变换

以第一个相机为参照系，设第二个相机旋转和平移为 $R$，$t$（对应李代数为 $\xi$）
同时，两相机的内参相同，记为 $K$
为清楚起见，列写完整的投影方程：

其中 $Z$₁ 是 $P$ 的深度， $Z$₂ 是 $P$ 在第二个相机坐标系下的深度
也就是 $RP+t$ 的第三个坐标值
由于 exp(ξ^) 只能和齐次坐标相乘，所以乘完之后要取出前三个元素
这和之前的相机模型部分的内容是一致的

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

直接法的思路是根据当前相机的位姿估计值，来寻找 $p$₂ 的位置
但若相机位姿不够好， $p$₂ 的外观和 $p$₁ 会有明显差别
于是，为了减小这个差别，优化相机的位姿，来寻找与 $p$₁ 更相似的 $p$₂
这同样可以通过解一个优化问题，但此时最小化的不是重投影误差
而是 光度误差（Photometric Error），也就是 P 的两个像的 亮度误差：

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

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

注意这里的优化变量是相机位姿 $\xi$
为了求解这个优化问题，关心误差 e 是如何随着相机位姿 ξ 变化的，需要分析它们的导数关系
因此，使用李代数上的扰动模型，给 $exp(\xi )$ 左乘一个小扰动 $exp(\delta \xi )$，得：

记：

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

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

1. $\partial$$I$₂$/$$\partial$$u$ 为 $u$ 处的像素梯度

2. $\partial$$u/$$\partial$$q$ 为投影方程关于相机坐标系下的三维点的导数
   记 $q=[X,Y,Z]$^T，根据 视觉SLAM笔记（36） 3D-2D: PnP 的推导，导数为：
   

3. $\partial$$q/$$\partial$$\delta \xi$ 为变换后的三维点对变换的导数
   这在 视觉SLAM笔记（17） 李代数求导与扰动模型 已经介绍过了：
   
   在实践中，由于后两项只与三维点 $q$ 有关
   而与图像无关，经常把它合并在一起：
   
   这个 2 × 6 的矩阵在上一讲中也出现过

于是，推导了误差相对于李代数的雅可比矩阵：

对于 N 个点的问题，可以用这种方法计算优化问题的雅可比
然后使用 G-N 或 L-M 计算增量，迭代求解
至此，推导了直接法估计相机位姿的整个流程

---

2. 已知位置的空间点P

在上面的推导中， $P$ 是一个已知位置的空间点，它是怎么来的呢？
在 RGB-D 相机下，可以把任意像素反投影到三维空间，然后投影到下一个图像中
如果在单目相机中，这件事情要更为困难，因为还需考虑由 $P$ 的深度带来的不确定性

---

3. 直接法分类

现在先来考虑简单的情况，即 $P$ 深度已知的情况
根据 $P$ 的来源，可以把直接法进行分类：

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

可以看到，从稀疏到稠密重构，都可以用直接法来计算
它们的计算量是逐渐增长的
稀疏方法可以快速地求解相机位姿，而稠密方法可以建立完整地图
具体使用哪种方法，需要视机器人的应用环境而定

特别地，在低端的计算平台上
稀疏直接法可以做到非常快速的效果，适用于实时性较高且计算资源有限的场合

---

4. 优缺点

大体来说，它的优点如下：

• 可以省去计算特征点、描述子的时间

• 只要求有像素梯度即可，无须特征点
  因此，直接法可以在特征缺失的场合下使用比较极端的例子是只有渐变的一张图像
  它可能无法提取角点类特征，但可以用直接法估计它的运动

• 可以构建半稠密乃至稠密的地图，这是特征点法无法做到的

另一方面，它的缺点也很明显：

• 非凸性——直接法完全依靠梯度搜索，降低目标函数来计算相机位姿
  其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数
  这使得优化算法容易进入极小，只在运动很小时直接法才能成功

• 单个像素没有区分度，找一个和他像的实在太多了！
  于是要么计算图像块，要么计算复杂的相关性
  由于每个像素对改变相机运动的“意见”不一致
  只能少数服从多数，以数量代替质量

• 灰度值不变是很强的假设
  如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗
  光照变化时亦会出现这种情况
  特征点法对光照具有一定的容忍性
  而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败
  针对这一点，目前的直接法开始使用更细致的光度模型标定相机
  以便在曝光时间变化时也能让直接法工作

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（42） 光流法跟踪特征点
视觉SLAM笔记（41） 光流
视觉SLAM笔记（40） 特征点的缺陷
视觉SLAM笔记（39） 求解 ICP
视觉SLAM笔记（38） 3D-3D: ICP

---