Visp系列学习四：visp 跟踪(一)

Block Tracking

参考：http://fumacrom.com/1mirO

实验环境：ubuntu16 + ros kinetic + kinectV2 深度相机
实验过程：编写附录所示的代码，运行相机启动指令，运行程序。
鼠标点击门上后，跟踪块会以门为中心，移动相机可以看到跟踪效果。
实验效果：

Keypoint tracking

参考：http://fumacrom.com/1mirt
　近来在研究跟踪，跟踪的方法其实有很多，如粒子滤波（pf）、meanshift跟踪，以及KLT跟踪或叫Lucas光流法，这些方法各自有各自的有点，对于粒子滤波而言，它能够比较好的在全局搜索到最优解，但其求解速度相对较慢，由于其是基于颜色直方图的计算，所以对相同颜色东西不太能够区别，meanshift方法很容易陷入局部最优，但速度还是挺快，所以现在很有一些人是将meanshift跟pf结合做跟踪，恰好在很多方面能够互补。

Kanade-Lucas-Tomasi方法，在跟踪方面表现的也不错，尤其在实时计算速度上，用它来得到的，是很多点的轨迹“trajectory”，并且还有一些发生了漂移的点，所以，得到跟踪点之后要进行一些后期的处理，说到Kanade-Lucas-Tomasi方法，首先要追溯到Kanade-Lucas两人在上世纪80年代发表的paper：An Iterative Image Registration Technique with an Application to Stereo Vision，这里讲的是一种图像点定位的方法，即图像的局部匹配，将图像匹配问题，从传统的滑动窗口搜索方法变为一个求解偏移量d的过程，后来Jianbo Shi和Carlo Tomasi两人发表了一篇CVPR(94’)的文章Good Features To Track，这篇文章，主要就是讲，在求解d的过程中，哪些情况下可以保证一定能够得到d的解，这些情况的点有什么特点（后来会发现，很多时候都是寻找的角点）。

KLT算法的几个前提假设：

• 1）亮度恒定
• 2）时间连续或者是运动是“小运动”
• 3）空间一致，临近点有相似运动，保持相邻

下面看一下在visp上实现KLT的效果。

实验的代码如附录所示。

首先roslaunch启动kinect后，通过vpROSGrabber获取对应的图像话题，然后根据vpImageConvert::convert将vpImage格式的图像转换成cv::Mat格式。
左击鼠标选择对应的点，右击鼠标确定后既可开始进行跟踪。这些跟踪的点如下图的红色十字所示。

附程序

Block Tracking

#include<visp3/core/vpConfig.h>
#include<visp3/gui/vpDisplayX.h>
#include<visp3/blob/vpDot2.h>
#include<visp3/gui/vpDisplayGDI.h>
#include<visp3/gui/vpDisplayOpenCV.h>
#include <visp_ros/vpROSGrabber.h>

int main()
{
    vpImage<unsigned char> I; //Create a gray level image container
    vpROSGrabber g; // Create a grabber based on ROS
    g.setImageTopic("/kinect2/qhd/image_color"); //get ros topic
    g.open(I); 
    vpDisplayX d(I, 0, 0, "camera view");//show iamge on screen

    // cv::Mat frame;//opencv图像显示
    // g >> frame; 

    vpImagePoint germ;
    bool init_done = false;
    vpDot2 blob; //2D 
    blob.setGraphics(true);
    blob.setGraphicsThickness(2);
    // vpImageConvert::convert(frame, I);

    while(1) {
        try{
            g.acquire(I);
            // g>>frame;
            // vpImageConvert::convert(frame, I);
            vpDisplay::display(I); 

            if (!init_done) {
                vpDisplay::displayText(I, vpImagePoint(10, 10), "Click in the blob to initialize the tracker", vpColor::green);
                if (vpDisplay::getClick(I, germ, false)) {
                blob.initTracking(I, germ);
                init_done = true;
                }
            } 
            else {
                blob.track(I);
            }
            vpDisplay::flush(I); 
        }catch(...){
            init_done = false;
        } 
    } 
}

KLT tracking

#include <visp3/core/vpImageConvert.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpVideoReader.h>
#include <visp3/klt/vpKltOpencv.h>
#include <visp_ros/vpROSGrabber.h> 
int main(int argc, const char *argv[])
{ 
  try {
    bool opt_init_by_click = false;
    for (int i = 0; i < argc; i++) {
      if (std::string(argv[i]) == "--init-by-click")
        opt_init_by_click = true;
      else if (std::string(argv[i]) == "--help") {
        std::cout << "Usage: " << argv[0] << " [--init-by-click] [--help]" << std::endl;
        return 0;
      }
    }
    cv::Mat cvI;
    vpImage<unsigned char> I; //Create a gray level image container
    vpROSGrabber g; // Create a grabber based on ROS
    g.setImageTopic("/kinect2/qhd/image_color"); //get ros topic
    g.open(I); 
    g.acquire(I);
    vpImageConvert::convert(I, cvI); 
    vpDisplayOpenCV d(I, 0, 0, "Klt tracking");
    vpDisplay::display(I);
    vpDisplay::flush(I);
    vpKltOpencv tracker;
    tracker.setMaxFeatures(200);
    tracker.setWindowSize(10);
    // To detect more keypoints, you may decrease the quality parameter set with the following line
    tracker.setQuality(0.01);
    tracker.setMinDistance(15);
    tracker.setHarrisFreeParameter(0.04);
    tracker.setBlockSize(9);
    tracker.setUseHarris(1);
    tracker.setPyramidLevels(3);
    // Initialise the tracking

    if (opt_init_by_click) {
      vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
 
      std::vector<cv::Point2f> feature;
 
      vpImagePoint ip;
      do {
        vpDisplay::displayText(I, 10, 10, "Left click to select a point, right to start tracking", vpColor::red);
        if (vpDisplay::getClick(I, ip, button, false)) {
          if (button == vpMouseButton::button1) {
            feature.push_back(cv::Point2f((float)ip.get_u(), (float)ip.get_v()));
            vpDisplay::displayCross(I, ip, 12, vpColor::green);
          }
        }
        vpDisplay::flush(I);
        vpTime::wait(20);
      } while (button != vpMouseButton::button3);
 
      tracker.initTracking(cvI, feature);
 
    } else {
      tracker.initTracking(cvI);
    }
    std::cout << "Tracker initialized with " << tracker.getNbFeatures() << " features" << std::endl;
    while (1) {
      g.acquire(I);
      vpDisplay::display(I);
      vpImageConvert::convert(I, cvI); 
      tracker.track(cvI);
      tracker.display(I, vpColor::red);
      vpDisplay::flush(I);
    }
    vpDisplay::getClick(I);
 
    return 0;
  } catch (const vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
 
}