OpenCV3とPython3を使った物体追跡（マウスで指定した特徴点をLucas-Kanade法で追跡する）

# はじめに
今回は、マウスクリックで動画に対して追跡対象を指定し、リアルタイムに３次元空間内の物体を追跡をしてみます。テンプレートマッチングと比較して、計算量が少なく、回転しても追跡し続けることが可能な、特徴量を利用するアルゴリズムを使います。

# OpenCV
OpenCV(Open Source Computer Vision Library)はBSDライセンスの映像/画像処理ライブラリ集です。画像のフィルタ処理、テンプレートマッチング、物体認識、映像解析、機械学習などのアルゴリズムが多数用意されています。

■ OpenCVを使った動体追跡の例 (OpenCV Google Summer of Code 2015)
[https://www.youtube.com/watch?v=OUbUFn71S4s](https://www.youtube.com/watch?v=OUbUFn71S4s)

■ インストールと簡単な使い方はこちら
[OpenCV 3（core + contrib）をPython 3の環境にインストール＆OpenCV 2とOpenCV 3の違い＆簡単な動作チェック](http://qiita.com/olympic2020/items/d5d475a446ec9c73261e)

■ 静止画像のフィルター処理についてはこちら
[OpenCVでエッジ検出してみる](http://qiita.com/olympic2020/items/2c3a2bfefe73ab5c86a4)
[OpenCVで各種フィルター処理をする（グラディエント、ハイパス、ラプラシアン、ガウシアン）](http://qiita.com/olympic2020/items/93e01ef22e46b14a60a9)
[OpenCVで特徴点を抽出する（AgastFeature, FAST, GFTT, MSER, AKAZE, BRISK, KAZE, ORB, SimpleBlob）](http://qiita.com/olympic2020/items/62989573a30ec1d8180b)

■ 動画ファイルの処理についてはこちら
[OpenCVで動画をリアルタイムに変換してみる](http://qiita.com/olympic2020/items/ce00fab38d829965db3b)
[OpenCVでWebカメラ/ビデオカメラの動画をリアルタイムに変換してみる](http://qiita.com/olympic2020/items/12a2eceaf65f142ec3df)
[OpenCVでオプティカルフローをリアルタイムに描画する（Shi-Tomasi法、Lucas-Kanade法）](http://qiita.com/olympic2020/items/772549d0fc3c89fb3cc4)


# 機能概要
今回は以下のような機能のプログラムを作成します。
+ マウスクリックで追跡する点を指定  
    　　cv2.setMouseCallback()
+ 追跡する特徴点を複数指定することが可能  
+ 追跡中の特徴点を動画に表示
+ 画像ピラミッド型Lucas-Kanade法で２フレーム間の特徴点の動きを検出  
    　　cv2.calcOpticalFlowPyrLK()
+ 計算は浮動小数点まで高精度に行う  
    　　cv2.cornerSubPix()
+ 特徴点として認識できなくなった点を自動的に削除
+ 既存の特徴点の近傍をマウスクリックすると特徴点を削除
+ 「Esc」キーを押すと終了
+ 「s」キーを押すと一時停止
+ 一時停止中に「s」キーを押すとステップ実行
+ 一時停止中に「r」キーを押すとリスタート
+ 動画が最後まで再生されたら終了

動画に対してマウスクリックで特徴点を指定する場合、一時停止機能は結構重要です。
「s」キーで動画を一時停止させ、ゆっくりと特徴点を指定してみてください。

# プログラム
+ 動作環境
    - python: 3.5.1
    - OpenCV: 3.1.0

+ 動画データ
    　OpenCVに付属しているサンプル動画を利用しました。
    　　OpenCV\opencv\sources\samples\data\768x576.avi

```motion.py
import cv2
import numpy as np

# Esc キー
ESC_KEY = 0x1b
# s キー
S_KEY = 0x73
# r キー
R_KEY = 0x72
# 特徴点の最大数
MAX_FEATURE_NUM = 500
# 反復アルゴリズムの終了条件
CRITERIA = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
# インターバル （1000 / フレームレート）
INTERVAL = 30
# ビデオデータ
VIDEO_DATA = '768x576.avi'

class Motion:
    # コンストラクタ
    def __init__(self):
        # 表示ウィンドウ
        cv2.namedWindow("motion")
        # マウスイベントのコールバック登録
        cv2.setMouseCallback("motion", self.onMouse)
        # 映像
        self.video = cv2.VideoCapture(VIDEO_DATA)
        # インターバル
        self.interval = INTERVAL
        # 現在のフレーム（カラー）
        self.frame = None
        # 現在のフレーム（グレー）
        self.gray_next = None
        # 前回のフレーム（グレー）
        self.gray_prev = None
        # 特徴点
        self.features = None
        # 特徴点のステータス
        self.status = None

    # メインループ
    def run(self):
        
        # 最初のフレームの処理
        end_flag, self.frame = self.video.read()
        self.gray_prev = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY)

        while end_flag:
            # グレースケールに変換
            self.gray_next = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY)

            # 特徴点が登録されている場合にOpticalFlowを計算する
            if self.features is not None:
                # オプティカルフローの計算
                features_prev = self.features
                self.features, self.status, err = cv2.calcOpticalFlowPyrLK( \
                                                    self.gray_prev, \
                                                    self.gray_next, \
                                                    features_prev, \
                                                    None, \
                                                    winSize = (10, 10), \
                                                    maxLevel = 3, \
                                                    criteria = CRITERIA, \
                                                    flags = 0)

                # 有効な特徴点のみ残す
                self.refreshFeatures()
                
                # フレームに有効な特徴点を描画
                if self.features is not None:
                    for feature in self.features:
                        cv2.circle(self.frame, (feature[0][0], feature[0][1]), 4, (15, 241, 255), -1, 8, 0)
            
            # 表示
            cv2.imshow("motion", self.frame)
            
            # 次のループ処理の準備
            self.gray_prev = self.gray_next
            end_flag, self.frame = self.video.read()
            if end_flag:
                self.gray_next = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY)

            # インターバル
            key = cv2.waitKey(self.interval)
            # "Esc"キー押下で終了
            if key == ESC_KEY:
                break
            # "s"キー押下で一時停止
            elif key == S_KEY:
                self.interval = 0
            elif key == R_KEY:
                self.interval = INTERVAL
                
            
        # 終了処理
        cv2.destroyAllWindows()
        self.video.release()


    # マウスクリックで特徴点を指定する
    #     クリックされた近傍に既存の特徴点がある場合は既存の特徴点を削除する
    #     クリックされた近傍に既存の特徴点がない場合は新規に特徴点を追加する
    def onMouse(self, event, x, y, flags, param):
        # 左クリック以外
        if event != cv2.EVENT_LBUTTONDOWN:
            return

        # 最初の特徴点追加
        if self.features is None:
            self.addFeature(x, y)
            return

        # 探索半径（pixel）
        radius = 5
        # 既存の特徴点が近傍にあるか探索
        index = self.getFeatureIndex(x, y, radius)

        # クリックされた近傍に既存の特徴点があるので既存の特徴点を削除する
        if index >= 0:
            self.features = np.delete(self.features, index, 0)
            self.status = np.delete(self.status, index, 0)

        # クリックされた近傍に既存の特徴点がないので新規に特徴点を追加する
        else:
            self.addFeature(x, y)

        return


    # 指定した半径内にある既存の特徴点のインデックスを１つ取得する
    #     指定した半径内に特徴点がない場合 index = -1 を応答
    def getFeatureIndex(self, x, y, radius):
        index = -1
        
        # 特徴点が１つも登録されていない
        if self.features is None:
            return index
        
        max_r2 = radius ** 2
        index = 0
        for point in self.features:
            dx = x - point[0][0]
            dy = y - point[0][1]
            r2 = dx ** 2 + dy ** 2
            if r2 <= max_r2:
                # この特徴点は指定された半径内
                return index
            else:
                # この特徴点は指定された半径外
                index += 1
                
        # 全ての特徴点が指定された半径の外側にある
        return -1


    # 特徴点を新規に追加する
    def addFeature(self, x, y):
        
        # 特徴点が未登録
        if self.features is None:
            # ndarrayの作成し特徴点の座標を登録
            self.features = np.array([[[x, y]]], np.float32)
            self.status = np.array([1])
            # 特徴点を高精度化
            cv2.cornerSubPix(self.gray_next, self.features, (10, 10), (-1, -1), CRITERIA)

        # 特徴点の最大登録個数をオーバー
        elif len(self.features) >= MAX_FEATURE_NUM:
            print("max feature num over: " + str(MAX_FEATURE_NUM))

        # 特徴点を追加登録
        else:
            # 既存のndarrayの最後に特徴点の座標を追加
            self.features = np.append(self.features, [[[x, y]]], axis = 0).astype(np.float32)
            self.status = np.append(self.status, 1)
            # 特徴点を高精度化
            cv2.cornerSubPix(self.gray_next, self.features, (10, 10), (-1, -1), CRITERIA)
            

    # 有効な特徴点のみ残す
    def refreshFeatures(self):
        # 特徴点が未登録
        if self.features is None:
            return
        
        # 全statusをチェックする
        i = 0
        while i < len(self.features):
            
            # 特徴点として認識できず
            if self.status[i] == 0:
                # 既存のndarrayから削除
                self.features = np.delete(self.features, i, 0)
                self.status = np.delete(self.status, i, 0)
                i -= 1
            
            i += 1


if __name__ == '__main__':
    Motion().run()
```

# 実行結果
　プログラムは、OpenCVのサンプル動画（768x576.avi）を使うものでしたが、いつも同じ画面で代り映えしませんよね。
　動画は何でもＯＫです。プログラムの`VIDEO_DATA`部分を好きな動画に変更してください。
　例えば、実行結果の実例として、リオデジャネイロ オリンピック 2016 体操個人総合 決勝、鉄棒をする内村選手を使ってみます。こちらの動画は、鉄棒をする内村選手も動いていますが、背景のフレームも内村選手に合わせて上下に動いています。「s」キーで動画を一時停止させ、内村選手の靴下に追従する点を設定します。黄色い点が特徴点を表しています。動画を再生すると、黄色い点が内村選手の靴下を追跡している様子が分かります。（内村選手のつま先が綺麗にそろっているため、同じ特徴点として認識し続けていられるとも言えますね。）

![u1.png](https://qiita-image-store.s3.amazonaws.com/0/107056/cb625729-451d-7e6d-aad2-247109e45dca.png)

![u2.png](https://qiita-image-store.s3.amazonaws.com/0/107056/741564cd-9851-cd68-0d9a-045f758adaff.png)

![u3.png](https://qiita-image-store.s3.amazonaws.com/0/107056/e540f5e1-adf5-62b4-0bdf-d58286e7cd45.png)

![u4.png](https://qiita-image-store.s3.amazonaws.com/0/107056/178c785c-e6bc-a21d-72cf-2e4a6294653c.png)

![u5.png](https://qiita-image-store.s3.amazonaws.com/0/107056/fc16755d-a323-8bce-0bed-adac831e8f9b.png)

# プログラムの課題
実際にプログラムを動作させてみると、今回のプログラム（単純にcv2.calcOpticalFlowPyrLK()を使うだけの方法）には、３つの課題があることが分かります。

1. 背景の特徴点と追跡中の特徴点が重なると誤認が発生し、背景の特徴点に捕まってしまう。  
2. 追跡中の特徴点が隠れると追跡が終わってしまい、特徴点が現れても再追跡してくれない。  
3. 指定した特徴点に特徴がないと、すぐに追跡が終わってしまう。

いずれも計算量が増えてしまうものの、対応方法はあります。
そのうち対応版も作ってみようと思います。