第八讲视觉里程计2

不提取特征点计算VO：

一是通过其他方式寻找配对点（光流法，跟踪特征点的运动），仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动仍使用对极几何、PnP或ICP算法。依然要求提取到的关键点有可区别性，即角点。
二是无配对点（直接法，计算特征点在下一时刻图像中的位置），根据图像的像素灰度信息同时估计相机运动和点的投影，不要求提取到的点必须为角点。
光流描述了像素在图像中的运动，而直接法则附带着一个相机运动模型。

2、2D光流

Lucas-Kanade光流

3、实践：LK光流

4、直接法

直接法的推导

直接法的讨论

2、2D光流

光流是一种描述像素随时间在图像之间运动的方法。分为稀疏光流（Lucas-Kanade光流）和稠密光流（Horn-Schunck光流）。

Lucas-Kanade光流

灰度不变假设：理想情况下，同一个空间点的像素灰度值，在各个图像中是固定不变的。 $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{dx}{dt}$ 像素在x轴的运动速度，记为u
- $\frac{dy}{dt}$ 像素在y轴的运动速度，记为v
- $\frac{\partial I}{\partial x}$ 图像在该点处x方向的梯度，记为 $I_x$
- $\frac{\partial I}{\partial y}$ 图像在该点处y方向的梯度，记为 $I_y$
- $\frac{\partial I}{\partial t}$ 图像灰度对时间的变化量，记为 $I_t$
以上写成矩阵形式： $\begin{bmatrix} I_x & I_y \end{bmatrix}\begin{bmatrix} u\\ v \end{bmatrix}=-I_t$
求解的是像素运动u,v。上式为二元一次方程，无法求解。必须引入额外的约束来计算u,v。LK光流中，假设某一个窗口内的像素具有相同的运动。考虑一个w×w的窗口，含有w^2数量的像素。该窗口内像素具有同样的运动，因此有w^2个方程： $\begin{bmatrix} I_x & I_y \end{bmatrix}_k\begin{bmatrix} u\\ v \end{bmatrix}=-I_{tk},k=1,2,...,w^2$
简化后： $A\begin{bmatrix} u\\ v \end{bmatrix}=-b$ 。这是关于u,v的超定线性方程，通过最小二乘解： $\begin{bmatrix} u\\ v \end{bmatrix}=-(A^TA)^{-1}A^Tb$ 。
如此，就得到了像素在图像间的运动u,v。当t取离散时刻时，可以估计某块像素在若干个图像出现的位置。

3、实践：LK光流

OpenCV光流：

cv::calOpticalFlowPyrLK()

高斯牛顿法实现光流：

光流也可以看成一个优化问题：通过最小化灰度误差估计最优的像素偏移。
即求解：。
- 雅可比为第二个图像在 $x+\Delta x,y+\Delta y$ 处的梯度。
- 在反向光流中，也可以用第一个图像的梯度 $I_1(x,y)$ 来代替，且 $I_1(x,y)$ 的梯度保持不变。

多层光流：

我们把光流写成优化问题，就必须假设优化的初始值靠近最优值，才能在一定程度上保障算法的收敛。如果相机运动过快，单层图像光流法容易达到一个局部极小值，这时引入图像金字塔来改善。
图像金字塔是指对同一个图像进行缩放，得到不同分辨率的图像。计算光流时，先从顶层图像开始计算，然后把上一层的追踪结果作为下一层光流的初始值。该过程也成为由粗至精（Coarse-to-fine）的光流。
由粗至精好处：当原始图像运动较大时，在顶层图像看运动仍然在一个很小的范围内。

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

4、直接法

直接法的推导

目标是求第一个相机到第二个相机的相对位姿变换。

完整的投影方程（Z1是P的深度，Z2是P在第二个相机坐标系下的深度）：

$p_1=\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}_1=\frac{1}{Z_1}KP$

$p_2=\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}_2=\frac{1}{Z_2}K(RP+t)=\frac{1}{Z_2}K(TP)_{1:3}$

直接法的思路是根据当前相机的位姿估计值寻找p2的位置。若相机的位姿不够好，p2和p1外观会有明显的差别。为了减小这一差别，需要优化相机位姿，来寻找与p1更相似的p2。此时最小化的不是重投影误差，而是光度误差，也就是P的两个像素的亮度误差： $e=I_1(p_1)-I_2(p_2)$ 。
上式e是标量,优化目标为该误差的二范式： $\mathop{min}\limits_{T}J(T)=||e||_2^2$
灰度不变假设：假设一个空间点在各个视角下成像的灰度是不变的。N个空间点Pi，则整个相机位姿估计问题： $\mathop{min}\limits_{T}J(T)=\sum_{i=1}^{N}e_i^Te_i,e_i=I_1(p_1,i)-I_2(p_2,i)$
这里优化变量是相机位姿T，而不像光流那样优化各个特征点的运动。为了求解该优化问题，我们关心误差e如何随着相机位姿T变化，需分析它们的导数关系。
- $e(T)=I_1(p_1)-I_2(u)$ ，其中 $q=TP$ , $u=\frac{1}{Z_2}Kq$
- 考虑李代数的左扰动模型，利用一阶泰勒展开：
  - $\frac{\partial I_2}{\partial u}$ 为u处的像素梯度
  - $\frac{\partial u}{\partial q}$ 为投影方程关于相机坐标系下的三维点的导数
  - $\frac{\partial q}{\partial \delta \xi }$ 为变换后的三维点对变换的导数， $\frac{\partial q}{\partial \delta \xi }=[I,-q^{\wedge}]$
  - 后两项只与三维点q有关，与图像无关，经常合并一起： $\frac{\partial u}{\partial \delta \xi }$ ，2×6矩阵
推导出误差对于李代数的雅可比矩阵： $J=\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial \delta \xi }$
然后使用高斯牛顿或列文伯格-马夸尔特方法计算增量，迭代求解。

直接法的讨论

上述推导中，P是已知位置的空间的，它的获取方式：

RGB-D相机，可以将任意像素饭投影到三维空间，然后再投影到下一幅图像
双目相机，根据视差计算像素深度
单目相机，还需要考虑P的深度带来的不确定性。详细建13讲

这里只考虑简单的情况，依旧是P深度已知，根据P的来源，直接法分类：

稀疏直接法：P来自于稀疏关键点。速度快，但只能计算稀疏的重构
半稠密直接法：P来自部分像素。若像素梯度为0，则雅可比矩阵为0，不会对计算运动增量有任何贡献。因此，可以只考虑带有梯度的像素点，舍弃梯度不明显的地方。
稠密直接法：P为全部像素。可以建立完整地图，但需要GPU加速。

5、实践：直接法

根据视差图获取深度信息？

int disparity = disparity_img.at<uchar>(y, x);
double depth = fx * baseline / disparity;

直接法队特征点不敏感，代码中随机在第一张图像上选取点，不使用任何角点或特征点。

直接法迭代过程描述

直接法完全依靠优化来求解相机位姿。如果想要得到正确的优化结果，必须保证大部分像素梯度能够把优化引导到正确的方向。
对于参考图像，测量到一个灰度值为299的像素。另外由于我们知道它的深度，可以推断出空间点P的位置。
此外，我们得到一幅新图像，需要估计它的相机位姿。这个位姿是由一个初值不断优化迭代得到的。假设初值较差，在这个初值下，空间点P投影后的像素灰度值是126。于是此像素误差为229-126=103。为了减小这个误差，我们希望微调相机的位姿，使像素更亮一些。
怎样知道哪里像素更亮呢？就需要用到局部的像素梯度。为了提高亮度，建议优化算法微调相机，使P的像往梯度增加的方向移动。
优化算法不能只听这个像素的一面之词，还需听取其他像素的建议。综合听取许多像素意见后，选择一个我们方向，计算出一个更新量 $exp(\xi ^\wedge)$ 。加上更新量后，图像从I2移动到I2'，这次更新后，误差变小了。理想情况下，期望误差不断下降，最后收敛。
实际中，沿着图像梯度前进，很容易由于图像本身的非凸性（或噪声）落进一个局部极小值中，无法继续优化。只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接法才成立。