サイトアイコン CV & Technologies

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

空撮動画の解析の流れを示しています。ドローンなどを使って真上から高度一定で撮影するという条件のもとで、カメラの座標推定から高精度なパノラママップの作成までのチュートリアルです。難易度は高めです。

最近のGPSの性能は上がってきていますが、数cm以下の誤差で抑えられるようなGPS受信機は手頃な価格では手に入りません。そこで、上空から真下に向けて撮影した動画を元にカメラの移動を計算する方法を解説します。下のサンプル動画は、右クリックからダウンロードすることができます。

なお、空撮に関しては、高価なマルチコプター(複数の羽をもつヘリコプター)を使用するほど大きなカメラを搭載可能で、高画質の映像を得られる傾向があります。ですが、安価なマルチコプターでも空撮が可能な機種はありますので、それを利用するのも良いと思います。DJIのドローンはとても安定しているのでおすすめです。

ただし、このような小型のドローンを飛ばすために、免許などは不要なものの、飛行が制限されている場所もあるので注意が必要です。詳細なルールは国土交通省のホームページや、ユーザー登録が必要となりますが、ドローン専用飛行支援地図サービスなどを参照してください。

画像を表示する

最終的な目的はカメラの移動の推定ですが、まずは動画を再生して表示するだけのプログラムを作成します。動画へのパスは各々で動画の置いてある場所に合わせてください。

#include "opencv/cv.h"
#include "opencv/highgui.h"
using namespace cv;

int main(){
    Mat img;
    VideoCapture cap("aerial.mp4");
    int max_frame=cap.get(CV_CAP_PROP_FRAME_COUNT);
    for(int i=0; i<max_frame;i++){ cap>>img;
        imshow("Video",img);
        waitKey(1);
    }
    return 0;
}

テンプレートマッチングにより画像の移動を推定する

カメラが少しづつ移動しているなら、n番目のフレームの画像とn+1番目のフレームの画像は少しだけずれていることになります。ここで、カメラの移動がそれほど速くなく、しかも回転が僅かであると仮定します。この仮定の元ではn番目のフレームの画像の中心付近の部分と似たような領域が、n+1番目のフレームの画像の中央付近にも存在するということが言え、さらに回転が僅かであるという仮定のもとで平行移動したものとすることができ、高度一定の飛行なら拡大縮小もないとすることができます。このような制限付きで似たような画像を見つけるにはテンプレートマッチングを使います。

ROIを設定する

「画像の中心付近の部分」というのを、今回は上下左右に20%の余白を取った中央付近の領域ということにします。OpenCVではこのように設定した領域をROI(関心領域:Region Of Interest)として扱うと便利です。ROIはMatから一部分を切り取ることで作ることができるMatです。切り取る矩形はRect(矩形左上X座標,矩形左上Y座標,幅,高さ)の形式で指定します。下の例ではimgから左に200、上に100の余白を空けた、一片400の正方形領域を切り取ったものをroi_imgとして保持するようなコードです。

Mat roi_img(img,Rect(200, 100, 400, 400));

matchTemplate関数でテンプレートマッチングを行う

OpenCVではmatchTemplate関数を使うことで、簡単にテンプレートマッチングを実装できます。この関数は、第一引数に大きなMat、第二引数に小さなMatを指定して、小さなMatが大きなMatのどの位置に似ているかということを計算します。下のコードではmax_ptに最もうまくマッチした場所の大きなMat上での座標(矩形の左上座標)を保持するようにしています。なお、#define文でROIの位置と大きさを設定しています。#define AAAA BBBB という文は、AAAAをBBBBに置き換えるという意味になります。

#include "opencv/cv.h"
#include "opencv/highgui.h"
using namespace cv;

#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)

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;
    
    //初回でのテンプレートマッチング用の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);
        
        //次フレームでのテンプレートマッチング用のROIを保存
        Mat roi_img(img,Rect(ROI_MARGIN_X,ROI_MARGIN_Y,ROI_SIZE_X,ROI_SIZE_Y));
        last_roi_img=roi_img.clone();
        
        imshow("Video",img);
        waitKey(1);
    }
    return 0;
}

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

今回のコードの場合には、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;
}

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

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

#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;
}

動画からパノラママップを作成する

さて、いよいよ今回の空撮動画の解析も最終回です。総仕上げとして、パノラマのようなマップを作ります。下のコードでは事前にどれくらいの大きさのマップになるかを計算しておく必要があります。今回の動画ではX座標は-1500~1500、y座標は-3000~1000の大きさがあれば十分であることがわかっているものとします。したがって、幅3000、高さ4000のマップになります。重なりの部分については次々と画像を上書きしていくことで合成します。

処理の流れ

下のコードでは、まず、前回までのチュートリアルで計算したように2つのROIからカメラのワールド座標と初期状態からの回転角度を求めています。今回、付け加えられた処理はパノラマ作成におけるモザイキング処理、すなわち、画像同士の貼り合わせ処理です。様々な実装手法がありえますが、今回は画像をカメラの角度に応じて回転させた画像を、ワールド座標の位置に順次重ねる処理を行います。ただし、回転させた画像は撮影領域の周りに黒の背景がついたものになっているため、そのまま重ねていくとうまくいきません。したがって、重ね合わせの時に黒色の部分を除いて重ね合わせています。具体的には回転画像を低い閾値で2値化してマスク画像を作成しています。これでも重ね合わせる画像の境界部分に黒色がわずかに入り込むことがあるため、余裕をもたせるためにマスク画像に収縮処理を行っています。

#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)

#define map_offset_x 1500
#define map_offset_y 3000

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; //右回りが正
    Mat map_img = Mat::zeros(4000, 3000, CV_8UC3);
    Mat dst_img= Mat(r(v_h,v_w,0,0),r(v_h,v_w,0,0), CV_8UC3);
    Rect dst_ROI(r(v_h,v_w,0,0)/2-v_w/2, r(v_h,v_w,0,0)/2-v_h/2, v_w, v_h);
    Mat element = Mat::ones(3,3,CV_8UC1);
    
    //初回でのテンプレートマッチング用の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つ目ここまで-----------------------------------------------------
        
        //次フレームでのテンプレートマッチング用の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();
        //次フレームでのテンプレートマッチング用の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 <<i<<" / " <<max_frame<<" (" << position_x << ", " << position_y << ") angle = "<<angle<<endl;
        
        //ここからパノラマ作成
        Mat rot_dst_img(dst_img,dst_ROI);
        img.copyTo(rot_dst_img);
        
        // 回転:  [deg] と スケーリング: 1.0 [倍]
        float angle_d = -angle*180.0/3.141592, scale = 1.0;
        // 中心:画像中心
        cv::Point2f center(dst_img.cols*0.5, dst_img.rows*0.5);
        // 2次元の回転行列を計算
        const cv::Mat affine_matrix = cv::getRotationMatrix2D( center, angle_d, scale );
        warpAffine(dst_img, rot_dst_img, affine_matrix, dst_img.size());
        
        Mat map_roi_img(map_img,Rect(position_x+map_offset_x-dst_img.cols/2,position_y+map_offset_y-dst_img.rows/2,dst_img.cols,dst_img.rows));
        Mat mask=rot_dst_img.clone();
        cvtColor(mask,mask,CV_BGR2GRAY);
        threshold(mask,mask,1,255,THRESH_BINARY);
        erode(mask,mask, element, Point(-1,-1), 1);
        rot_dst_img.copyTo(map_roi_img,mask);
        imshow("Map",map_img);
        
        waitKey(1);
    }
    imwrite("map.png", map_img);
    imshow("Map",map_img);
    waitKey(0);
    return 0;
}

モバイルバージョンを終了