空撮動画からカメラ位置を推定

テンプレートマッチングから得られた情報からカメラの移動を推定する

今回のコードの場合には、max_ptに最もうまくマッチした場所の大きなMat上での座標（矩形の左上座標）を保持するようにしています。したがって、その座標とROIの左上座標との差を計算すればカメラの移動が計算できます。今回は、X方向の移動とY方向の移動量をそれぞれ求め、三平方の定理から移動速度も求めて表示しています。速度の単位はpixel/frameで、1フレームあたりに進行したピクセル距離です。なお、右方向がXの正の方向で、下方向がYの正の方向としています。したがって、下のスクリーンショットの時点ではカメラは左上に移動していたと推定されています。また、標準出力に初期位置を(0,0)とした場合の現在位置を (-11, -135) というような (X座標, Y座標) 形式で出力するようにもしています。

#include "opencv/cv.h"
#include "opencv/highgui.h"
#include <iostream>
#include <math.h>

using namespace cv;
using namespace std;

#define ROI_MARGIN_X (0.2*v_w)
#define ROI_MARGIN_Y (0.2*v_h)
#define ROI_SIZE_X (v_w-ROI_MARGIN_X*2)
#define ROI_SIZE_Y (v_h-ROI_MARGIN_Y*2)

double r(double x1,double y1,double x2,double y2){
    return pow((pow((x1-x2),2)+pow((y1-y2),2)),0.5);
}

int main(){
    VideoCapture cap("aerial.mp4");
    int v_w=cap.get(CV_CAP_PROP_FRAME_WIDTH);
    int v_h=cap.get(CV_CAP_PROP_FRAME_HEIGHT);
    Mat img;
    Mat result_img;
    int position_x=0;
    int position_y=0;
    
    //初回でのテンプレートマッチング用のROIを作成
    cap>>img ;
    Mat last_roi_img(img,Rect(ROI_MARGIN_X,ROI_MARGIN_Y,ROI_SIZE_X,ROI_SIZE_Y));
    int max_frame=cap.get(CV_CAP_PROP_FRAME_COUNT);
    for(int i=1; i<max_frame;i++){ cap>>img ;
        
        //テンプレートマッチング
        matchTemplate(img,last_roi_img,result_img, CV_TM_CCOEFF_NORMED);
        Point max_pt;
        double maxVal;
        minMaxLoc(result_img, NULL, &maxVal, NULL, &max_pt);
        
        //探索結果の場所に矩形を描画
        Rect roi_rect(0, 0, ROI_SIZE_X, ROI_SIZE_Y);
        roi_rect.x = max_pt.x;
        roi_rect.y = max_pt.y;
        rectangle(img, roi_rect, Scalar(0,255,255), 1);
        
        //速度の表示
        char v_c[256];
        sprintf(v_c, "x %d y %d px/F", (int)(ROI_MARGIN_X-max_pt.x),(int)(ROI_MARGIN_Y-max_pt.y));
        putText(img, v_c, Point(0,80), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0,0,255), 1, CV_AA);
        char vr_c[256];
        sprintf(vr_c, "%3.3f px/F", r(max_pt.x,max_pt.y,ROI_MARGIN_X,ROI_MARGIN_Y));
        putText(img, vr_c, Point(0,120), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0,0,255), 1, CV_AA);
        
        //次フレームでのテンプレートマッチング用のROIを保存
        Mat roi_img(img,Rect(ROI_MARGIN_X,ROI_MARGIN_Y,ROI_SIZE_X,ROI_SIZE_Y));
        last_roi_img=roi_img.clone();
        
        //位置推定
        position_x+=(ROI_MARGIN_X-max_pt.x);
        position_y+=(ROI_MARGIN_Y-max_pt.y);
        cout << "(" << position_x << ", " << position_y << ")"<<endl;
        
        imshow("Video",img);
        waitKey(1);
    }
    return 0;
}