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()よりも高速なのでオススメです。
最後に、こちらがキャリブレーション前の画像。
こちらがキャリブレーション後の画像です。
歪みね...いえ、なんでもありません。※OpenCV 2.x系用のサンプルも作りました。
続きを読む
