【视觉SLAM十四讲】视觉里程计—直接法

本文为视觉 SLAM 学习总结，讲解直接法的相关知识。

直接法的引出

我们在上一讲中的特征法中，提取特征点计算描述子十分耗时，并且特征匹配使用暴力匹配的方法，时间复杂度为 $O(n^2)$O(n2)，但计算位姿较快。我们希望找一种方法能够代替特征法中的特征点提取和匹配的过程。

我们可以通过光流的方法寻找匹配点，也可以使用直接法，不匹配点对。光流描述了像素在图像中的运动，直接法附带一个相机运动模型。

光流

一般分为稀疏光流和稠密光流。稀疏以 LK 光流为代表，稠密以 HS 光流为代表，本质上是估计像素在不同时刻图像中的运动。因为 SLAM 中我们只需要对特征点进行匹配并计算位姿，因此仅介绍稀疏的 LK 光流。

设 $t$t 时刻位于 $x,y$x,y 处的像素点的灰度值为 $I(x,y,t)$I(x,y,t)，在 $t+dt$t+dt 时刻，该像素运动到 $(x+dx,y+dy)$(x+dx,y+dy)。

灰度不变性假设：同一空间点的像素灰度值，在各图像中不变。则有：
$I(x+dx,y+dy,t+dt)=I(x,y,t)$I(x+dx,y+dy,t+dt)=I(x,y,t)
灰度不变假设是理想状态，实际中不成立。

对 $t+dt$t+dt 时刻的灰度进行泰勒展开保留一阶项：
$I(x+dx,y+dy,t+dt)≈I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt$I(x+dx,y+dy,t+dt)≈I(x,y,t)+∂x∂I​dx+∂y∂I​dy+∂t∂I​dt
由于灰度不变，则：
$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$∂x∂I​dx+∂y∂I​dy+∂t∂I​dt=0
因此
$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t}$∂x∂I​dtdx​+∂y∂I​dtdy​=−∂t∂I​
设 $\frac{dx}{dt},\frac{dy}{dt}$dtdx​,dtdy​ 分别为 $u,v$u,v。这是一个二元一次方程，欠定，需要引入额外的约束。假设一个窗口 $(w\times w)$(w×w) 内光度不变，我们共有 $w^2$w2 个方程：
$\left[ \begin{matrix} I_x & I_y \end{matrix} \right]_k\left[ \begin{matrix} u \\ v \end{matrix} \right]=-I_{tk},\quad k=1,…,w^2$[Ix​​Iy​​]k​[uv​]=−Itk​,k=1,…,w2
记：
$A=\left[ \begin{matrix} [I_x & I_y]_1\\ \vdots\\ [I_x & I_y]_k \end{matrix} \right],\quad b=\left[ \begin{matrix} I_{t1}\\ \vdots\\ I_{tk} \end{matrix} \right]$A=⎣⎢⎡​[Ix​⋮[Ix​​Iy​]1​Iy​]k​​⎦⎥⎤​,b=⎣⎢⎡​It1​⋮Itk​​⎦⎥⎤​
于是整个方程：
$A\left[ \begin{matrix} u \\ v \end{matrix} \right]=-b$A[uv​]=−b
关于 $u,v$u,v 超定线性方程的求解，传统解法是求最小二乘解（详细推导见《矩阵论》）：
$\left[ \begin{matrix} u \\ v \end{matrix} \right]^*=-(A^TA)^{-1}A^Tb$[uv​]∗=−(ATA)−1ATb

• 我们可以将光流看作最小化像素误差的非线性优化。

• 每次泰勒一阶近似，在离优化点较远时效果不佳，往往需要迭代多次。

• 运动较大时使用图像金字塔，缩放分辨率，从粗到精

• 只需提取第一个图像的角点，不同计算描述子，直接跟踪像素轨迹。

实践：光流

首先我们对第一帧提取 FAST 角点：

if (index ==0 )
{
    // 对第一帧提取FAST特征点
    vector<cv::KeyPoint> kps;
    cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
    detector->detect( color, kps );
    for ( auto kp:kps )
        keypoints.push_back( kp.pt );
    last_color = color;
    continue;
}

对其他帧用 LK 跟踪特征点：

vector<cv::Point2f> next_keypoints; 
vector<cv::Point2f> prev_keypoints;
for ( auto kp:keypoints )
    prev_keypoints.push_back(kp);
vector<unsigned char> status;
vector<float> error; 
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;

把跟丢的点删掉：

// 把跟丢的点删掉
int i=0; 
for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
{
    if ( status[i] == 0 )
    {
        iter = keypoints.erase(iter);
        continue;
    }
    *iter = next_keypoints[i];
    iter++;
}
cout<<"tracked keypoints: "<<keypoints.size()<<endl;
if (keypoints.size() == 0)
{
    cout<<"all keypoints are lost."<<endl;
    break; 
}

画出 keypoints：

cv::Mat img_show = color.clone();
for ( auto kp:keypoints )
    cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
cv::imshow("corners", img_show);
cv::waitKey(0);
last_color = color;

角点固定不动，边缘会沿着边滑动，因为点沿着边看起来一样，区块附近点很相似，更容易移动到别的地方去。

直接法

光流法仅估计了相机间的平移。但

• 不知道相机怎么走，未使用深度信息，未使用相机模型，即没有考虑到相机的旋转和图像的缩放
• 灰度不变性没有考虑到相机的旋转和图像的缩放

直接法的推导

假设有两帧，运动未知，但有初始估计 $R,t$R,t，可以为 0。在第一帧看到了点 $P$P，投影为 $p_1$p1​，按照初始估计， $P$P 在第二帧投影为 $p_2$p2​。

投影关系：
$p_1=\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right]_1=\frac{1}{Z_1}KP$p1​=⎣⎡​uv1​⎦⎤​1​=Z1​1​KP

$p_2=\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right]_2=\frac{1}{Z_1}K(RP+t)=\frac{1}{Z_1}K(\exp(\hat{\xi})P)_{1:3}$p2​=⎣⎡​uv1​⎦⎤​2​=Z1​1​K(RP+t)=Z1​1​K(exp(ξ^​)P)1:3​

为了估计相机的位姿，我们需要最小化光度误差：
$e=I_1(p_1)-I_2(p_2)$e=I1​(p1​)−I2​(p2​)
优化的目标函数为：
$\min\limits_\xi J(\xi)=||e||^2$ξmin​J(ξ)=∣∣e∣∣2
我们仍然基于灰度不变假设，有许多个空间点 $P$P：
$\min\limits_\xi J(\xi)=\sum\limits_{i=1}^Ne_i^Te_i, \quad e_i=I_1(p_{1,i})-I_2(p_{2,i})$ξmin​J(ξ)=i=1∑N​eiT​ei​,ei​=I1​(p1,i​)−I2​(p2,i​)
我们关心误差 $e$e 是如何随着相机位姿 $\xi$ξ 变化的，需要分析它们的导数，使用李代数上的扰动模型，给 $\exp(\xi)$exp(ξ) 左乘一个小扰动 $\exp(\delta\xi)$exp(δξ)：
$e(\xi\oplus \delta\xi)=I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K\exp(\delta\hat{\xi})\exp(\hat{\xi})P)$e(ξ⊕δξ)=I1​(Z1​1​KP)−I2​(Z2​1​Kexp(δξ^​)exp(ξ^​)P)
对 $\exp(\delta\hat{\xi})$exp(δξ^​) 一阶泰勒展开：
$≈I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K(1+\delta\hat{\xi})\exp(\hat{\xi})P)$≈I1​(Z1​1​KP)−I2​(Z2​1​K(1+δξ^​)exp(ξ^​)P)

$=I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K\exp(\hat{\xi})P+\frac{1}{Z_2}K\delta\hat{\xi}\exp(\hat{\xi})P)$=I1​(Z1​1​KP)−I2​(Z2​1​Kexp(ξ^​)P+Z2​1​Kδξ^​exp(ξ^​)P)

记 $q=\delta\hat{\xi}\exp(\hat{\xi})P),\quad u=\frac{1}{Z_2}Kq$q=δξ^​exp(ξ^​)P),u=Z2​1​Kq，为 $P$P 在扰动后位于第二个相机坐标系下的坐标，$u$u 为其像素坐标。

利用一阶泰勒展开有：
$e(\xi\oplus \delta\xi)=I_1(\frac{1}{Z_1}KP)-I_2(\frac{1}{Z_2}K\exp(\hat{\xi})P)-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta\xi}\partial \delta\xi$e(ξ⊕δξ)=I1​(Z1​1​KP)−I2​(Z2​1​Kexp(ξ^​)P)−∂u∂I2​​∂q∂u​∂δξ∂q​∂δξ

$=e(\xi)-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial q}\frac{\partial q}{\partial \delta\xi}\partial \delta\xi$=e(ξ)−∂u∂I2​​∂q∂u​∂δξ∂q​∂δξ

其中：

• $\frac{\partial I_2}{\partial u}$∂u∂I2​​ 为 $u$u 处的像素梯度

• $\frac{\partial u}{\partial q}$∂q∂u​ 为像素对投影点的导数，记 $q=[X,Y,Z]^T$q=[X,Y,Z]T，根据视觉里程计中的推导：
  $\frac{\partial u}{\partial q}=\left[ \begin{matrix} \frac{\partial u}{\partial X} &\frac{\partial u}{\partial Y}&\frac{\partial u}{\partial Z} \\ \frac{\partial v}{\partial X} &\frac{\partial v}{\partial Y}&\frac{\partial v}{\partial Z} \\ \end{matrix} \right]=\left[ \begin{matrix} \frac{f_x}{Z} &0&-\frac{f_xX}{Z^2} \\ 0 &\frac{f_y}{Z}&\frac{f_yY}{Z^2} \\ \end{matrix} \right]$∂q∂u​=[∂X∂u​∂X∂v​​∂Y∂u​∂Y∂v​​∂Z∂u​∂Z∂v​​]=[Zfx​​0​0Zfy​​​−Z2fx​X​Z2fy​Y​​]

• 为投影点对位姿的导数，李代数中已经推导：
  $\frac{\partial q}{\partial \delta\xi}=[I,\hat{-q}]$∂δξ∂q​=[I,−q^​]

雅可比矩阵为：
$J=-\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial \delta\xi}$J=−∂u∂I2​​∂δξ∂u​
可以看出，直接法的雅可比有一个图像梯度因子，因此在图像不明显的地方对相机运动估计的贡献很小。

根据使用图像信息的不同，直接法可分为：

• 稀疏直接法，只处理稀疏角点或关键点
• 稠密直接法：使用所有像素
• 半稠密直接法：使用部分梯度明显的像素

实践：RGB-D 的直接法

稀疏直接法

我们要优化以李群方式表示的 6 *度的相机位姿 VertexSE3Expmap。然后计算直接法中的光度误差，定义一个一元边（只估计一个误差）。

定义误差：

virtual void computeError()
{
    const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
    Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
    // 相机内参计算图像坐标
    float x = x_local[0]*fx_/x_local[2] + cx_;
    float y = x_local[1]*fy_/x_local[2] + cy_;
    // check x,y is in the image
    if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
    {
        _error ( 0,0 ) = 0.0;
        this->setLevel ( 1 );
    }
    else {// 第二个图中灰度值-第一个图的灰度值
        _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
    }
}

然后写出雅可比：

Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
jacobian_uv_ksai ( 0,3 ) = invz *fx_;
jacobian_uv_ksai ( 0,4 ) = 0;
jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
jacobian_uv_ksai ( 1,3 ) = 0;
jacobian_uv_ksai ( 1,4 ) = invz *fy_;
jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
// 计算梯度
jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

_jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;

然后将图像的点给它，让它计算相机的位姿。用 g2o 建立优化问题，将定义的点和边放入即可。

得到图像间的匹配关系及相机位姿：

半稠密直接法

梯度大于某个值时，将其作为被测量量。

直接法的讨论

由于直接法的目标函数优化包含了像素的信息，因此图像的性质直接影响了优化的进行。如果图像间同一像素忽明忽暗，则灰度梯度不再是凸函数，会陷入局部最优解。可以构建图像金字塔，当图像分辨率很小时，图像较为模糊，灰度变化不大，非凸性变弱。因此先在粗糙的图像下计算位姿，不断提高分辨率，找到更精确的位姿。

同时直接法假设了灰度不变性，容易受到光照的影响。

优势：

• 没有特征提取和匹配的时间，不必显式估计每个点的运动，计算规模较小
• 选择的点只需要有像素梯度，不必是角点
• 可稠密或半稠密。可以选很多点

劣势：与光流相似，对图像质量要求较高。