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

左右2つのROIを設定し、それぞれでテンプレートマッチングを行い回転を検出する

前回までは1つのROIを設定していました。今回は左右2つのROIを設定し、別々にテンプレートマッチングを行います。仮に右側のROIと左側のROIの進む速度が異なって検出されるのなら、カメラは回転しているということが推定できます。このようにして初期状態に対する現在進行する角度を計算し、この角度と回転行列を用いて移動速度をx =ｘcosθ-ｙsinθ、y’ =ｘsinθ+ｙcosθとして変換することで、座標推定をより厳密にしています。

#include "opencv/cv.h"
#include "opencv/highgui.h"
#include <iostream>
#include <math.h>
using namespace cv;
using namespace std;

#define ROI_MARGIN_X1 (0.2*v_w)
#define ROI_MARGIN_X1_X2 (0.2*v_w)
#define ROI_MARGIN_Y (0.2*v_h)
#define ROI_SIZE_X ((v_w-ROI_MARGIN_X1*2-ROI_MARGIN_X1_X2)/2)
#define ROI_SIZE_Y (v_h-ROI_MARGIN_Y*2)
#define ROI_MARGIN_X2 (ROI_MARGIN_X1+ROI_SIZE_X+ROI_MARGIN_X1_X2)

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_img1;
    Mat result_img2;
    int d_x=0;
    int d_y=0;
    int position_x=0;
    int position_y=0;
    double angle=0; //右回りが正
    
    //初回でのテンプレートマッチング用のROIを作成
    cap>>img ;
    Mat last_roi_img1(img,Rect(ROI_MARGIN_X1,ROI_MARGIN_Y,ROI_SIZE_X,ROI_SIZE_Y));
    Mat last_roi_img2(img,Rect(ROI_MARGIN_X2,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;
        
        //テンプレートマッチング1----------------------------------------------
        matchTemplate(img,last_roi_img1,result_img1, CV_TM_CCOEFF_NORMED);
        Point max_pt1;
        double maxVal1;
        minMaxLoc(result_img1, NULL, &maxVal1, NULL, &max_pt1);
        //1つ目ここまで-----------------------------------------------------
        
        //テンプレートマッチング2----------------------------------------------
        matchTemplate(img,last_roi_img2,result_img2, CV_TM_CCOEFF_NORMED);
        Point max_pt2;
        double maxVal2;
        minMaxLoc(result_img2, NULL, &maxVal2, NULL, &max_pt2);
        //2つ目ここまで-----------------------------------------------------
        
        //探索結果の場所に矩形を描画1
        Rect roi_rect1(0, 0, ROI_SIZE_X, ROI_SIZE_Y);
        roi_rect1.x = max_pt1.x;
        roi_rect1.y = max_pt1.y;
        rectangle(img, roi_rect1, Scalar(0,255,255), 1);
        //次フレームでのテンプレートマッチング用のROIを保存1
        Mat roi_img1(img,Rect(ROI_MARGIN_X1,ROI_MARGIN_Y,ROI_SIZE_X,ROI_SIZE_Y));
        last_roi_img1=roi_img1.clone();
        
        //探索結果の場所に矩形を描画2
        Rect roi_rect2(0, 0, ROI_SIZE_X, ROI_SIZE_Y);
        roi_rect2.x = max_pt2.x;
        roi_rect2.y = max_pt2.y;
        rectangle(img, roi_rect2, Scalar(0,255,255), 1);
        //次フレームでのテンプレートマッチング用のROIを保存2
        Mat roi_img2(img,Rect(ROI_MARGIN_X2,ROI_MARGIN_Y,ROI_SIZE_X,ROI_SIZE_Y));
        last_roi_img2=roi_img2.clone();
        
        angle-=atan((double)(max_pt1.y-max_pt2.y)/(max_pt1.x-max_pt2.x));
        
        //位置推定
        d_x=(ROI_MARGIN_X1-max_pt1.x)/2+(ROI_MARGIN_X2-max_pt2.x)/2;
        d_y=(ROI_MARGIN_Y-max_pt1.y);
        position_x+=d_x*cos(angle)-d_y*sin(angle);
        position_y+=d_x*sin(angle)+d_y*cos(angle);
        
        cout << "(" << position_x << ", " << position_y << ") angle = "<<angle<<endl;
        
        imshow("Video",img);
        waitKey(1);
    }
    return 0;
}