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

视觉里程计（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算子与原图卷积的结果作为特征点的描述子。在位姿的计算方面，则通过$RANSAC$迭代的方式，每次迭代随机抽取3个点，根据这三个点，用高斯牛顿法计算出一个$RT$矩阵，表示两帧图像之间，相机的姿态变换。而位姿的计算也是libviso 中较为抽象的一部分，接下来，本文将在读者已经对立体视觉的基本原理，以及libviso的场景流匹配熟悉的前提下，对这个过程进行详细分析。

1 运动描述

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

$\textbf{T}=\left \{r_{x},r_{y},r_{z},t_{x},t_{y},t_{z} \right \}$

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

2 位姿计算过程

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

2.1 参数的更新

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

假设在当前的梯度下降迭代(第i次)中，参数的值为
$Ti={r_{xi},r_{yi},r_{zi},t_{xi},t_{yi},t_{zi}}$
，当前的迭代，是根据$RANSAC$中抽取的3个点，更新这6个参数的值，使之更加接近正确解。

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

$R_{x}=\begin{bmatrix} 1 & 0 & 0\\ 0 & cos(x) & -sin(x)\\ 0 & sin(x) & cos(x) \end{bmatrix}$

$R_{y}=\begin{bmatrix} cos(y) & 0 & sin(y)\\ 0 & 1 & 0\\ -sin(y) & 0 & cos(y) \end{bmatrix}$

$R_{z}=\begin{bmatrix} cos(z) & -sin(z) &0\\ sin(z) & cos(z) & 0\\ 0 & 0 & 1 \end{bmatrix}$

$R=R_{x}*R_{y}*R_{z}= \begin{bmatrix} \begin{smallmatrix} cos(y)cos(z) & -cos(y)sin(z) & sin(y)\\ cos(x)sin(z)+cos(z)sin(x)sin(y) & cos(x)cos(z)-sin(x)sin(y)sin(z) & -cos(y)sin(x)\\ sin(x)sin(z)-cos(x)cos(z)sin(y) & cos(z)sin(x)+cos(x)*sin(y)*sin(z) & cos(x)cos(y) \end{smallmatrix} \end{bmatrix}$

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

假设$X_{1p},Y_{1p},Z_{1p}$为左图匹配点对应的三维坐标，根据当前的参数$R,tx,ty,tz$，可以计算出在目前参数下，$(X1c,Y1c,Z1c)$的估计值：

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

根据对极几何原理,$(X2c,Y2c,Z2c)$，可以通过以下方式求得

$\begin{bmatrix} X_{2c}\\ Y_{2c}\\ Z_{2c} \end{bmatrix} = \begin{bmatrix} X_{1c}-b\\ Y_{1c}\\ Z_{1c} \end{bmatrix}$

其中b为立体相机基线长度。将$(X1c,Y1c,Z1c) ,(X2c,Y2c,Z2c)$ 变换回对应的二维坐标的过程，称为重投影，具体计算方式如下：

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

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

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

$J_{u,v}|_{T} = \begin{bmatrix} \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{bmatrix}$

其中，
$\frac{\partial u}{\partial r_{x}} = \frac{ZX_{r_{x}}- XZ_{r_{x}}}{Z^{2}}$

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

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

$ry,rz$的偏导数以此类推，由上述关于$tx$偏导数的表达式，可得：

$\begin{bmatrix} X{t_{x}}\\ Y{t_{x}}\\ Z{t_{x}} \end{bmatrix}= \begin{bmatrix} 1\\ 0\\ 0 \end{bmatrix}$

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

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

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

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

$J =\begin{bmatrix} \frac{\partial u_{1c1}}{\partial r_{x}} & \frac{\partial v_{1c1}}{\partial r_{x}}& \frac{\partial u_{2c1}}{\partial r_{x}} & \frac{\partial v_{2c1}}{\partial r_{x}}&\cdots\ & \frac{\partial u_{1cN}}{\partial r_{x}} & \frac{\partial v_{1cN}}{\partial r_{x}}& \frac{\partial u_{2cN}}{\partial r_{x}} & \frac{\partial v_{2cN}}{\partial r_{x}}\\ \frac{\partial u_{1c1}}{\partial r_{y}} & \frac{\partial v_{1c1}}{\partial r_{y}}& \frac{\partial u_{2c1}}{\partial r_{y}} & \frac{\partial v_{2c1}}{\partial r_{y}}&\cdots\ & \frac{\partial u_{1cN}}{\partial r_{y}} & \frac{\partial v_{1cN}}{\partial r_{y}}& \frac{\partial u_{2cN}}{\partial r_{y}} & \frac{\partial v_{2cN}}{\partial r_{y}}\\ \frac{\partial u_{1c1}}{\partial r_{z}} & \frac{\partial v_{1c1}}{\partial r_{z}}& \frac{\partial u_{2c1}}{\partial r_{z}} & \frac{\partial v_{2c1}}{\partial r_{z}}&\cdots\ & \frac{\partial u_{1cN}}{\partial r_{z}} & \frac{\partial v_{1cN}}{\partial r_{z}}& \frac{\partial u_{2cN}}{\partial r_{z}} & \frac{\partial v_{2cN}}{\partial r_{z}}\\ \frac{\partial u_{1c1}}{\partial t_{x}} & \frac{\partial v_{1c1}}{\partial t_{x}}& \frac{\partial u_{2c1}}{\partial t_{x}} & \frac{\partial v_{2c1}}{\partial t_{x}}&\cdots\ & \frac{\partial u_{1cN}}{\partial t_{x}} & \frac{\partial v_{1cN}}{\partial t_{x}}& \frac{\partial u_{2cN}}{\partial t_{x}} & \frac{\partial v_{2cN}}{\partial t_{x}}\\ \frac{\partial u_{1c1}}{\partial t_{y}} & \frac{\partial v_{1c1}}{\partial t_{y}}& \frac{\partial u_{2c1}}{\partial t_{y}} & \frac{\partial v_{2c1}}{\partial t_{y}}&\cdots\ & \frac{\partial u_{1cN}}{\partial t_{y}} & \frac{\partial v_{1cN}}{\partial t_{y}}& \frac{\partial u_{2cN}}{\partial t_{y}} & \frac{\partial v_{2cN}}{\partial t_{y}}\\ \frac{\partial u_{1c1}}{\partial t_{z}} & \frac{\partial v_{1c1}}{\partial t_{z}}& \frac{\partial u_{2c1}}{\partial t_{z}} & \frac{\partial v_{2c1}}{\partial t_{z}}&\cdots\ & \frac{\partial u_{1cN}}{\partial t_{z}} & \frac{\partial v_{1cN}}{\partial t_{z}}& \frac{\partial u_{2cN}}{\partial t_{z}} & \frac{\partial v_{2cN}}{\partial t_{z}}\\ \end{bmatrix}$

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

$\vec{r} = \begin{bmatrix} r_{1c1}\\ r_{2c1} \\ r_{1c2} \\ \vdots \\ r_{1cN} \\ r_{2cN} \end{bmatrix}$

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

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

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

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

2.2 最优位姿选取

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

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

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

3 小结

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

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