KLT: Kanade-Lucas-Tomasi Feature Trackerをリアルタイムで動かして特徴点追跡をするまで

こちらの記事では、KLT法(KLT: Kanade-Lucas-Tomasi Feature Tracker)をmac、あるいはlinux上で読み込み、リアルタイムで特徴点抽出、追跡をするまでを説明します。
筆者のPC環境は、一昔前のMac book air(Core 2 Duo)です。言語はC++のみを使います。gccのバージョンは4.2.1です。画像の読み込み部分にOpenCVを使っています。

この技術についての投稿は、一つの索引用の記事にまとめられています。
全体を俯瞰する場合はそちらを御覧ください。

m-yoshi-1700.hatenablog.com

ライブラリをダウンロード

以下のサイトからライブラリをダウンロードして展開します。
https://cecas.clemson.edu/~stb/klt/installation.html

ライブラリのビルド

ライブラリをmakeします

cd klt
make

make後、エラーがなく、libklt.aという静的リンクライブラリが生成されていれば完了です。

読み込み側のプログラム

以下の様にプログラムを作成します。
非常に汚いプログラムで申し訳ありません...

sample.cpp:

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

#include "klt.h"

using namespace std;
using namespace cv;


Rect box;
bool dragging = false;
bool reset = false;

bool track = false;

typedef struct {
  int xdiff;
  int ydiff;
} TrackResult;

void my_mouse_callback(int event, int x, int y, int flags, void* param){
  switch (event){
  case cv::EVENT_MOUSEMOVE:
    if (dragging){
      box.width = x - box.x;
      box.height = y - box.y;
    }
    break;

  case cv::EVENT_LBUTTONDOWN:
    track = false;
    reset = false;
    dragging = true;
    box = cv::Rect(x, y, 0, 0);
    break;

  case cv::EVENT_LBUTTONUP:
    dragging = false;
    reset = true;
    // track = true;
    if (box.width < 0){
      box.x += box.width;
      box.width *= -1;
    }
    if (box.height < 0){
      box.y += box.height;
      box.height *= -1;
    }
    break;
  }
}

int main(int argc, char** argv)
{
  cv::VideoCapture cap(0);
  if(!cap.isOpened())
    return -1;

  int nFeatures = 20;

  KLT_TrackingContext tc;// =
  KLT_FeatureList fl;// = KLTCreateFeatureList(nFeatures);

  Mat previousFrame;

  cv::namedWindow("window");
  cv::setMouseCallback("window", my_mouse_callback);

  // box = Rect(0, 0, 640, 480);
  box = Rect(0, 0, 320, 240);
  reset = true;
  
  while(1){
    cv::Mat frame;
    cap >> frame;

    cv::resize(frame, frame, cv::Size(), 0.5, 0.5);
    
    Mat grayImage;
    cv::cvtColor(frame, grayImage, CV_BGR2GRAY);

    vector<Point> points;

    vector<Point> beforeTrack;
    vector<TrackResult> afterTrack;

    if(reset){
      tc = KLTCreateTrackingContext();

      tc->sequentialMode = TRUE;
      tc->writeInternalImages = FALSE;
      // tc->affineConsistencyCheck = -1;

      tc->mindist = 20;
      tc->window_width  = 3;
      tc->window_height = 3;
      KLTChangeTCPyramid(tc, 10);
      KLTUpdateTCBorder(tc);
  
      KLTPrintTrackingContext(tc);
      
      fl = KLTCreateFeatureList(nFeatures);
      KLTSelectGoodFeatures(tc, grayImage(box).data, box.width, box.height, fl);

      reset = false;
      track = true;

      previousFrame = grayImage.clone();
      continue;
    }
    if (track){
      beforeTrack.clear();
      for(int i = 0; i < nFeatures; ++i){
	KLT_Feature feature = fl->feature[i];
	Point point(feature->x, feature->y);

	beforeTrack.push_back(point);
      }
      
      KLTTrackFeatures(tc,
		       (previousFrame(box)).data,
		       (grayImage(box)).data,
		       box.width,
		       box.height,
		       fl);

      afterTrack.clear();
      for(int i = 0; i < nFeatures; ++i){
	KLT_Feature feature = fl->feature[i];
	if (feature->val < 0)
	  continue;

	TrackResult result;
	result.xdiff = feature->x - beforeTrack[i].x;
	result.ydiff = feature->y - beforeTrack[i].y;
	if (result.xdiff == 0 && result.ydiff == 0)
	  continue;
	afterTrack.push_back(result);
      }

      KLTReplaceLostFeatures(tc, (grayImage(box)).data, box.width, box.height, fl);
      points.reserve(fl->nFeatures);
    }

    if (track){
      for(int i = 0; i < fl->nFeatures; ++i){
	KLT_Feature feature = fl->feature[i];
	Point center(box.x + feature->x, box.y + feature->y);

	if (center.x < 0 || center.y < 0|| center.x > grayImage.cols || center.y > grayImage.rows)
	  continue;

	points.push_back(center);
      
	circle(frame, center, 3, Scalar(0, 255, 0), -1);
      }

      double xsum = 0;
      double ysum = 0;
      for(int i = 0; i < afterTrack.size(); ++i){
	xsum += afterTrack[i].xdiff;
	ysum += afterTrack[i].ydiff;
      }

      xsum /= afterTrack.size();
      ysum /= afterTrack.size();

      TrackResult diff;

      diff.xdiff = (int)xsum;
      diff.ydiff = (int)ysum;

	diff.xdiff = 0;
	diff.ydiff = 0;

      box.x += diff.xdiff;
      box.y += diff.ydiff;

      for(int i = 0; i < nFeatures; ++i){
	fl->feature[i]->x += diff.xdiff;
	fl->feature[i]->y += diff.ydiff;
      }

    }

    previousFrame = grayImage.clone();

    cv::imshow("window", frame);

    int key = cv::waitKey(1);
    if(key == 113)
      break;
    else if(key == 115)
      cv::imwrite("img.png", frame);
  }
  cv::destroyAllWindows();
  return 0;
}

cmake用のCMakeLists.txtも用意します。
CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

set(cmake_build_type debug)

project(cv_shokyu)

find_package(opencv)
find_package(boost)

include_directories("${PROJECT_SOURCE_DIR}")
include_directories("/usr/local/include")
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(${Boost_INCLUDE_DIRS})
include_directories("${PROJECT_SOURCE_DIR}/klt")

link_directories("/usr/local/lib")

add_library(klt
    STATIC
    IMPORTED
)

set_target_properties(klt
	PROPERTIES
	IMPORTED_LOCATION ${CMAKE_SOURCE_DIR}/klt/libklt.a)

add_executable(sample sample.cpp)

target_link_libraries(sample klt ${OpenCV_LIBRARIES} ${Boost_LIBRARIES})