这篇关于libviso的文章，本人已投稿在泡泡机器人微信公众号中，放到这里，作学习笔记用。
libviso一直以来被称为在视觉里程计(VO)中的老牌开源算法。它通过corner,chessboard两种kernel的响应以及非极大值抑制的方式提取特征，并用sobel算子与原图卷积的结果作为特征点的描述子。在位姿的计算方面，则通过RANSAC迭代的方式，每次迭代随机抽取3个点，根据这三个点，用高斯牛顿法计算出一个RT矩阵，表示两帧图像之间，相机的姿态变换。而位姿的计算也是libviso 中较为抽象的一部分，接下来，本文将在读者已经对立体视觉的基本原理，以及libviso的场景流匹配熟悉的前提下，对这个过程进行详细分析。
1 运动描述
在libviso的实际位姿计算过程中，实质上是通过含有6个变量的向量$ extbf{T}$来表示位资变换的：
$$ extbf{T}=left {r_{x},r_{y},r_{z},t_{x},t_{y},t_{z} ight }$$
其中$r_{x},r_{y},r_{z}$和$t_{x},t_{y},t_{z}$分别表示两帧之间相机绕${x,y,z}$轴之间的旋转和平移。计算出这6个变量之后，再转换成描述两帧之间位置变化的${R| extbf{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次)中，参数的值为$ extbf{$T_{i}$}=left { r_{xi},r_{yi},r_{zi},t_{xi},t_{yi},t_{zi} ight }$，当前的迭代，是根据RANSAC中抽取的3个点，更新这6个参数的值，使之更加接近正确解。
对于$r_{x}$,$r_{y}$,$r_{z}$首先，计算这三个量对应的旋转矩阵$R$，如下所示(为了简便，以下$x$指代$r_{x}$,$y$与$z$ 以此类推)
$$R_{x}=egin{bmatrix}
1 & 0 & 0\
0 & cos(x) & -sin(x)\
0 & sin(x) & cos(x)
end{bmatrix}$$
$$R_{y}=egin{bmatrix}
cos(y) & 0 & sin(y)\
0 & 1 & 0\
-sin(y) & 0 & cos(y)
end{bmatrix}$$
$$R_{z}=egin{bmatrix}
cos(z) & -sin(z) &0\
sin(z) & cos(z) & 0\
0 & 0 & 1
end{bmatrix}$$
$$R=R_{x}*R_{y}*R_{z}=egin{bmatrix}egin{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的偏导数，如下所示:
假设X1p,Y1p,Z1p为左图匹配点对应的三维坐标，根据当前的参数$R,t_{x},t_{y},t_{z}$，可以计算出在目前参数下，$(X_{1c},Y_{1c},Z_{1c})$ 的估计值：
$$ egin{bmatrix}
X_{1c}\
Y_{1c}\
Z_{1c}
end{bmatrix}
= R*
egin{bmatrix}
X_{1p}\
Y_{1p}\
Z_{1p}
end{bmatrix}
+
egin{bmatrix}
t_{x}\
t_{y}\
t_{z}
end{bmatrix}$$
根据对极几何原理,$(X_{2c},Y_{2c},Z_{2c})$ ，可以通过以下方式求得
$$egin{bmatrix}
X_{2c}\
Y_{2c}\
Z_{2c}
end{bmatrix}
=
egin{bmatrix}
X_{1c}-b\
Y_{1c}\
Z_{1c}
end{bmatrix} $$
其中b为立体相机基线长度。将$(X_{1c},Y_{1c},Z_{1c})$ ,$(X_{2c},Y_{2c},Z_{2c})$ 变换回对应的二维坐标的过程，称为重投影，具体计算方式如下：
$$v = ffrac{Y}{Z}$$
$$u = ffrac{X}{Z}$$
给定一个重投影后的二维坐标点$(u,v)$，其关于$T_{i}$的雅克比矩阵的计算方式如下：
$$J_{u,v}|_{T} = egin{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关于$r_{x}$的偏导数以此类推$X_{r_{x}}$表示的是$X$在$r_{x}$方向上的偏导数，$X$,$Y$,$Z$为当前帧下的三维点坐标（即$X_{1c},Y_{1c},Z_{1c}$或 $X_{2c},Y_{2c},Z_{2c}$），
通过上一帧的三维点以上述的公式计算可得，而其偏导数则通过下面的公式计算：
$$egin{bmatrix}
X{r_{x}}\
Y{r_{x}}\
Z{r_{x}}
end{bmatrix}
=
egin{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*egin{bmatrix}
X{p}\
Y{p}\
Z{p}
end{bmatrix}
+egin{bmatrix}
t_{x}\
t_{y}\
t_{z}
end{bmatrix})}{partial r_{x}}}
=
frac{partial R}{partial r_{x}}*egin{bmatrix}
X{p}\
Y{p}\
Z{p}
end{bmatrix}$$
$r_{y}$,$r_{z}$的偏导数以此类推，由上述关于$t_{x}$偏导数的表达式，可得：
$$egin{bmatrix}
X{t_{x}}\
Y{t_{x}}\
Z{t_{x}}
end{bmatrix}=
egin{bmatrix}
1\
0\
0
end{bmatrix}$$
基于上述计算方式，可以算出抽样得到的三组点关于$(u_{1c},v_{1c})$以及$(u_{2c},v_{2c})$的雅可比矩阵（对于每一组来说，都用一个$4*6$的矩阵来表示）。接下来，还需要计算残差，供最优化$ extbf{T}$使用。
对于一组点，残差实质上就是$(u,v)$的观测值（匹配点坐标）与其重投影后的坐标的差值，再乘以权重：
$$
extbf{r}=w*[(u,v)-(hat{u},hat{v})]
$$
乘上权重$w$是为了减少标定参数不准确带来的误差，远离相机主点的特征点会赋予更低的权值，具体计算方式为：
$$w = frac{c_{u}}{|u-c_{u}|}$$
2.1.2 高斯牛顿迭代
在libviso的迭代过程中使用的高斯分布，实际上用了一个小技巧：将二次偏导数省略，只通过雅克比矩阵来进行迭代。通过计算三组点中的雅克比矩阵，最终，我们可以得到这样的一个矩阵：
$$J
=egin{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^{
m T}$,$vec{b}=Jvec{r}$,其中$vec{r}$为残差组成的向量：
$$vec{r} = egin{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迭代。然而，每次迭代之后，我们都要判断计算出来的$ extbf{T}$的精确度。在libviso中，精确度是通过局内点(inlier)的个数来衡量的，若通过当前的位姿$ extbf{T}$求得的局内点更多，就将这个位姿替换成当前最好的位姿。
局内点的计算方式如下：先计算出$(hat{u},hat{v})$,随后，满足下式的点即为局内点：
$$
sum_{iin {1c,2c}}(hat{u_{i}}-u_{i})^{2}+(hat{v_{i}}-v_{i})^{2} < t
$$
其中t为人为设定的参数。
经过多次RANSAC迭代，最终能够快速鲁棒地找到两帧之间的位姿变换。这里的位姿变换是通过$T$来表征的，而最终需要换算到${R| extbf{t}}$矩阵，所需要的公式在此不再赘述。
3 小结
本文详细地描述了在libviso中用到的位姿求解的过程。其主要思路是利用随机抽样，每次从匹配点中取出3组，进行高斯牛顿迭代，求出基于这3组点的位姿，再通过内点判断这个位姿的精确度，多次抽样——迭代后，得到一个准确的位姿。抽样能够能够加快高斯牛顿法的收敛速度，而RANSAC的思路则保证了这种抽样的鲁棒性，降低了动态点对算法的影响，是一种值得借鉴的思路。
libviso的project主页位于http://www.cvlibs.net/software/libviso/,另外，我将其中的位姿求解模块单独抽取出来，供读者参考：https://github.com/RichardChe/libviso_pose_estimation