AR.Drone2.0とcv::VideoCapture

AR.Drone2.0限定ですが、cv::VideoCaptureを使えば比較的簡単に画像を取得することができます。

#include <stdio.h>

// OpenCV
#include <opencv2/opencv.hpp>

int main(void)
{
cv::VideoCapture cap;

// ビデオを開く
if (!cap.open("tcp://192.168.1.1:5555") {
printf("AR.Droneを接続してください\n");
return -1;
}

// メインループ
while (!GetAsyncKeyState(VK_ESCAPE)) {
// 画像の取得
cv::Mat image;
cap >> image;

// 表示
cv::imshow("AR.Drone", image);
cv::waitKey(1);
}

return 0;
}
だったらFFmpeg入れる必要ないんじゃない?

と、思うでしょう?私もそう思っていました。

cv::VideoCaptureを用いてAR.Droneから動画を取得する場合、動画に遅延が発生します。
ワンテンポどころではない遅延なので、リアルタイムで画像処理しながら動かすのには向きませんね。

また、cv::VideoCaptureでデコードできるのはH.264の動画だけです(現在のところは)。
MPEG4はなぜか初期化で止まります。

CV DroneがあえてFFmpegで真面目に動画をデコードしているのには理由があるのです。
スポンサーサイト



AR.Droneでデッドレコニング

デッドレコニング(Dead-reckoning)とは、エンコーダやセンサの情報から機体の相対的な位置・姿勢を推定することをいいます。

AR.Droneには加速度センサやジャイロスコープといったセンサが搭載されてあり、ユーザはAR.Droneが推定した速度・姿勢を用いることができます。

Navdataのvx、vy、vzはAR.Drone自身からみた速度なので回転行列を使ってワールド座標に変換します。

 rotation_mat.jpg
  ※上からZ軸周り、Y軸周り、X軸周りに関する回転行列

数Cのやつですね。懐かしい、今もあるのでしょうか?

実際の行列演算はcv::Matにやってもらいます(プログラム74行目)。楽ですね。


#include "ardrone/ardrone.h"

// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Droneクラス
ARDrone ardrone;

// 初期化
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}

// バッテリ残量
printf("Battery = %d%%\n", ardrone.getBatteryPercentage());

// マップ
IplImage *map = cvCreateImage(cvSize(500, 500), IPL_DEPTH_8U, 3);
cvZero(map);

// 位置
cv::Mat P = cv::Mat::zeros(3, 1, CV_64FC1);

// メインループ
while (1) {
// キー入力
int key = cvWaitKey(33);
if (key == 0x1b) break;

// 更新
if (!ardrone.update()) break;

// 画像を取得
IplImage *image = ardrone.getImage();

// 姿勢
double roll = ardrone.getRoll();
double pitch = ardrone.getPitch();
double yaw = ardrone.getYaw();

// 速度取得
double vx, vy, vz;
double velocity = ardrone.getVelocity(&vx, &vy, &vz);

// 回転行列
double _RX[] = { 1.0, 0.0, 0.0,
0.0, cos(roll), -sin(roll),
0.0, sin(roll), cos(roll)};
double _RY[] = { cos(pitch), 0.0, sin(pitch),
0.0, 1.0, 0.0,
-sin(pitch), 0.0, cos(pitch)};
double _RZ[] = { cos(yaw), -sin(yaw), 0.0,
sin(yaw), cos(yaw), 0.0,
0.0, 0.0, 1.0};
cv::Mat RX(3, 3, CV_64FC1, _RX);
cv::Mat RY(3, 3, CV_64FC1, _RY);
cv::Mat RZ(3, 3, CV_64FC1, _RZ);

// 時間
static int64 last = cv::getTickCount();
double dt = (cv::getTickCount() - last) / cv::getTickFrequency();
last = cv::getTickCount();

// 移動量
double _M[] = {vx * dt, vy * dt, vz * dt};
cv::Mat M(3, 1, CV_64FC1, _M);

// デッドレコニング
P = P + RZ * RY * RX * M;

// 位置 (x, y, z)
double pos[3] = {P.at<double>(0,0), P.at<double>(1,0), P.at<double>(2,0)};
printf("x = %3.2f, y = %3.2f, z = %3.2f", pos[0], pos[1], pos[2]);

// 離陸・着陸
if (key == ' ') {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}

// 速度指令
double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
if (key == 0x260000) x = 1.0;
if (key == 0x280000) x = -1.0;
if (key == 0x250000) r = 1.0;
if (key == 0x270000) r = -1.0;
if (key == 'q') z = 1.0;
if (key == 'a') z = -1.0;
ardrone.move3D(x, y, z, r);

// カメラ切り替え
static int mode = 0;
if (key == 'c') ardrone.setCamera(++mode%4);

// 表示
cvDrawCircle(map, cvPoint(-pos[1]*30.0 + map->width/2, -pos[0]*30.0 + map->height/2), 2, CV_RGB(255,0,0));
cvShowImage("map", map);
cvShowImage("camera", image);
}

// さようなら
ardrone.close();
cvReleaseImage(&map);

return 0;
}
動かした結果がこちらです。

ardrone1_square.jpg

本当は1mぐらい上がったのですが、こちらのAR.Drone1.0はvzがゼロになるという呪いにかかっているため、z位置が0.2mまでという結果になっています。

Devil May Cry 4 を遊んでみた

久しぶりにブラッディパレスやりました。

やっぱダンテよりネロの方が楽です。

dmc4_bp.jpg


チャージショット3無双でした。

たまにはゲームしたっていいじゃない。

※ 23:00追記
 そういえばParrotからSDKの最新版(2.0.1)がリリースされましたね。
 https://projects.ardrone.org/boards/1/topics/show/5346

 SDKのソースコードには全く興味ありませんが、AR.Drone1.0の最新ファームが入っているので、
 今度インストールしてみましょう。

CV Droneとライセンス

CV Droneはオープンソースで開発を進めていますが、作っている本人はライセンス周りをよく分かっていなかったりします。

CV Droneで使われているライブラリは
OpenCV 2.4.3
FFmpeg 1.0
stdint.h for Visual C++
の3つです。

そして、それぞれのライセンスは
・OpenCV 修正BSD
・FFmpeg GPL または LGPL
・stdint.h 修正BSD(たぶん)
です。

各ライセンスを大まかに説明するとこんな感じです。
・GPL
 - 改変・再配布OKだが2次創作物もGPLが適用される、ソースコード公開義務あり
・LGPL
 - クレジットを表記すれば改変・再配布OK、ソースコード公開の義務あり
・修正BSDライセンス
 - 著作権情報とライセンス条文、無保証を明記すれば改変・再配布OK、さらにソースコード公開の義務なし

OpenCVやstdio.hはBSDライセンスなので、割と自由に使えます。
FFmpegは特殊で、ビルド構成によってGPLかLGPLかが変わります。

GPL/LGPLはソースコードの公開義務があるため、場合によっては都合が悪くなるかもしれませんね。
加えて、GPLは他のライセンスと競合するため、ここ最近は採用率が減っているそうです。

これまでのCV Droneは「Zeranoe FFmpeg」で配布されているバイナリを使っていたためGPLであり、CV Droneの派生プログラムもGPLにする必要がありました。ちょっと使いにくかったかもしれませんね。

まぁ、そんなわけでLGPL版を自分でビルドしました!
ビルドの方法はまた後日書きます。

新しいCV DroneはLGPLと修正BSDのデュアルライセンスです。
さらに自由になったCV Droneを試してみましょう!機能的にはあまり変わっていませんが。

githubでも何人かフォークしてくれてるので、そのうちCV Droneの派生版が出るのでは?

あけまして

新年、あけましておめでとうございます。

ノリで作ったこのブログも無事新年を迎えることができました。

AR.Droneについては基礎となる部分ができたので、今後は応用がメインとなります。

「複数台の制御ができた」とか「PTAMと統合できた」という声もちらほら聞きます。

これは負けてられませんね!

それでは皆様、本年もよろしくお願いします。

TwitterとFacebookも復活させようかな?
プロフィール

puku

Author:puku
暇な時はゲームかプログラミングしてる人だよ。
だいたい月1更新。
CV Drone はこちら(GitHub)

最近はQiitaでOnsenUI2で遊んでいる。

最新記事
最新コメント
最新トラックバック
検索フォーム
カレンダー
12 | 2013/01 | 02
- - 1 2 3 4 5
6 7 8 9 10 11 12
13 14 15 16 17 18 19
20 21 22 23 24 25 26
27 28 29 30 31 - -
月別アーカイブ
カテゴリ
スポンサードリンク
RSSリンクの表示
FC2カウンター
リンク
ブロとも申請フォーム

この人とブロともになる

アクセスランキング
[ジャンルランキング]
コンピュータ
607位
アクセスランキングを見る>>

[サブジャンルランキング]
プログラミング
120位
アクセスランキングを見る>>
FC2ブログランキング

FC2Blog Ranking

QRコード
QR