libviso中的姿态解算【转载】

　　这篇关于libviso的文章，本人已投稿在泡泡机器人微信公众号中，放到这里，作学习笔记用。

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

1 运动描述

在libviso的实际位姿计算过程中，实质上是通过含有6个变量的向量$\textbf{T}$来表示位资变换的：

　　$$\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|\textbf{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次)中，参数的值为$\textbf{$T_{i}$}=\left \{ r_{xi},r_{yi},r_{zi},t_{xi},t_{yi},t_{zi}\right \}$，当前的迭代，是根据RANSAC中抽取的3个点，更新这6个参数的值，使之更加接近正确解。

　　对于$r_{x}$,$r_{y}$,$r_{z}$首先，计算这三个量对应的旋转矩阵$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的偏导数，如下所示:

　　假设X1p,Y1p,Z1p为左图匹配点对应的三维坐标，根据当前的参数$R,t_{x},t_{y},t_{z}$，可以计算出在目前参数下，$(X_{1c},Y_{1c},Z_{1c})$ 的估计值：
$$ \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}$$
根据对极几何原理,$(X_{2c},Y_{2c},Z_{2c})$ ，可以通过以下方式求得
$$\begin{bmatrix}
X_{2c}\\
Y_{2c}\\
Z_{2c}
\end{bmatrix}
=
\begin{bmatrix}
X_{1c}-b\\
Y_{1c}\\
Z_{1c}
\end{bmatrix} $$
其中b为立体相机基线长度。将$(X_{1c},Y_{1c},Z_{1c})$ ,$(X_{2c},Y_{2c},Z_{2c})$ 变换回对应的二维坐标的过程，称为重投影，具体计算方式如下：
$$v = f\frac{Y}{Z}$$
$$u = f\frac{X}{Z}$$
给定一个重投影后的二维坐标点$(u,v)$，其关于$T_{i}$的雅克比矩阵的计算方式如下：
$$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关于$r_{x}$的偏导数以此类推$X_{r_{x}}$表示的是$X$在$r_{x}$方向上的偏导数，$X$,$Y$,$Z$为当前帧下的三维点坐标（即$X_{1c},Y_{1c},Z_{1c}$或 $X_{2c},Y_{2c},Z_{2c}$），
通过上一帧的三维点以上述的公式计算可得，而其偏导数则通过下面的公式计算：
$$\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}$$
$r_{y}$,$r_{z}$的偏导数以此类推，由上述关于$t_{x}$偏导数的表达式，可得：
$$\begin{bmatrix}
X{t_{x}}\\
Y{t_{x}}\\
Z{t_{x}}
\end{bmatrix}=
\begin{bmatrix}
1\\
0\\
0
\end{bmatrix}$$

基于上述计算方式，可以算出抽样得到的三组点关于$(u_{1c},v_{1c})$以及$(u_{2c},v_{2c})$的雅可比矩阵（对于每一组来说，都用一个$4*6$的矩阵来表示）。接下来，还需要计算残差，供最优化$\textbf{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}$,$\vec{b}=J\vec{r}$,其中$\vec{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迭代。然而，每次迭代之后，我们都要判断计算出来的$\textbf{T}$的精确度。在libviso中，精确度是通过局内点(inlier)的个数来衡量的，若通过当前的位姿$\textbf{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|\textbf{t}}$矩阵，所需要的公式在此不再赘述。

3 小结

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

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