KLT稀疏光流法跟踪特征点详解

由于要做SLAM的project，而KLT是这个项目中第一个算法，做个记录顺便让自己记住。

定义一个矩阵frame,用于盛放caputure对象发过来的每一帧图像，gray是frame对应的灰度图，features中盛放着通过detectFeatures()在gray中识别出来的特征的坐标。
frame——当前帧（彩色）
gray——当前帧（灰度）
prev_frame,prev_gray盛放着上一帧的彩色图和灰度图。
prev_frame——上一帧（彩色）
prev_gray——上一帧（灰度）

fpts中存放着通过KLT算法筛选过的角点，这些点首先得是角点features，而这些点中，会运动的点且在当前帧中仍然存在着的点才回保存在fpts中。
fpts[0]——上一帧KLT算法筛选过的特征点的上一帧坐标的集合
fpts[1]——当前帧KLT算法筛选过的特征点的当前坐标的集合

先考虑一般的情况，最后考虑第一帧和第二帧。
首先capture会一直在读取视频或者录像中的每一帧，当前帧，给frame和gray，现在会判断上一帧KLT算法筛选过的特征点fpts[0]的数量是否大于40。先考虑大于40的情况，那接下来就会进行KLT特征点跟踪了，在klTrackFeature()中，首先给calcOpticalFlowPyrLK()输入上一帧和当前帧的灰度图，prev_gray，gray，同时也输入fpts[0]，这个封装的函数会输出当前特征点fpts[1]（一定要注意fpts[1]是calcOpticalFlowPyrLK()计算得到的输出值），然后一个个的比对这两帧对应特征点fpts[0][i]和fpts[1][i]，如果位移大于2且能被calcOpticalFlowPyrLK()识别（status标志符为1），则在fpts[1]和initPoint保留这些点，其余的点都删掉（也就是一些从图像中消失或者不再运动的点）。

Status——特征点跟踪成功标志位

initPoint——保存着所有被KLT算法筛选过的特征点的初始坐标。它的作用就是在画轨迹线的时候和fpts[1]中的点一一连起来，显示出特征点位移特性。

当前帧的历史作用完成了，那么接下来，把当前帧的所有信息，都赋值给上一帧，把frame，gray，fpts[1],给prev_frame,prev_gray和fpts[0]，下一帧来的时候，frame，gray会继续由capture提供，fpts[1]会由calcOpticalFlowPyrLK()计算得到。接下来capture会读取下一帧，重复上面的循环。随着循环的进行，一些点会出画面，特征点越来越少，少于40的时候，我们要对特征进行重新识别。对当前帧gray进行特征提取，得到features，把这些特征点放在fpts[0]和initPoints的后面，然后再重复klTrackFeature()。

对于第一帧，我们要把第一帧的信息同时给frame和pre_frame，这个是时候，信息都是一样的。在第二帧来的时候，才是真正的开始进行对比。

#include<opencv2/opencv.hpp>
#include<iostream>

using namespace cv;
using namespace std;

Mat frame,gray; //当前帧的彩色图和灰色图
Mat prev_frame,prev_gray; //上一帧的彩色图和灰色图
vector<Point2f> features;//当前帧的使用tomasi角点检测-特征数据

vector<Point2f> iniPoints;//初始化（每次重采样时的）特征数据，作用是为了绘制跟踪轨迹
vector<Point2f> fpts0,fpts1;// 保存当前帧和前一帧特征点位置

vector<uchar> status; //特征点跟踪成功标志位
vector<float> errors;//跟踪时候区域误差和

void featureGet(Mat &frame,Mat &gray,Mat &prev_frame,Mat &prev_gray,vector<Point2f> &initPoints,vector<Point2f> &fpts0,vector<Point2f> &fpts1);
void detectFeatures(Mat &inFrame, Mat &ingray, int addPoint); 
void klTrackFeature();
void drawTrackLines();
//以上为声明全局变量


int main(int argc, char** argv){
//VideoCapture capture(0);识别摄像头
  VideoCapture capture; //实例化一个capture对象，对video进行每一帧采样
  capture.open("./V91017-133736.mp4");
  if(!capture.isOpened()){ //判断该目录下是否存在目标视频
    printf("Could not load video \n");
  return -1;
  }

  namedWindow("camera_input",CV_WINDOW_NORMAL); //创建一个显示窗口
  resizeWindow("camera_input",640,480);
  Mat frame;
  int i = 0;
  Mat R;
  Mat t;
  while(capture.read(frame)){
  //flip(frame,frame,1);
    featureGet(frame,gray,prev_frame,prev_gray,initPoints,fpts0,fpts1);
    //pose_estimation_2d2d (fpts0,fpts1,R,t);
    char c = waitKey(50);
    if (c==27){
      break;
    } 
    i++;
  }
    waitKey(0);
    return 0;
} 

void featureGet(Mat &frame,Mat &gray,Mat &prev_frame,Mat &prev_gray,vector<Point2f> &initPoints,vector<Point2f> &fpts0,vector<Point2f> &fpts1){
    cvtColor(frame,gray,COLOR_BGR2GRAY); //把frame转换成灰度图赋值给gray
    if(fpts0.size() < FEATURE_NUMBER){ //如果现存的特征点数量小于规定值，则对当前帧进行重新特征提取给initpoint（因为每出现新的一帧，就会有特征点损失）
      detectFeatures(frame,gray,fpts0.size()); //从灰度图中识别出特征赋值给features
      fpts0.insert(fpts0.end(),features.begin(),features.end()); //把features中全部元素放到fpts[0]的结尾
      initPoints.insert(initPoints.end(),features.begin(),features.end()); //把features中全部元素放到initPoints的结尾
    }
    else{
      //printf("当前正在跟踪现有特征点\n");
    }

    if(prev_gray.empty()){
      gray.copyTo(prev_gray);
    }
    
    //find_feature_matches ( prev_gray, gray,dynamic_cast(fpts0),dynamic_cast(fpts1),matches );
    klTrackFeature();
    drawFeature(frame);

    //更新前一帧数据:把现在的帧赋值给前一帧
    gray.copyTo(prev_gray);
    frame.copyTo(prev_frame);
    imshow("camara_input",frame); //在之前创建的显示窗口显示当前帧图像

}


void detectFeatures(Mat &inFrame, Mat &ingray, int addPoint){
  double maxCorners = FEATURE_NUMBER;
  double qualitylevel = 0.01;
  double minDistance = 30; //两个特征点最小距离
  double blockSize = 3;
  double k = 0.04;
  goodFeaturesToTrack(ingray,features,maxCorners-addPoint,qualitylevel,minDistance,Mat(),blockSize,false,k);
  //cout<<"detect features:"<<features.size()<<endl;
}

void drawFeature(Mat &inFrame){
  for (size_t t=0;t<fpts0.size();t++){ //???????????
    circle(inFrame,fpts0[t],2,Scalar(0,0,255),2,8,0);
  }
}

void klTrackFeature(){ //不太懂啊
    //KLT光流法跟踪
  calcOpticalFlowPyrLK(prev_gray,gray,fpts0,fpts1,status,errors); //输入前一帧图像，输入后一帧图像，输入前一帧特征点，输出后一帧特征点
  int k=0;
  //特征点过滤
  for(int i=0;i<fpts1.size();i++){
    //double dist = abs(fpts0[i].x-fpts1[i].x) + abs(fpts0[i].y-fpts1[i].y);
    if(status[i]){
    //if(dist > 2 && status[i]){ //作用：过滤掉不动的点，和跟踪失败的点。status[i]表示第i个特征点还处于跟踪状态
      initPoints[k] = initPoints[i]; //删除损失跟踪点和不动的特征点
      fpts0[k++] = fpts1[i];  //保存跟踪特征点（和上一句话一个意思）
      //上一句不好理解，这么写fpts[1][k] = fpts[1][i];k++;
    }
  }
  //保存特征点并绘制跟踪轨迹
  initPoints.resize(k);
  fpts1.resize(k);
  drawTrackLines();
  std::swap(fpts1,fpts0); //之所以是交换，是因为现在一直在考察现有的特征点，
}

void drawTrackLines(){
  for (size_t t=0;t<fpts1.size();t++){
    line(frame,initPoints[t],fpts1[t],Scalar(0,255,0),2,8,0);
    circle(frame,fpts1[t],2,Scalar(0,0,255),2,8,0);
  }
}



//1.输入第一帧图片，保存到frame,pre_frame,gray,pre_gray中去
//2.把gray中识别的特征点加到iniPoints，fpts[0]输入第二帧图片，保存到frame,gray中去
//3.用KLT算法，得到fpts[1]，过滤掉iniPoints，fpts[1]中运动量小的点和没有识别到光流的点，交换fpts[0]，fpts[1]，画出跟踪轨迹        
//4.画出特征点
//5.把当前帧图像赋值给上一帧
//6.采样下一帧图片，如果特征点数量少于阙值，返回步骤2，否则返回步骤3

1.输入第一帧图片，保存到frame,pre_frame,gray,pre_gray中去
2.把gray中识别的特征点加到iniPoints，fpts[0]输入第二帧图片，保存到frame,gray中去
3.用KLT算法，得到fpts[1]，过滤掉iniPoints，fpts[1]中运动量小的点和没有识别到光流的点，交换fpts[0]，fpts[1]，画出跟踪轨迹
4.画出特征点
5.把当前帧图像赋值给上一帧
6.采样下一帧图片，如果特征点数量少于阙值，返回步骤2，否则返回步骤3

CMakeLists如下，

cmake_minimum_required( VERSION 2.8 )
project( useKLT )

# 添加c++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
#set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS "-o3")

# 寻找OpenCV库
find_package( OpenCV 3.1 REQUIRED )

# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )
include_directories( "/usr/include/eigen3" )

add_executable( useKLT useKLT.cpp )
# 链接OpenCV库
target_link_libraries( useKLT ${OpenCV_LIBS} )

由网上视频学习整理。

光流法在工程上的运用：

【SLAM】VINS-MONO解析——feature_tracker