第8讲视觉里程计2 --- 光流

特征点法的缺陷

尽管特征点法在视觉里程计中占据主流地位，但研究者们还是认识到它至少有以下几个缺点：

（1）关键点的提取和描述子的计算十分耗时。

（2）使用特征点时，忽略了除特征点以外的所有信息。一幅图像有几十万个像素，而特征点仅有几百个，因此只使用特征点丢弃了大部分可能有用的图像信息。

（3）相机有时候会运动到特征缺失的地方，这些地方往往没有明显的纹理信息。例如：有时候我们会面对一堵白墙，或者一个空荡荡的走廊，这些场景下特征点数量会明显减少，我们可能找不到足够的匹配点来计算相机运动。

避免特征点法的几个思路

（1）保留特征点，但是只计算关键点，不计算描述子。同时，使用光流法来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但是光流本身的计算需要一定时间。

（2）只计算关键点，不计算描述子，同时使用直接法来计算特征点在下一时刻图像中的位置。

（3）既不计算关键点，也不计算描述子，而是根据像素灰度的差异，直接计算相机运动。

直接法的思想

直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子，于是既回避了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化，直接法就能工作。

根据使用像素的数量，直接法分为稀疏、稠密、半稠密三种。

一、光流（Optical Flow）

直接法是从光流演变而来的。为了说明直接法，我们先来介绍一下光流。

光流是一种描述像素随着时间在图像之间运动的方法。随着时间的流逝，同一个像素会在图像中运动，而我们希望追踪它的运动过程。其中，计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。

本节以Lucas-Kanade光流为代表，也可以称为LK光流。

Lucas-Kanade光流

在LK光流，我们认为来自相机的图像是随时间变化的，图像可以看做时间的函数： $I(t)$ 。

那么，一个在 $t$ 时刻，位于 $(x,y)$ 处的像素，它的灰度可以写成： $I(x,y,t)$ 。

现在我们来考虑下面这个场景。

某个固定的空间点，它在 $t$ 时刻的像素坐标为 $(x,y)$ 。由于图像的运动，它的像素坐标将发生变化。我们希望估计这个空间点在其他时刻图像位置。怎么估计呢？光流大法来了，好好看！

首先，要引入光流法的基本假设。

灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。（灰度不变假设是一个很强的假设，在实际当中很可能不成立。然而，所有算法都是在一定假设下工作的。如果我们什么假设都不做，就无法设计实用的算法。所以，我们先暂且认为该假设是成立的，看看光流到底是个什么鬼！）

对于 $t$ 时刻位于 $(x,y)$ 处的像素，我们设 $t+dt$ 时刻它运动到了 $(x+dx,y+dy)$ 处，

由灰度不变假设，我们有 $I(x+dx,y+dy,t+dt)=I(x,y,t)$

把上式的左边在 $(x,y,t)$ 处进行泰勒展开（泰勒展开式如果忘了可以看多元函数的泰勒展开式），可以得到下列等式：

$I(x+dx,y+dy,t+dt)\approx I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt$ ，

由灰度不变假设，可以得到 $\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$ ,

两边同时除以 $dt$ ，得到 $\frac{\partial I}{\partial x}\frac{\mathrm{d} x}{\mathrm{d} t} + \frac{\partial I}{\partial y}\frac{\mathrm{d} y}{\mathrm{d} t} = -\frac{\partial I}{\partial t}$

（注意一下数学意义： $\frac{\mathrm{d} x}{\mathrm{d} t}$ 为像素在x轴上运动速度， $\frac{\mathrm{d} y}{\mathrm{d} t}$ 为像素在y轴上的运动速度，把他们记作 $u,v$ 。 $\frac{\partial I}{\partial x}$ 可以看做图像在该点处沿 $x$ 方向的梯度， $\frac{\partial I}{\partial y}$ 可以看做图像在该点处沿 $y$ 方向的梯度，记为 $I_{x},I_{y}$ ）

把图像灰度对时间的变化率记为 $I_{t}$ ，将上式写成矩阵的形式有： $\begin{pmatrix} I_{x} & I_{y} \end{pmatrix} \begin{pmatrix} u \\ v \end{pmatrix} = -I_{t}$

我们要计算的是像素在 $x,y$ 方向上的运动速度 $u,v$ ，但是仅凭这一个等式计算出两个未知量是不可能的。因此，必须要引入额外的约束来计算 $u,v$ 。

在LK光流中，我们假设某一个窗口内的像素具有相同的运动。

现在考虑一个大小为 $w*w$ 的窗口，它含有 $w^{2}$ 个数量的像素。

由于该窗口内的像素具有相同的运动，因此我们可以得到 $w^{2}$ 个这样的方程： $\begin{pmatrix} I_{x} & I_{y} \end{pmatrix}_{k} \begin{pmatrix} u \\ v \end{pmatrix} = -I_{t}_{k} , k=0,1,2,...,w^2$

记：

$A=\begin{bmatrix} \begin{bmatrix} I_x&I_y \end{bmatrix}_1 \\ . \\ . \\ . \\ \begin{bmatrix} I_x&I_y \end{bmatrix}_k \end{bmatrix}$ ， $\vec{b}=\begin{bmatrix} I_{t1}\\ .\\ .\\ .\\ I_{tk} \end{bmatrix}$ ，

于是整个方程就可以写成 $A\begin{bmatrix} u\\ v \end{bmatrix} =-\vec{b}$

这是一个关于 ${\color{Red} u,v}$ 的超定线性方程，传统解法是最小二乘解（我也忘了最小二乘解，之后再补吧，还是那个想法，现在最重要的就是迅速入门，掌握SLAM的大体框架和方法）。

直接给出解： $\begin{bmatrix} u\\ v \end{bmatrix}^{*} =-(A^TA)^{-1}A^T\vec{b}$

这样就得到了像素在图像间的运动速度 ${\color{Red} u,v}$ 。当 ${\color{Red} t}$ 取离散的时刻而不是连续时间时，我们可以估计某块像素在若干个图像中出现的位置。

实践：LK光流

使用TUM公开数据集

先对这个数据集做一个简单的介绍。TUM数据集是来自于慕尼黑工业大学（TUM）提供的公开RGB-D数据集。这里只使用了其中一部分的图像。下面解释一下它的数据格式：

解压后文件中是这个样子的。

rgb.txt和depth.txt记录了各文件的采集时间和对应的文件名
rgb/和depth/目录存放着采集到的PNG格式图像文件。彩色图像为8位3通道，深度图为16位单通道图像。文件名即采集时间。
groundtruth.txt问外部运动捕捉系统采集到的相机位姿，格式为 $(time,t_x,t_y,t_z,q_x,q_y,q_z,q_w)$ ，我们可以把它看成标准轨迹。

注：彩色图、深度图、标准轨迹的采集都是独立的，在使用数据之前，需要根据采集时间对数据进行一次时间上的对齐，以便彩色图和深度图进行配对。对齐原则：我们可以把采集时间相近于一个阈值内的数据，看成是一对图像，并把相近时间的位姿，看作是该图像的真实采集位置。

注：TUM提供了一个python脚本associate.py来帮我们完成这些工作，运行python associate.py rgb.txt depth.txt > associate.txt即可。

源代码：

#include <iostream>
#include <fstream>  //进行文件io
#include <list>
#include <vector>
#include <chrono>

//opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>

//namespace
using namespace std;

int main(int argc, char **argv)
{
    if(argc!=2){
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    string path_to_dataset=argv[1]; //数据集路径
    string associate_file=path_to_dataset+"/associate.txt";

    ifstream fin(associate_file);   //输入文件流对象，如果提供了文件名，则open()函数会自动调用

    string rgb_file, depth_file, time_rgb, time_depth;
    list<cv::Point2f> key_points;
    cv::Mat color, depth, last_color;

    for(int index=0; index<100; index++){
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;    //注意这个要和associate.txt中的数据意义对应
        color=cv::imread(path_to_dataset+"/"+rgb_file);
        depth=cv::imread(path_to_dataset+"/"+depth_file, -1);

        if(index==0){   //只对第一帧图像提取关键点
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector=cv::FastFeatureDetector::create();
            detector->detect(color, kps);   //提取到的关键点保存在kps当中
            for(auto kp:kps){
                key_points.push_back(kp.pt);    //kp.pt返回关键点的坐标
            }
            last_color=color;
            continue;
        }

        if(color.data==nullptr || depth.data==nullptr)
            continue;

        //对其他帧用LK跟踪特征点
        vector<cv::Point2f> next_key_points;
        vector<cv::Point2f> prev_key_points;
        for(auto kp:key_points){
            prev_key_points.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_key_points, next_key_points, status, error);
        //status: 输出状态向量，如果找到了对应特征的流，则将向量的相应元素设置为1；否则，置0
        //error: 误差输出向量，vector的每个元素被设置为对应特征的误差
        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=key_points.begin(); iter!=key_points.end(); i++){
            if(status[i]==0){
                iter=key_points.erase(iter);    //删除跟丢的点
                continue;
            }
            //下面这两句代码不是很懂
            *iter=next_key_points[i];
            iter++;
        }
        cout<<"tracked keypoints: "<<key_points.size()<<endl;
        if(key_points.size()==0){
            cout<<"all keypoints are lost. "<<endl;
            break;
        }
        //画出keypoints
        cv::Mat img_show=color.clone();
        for(auto kp:key_points){
            cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
        }
        cv::imshow("corners", img_show);
        cv::waitKey(0);
        last_color=color;
    }
    return 0;
}

CMakeLists.txt的内容：

cmake_minimum_required(VERSION 3.12)
project(useLK)

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

find_package(OpenCV 3.4 REQUIRED)

include_directories(
        ${OpenCV_INCLUDE_DIRS}
)

add_executable(useLK main.cpp)

target_link_libraries(useLK ${OpenCV_LIBS})

运行结果：只放第一帧的结果

从这里应该可以想到：角点具有更好的辨识度，边缘次之，区块最小。