视觉里程计2

习题二:直接法

2.1 单层直接法

我们说直接法是光流的直观拓展。在光流中,我们估计的是每个像素的平移(在additive的情况下)。而在直接法当中,我们最小化光流误差,来估计相机的旋转和平移(以李代数的形式)。现在我们将使用和前一个习题非常相似的做法来实现直接法,请同学体现二者之间的紧密联系。
本习题中,你将使用Kitti数据集中的一些图像。给定left.pngdisparity.png,我们知道,通过这两个图可以得到left.png中任意一点的3D信息。现在,请你使用直接法,估计图像000001.png000005.png的相机位姿。我们称left.png为参考图像(reference,简称ref),称000001.png -000005.png中任意一图为当前图像(current,简称cur),如下图所示。设待估计的目标为 $ T_ {cur,ref} $ ,那么在ref中取一组点 $ { p_ i } $ ,位姿可以通过最小化下面的目标函数求解:

$$ \mathbf{T}_{\mathrm{cur}, \mathrm{ref}}=\frac{1}{N} \sum_{i=1}^{N} \sum_{W_{i}}\left\|I_{\mathrm{ref}}\left(\mathbf{p}_{i}\right)-I_{\mathrm{cur}}\left(\pi\left(\mathbf{T}_{\mathrm{cur}, \mathrm{ref}} \pi^{-1}\left(\mathbf{p}_{i}\right)\right)\right)\right\|_{2}^{2} $$

其中N为点数,π函数为针孔相机的投影函数 $ R^3 → R^2 $,$π^{−1}$为反投影函数,$ W_ i$为第i个点周围的小窗口。同光流法,该问题可由Gauss-Newton函数求解。请回答下列问题,然后实现code/direct_method.cpp中的DirectPoseEstimationSingleLayer函数。 然后回答一下问题:

  1. 该问题中的误差项是什么?
  2. 误差相对于自变量的雅可比维度是多少?如何求解?
  3. 窗又可以取多大?是否可以取单个点?

下面是一些实现过程中的提示:

  1. 这次我们在参考图像中随机取1000个点,而不是取角点。请思考为何不取角点,直接法也能工作。
  2. 由于相机运动,参考图像中的点可能在投影之后,跑到后续图像的外部。所以最后的目标函数要对投影在内部的点求平均误差,而不是对所有点求平均。程序中我们以good标记出投影在内部的点。
  3. 单层直接法的效果不会很好,但是你可以查看每次迭代的目标函数都会下降。

答案:

1.直接法的目标函数为:

$$ \underset{\xi}{\operatorname{argmin}} \frac{1}{N} \sum_{i=0}^{N} \sum_{W_{i}}\left\|I_{1}\left(p_{1}, i\right)-I_{2}\left(p_{2}, i\right)\right\|^{2} $$

误差项为:

$$ g(\xi)=I_{1}\left(p_{1}, i\right)-I_{2}\left(p_{2}, i\right) $$

其中$p_1,p_2$为相机归一化平面的坐标,$\xi$为$R,t$对应的李代数

$$ p_1 = \frac{1}{Z_1}KP \\ p_2 = \frac{1}{Z_2}K(RP+t) = \frac{1}K(exp(\xi)P)_{1:3} $$

2.误差项相对于自变量的雅克比维度为$1\times 6$,大小计算可以通过自变量的个数进行判断,例如本题目中位姿的自由度为6,误差函数中的光度的变化,一维的数据,所以雅克比矩阵为$1\times 6$。上一讲中,求解PnP问题时,误差函数中自变量是像素的坐标,是一个二维的,所以对应的雅克比矩阵大小为$2\times 6$。具体的求解过程如下:

$$ J(\xi)=\frac{\partial g}{\partial \delta \xi}=-\frac{\partial I_{2}}{\partial P_{u v}} \frac{\partial P_{u v}}{\partial P_{c}} \frac{\partial P_{c}}{\partial \delta \xi} $$

其中:

$$ \frac{\partial P_{c}}{\partial \delta \xi}=\left[I,-P_{c}^{\wedge}\right] $$

$$ \frac{\partial P_{u v}}{\partial P_{c}}=\left[ \begin{array}{ccc}{\frac{f_{x}}{z_{c}}} & {0} & {-\frac{f_{x} x_{c}}{z_{c}^{2}}} \\ {0} & {\frac{f_{y}}{z_{c}}} & {-\frac{f_{y} y_{c}}{z_{c}^{2}}}\end{array}\right] $$

最后带入所有的公式,我们可以求得雅克比矩阵为:

$$ J(\xi)=-\frac{\partial I_{2}}{\partial P_{u v}} \frac{\partial P_{u v}}{\partial \delta \xi}=-\frac{\partial I_{2}}{\partial P_{u v}} \left[ \begin{array}{ccccc}{\frac{f_{x}}{z_{c}}} & {0} & {-\frac{f_{x} x_{c}}{z_{c}^{2}}} & {-\frac{f_{x} x_{c} y_{c}}{z_{c}^{2}}} & {f_{x}+\frac{f_{x} x_{c}^{2}}{z_{c}^{2}}} & {-\frac{f_{x} y_{c}}{z_{c}}} \\ {0} & {\frac{f_{y}}{z_{c}}} & {-\frac{f_{y} y_{c}}{z_{c}^{2}}} & {-f_{y}-\frac{f_{y} y_{c}^{2}}{z_{c}^{2}}} & {\frac{f_{y} x_{c} y_{c}}{z_{c}^{2}}} & {\frac{f_{y} x_{c}}{z_{c}}}\end{array}\right] $$

3.窗口的大小和误差以及运算量有关。在本题目中,同一个窗口内计算了像素梯度,但是采用了相同的深度信息,当像素块越大,像素块内的深度信息误差也越大;同时,窗口大小的增大,两帧之间的关键点的计算量将会呈指数增长。所以窗口不能取太大; 当然窗口可以取单个点。但是这样会导致像素梯度计算误差增大,在本题目中,取单个点之后估计结果比加窗口之后的小,误差增大。

题记:

  1. 对于本题目中,为什么随机选取角点也可以工作的原因在于:直接法是基于灰度不变的假设,对于任意点,在不同的图像中的灰度值均相同,从而通过大量随机点的光度误差最小化,求出相机的位姿,因此随机选取角点也可以正常工作; 此外,说一点自己的看法,SVO中使用的是FAST角点,以及ICIA光流法进行跟踪,之所以使用FAST特征点而不是随机的选取角点,是为了保证能够跟踪正确,即确保选取的角点周围存在光度的梯度,减少不必要的计算。

最终的效果如下所示,我们能够清晰的看到相机是前向运动的。

2.2 多层直接法

下面,类似于光流,我们也可以把直接法以coarse-to-fine的过程,拓展至多层金字塔。多层金字塔的直接法允许图像在发生较大运动时仍能追踪到所有点。下面我们使用缩放倍率为2的四层金字塔,实现金字塔上的直接法。请实现DirectPoseEstimationMultiLayer函数,下面是一些提示:

  1. 在缩放图像时,图像内参也需要跟着变化。那么,例如图像缩小一倍,$f_x,f_y,c_x,c_y$应该如何变化?
  2. 根据coarse-to-fine的过程,上一层图像的位姿估计结果可以作为下一层图像的初始条件。
  3. 在调试期间,可以画出每个点在refcur上的投影,看看它们是否对应。若准确对应,则说明位姿估计是准确的。

作为验证,图像000001000005的位姿平移部分应该接近:

$$ t_1 = [0.005876, −0.01024, −0.0725]^T \\ t_5 = [0.0394, −0.0592, −3.9907]^T $$

可以看出车辆基本是笔直向前开的。

答案:

  1. 缩放图像的时候,图像内参也需要跟着变化,首先平移参数$c_x,c_y$跟随图像进行缩放。$f_x,f_y$也需要进行相应的缩放,注意,此时$f_x,f_y$的单位是像素,表示焦距(单位为米)与缩放倍数(单位为像素/米)的乘积。
  2. 在使用图像金字塔的时候,和光流时一样,我们采用的时候opencvresize函数,但是在实际的应用过程中,OpenCV改变图像大小的操作有两类resize与图像金字塔,但是这两类操作差别还是比较的。 resize函数的目标大小可以是任意的大小(不为0),可以不保持长宽比率;图像金字塔包含两个函数,pyrDownpyrUp分别表示向下降采样与向上升采样,但是二者并不是互为逆操作,但是此时保持的是长宽比率是恒定的。这两个操作实现图像金字塔的经典操作,他们仅仅是分别代表一次采样操作,也就是说,向下(或者向上)进行相邻层次的金字塔采样,调用一次pyrDown函数只能降低到原图像尺寸的1/2;反之,调用pyrUp目标图像则为原图像尺寸的2倍。因为它们内部都给定了一次采样尺寸的约束。也就是说采样之后,长宽比基本是不变的。

最终结果如图所示,和验证结果相差不大。

同时下图展示了第一张图像进行金字塔的过程图(作为参考帧,右为当前帧):

2.3 延伸讨论

现在你已经实现了金字塔上的Gauss-Newton直接法。你可以调整实验当中的一些参数,例如图像点数、每个点周围小块的大小等等。请思考下面问题:

  1. 直接法是否可以类似光流,提出inversecompositional的概念?它们有意义吗?
  2. 请思考上面算法哪些地方可以缓存或加速?
  3. 在上述过程中,我们实际假设了哪两个patch不变?
  4. 为何可以随机取点?而不用取角点或线上的点?那些不是角点的地方,投影算对了吗?
  5. 请总结直接法相对于特征点法的异同与优缺点。

答案:

  1. 理论上认为,直接法可类似光流法提出inverse、compositional概念,目前这一块存疑,后续会有更深入的补充。
  2. 加速的思路有两种,第一种是直接更改窗口的大小,减少计算量。第二种办法是当窗口大小固定时,假设小窗口内的 $8\times 8$ 个像素,对应在相机坐标系下是同一个点,即小窗口内的所有像素在相机坐标系下的坐标相等,这样每一个窗口只需要计算一次雅各比矩阵,进而提高效率。
  3. 在本题目中有两个地方,第一个是patch内关键点的灰度值不变假设;第二个是patch内点的深度信息不变(采用了一样的深度信息$Z_2$)。
  4. 因为直接法是基于灰度不假设,对于任意点,在不同图像中的灰度值均相同,从而通过大量随机点的光度误差最小值,求出相机位姿。因此,无所谓是否为角点。但是选取角点的话,基本能够确保该点的梯度值存在,例如半直接法SVO中选取的是FAST特征点。 在本题目中,那些不是角点的随机点,跟踪的结果基本上是正确的,但是在图像右上方白墙上有些点的跟踪结果有偏差。
  5. 直接法和特征点法的比较:

    直接法的优点:

    • 可以省去计算特征点和描述子的时间;
    • 只要求有像素梯度即可,不需要特征点。因此直接法可以在特征缺失的场合下使用。比较极端的例子是只有渐变的一副图像。他可能无法提取角点类特征,但可以用直接法估计他的运动;
    • 可以构建半稠密乃至稠密的地图,这是特征点法无法做到的。

    直接法的缺点:

    • 非凸性,容易陷入局部极小值;
    • 单个像素没有区分度;
    • 灰度值不变的假设是很强的假设,相机存在曝光、或者物体有高光或者阴影、相机运动过快时,均容易失败。

    特征点法的优点:

    • 对光照有一定的容忍度;
    • 运动过大时,只要图像中还有匹配的点,则不容易丢失,有更好的鲁棒性。

    特征点法的缺点:

    • 当环境纹理的特征点少的时候,容易失败,当特征点过于集中时容易产生退化;
    • 相机运动过快,图像模糊的时候容易失败;
    • 特征点计算耗时。

完整程序链接:https://github.com/XLMaverick/Visual-Localization-Percessing/tree/master/vslam_fourteen_lectures/direct_method