よいお年を

年の瀬の今日、みなさまいかがお過ごしでしょうか?

CV Droneは開発初期と比べて遥かに安定して動作するようになりました。
バグ報告・検証してくださった皆様、本当にありがとうございました。

最新版はAR.Drone2.0のUSB録画機能が使えるようになっています。

新年は凧ではなくAR.Droneを飛ばして空撮!なんていかがでしょうか?
あ、私はコタツで丸くなっています。

他のプロジェクトも進行しているため、来年から不定期更新となりますが今後ともよろしくお願いします。

それではよい年を。
スポンサーサイト



連続したメモリ領域を持つ多次元配列

通常、C言語/C++で多次元配列を作る時は

// 2x3の配列
int a[2][3];
としますね。

この方法はとても便利ですが、要素数は定数でなければならず、開発環境によっては大きなサイズの配列を作ることができません(https://msdn.microsoft.com/ja-jp/library/s0z0bbfe.aspx)。

これを解決するには、mallocなどで配列を動的に確保します。

#include <stdlib.h>

// 2x3の配列
int **b = (int**)malloc(2 * sizeof(int*));
for (int i = 0; i < 2; i++) b[i] = (int*)malloc(3 * sizeof(int*));

// 解放
for (int i = 0; i < 2; i++) free(b[i]);
free(b);
普通に[]でアクセスするのは問題ありませんが、配列まるごとmemcpyしようとするとエラーが起きます。

先程の配列のアドレスを表示してみましょう。

// 表示
printf("b:\n");
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
printf("%p\n", &b[i][j]);
}
}
printf("\n");
結果はこうなります。
配列版 malloc版
a[0][0]
a[0][1]
a[0][2]
a[1][0]
a[1][1]
a[1][2]
0141FF24
0141FF28
0141FF2C
0141FF30
0141FF34
0141FF38
b[0][0]
b[0][1]
b[0][2]
b[1][0]
b[1][1]
b[1][2]
001C1A10 ←アドレスは
001C1A14 ←4バイトずつ
001C1A18 ←増える
001C1A28 ←アドレスのずれが発生
001C1A2C
001C1A30

原因は5行目でfor文を使って行インデックス別に列要素をmallocで確保しているためです。

メモリ領域を連続にするには、このように行列の全要素を確保した後でインデックスを与えます。

// メモリ領域が連続な2x3の配列
int **c = (int**)malloc(2 * sizeof(int*));
c[0] = (int*)malloc(2 * 3 * sizeof(int));
for (int i = 1; i < 2; i++) c[i] = c[i-1] + 3;

// 解放
free(c[0]);
free(c);
アドレスはこのように、
malloc改良版
c[0][0]
c[0][1]
c[0][2]
c[1][0]
c[1][1]
c[1][2]
002A1A10
002A1A14
002A1A18
002A1A1C
002A1A20
002A1A24

連続していることが確認できます。

おまけ

続きを読む

Win32APIでスプラッシュウィンドウ

WordとかVisual Studioとかで起動時に「もやぁ~」と出てくるアレを再現したくて。

スプラッシュウィンドウ(スプラッシュスクリーン)の表示ってC言語でやろうとするとかなり大変ですよね。

昔書いたコードを整理していたらWin32API使ったプログラムが見つかったので紹介します。

スプラッシュウィンドウを表示する意味は、体感での起動時間を短縮することです。
単純にウィンドウを出して画像を表示するだけはダメ、ということですね。

時間のかかる処理と並行して画面を表示するには...

そうだね、マルチスレッドだね!

メインスレッドではプログラムの初期化を、子スレッドではスプラッシュウィンドウの表示を行います。
今回はイベントオブジェクトを使って表示タイミングを調整しています。

今回作成するプログラムのヘッダはこちら。設計の方針としては、時間のかかる関数を「表示」と「非表示」の機能を持つ関数で挟むような感じでいこうと思います。

#ifndef __HEADER_SPLASH__
#define __HEADER_SPLASH__

// 透過ウィンドウの為のマクロ
#ifndef _WIN32_WINNT
#define _WIN32_WINNT 0x0500
#endif

// 画像
#include <windows.h>
#pragma comment (lib, "msimg32.lib")

// スレッド
#include <process.h>

// --------------------------------------------------------------------------
// void showSplash(画像ファイル)
// スプラッシュウィンドウの表示
// 戻り値 なし
// --------------------------------------------------------------------------
void showSplash(const char *filename);

// --------------------------------------------------------------------------
// void hideSplash(引数なし)
// スプラッシュウィンドウ非表示
// 戻り値 なし
// --------------------------------------------------------------------------
void hideSplash(void);

#endif

せっかくなのでフェードイン効果と、画像の角を丸くする処理も加えてみましょう。

ウィンドウをフェードイン、つまり半透明にするには「レイヤードウィンドウ」という機能を使います。

やり方は比較的簡単で、CreateWindowExを呼ぶときにWS_EX_LAYEREDのスタイルを設定し、SetLayeredWindowAttributesで透過率を設定します。

角丸にする方法については、今回はCreateRoundRectRgnを採用しています。

関数の中身はこちらです。


#include "splash.h"

// タイマーID
#define TIMER_ID 1

// グローバル変数
HANDLE hEvent;
HDC hMemDC;
HBITMAP hBitmap;
BITMAP bitmap;

// --------------------------------------------------------------------------
// void windowProc(ウィンドウハンドル, メッセージ, パラメータ1, パラメータ2)
// ウィンドウプロシージャ
// 戻り値 0
// --------------------------------------------------------------------------
LRESULT CALLBACK windowProc(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
{
static int action = 0;
static int alpha = 0;

switch (uMsg) {
// ウィンドウが生成された
case WM_CREATE: {
// 変数の初期化
alpha = 0;
action = 0;
SetTimer(hWnd, TIMER_ID, 30, NULL);
return 0;
}
// タイマーイベント
case WM_TIMER: {
alpha += 10;
if (alpha > 255) alpha = 255;
SetLayeredWindowAttributes(hWnd, 0, (BYTE)alpha, LWA_ALPHA);
return 0;
}
// ウィンドウの更新
case WM_PAINT: {
// ビットマップ描画
PAINTSTRUCT ps;
HDC hdc = BeginPaint(hWnd , &ps);
BitBlt(hdc, 0, 0, bitmap.bmWidth, bitmap.bmHeight, hMemDC, 0, 0, SRCCOPY);
EndPaint(hWnd , &ps);
return 0;
}
// ウィンドウの破棄
case WM_DESTROY: {

KillTimer(hWnd, TIMER_ID);
PostQuitMessage(0);
return 0;
}
}

return DefWindowProc(hWnd, uMsg, wParam, lParam);
}

// --------------------------------------------------------------------------
// void threadFunc(引数)
// スレッド関数
// 戻り値 なし
// --------------------------------------------------------------------------
void threadFunc(void *arg)
{
// ウィンドウクラスの登録
char name[] = "splash_test";
WNDCLASSEX wc;
wc.cbSize = sizeof(WNDCLASSEX);
wc.style = 0;
wc.lpfnWndProc = windowProc;
wc.cbClsExtra = 0;
wc.cbWndExtra = 0;
wc.hInstance = (HINSTANCE)GetModuleHandle(NULL);
wc.hIcon = NULL;
wc.hCursor = LoadCursor(NULL, IDC_ARROW);
wc.hbrBackground = NULL;
wc.lpszMenuName = NULL;
wc.lpszClassName = name;
wc.hIconSm = NULL;
RegisterClassEx(&wc);

// メモリデバイスコンテキストを生成
hMemDC = CreateCompatibleDC(NULL);
SelectObject(hMemDC, hBitmap);
GetObject(hBitmap, sizeof(BITMAP), &bitmap);

// スプラッシュスクリーンを画面中央に作成
RECT rect;
GetWindowRect(GetDesktopWindow(), &rect);
HWND hWnd = CreateWindowEx(WS_EX_LAYERED|WS_EX_TOOLWINDOW,
name,
name,
WS_POPUP,
(rect.left + rect.right)/2 - bitmap.bmWidth/2,
(rect.top + rect.bottom)/2 - bitmap.bmHeight/2,
bitmap.bmWidth,
bitmap.bmHeight,
NULL,
NULL,
wc.hInstance,
arg);

// ウィンドウを角丸に変更
HRGN hRGN = CreateRoundRectRgn(0, 0, bitmap.bmWidth, bitmap.bmHeight, 30, 30);
SetWindowRgn(hWnd, hRGN, TRUE);

// 表示
ShowWindow(hWnd, TRUE);
UpdateWindow(hWnd);

// イベントを起こす
SetEvent(hEvent);

// メッセージループ
MSG msg;
while (GetMessage(&msg, NULL, 0, 0) > 0) {
TranslateMessage(&msg);
DispatchMessage(&msg);
}

// メモリ解放
DeleteDC(hMemDC);
DeleteObject(hBitmap);
DeleteObject(hRGN);
}

// --------------------------------------------------------------------------
// void showSplash(画像ファイル)
// スプラッシュウィンドウの表示
// 戻り値 なし
// --------------------------------------------------------------------------
void showSplash(const char *filename)
{
// イベントの生成
hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);

// ビットマップ読込み
hBitmap = (HBITMAP)LoadImage(NULL, filename, IMAGE_BITMAP, 0, 0, LR_LOADFROMFILE);

// スレッド生成
HANDLE thread = (HANDLE)_beginthread(threadFunc, NULL, hEvent);

// 表示のタイミングを合わせる為のウェイト
if (thread != INVALID_HANDLE_VALUE) WaitForSingleObject(hEvent, 1000);
CloseHandle(hEvent);
}

// --------------------------------------------------------------------------
// void hideSplash(引数なし)
// スプラッシュウィンドウ非表示
// 戻り値 なし
// --------------------------------------------------------------------------
void hideSplash(void)
{
HWND hWnd = FindWindow("splash_test", NULL);

// ウィンドウ消去
if (hWnd) SendMessage(hWnd, WM_DESTROY, 0, 0);
}

なんという自己満足。

AR.Droneはどこへ行ったのか。

CV Drone 日本語版 更新

CV Droneの日本語版はもう作らないと言ったな...あれは嘘だ。

コッソリ作ってました。

今回の更新は、ブログやフォーラム等から受けたバグ報告やリクエストを日本語版にフィードバックする形となります。

CV Drone 日本語版の記事へ

初版から修正された点は以下の通りです。内部処理に関するものがほとんどですね。
・WatchDogコマンドの定期的な送信の追加
・終了→緊急停止 から 終了→着陸に変更
・Navdata取得の別スレッド化
・速度指令が[%]だったので[m/s]に変更
・Visual Studio 2012への対応
・アニメーションへの対応
・AR.Drone FW2.2.9への対応
・UVLCデコーダを全チャンネル対応に
・複数台制御への対応
・H.264デコードのメモリリークの修正

拙いプログラムですが、皆様の趣味や研究に役立てていただければ幸いです。

ビデオ取得の高速化

ライブラリの実装的な話をちょっと。

AR.Droneは30fpsで動画を送ってきます。

表示だけなら十分ですが、画像処理も行うならもう少しスピードが欲しいところです。

そこで、動画取得の部分をマルチスレッド化します。

必要なヘッダは以下の2つ
・windows.h
・process.h

以下、ardrone.hから抜粋。
ARDroneクラスには、内部で画像を更新するgetVideo()関数があります。

この関数は動画のデコードが入っているため処理が重く、これを別スレッドに回すことができれば処理時間がだいぶ稼げると考えられます。

スレッド間でやりとりされるデータはpFrameBGRであり、このデータは画像処理を扱う上で破綻しないように"安全"に更新されなければなりません。

そこで「ミューテックス」を使います。これでデータをロックし、スレッドを同期させます。

スレッドの生成については、クラスで実装されていることもあり、thisポインタを用いたちょっとしたテクニックを使っています。

スレッドが開始されるとrun()→loop()と実行されます。

loop()の処理はこのような単純な無限ループです。
getVideo()の中身はビデオのデコード処理ですが、データを書き出す部分をWaitForSingleObject()関数とReleaseMutex()関数で囲んでいます。
そしてgetImage()でも同じように、データをIplImageに変換する部分をWaitForSingleObject()とReleaseMutex()で囲みます。
こうすることでユーザは任意のタイミングで、且つ高速に画像を取得できるようなります。

終了処理は以下の通り。
公式のSDKではもっと多くのスレッドに分けられています。
というかやりすぎです。SDKのとっつきにくさは大体コイツのせいです。どうしてこうなった。
※2013/02/0 8結局 CV Droneも同じになりました。でも公式SDKより遙かにマシな実装です。てかこうなるだろ、普通。

マルチスレッドプログラミングは難しいですが、その分面白いですよ!

AR.Drone2.0で720pの画像を取得するには?

AR.Drone 2.0の情報って意外と少ないですよね。

デベロッパーガイドに載っていないことも多く、苦労した覚えがあります。

公式のフォーラムにはとても助けられました。

AR.Drone 2.0になってHD画質で撮れるようになったのに使わないなんてもったいない。
と、いうわけでさっそくやっていきましょう。

AT*CONFIGで送信可能なvideo:video_codecは何種類かありますが使うのは2つだけです。
・H264_360P_CODEC 0x81
・H264_720P_CODEC 0x83

まずは設定用のIDを登録します。

// コンフィグ用のIDを設定
sockCommand.sendf("AT*CONFIG_IDS=%d,\"%s\",\"%s\",\"%s\"\r", seq++, ARDRONE_SESSION_ID, ARDRONE_PROFILE_ID, ARDRONE_APPLOCATION_ID);
sockCommand.sendf("AT*CONFIG=%d,\"custom:session_id\",\"%s\"\r", seq++, ARDRONE_SESSION_ID);
Sleep(100);
sockCommand.sendf("AT*CONFIG_IDS=%d,\"%s\",\"%s\",\"%s\"\r", seq++, ARDRONE_SESSION_ID, ARDRONE_PROFILE_ID, ARDRONE_APPLOCATION_ID);
sockCommand.sendf("AT*CONFIG=%d,\"custom:profile_id\",\"%s\"\r", seq++, ARDRONE_PROFILE_ID);
Sleep(100);
sockCommand.sendf("AT*CONFIG_IDS=%d,\"%s\",\"%s\",\"%s\"\r", seq++, ARDRONE_SESSION_ID, ARDRONE_PROFILE_ID, ARDRONE_APPLOCATION_ID);
sockCommand.sendf("AT*CONFIG=%d,\"custom:application_id\",\"%s\"\r", seq++, ARDRONE_APPLOCATION_ID);
Sleep(100);
そしてAT*CONFIGを送ります。
公開中のライブラリにこっそり入っています(コメント化されていますが)。

// 720pで出力
sockCommand.sendf("AT*CONFIG_IDS=%d,\"%s\",\"%s\",\"%s\"\r", seq++, ARDRONE_SESSION_ID, ARDRONE_PROFILE_ID, ARDRONE_APPLOCATION_ID);
sockCommand.sendf("AT*CONFIG=%d,\"video:video_codec\",\"%d\"\r", seq++, 0x83);
Sleep(100);
↓撮れた画像がこちら
720p.jpg

↓360pと比べると大きいですね
360p.jpg

↓お腹のカメラはこのように黒帯が出ます。
720p_under.jpg

解像度を変更した初回は画像が止まります。この場合はプログラムを再起動しましょう。

これやってる人少ないだろうなぁ。

AR.Droneを動かそう(カメラの切り替え)

今回はカメラの切り替えについてやっていきましょう。
AT*CONFIGというコマンドを用います。

AR.Drone 1.0では、
とすればカメラの切り替えが可能です。

チャンネルは0~4まであり、
0: 前カメラ
img0.jpg

1: 下カメラ
img1.jpg

2: 前カメラ+下カメラ
img2.jpg

3: 下カメラ+前カメラ
img3.jpg

4: ザッピング(0~3まで順番に切り替え)
となっています。

githubで公開中のプログラムはAR.Drone1.0の全チャンネルを解像度320x240で取得可能です。

AR.Drone 2.0ではコンフィグ用のIDを送ってあげないとカメラ切り替えが出来なくなっています。

このように、初期化する際にIDを登録します。
このIDは8桁の16進数なら何でも良いそうです。

AT*CONFIGを送った場合、AR.Droneの設定が完了するまで他のAT*CONFIGは無視されます。

そのため、Sleep()を入れて少し待ちます。
本当はナビゲーションデータの状態フラグをチェックした方が良いのですが。

ちなみにAR.Drone2.0はチャンネル0と1しか見ることが出来ません。これはデベロッパーガイドにも記載されています。

AT*CONFIGでカメラの切り替えも出来るのなら、画像の解像度も...?

フフフ、もちろんできますよ!

では次回!

AR.Droneのカメラキャリブレーション

AR.Droneのカメラは広角レンズが入っているらしく、レンズの外側がだいぶ歪んで見えますね。

OpenCVにはこのような歪みを補正するアルゴリズムcvCalibrateCamera2()があります。

これを使ってカメラの歪み補正を行いましょう。

まずはカメラの内部パラメータを推定します。

有名なのはチェスボードパターンを使った手法です。

#include "ardrone/ardrone.h"

// チェスボードパターンのパラメータ
#define PAT_ROW (7) // 行
#define PAT_COL (10) // 列
#define PAT_SIZE (PAT_ROW*PAT_COL)
#define CHESS_SIZE (24.0) // パターンサイズ [mm]

// --------------------------------------------------------------------------
// cvDrawText(画像, 描画する位置, メッセージ)
// 画像の指定されたちに書式付きテキストを書き込みます
// 戻り値 なし
// --------------------------------------------------------------------------
inline void cvDrawText(IplImage *image, CvPoint point, const char *fmt, ...)
{
// フォント
static CvFont font = cvFont(1.0);

// 書式を適用
char text[256];
va_list ap;
va_start(ap, fmt);
vsprintf(text, fmt, ap);
va_end(ap);

// 描画
cvPutText(image, text, point, &font, CV_RGB(0, 255, 0));
}

// --------------------------------------------------------------------------
// cvAsk(メッセージ)
// メッセージボックスを表示
// 戻り値 いいえ:0 はい:1
// --------------------------------------------------------------------------
inline int cvAsk(const char *message, ...)
{
char *arg;
char str[256];

// 書式を適用
va_start(arg, message);
vsprintf(str, message, arg);
va_end(arg);

// メッセージボックスを表示
#ifndef _WIN32
return (MessageBox(NULL, str, "QUESTION", MB_YESNO|MB_ICONQUESTION|MB_TOPMOST|MB_SETFOREGROUND) == IDYES);
#else
char c = 'n';
printf(str);
scanf("%c", &c);
return (c == 'y' || c == 'Y');
#endif
}

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

// 初期化
if (!ardrone.open()) {
printf("AR.Droneの初期化に失敗しました\n");
return -1;
}

// 画像
std::vector<IplImage*> images;
printf("Spaceキーを押してサンプルを撮りましょう\n");

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

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

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

// グレースケールに変換
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, gray, CV_BGR2GRAY);

// チェスボードパターンの検出
int corner_count = 0;
CvSize size = cvSize(PAT_COL, PAT_ROW);
CvPoint2D32f corners[PAT_SIZE];
int found = cvFindChessboardCorners(gray, size, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH+CV_CALIB_CB_NORMALIZE_IMAGE|CV_CALIB_CB_FAST_CHECK);

// 検出した
if (found) {
// チェスボードコーナーを表示
cvDrawChessboardCorners(image, size, corners, corner_count, found);

// Spaceが押された
if (key == ' ') {
// バッファに追加
images.push_back(gray);
}
else {
// 解放
cvReleaseImage(&gray);
}
}
// 検出しなかった
else {
// 解放
cvReleaseImage(&gray);
}

// 表示
cvDrawText(image, cvPoint(15, 20), "NUM = %d", (int)images.size());
cvShowImage("camera", image);
}

// ウィンドウの破棄
cvDestroyWindow("camera");

// 十分なサンプルがある
if (images.size() > 4) {
// サンプルの数
const int num = (int)images.size();

//// デバッグ用
//for (int i = 0; i < num; i++) {
// char name[256];
// sprintf(name, "images[%d/%d]", i+1, num);
// cvShowImage(name, images[i]);
// cvWaitKey(0);
// cvDestroyWindow(name);
//}

// 保存するか尋ねる
if (cvAsk("パラメータを保存しますか? (y/n)\n")) {
// 「はい」だったらチェスボードパターンを検出しなおしてパラメータ推定へ
int *p_count = (int*)malloc(sizeof(int) * num);
CvPoint2D32f *corners = (CvPoint2D32f*)cvAlloc(sizeof(CvPoint2D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
// チェスボードパターンの検出
int corner_count = 0;
CvSize size = cvSize(PAT_COL, PAT_ROW);
int found = cvFindChessboardCorners(images[i], size, &corners[i * PAT_SIZE], &corner_count);

// サブピクセル精度に変換
cvFindCornerSubPix(images[i], &corners[i * PAT_SIZE], corner_count, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
p_count[i] = corner_count;
}

// チェスボードパターンの実寸
CvPoint3D32f *objects = (CvPoint3D32f*)cvAlloc(sizeof(CvPoint3D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
for (int j = 0; j < PAT_ROW; j++) {
for (int k = 0; k < PAT_COL; k++) {
objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;
}
}
}

// 計算用の行列を生成
CvMat object_points, image_points, point_counts;
cvInitMatHeader(&object_points, num * PAT_SIZE, 3, CV_32FC1, objects);
cvInitMatHeader(&image_points, num * PAT_SIZE, 1, CV_32FC2, corners);
cvInitMatHeader(&point_counts, num, 1, CV_32SC1, p_count);

// カメラ行列と歪み係数を推定
printf("カメラパラメータの推定をしています...");
CvMat *intrinsic = cvCreateMat(3, 3, CV_32FC1);
CvMat *distortion = cvCreateMat(1, 4, CV_32FC1);
cvCalibrateCamera2(&object_points, &image_points, &point_counts, cvGetSize(images[0]), intrinsic, distortion);
printf("完了!\n");

// XMLファイルへ書き出し
printf("XMLファイルを出力しています...");
CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite(fs, "intrinsic", intrinsic);
cvWrite(fs, "distortion", distortion);
cvReleaseFileStorage(&fs);
printf("完了!\n");

// 解放
free(p_count);
cvFree(&corners);
cvFree(&objects);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
}

// 解放
for (int i = 0; i < num; i++) cvReleaseImage(&images[i]);
}

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

return 0;
}

このサンプルプログラムはOpenCV.jpのサンプルを元に使いやすくしたものです。

起動したらスペースキーを押してバシャバシャ撮りましょう。
※あらかじめチェスボードパターンPDFを印刷しておいて下さい。

Escを押すとパラメータを保存するかどうか聞いてくるので「はい」を選択してください。

これで準備完了です。

画像の歪みを補正するにはcvInitUndistortMap()とcvRemap()を使います。

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

// 初期化
if (!ardrone.open()) {
printf("AR.Droneの初期化に失敗しました\n");
return -1;
}

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

// 歪みデータ読み込み
CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_READ);
CvMat *intrinsic = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "intrinsic"));
CvMat *distortion = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "distortion"));

// 歪み補正マップ初期化
CvMat *mapx = cvCreateMat(image->height, image->width, CV_32FC1);
CvMat *mapy = cvCreateMat(image->height, image->width, CV_32FC1);
cvInitUndistortMap(intrinsic, distortion, mapx, mapy);

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

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

// 画像の取得
image = ardrone.getImage();

// 歪み補正
cvRemap(image, image, mapx, mapy);

// 表示
cvShowImage("camera", image);
}

// メモリ解放
cvReleaseMat(&mapx);
cvReleaseMat(&mapy);
cvReleaseFileStorage(&fs);

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

return 0;
}
cvRemap()はcvUndistort2()よりも高速なのでオススメです。

最後に、こちらがキャリブレーション前の画像。
before.jpg


こちらがキャリブレーション後の画像です。

after.jpg

歪みね...いえ、なんでもありません。

※OpenCV 2.x系用のサンプルも作りました。

続きを読む

AR.Droneでオプティカルフロー

AR.Droneでオプティカルフローとか他の人が2年ぐらい前にやってるんですけど、

見た目が面白いのでついやっちゃうんですよね。

#include "ardrone/ardrone.h"

#define KEY_DOWN(key) (GetAsyncKeyState(key) & 0x8000)
#define KEY_PUSH(key) (GetAsyncKeyState(key) & 0x0001)

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

// 初期化
if (!ardrone.open()) {
printf("ARDroneの初期化に失敗しました\n");
return -1;
}

// AR.Droneの画像
IplImage *image = ardrone.getImage();

// オプティカルフロー用
int corner_count = 50;
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
IplImage *prev = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, prev, CV_BGR2GRAY);
IplImage *eig_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *tmp_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage *prev_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
IplImage *curr_pyramid = cvCreateImage(cvSize(image->width+8, image->height/3), IPL_DEPTH_8U, 1);
CvPoint2D32f *corners1 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));
CvPoint2D32f *corners2 = (CvPoint2D32f*)malloc(corner_count * sizeof(CvPoint2D32f));

// メインループ
while (!GetAsyncKeyState(VK_ESCAPE)) {
// AR.Droneの更新
if (!ardrone.update()) break;

// 画像の取得
image = ardrone.getImage();

// 離陸・着陸
if (KEY_PUSH(VK_SPACE)) {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}

// AR.Droneが飛行状態
if (!ardrone.onGround()) {
// 速度指令
double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
if (KEY_DOWN(VK_UP)) vx = 0.5;
if (KEY_DOWN(VK_DOWN)) vx = -0.5;
if (KEY_DOWN(VK_LEFT)) vr = 0.5;
if (KEY_DOWN(VK_RIGHT)) vr = -0.5;
if (KEY_DOWN('Q')) vz = 0.5;
if (KEY_DOWN('A')) vz = -0.5;
ardrone.move3D(vx, vy, vz, vr);
}

// グレースケールに変換
cvCvtColor(image, gray, CV_BGR2GRAY);

// 疎な特徴点を検出
corner_count = 50;
cvGoodFeaturesToTrack(prev, eig_img, tmp_img, corners1, &corner_count, 0.1, 5.0, NULL);

// コーナーが見つかった
if (corner_count > 0) {
char *status = (char*)malloc(corner_count * sizeof(char));

// オプティカルフローを計算
CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.3);
cvCalcOpticalFlowPyrLK(prev, gray, prev_pyramid, curr_pyramid, corners1, corners2, corner_count, cvSize(10, 10), 3, status, NULL, criteria, 0);

// フローの描画
for (int i = 0; i < corner_count; i++) {
cvCircle(image, cvPointFrom32f(corners1[i]), 1, CV_RGB (255, 0, 0));
if (status[i]) cvLine(image, cvPointFrom32f(corners1[i]), cvPointFrom32f(corners2[i]), CV_RGB (0, 0, 255), 1, CV_AA, 0);
}

free(status);
}

// 過去のフレームを保存
cvCopy(gray, prev);

// 表示
cvShowImage("camera", image);
cvWaitKey(1);
}

// メモリ解放
cvReleaseImage(&gray);
cvReleaseImage(&prev);
cvReleaseImage(&eig_img);
cvReleaseImage(&tmp_img);
cvReleaseImage(&prev_pyramid);
cvReleaseImage(&curr_pyramid);
free(corners1);
free(corners2);

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

return 0;
}
↑グレースケール変換していますが、カラー画像でも問題無いようです。特徴点検出などのパラメータは適宜変更しましょう。

公開しているライブラリの\src\sampleにも入っていますので、気になる方は一度試してみては?
プロフィール

puku

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

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

最新記事
最新コメント
最新トラックバック
検索フォーム
カレンダー
11 | 2012/12 | 01
- - - - - - 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