视觉SLAM第八讲视觉里程计2--- LK光流---代码详细讲解

光流是一种描述像素随时间在图像之间运动的方法：
光流法有两个假设：（1）灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的；（2）假设某个窗口内的像素具有相同的运动
一、本讲的代码使用了三种方法来追踪图像上的特征点
（1）第一种：使用OpenCV中的LK光流；
（2）第二种：用高斯牛顿实现光流：单层光流；
（3）第三种：用高斯牛顿实现光流：多层光流。
其中高斯牛顿法，即最小化灰度误差估计最优的像素偏移。在具体函数实现中（即calculateOpticalFlow)，求解这样一个问题：

二、代码注释讲解

#include<opencv2/opencv.hpp>
#include<string>
#include<chrono>
#include<Eigen/Core>
#include<Eigen/Dense>

using namespace std;
using namespace cv;
string file_1="./LK1.png";   //第一张图像的路径
string file_2="./LK2.png";  //第二张图像的路径
  
//使用高斯牛顿法实现光流
//定义一个光流追踪类
class OpticalFlowTracker
{
 public:
    OpticalFlowTracker(    //带参构造函数，并初始化
      const Mat &img1_,
      const Mat &img2_,
      const vector<KeyPoint>&kp1_,
      vector<KeyPoint>&kp2_,
      vector<bool>&success_,
      bool inverse_=true,bool has_initial_=false):
      img1(img1_),img2(img2_),kp1(kp1_),kp2(kp2_),success(success_),inverse(inverse_),
      has_initial(has_initial_) {}
      
      //计算光流的函数
      void calculateOpticalFlow(const Range &range);  //range是一个区间，应该看作一个窗口
      
  private:
    const Mat &img1;
    const Mat &img2;
    const vector<KeyPoint> &kp1;
    vector<KeyPoint> &kp2;
    vector<bool> &success;
    bool inverse=true;
    bool has_initial=false;
  };
  //单层光流的函数声明
  void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint>&kp1,
    vector<KeyPoint>&kp2,
    vector<bool>&success,
    bool inverse=false,
    bool has_initial_guess=false
  );
  //多层光流的函数声明
  void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint>&kp1,
    vector<KeyPoint>&kp2,
    vector<bool> &success,
    bool inverse=false
       );
  
  //从图像中获取一个灰度值
  //采用双线性内插法，来估计一个点的像素：
  //f(x,y)=f(0,0)(1-x)(1-y)+f(1,0)x(1-y)+f(0,1)(1-x)y+f(1,1)xy
  inline float GetPixelValue(const cv::Mat &img,float x,float y)
  {
    //边缘检测
    if(x<0)
      x=0;
    if(y<0)
      y=0;
    if(x>=img.cols)
      x=img.cols-1;
    if(y>=img.rows)
      y=img.rows-1;
    uchar *data=&img.data[int(y)*img.step+int(x)];  //img.step:表示图像矩阵中每行包含的字节数;int(x)将x转换为int类型
    
    float xx=x-floor(x);   //floor(x)函数：向下取整函数，即返回一个不大于x的最大整数
    float yy=y-floor(y);
    
    return float(
      (1-xx)*(1-yy)*data[0]+
      xx*(1-yy)*data[1]+
      (1-xx)*yy*data[img.step]+
      xx*yy*data[img.step+1]
      );
  }
  
  //主函数
  int main(int argc,char**argv)
  {
    Mat img1=imread(file_1,0);  //以灰度读取图像
    Mat img2=imread(file_2,0);
    //特征点检测
    vector<KeyPoint>kp1;  //关键点 存放在容器kp1中
    Ptr<GFTTDetector> detector=GFTTDetector::create(500,0.01,20); //通过GFTTD来获取角点，参数：最大角点数目500;角点可以接受的最小特征值0.01;角点之间的最小距离20
    detector->detect(img1,kp1);
    
    //接下来实现在第二张图像中追踪这些角点，即追踪 kp1
    //第一种方法：单层光流
    vector<KeyPoint>kp2_single;
    vector<bool>success_single;
    OpticalFlowSingleLevel(img1,img2,kp1,kp2_single,success_single);
    
    //第二种方法：多层光流
    vector<KeyPoint>kp2_multi;
    vector<bool>success_multi;
    chrono::steady_clock::time_point t1=chrono::steady_clock::now();
    OpticalFlowMultiLevel(img1,img2,kp1,kp2_multi,success_multi,true);
    chrono::steady_clock::time_point t2=chrono::steady_clock::now();
    auto time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
     //输出使用高斯牛顿法所花费的时间
    cout << "optical flow by gauss-newton: " << time_used.count() << endl;

    
    //使用OpenCV中的LK光流
    vector<Point2f>pt1,pt2;
    for(auto &kp:kp1)     //kp1中存放的是第一张图像中的角点，通过遍历，将kp1存放在pt1中
      pt1.push_back(kp.pt);
    vector<uchar>status;
    vector<float>error;
    t1=chrono::steady_clock::now();
    //调用cv::calculateOpticalFlowPyrLK函数：
    //提供前后两张图像及对应的特征点，即可得到追踪后的点，以及各点的状态、误差;
    //根据status变量是否为1来确定对应的点是否被正确追踪到。
    cv::calcOpticalFlowPyrLK(img1,img2,pt1,pt2,status,error);
    t2=chrono::steady_clock::now();
    time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout << "optical flow by opencv: " << time_used.count() << endl;  //输出使用opencv所花费的时间
    
    //下面一部分代码实现绘图的功能
    //第一张图像：单层光流的效果图
     Mat img2_single;
     //将输入图像从一个空间转换到另一个色彩空间
     cv::cvtColor(img2,img2_single,CV_GRAY2BGR); //cvtColor(）函数实现的功能：将img2灰度图转换成彩色图img2_single输出
     for(int i=0;i<kp2_single.size();i++)
     {
       if(success_single[i])   //判断是否追踪成功
       {
	 //circle():画圆：参数：源图像，画圆的圆心坐标，圆的半径，圆的颜色，线条的粗细程度
	 //kp2_single[i].pt：用来取第i个角点的坐标；Scalar(0,250,0)：设置颜色，遵循B G R ，所以此图中为绿色
	 cv::circle(img2_single,kp2_single[i].pt,2,cv::Scalar(0,250,0),2);
	 //line():绘制直线：参数：要画的线所在的图像，直线起点，直线终点，直线的颜色（绿色）
	 cv::line(img2_single,kp1[i].pt,kp2_single[i].pt,cv::Scalar(0,250,0));
       }
     }
     
     //第二张图像：多层光流的效果图
     Mat img2_multi;
     cv::cvtColor(img2,img2_multi,CV_GRAY2BGR);
      for(int i=0;i<kp2_multi.size();i++)
     {
       if(success_multi[i])   
       {
	 cv::circle(img2_multi,kp2_multi[i].pt,2,cv::Scalar(250,0,0),2);
	  cv::line(img2_multi,kp1[i].pt,kp2_multi[i].pt,cv::Scalar(250,0,0));
       }
     }
     
     //第三张图像：使用OpenCV中的LK光流
     Mat img2_CV;
     cv::cvtColor(img2,img2_CV,CV_GRAY2BGR);
      for(int i=0;i<pt2.size();i++)
     {
       if(status[i])   
       {
	  cv::circle(img2_CV,pt2[i],2,cv::Scalar(0,0,250),2);
	  cv::line(img2_CV,pt1[i],pt2[i],cv::Scalar(0,0,250));
       }
     }
     
     //
     cv::imshow("tracked single level",img2_single);
     cv::imshow("tracked multi level",img2_multi);
     cv::imshow("tracked by opencv",img2_CV);
     cv::waitKey(0);
     return 0;
  }
  
  //接下来这一部分：具体函数的实现
  //第一个：单层光流函数的实现
  void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint>&kp1,
    vector<KeyPoint>&kp2,
    vector<bool>&success,
    bool inverse,
    bool has_initial)
  {
    //resize()函数：调整图像的大小；size（）函数：获取kp1的长度
    //初始化
    kp2.resize(kp1.size());
    success.resize(kp1.size());   //是否追踪成功的标志
    
    OpticalFlowTracker tracker(img1,img2,kp1,kp2,success,inverse,has_initial);  //创建类的对象tracker
    //调用parallel_for_并行调用OpticalFlowTracker::calculateOpticalFlow，该函数计算指定范围内特征点的光流
    //range():从指定的第一个值开始，并在到达指定的第二个值后终止
    parallel_for_(Range(0,kp1.size()),std::bind(&OpticalFlowTracker::calculateOpticalFlow,&tracker,placeholders::_1));
  }
  
  //类外实现成员函数
  void OpticalFlowTracker::calculateOpticalFlow(const Range &range)
  {
    //定义参数
    int half_patch_size=4;  //窗口的大小8×8
    int iterations=10;  //每个角点迭代10次
    for(size_t i=range.start;i<range.end;i++)
    {
      auto kp=kp1[i];   //将第一张图像中的第i个关键点kp1[i]存放在 kp 中
      double dx=0,dy=0; //初始化
      if(has_initial)
      {
	dx=kp2[i].pt.x-kp.pt.x;   //第i个点在第二张图像中的位置与第一张图像中的位置的差值
	dy=kp2[i].pt.y-kp.pt.y;
      }
      
      double cost=0,lastCost=0;
      bool succ=true;
      
      //高斯牛顿方程
      //高斯牛顿迭代
      Eigen::Matrix2d H = Eigen::Matrix2d::Zero();   //定义H，并进行初始化。
      Eigen::Vector2d b = Eigen::Vector2d::Zero();   //定义b，并初始化.
      Eigen::Vector2d J;   //定义雅克比矩阵2×1
      for(int iter=0;iter<iterations;iter++)
      {
	if(inverse==false)
	{
	  H=Eigen::Matrix2d::Zero();
	  b=Eigen::Vector2d::Zero();
	}
	else
	{
	  b=Eigen::Vector2d::Zero();
	}
	cost=0;
	//假设在这个8×8的窗口内像素具有同样的运动
	//计算cost和J
	for(int x=-half_patch_size;x<half_patch_size;x++)
	  for(int y=-half_patch_size;y<half_patch_size;y++)
	  {
	    //GetPixelValue（）计算某点的灰度值
	    //计算残差：I(x,y)-I(x+dx,y+dy)
	    double error=GetPixelValue(img1,kp.pt.x+x,kp.pt.y+y)-GetPixelValue(img2,kp.pt.x+x+dx,kp.pt.y+y+dy);;
	    if(inverse==false)
	    {
	      //雅克比矩阵为第二个图像在x+dx,y+dy处的梯度
	      J=-1.0*Eigen::Vector2d(0.5*(GetPixelValue(img2,kp.pt.x+dx+x+1,kp.pt.y+dy+y)-
	                                 GetPixelValue(img2,kp.pt.x+dx+x-1,kp.pt.y+dy+y)),
				     0.5*(GetPixelValue(img2,kp.pt.x+dx+x,kp.pt.y+dy+y+1)-
				         GetPixelValue(img2,kp.pt.x+dx+x,kp.pt.y+dy+y-1)));
	    }
	    else if(iter==0)  //如果是第一次迭代，梯度为第一个图像的梯度，反向光流法
	      //在反向光流中，I(x,y)的梯度是保持不变的，可以在第一次迭代时保留计算的结果，在后续的迭代中使用。
	      //当雅克比矩阵不变时，H矩阵不变，每次迭代只需要计算残差。
	    {
	      J=-1.0*Eigen::Vector2d(0.5*(GetPixelValue(img1,kp.pt.x+x+1,kp.pt.y+y)-
	                                 GetPixelValue(img1,kp.pt.x+x-1,kp.pt.y+y)),
				     0.5*(GetPixelValue(img1,kp.pt.x+x,kp.pt.y+y+1)-
				         GetPixelValue(img1,kp.pt.x+x,kp.pt.y+y-1)));
	    }
	    //计算H和b
	    b+=-error*J;
	    cost+=error*error;
	    if(inverse==false||iter==0)
	    {
	      H+=J*J.transpose();
	    }
	  }
	  //计算增量update，求解线性方程Hx=b
	  Eigen::Vector2d update=H.ldlt().solve(b);
	  if(std::isnan(update[0]))  //判断增量
	  {
	    //有时当我们遇到一个黑色或白色的方块，H是不可逆的，即高斯牛顿方程无解
	    cout<<"update is nan"<<endl;
	    succ=false;   //追踪失败
	    break;
	  }
	  if(iter>0&&cost>lastCost)
	  {
	    break;
	  }
	  dx+=update[0];
	  dy+=update[1];
	  lastCost=cost;
	  succ=true;
	  if(update.norm()<1e-2)
	  {
	    break;
	  }
      }
      success[i]=succ;
      kp2[i].pt=kp.pt+Point2f(dx,dy);
    }
  }//迭代完成
  
  //第二个：多层光流函数的实现
  void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint>&kp1,
    vector<KeyPoint>&kp2,
    vector<bool>&success,
    bool inverse)
  {
    int pyramids=4; //建立4层金字塔
    double pyramid_scale=0.5;  //金字塔每层缩小0.5
    double scales[]={1.0,0.5,0.25,0.125};
    
    //建立金字塔
    chrono::steady_clock::time_point t1=chrono::steady_clock::now();  //计算时间
    vector<Mat>pyr1,pyr2;
    for(int i=0;i<pyramids;i++)
    {
      if(i==0)
      {
	//将两张图像存放在pyr1,pyr2中
	pyr1.push_back(img1);    
	pyr2.push_back(img2);
      }
      else
      {
	Mat img1_pyr,img2_pyr;
	//对图像进行缩放，参数：原图，输出图像，输出图像大小
	cv::resize(pyr1[i-1],img1_pyr,cv::Size(pyr1[i-1].cols*pyramid_scale,pyr1[i-1].rows*pyramid_scale));
	cv::resize(pyr2[i-1],img2_pyr,cv::Size(pyr2[i-1].cols*pyramid_scale,pyr2[i-1].rows*pyramid_scale));
	pyr1.push_back(img1_pyr);
	pyr2.push_back(img2_pyr);
      }
    }
    chrono::steady_clock::time_point t2=chrono::steady_clock::now();
    auto time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"build pyramid time:"<<time_used.count()<<endl;
    
    //计算光流时，先从顶层的图像开始计算
    vector<KeyPoint>kp1_pyr,kp2_pyr;
    for(auto &kp:kp1)
    {
      auto kp_top=kp;
      kp_top.pt *=scales[pyramids-1];   //顶层
      kp1_pyr.push_back(kp_top);
      kp2_pyr.push_back(kp_top);
    }
    
    for(int level=pyramids-1;level>=0;level--)
    {
      success.clear();
      t1=chrono::steady_clock::now();
      OpticalFlowSingleLevel(pyr1[level],pyr2[level],kp1_pyr,kp2_pyr,success,inverse,true);
      t2=chrono::steady_clock::now();
      auto time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
      cout<<"track pyr"<<level<<"cost time:"<<time_used.count()<<endl;
      if(level>0)
      {
	for(auto &kp:kp1_pyr)
	  kp.pt/=pyramid_scale;
	for(auto &kp:kp2_pyr)
	  kp.pt/=pyramid_scale;
      }
    }
    for(auto &kp:kp2_pyr)
      kp2.push_back(kp);
  }

三、代码运行结果
1、构建完成后，打开程序所在的文件目录下，在此目录下打开终端，在终端输入：

2.运行结果图

小备注：对于代码的理解可能存在问题，欢迎各位指正，蟹蟹