视觉里程计（1）

数据集链接：Karlsruhe Dataset: Stereo Video Sequences + rough GPS Poses

github代码：
https://github.com/srv/viso2
https://github.com/BrainSpawnInfosphere/libviso2

libviso2【转载】

　libviso一直以来被称为在视觉里程计(VO)中的老牌开源算法。它通过corner,chessboard两种kernel的响应以及非极大值抑制的方式提取特征，并用sobel算子与原图卷积的结果作为特征点的描述子。在位姿的计算方面，则通过 $R A N S A C$ 迭代的方式，每次迭代随机抽取3个点，根据这三个点，用高斯牛顿法计算出一个 $R T$ 矩阵，表示两帧图像之间，相机的姿态变换。而位姿的计算也是libviso 中较为抽象的一部分，接下来，本文将在读者已经对立体视觉的基本原理，以及libviso的场景流匹配熟悉的前提下，对这个过程进行详细分析。

1 运动描述

在libviso的实际位姿计算过程中，实质上是通过含有6个变量的向量

$T = {r_{x}, r_{y}, r_{z}, t_{x}, t_{y}, t_{z}}$

来表示位姿变换的：
其中 $r_{x}, r_{y}, r_{z}$ 和 $t_{x}, t_{y}, t_{z}$ 分别表示两帧之间相机绕 $x, y, z$ 轴之间的旋转和平移。计算出这6个变量之后，再转换成描述两帧之间位置变化的 $R | t$ 矩阵。

2 位姿计算过程

在位姿计算的过程中，输入的是n组通过场景流匹配得到的二维坐标点，每组4个：
$(u_{1 c}, v_{1 c}), (u_{1 p}, v_{1 p}), (u_{2 c}, v_{2 c}), (u_{1 p}, v_{1 p})$ ，
分别代表左图、右图，当前时刻，上一时刻图像中的匹配点（这里用了与代码中相同的符号表示，1下标代表左图，2代表右图，c代表上一阵，p代表当前帧）,
$(X_{1 c}, Y_{1 c}, Z_{1 c}), (X_{1 p}, Y_{1 p}, Z_{1 p}), (X_{2 c}, Y_{2 c}, Z_{2 c}), (X_{1 p}, Y_{1 p}, Z_{1 p})$
是其对应的三维坐标，根据立体视觉的原理，三维坐标可以通过匹配点坐标结合相机内参数算出。输出是1中描述的6个变量。这6个变量是通过RANSAC迭代，在每次迭代中都从匹配点中随机抽取3个点，基于这3个点，通过高斯牛顿法的方式求出来的。计算出一次迭代中的参数之后，利用这个参数计算出局内点(inlier)的占比。最终取占比最高的参数，得到结果。下面将对每次迭代中进行的操作细节进行分析。

2.1 参数的更新

在每次迭代中，参数是通过梯度下降的方式求出来的。而高斯牛顿实质上也是一种通过迭代求解的方式。位姿解算的过程，可以看成是在 $R A N S A C$ 迭代中，再嵌套了一个迭代求解过程。高斯牛顿法计算的过程中，主要的计算工作包括两步：残差以及雅克比的计算，参数的迭代。
2.1.1 残差以及雅克比的计算

假设在当前的梯度下降迭代(第i次)中，参数的值为
$T i = r_{x i}, r_{y i}, r_{z i}, t_{x i}, t_{y i}, t_{z i}$
，当前的迭代，是根据 $R A N S A C$ 中抽取的3个点，更新这6个参数的值，使之更加接近正确解。

对于 $r_{x i}, r_{y i}, r_{z i}$ 首先，计算这三个量对应的旋转矩阵 $R$ ，如下所示(为了简便，以下 $x$ 指代 $r_{x}, y 与 z$ 以此类推)

$R_{x} = [\begin{matrix} 1 & 0 & 0 \\ 0 & c o s (x) & - s i n (x) \\ 0 & s i n (x) & c o s (x) \end{matrix}]$

$R_{y} = [\begin{matrix} c o s (y) & 0 & s i n (y) \\ 0 & 1 & 0 \\ - s i n (y) & 0 & c o s (y) \end{matrix}]$

$R_{z} = [\begin{matrix} c o s (z) & - s i n (z) & 0 \\ s i n (z) & c o s (z) & 0 \\ 0 & 0 & 1 \end{matrix}]$

$R = R_{x} * R_{y} * R_{z} = [\begin{matrix} \begin{matrix} c o s (y) c o s (z) & - c o s (y) s i n (z) & s i n (y) \\ c o s (x) s i n (z) + c o s (z) s i n (x) s i n (y) & c o s (x) c o s (z) - s i n (x) s i n (y) s i n (z) & - c o s (y) s i n (x) \\ s i n (x) s i n (z) - c o s (x) c o s (z) s i n (y) & c o s (z) s i n (x) + c o s (x) * s i n (y) * s i n (z) & c o s (x) c o s (y) \end{matrix} \end{matrix}]$

然后分别计算 $R$ 中关于 $x, y, z$ 的偏导数，如下所示:

视觉里程计（1）----libviso

视觉里程计（1）----libviso
假设 $X_{1 p}, Y_{1 p}, Z_{1 p}$ 为左图匹配点对应的三维坐标，根据当前的参数 $R, t x, t y, t z$ ，可以计算出在目前参数下， $(X 1 c, Y 1 c, Z 1 c)$ 的估计值：

$[\begin{matrix} X_{1 c} \\ Y_{1 c} \\ Z_{1 c} \end{matrix}] = R * [\begin{matrix} X_{1 p} \\ Y_{1 p} \\ Z_{1 p} \end{matrix}] + [\begin{matrix} t_{x} \\ t_{y} \\ t_{z} \end{matrix}]$

根据对极几何原理, $(X 2 c, Y 2 c, Z 2 c)$ ，可以通过以下方式求得

$[\begin{matrix} X_{2 c} \\ Y_{2 c} \\ Z_{2 c} \end{matrix}] = [\begin{matrix} X_{1 c} - b \\ Y_{1 c} \\ Z_{1 c} \end{matrix}]$

其中b为立体相机基线长度。将 $(X 1 c, Y 1 c, Z 1 c), (X 2 c, Y 2 c, Z 2 c)$ 变换回对应的二维坐标的过程，称为重投影，具体计算方式如下：

$u = f \frac{X}{Z}$

$v = f \frac{Y}{Z}$

给定一个重投影后的二维坐标点 $(u, v)$ ，其关于 $T i$ 的雅克比矩阵的计算方式如下：

$J_{u, v} |_{T} = [\begin{matrix} \frac{\partial u}{\partial r_{x}} & \frac{\partial u}{\partial r_{y}} & \frac{\partial u}{\partial r_{z}} & \frac{\partial u}{\partial t_{x}} & \frac{\partial u}{\partial t_{y}} & \frac{\partial u}{\partial t_{z}} \\ \frac{\partial v}{\partial r_{x}} & \frac{\partial v}{\partial r_{y}} & \frac{\partial v}{\partial r_{z}} & \frac{\partial v}{\partial t_{x}} & \frac{\partial v}{\partial t_{y}} & \frac{\partial v}{\partial t_{z}} \end{matrix}]$

其中，
$\frac{\partial u}{\partial r_{x}} = \frac{Z X_{r_{x}} - X Z_{r_{x}}}{Z^{2}}$

$v$ 关于 $r x$ 的偏导数以此类推 $X r x$ 表示的是 $X$ 在 $r x$ 方向上的偏导数， $X, Y, Z$ 为当前帧下的三维点坐标（即 $X 1 c, Y 1 c, Z 1 c 或 X 2 c, Y 2 c, Z 2 c ）$ ，
通过上一帧的三维点以上述的公式计算可得，而其偏导数则通过下面的公式计算：

$[\begin{matrix} X r_{x} \\ Y r_{x} \\ Z r_{x} \end{matrix}] = [\begin{matrix} \frac{\partial X_{c}}{\partial r_{x}} \\ \frac{\partial Y_{c}}{\partial r_{x}} \\ \frac{\partial Z_{c}}{\partial r_{x}} \end{matrix}] = \frac{\partial (R * [\begin{matrix} X p \\ Y p \\ Z p \end{matrix}] + [\begin{matrix} t_{x} \\ t_{y} \\ t_{z} \end{matrix}])}{\partial r_{x}} = \frac{\partial R}{\partial r_{x}} * [\begin{matrix} X p \\ Y p \\ Z p \end{matrix}]$

$r y, r z$ 的偏导数以此类推，由上述关于 $t x$ 偏导数的表达式，可得：

$[\begin{matrix} X t_{x} \\ Y t_{x} \\ Z t_{x} \end{matrix}] = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}]$

基于上述计算方式，可以算出抽样得到的三组点关于 $(u 1 c, v 1 c)$ 以及 $(u 2 c, v 2 c)$ 的雅可比矩阵（对于每一组来说，都用一个 $4 * 6$ 的矩阵来表示）。接下来，还需要计算残差，供最优化 $T$ 使用
　　对于一组点，残差实质上就是 $(u, v)$ 的观测值（匹配点坐标）与其重投影后的坐标的差值，再乘以权重：
　　
$r = w * [(u, v) - (\hat{u}, \hat{v})]$

乘上权重 $w$ 是为了减少标定参数不准确带来的误差，远离相机主点的特征点会赋予更低的权值，具体计算方式为：

$w = \frac{c_{u}}{| u - c_{u} |}$

2.1.2 高斯牛顿迭代
在libviso的迭代过程中使用的高斯分布，实际上用了一个小技巧：将二次偏导数省略，只通过雅克比矩阵来进行迭代。通过计算三组点中的雅克比矩阵，最终，我们可以得到这样的一个矩阵：

$J = [\begin{matrix} \frac{\partial u_{1 c 1}}{\partial r_{x}} & \frac{\partial v_{1 c 1}}{\partial r_{x}} & \frac{\partial u_{2 c 1}}{\partial r_{x}} & \frac{\partial v_{2 c 1}}{\partial r_{x}} & \dots & \frac{\partial u_{1 c N}}{\partial r_{x}} & \frac{\partial v_{1 c N}}{\partial r_{x}} & \frac{\partial u_{2 c N}}{\partial r_{x}} & \frac{\partial v_{2 c N}}{\partial r_{x}} \\ \frac{\partial u_{1 c 1}}{\partial r_{y}} & \frac{\partial v_{1 c 1}}{\partial r_{y}} & \frac{\partial u_{2 c 1}}{\partial r_{y}} & \frac{\partial v_{2 c 1}}{\partial r_{y}} & \dots & \frac{\partial u_{1 c N}}{\partial r_{y}} & \frac{\partial v_{1 c N}}{\partial r_{y}} & \frac{\partial u_{2 c N}}{\partial r_{y}} & \frac{\partial v_{2 c N}}{\partial r_{y}} \\ \frac{\partial u_{1 c 1}}{\partial r_{z}} & \frac{\partial v_{1 c 1}}{\partial r_{z}} & \frac{\partial u_{2 c 1}}{\partial r_{z}} & \frac{\partial v_{2 c 1}}{\partial r_{z}} & \dots & \frac{\partial u_{1 c N}}{\partial r_{z}} & \frac{\partial v_{1 c N}}{\partial r_{z}} & \frac{\partial u_{2 c N}}{\partial r_{z}} & \frac{\partial v_{2 c N}}{\partial r_{z}} \\ \frac{\partial u_{1 c 1}}{\partial t_{x}} & \frac{\partial v_{1 c 1}}{\partial t_{x}} & \frac{\partial u_{2 c 1}}{\partial t_{x}} & \frac{\partial v_{2 c 1}}{\partial t_{x}} & \dots & \frac{\partial u_{1 c N}}{\partial t_{x}} & \frac{\partial v_{1 c N}}{\partial t_{x}} & \frac{\partial u_{2 c N}}{\partial t_{x}} & \frac{\partial v_{2 c N}}{\partial t_{x}} \\ \frac{\partial u_{1 c 1}}{\partial t_{y}} & \frac{\partial v_{1 c 1}}{\partial t_{y}} & \frac{\partial u_{2 c 1}}{\partial t_{y}} & \frac{\partial v_{2 c 1}}{\partial t_{y}} & \dots & \frac{\partial u_{1 c N}}{\partial t_{y}} & \frac{\partial v_{1 c N}}{\partial t_{y}} & \frac{\partial u_{2 c N}}{\partial t_{y}} & \frac{\partial v_{2 c N}}{\partial t_{y}} \\ \frac{\partial u_{1 c 1}}{\partial t_{z}} & \frac{\partial v_{1 c 1}}{\partial t_{z}} & \frac{\partial u_{2 c 1}}{\partial t_{z}} & \frac{\partial v_{2 c 1}}{\partial t_{z}} & \dots & \frac{\partial u_{1 c N}}{\partial t_{z}} & \frac{\partial v_{1 c N}}{\partial t_{z}} & \frac{\partial u_{2 c N}}{\partial t_{z}} & \frac{\partial v_{2 c N}}{\partial t_{z}} \end{matrix}]$

$N$ 为抽样的点个数 $（ 3 ）$ ,令
$A = J J^{T}$
$b ⃗ = J r$ ⃗
,其中 $r$ ⃗ 为残差组成的向量：

$\vec{r} = [\begin{matrix} r_{1 c 1} \\ r_{2 c 1} \\ r_{1 c 2} \\ ⋮ \\ r_{1 c N} \\ r_{2 c N} \end{matrix}]$

最后，高斯牛顿法通过下面的公式，进行 $T$ 的迭代：

$T^{(i + 1)} = T^{(i)} + A^{- 1} \vec{r}$

往复迭代，直到 $T$ 收敛

$| T^{(i + 1)} - T^{(i)} | < ε$

2.2 最优位姿选取

从匹配点中选取3个，并通过高斯牛顿法，求出位姿 $T$ ，即完成了一次 $R A N S A C$ 迭代。然而，每次迭代之后，我们都要判断计算出来的T的精确度。在libviso中，精确度是通过局内点 $(i n l i e r)$ 的个数来衡量的，若通过当前的位姿 $T$

求得的局内点更多，就将这个位姿替换成当前最好的位姿。

　　局内点的计算方式如下：先计算出 $(\hat{u}, \hat{v})$ ,随后，满足下式的点即为局内点：
　　
　　 $\sum_{i \in 1 c, 2 c} (\hat{u_{i}} - u_{i})^{2} + (\hat{v_{i}} - v_{i})^{2} < t$
　　
　其中 $t$ 为人为设定的参数。
　经过多次 $R A N S A C$ 迭代，最终能够快速鲁棒地找到两帧之间的位姿变换。这里的位姿变换是通过 $T$
来表征的，而最终需要换算到 $R | t$ 矩阵，所需要的公式在此不再赘述。

3 小结

本文详细地描述了在libviso中用到的位姿求解的过程。其主要思路是利用随机抽样，每次从匹配点中取出3组，进行高斯牛顿迭代，求出基于这3组点的位姿，再通过内点判断这个位姿的精确度，多次抽样——迭代后，得到一个准确的位姿。抽样能够能够加快高斯牛顿法的收敛速度，而RANSAC的思路则保证了这种抽样的鲁棒性，降低了动态点对算法的影响，是一种值得借鉴的思路。

libviso的project主页位于
http://www.cvlibs.net/software/libviso/
,另外，我将其中的位姿求解模块单独抽取出来，供读者参考：
https://github.com/RichardChe/libviso_pose_estimation