AR.Drone使ってるくせにARってほとんどやったことないんですよね。
「AR ToolKit使わないの？」というコメントを以前頂いたり、実際にCV Droneに組み込んだものを見せてもらったりしました。
それで、いざやってみるか！と思ったのですが...ライセンスがGPLだったのであえなく没に。
でも今どき、AR（せめてマーカ有り）ぐらいできないとなーと思い、他を探したらありましたよ。
ArUcoOpenCVベースでBSDライセンスとか気が利きますね！
マーカの生成はここから↓
OVERVISIONのARマーカー生成アプリをarucoから切り出したので配布するただ、現在CV Droneが提供しているサンプルはこれではなく「Mastering OpenCV」の互換ライブラリを用いています（マーカ自体はArUcoのものが流用できます）。
いや、サンプルのためだけに依存するライブラリを増やすのはちょっとなー、という考えがあってですね...
// Marker detector
#include ".\3rdparty\packtpub\MarkerDetector.hpp"
// チェスボードのパラメータ
#define PAT_ROWS (7) // パターンの行数
#define PAT_COLS (10) // パターンの列数
#define CHESS_SIZE (24.0) // パターン一つの大きさ [mm]
// グローバル変数
ARDrone ardrone;
cv::Mat mapx, mapy;
CameraCalibration calibration;
// --------------------------------------------------------------------------
// buildProjectionMatrix(カメラ行列, 画面幅, 画面高さ)
// 画面の更新時に呼ばれる関数です
// 戻り値: カメラ行列から計算した透視投影行列
// --------------------------------------------------------------------------
Matrix44 buildProjectionMatrix(Matrix33 cameraMatrix, int screen_width, int screen_height)
{
float d_near = 0.01; // Near clipping distance
float d_far = 100; // Far clipping distance
// Camera parameters
float f_x = cameraMatrix.data[0]; // Focal length in x axis
float f_y = cameraMatrix.data[4]; // Focal length in y axis (usually the same?)
float c_x = cameraMatrix.data[2]; // Camera primary point x
float c_y = cameraMatrix.data[5]; // Camera primary point y
Matrix44 projectionMatrix;
projectionMatrix.data[0] = -2.0 * f_x / screen_width;
projectionMatrix.data[1] = 0.0;
projectionMatrix.data[2] = 0.0;
projectionMatrix.data[3] = 0.0;
projectionMatrix.data[4] = 0.0;
projectionMatrix.data[5] = 2.0 * f_y / screen_height;
projectionMatrix.data[6] = 0.0;
projectionMatrix.data[7] = 0.0;
projectionMatrix.data[8] = 2.0 * c_x / screen_width - 1.0;
projectionMatrix.data[9] = 2.0 * c_y / screen_height - 1.0;
projectionMatrix.data[10] = -(d_far + d_near) / (d_far - d_near);
projectionMatrix.data[11] = -1.0;
projectionMatrix.data[12] = 0.0;
projectionMatrix.data[13] = 0.0;
projectionMatrix.data[14] = -2.0 * d_far * d_near / (d_far - d_near);
projectionMatrix.data[15] = 0.0;
return projectionMatrix;
}
// --------------------------------------------------------------------------
// idle(引数なし)
// プログラムのアイドル時に呼ばれる関数です
// 戻り値: なし
// --------------------------------------------------------------------------
void idle(void)
{
// 再描画をリクエスト
glutPostRedisplay();
}
// --------------------------------------------------------------------------
// display(引数なし)
// 画面の更新時に呼ばれる関数です
// 戻り値: なし
// --------------------------------------------------------------------------
void display(void)
{
// 描画用のバッファクリア
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// 画像を取得
cv::Mat image_raw = ardrone.getImage();
cv::Mat image;
cv::remap(image_raw, image, mapx, mapy, cv::INTER_LINEAR);
// カメラ画像(RGB)表示
cv::Mat rgb;
cv::cvtColor(image, rgb, cv::COLOR_BGR2RGB);
cv::flip(rgb, rgb, 0);
glDepthMask(GL_FALSE);
glDrawPixels(rgb.cols, rgb.rows, GL_RGB, GL_UNSIGNED_BYTE, rgb.data);
// BGRAに変換
cv::Mat bgra;
cv::cvtColor(image, bgra, cv::COLOR_BGR2BGRA);
// データを渡す
BGRAVideoFrame frame;
frame.width = bgra.cols;
frame.height = bgra.rows;
frame.data = bgra.data;
frame.stride = bgra.step;
// マーカ検出
MarkerDetector detector(calibration);
detector.processFrame(frame);
std::vector<Transformation> transformations = detector.getTransformations();
// 射影変換行列を計算
Matrix44 projectionMatrix = buildProjectionMatrix(calibration.getIntrinsic(), frame.width, frame.height);
// 射影変換行列を適用
glMatrixMode(GL_PROJECTION);
glLoadMatrixf(projectionMatrix.data);
// ビュー行列の設定
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// デプス有効
glDepthMask(GL_TRUE);
// 頂点配列有効
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
// ビュー行列を退避
glPushMatrix();
// ラインの太さを設定
glLineWidth(3.0f);
// ライン頂点配列
float lineX[] = { 0, 0, 0, 1, 0, 0 };
float lineY[] = { 0, 0, 0, 0, 1, 0 };
float lineZ[] = { 0, 0, 0, 0, 0, 1 };
// 2D平面
const GLfloat squareVertices[] = {-0.5f, -0.5f,
0.5f, -0.5f,
-0.5f, 0.5f,
0.5f, 0.5f};
// 2D平面の色(RGBA)
const GLubyte squareColors[] = {255, 255, 0, 255,
0, 255, 255, 255,
0, 0, 0, 0,
255, 0, 255, 255};
// AR描画
for (size_t i = 0; i < transformations.size(); i++) {
// 変換行列を取得
const Transformation &transformation = transformations[i];
Matrix44 glMatrix = transformation.getMat44();
// ビュー行列にロード
glLoadMatrixf(reinterpret_cast<const GLfloat*>(&glMatrix.data[0]));
// 2D平面の描画
glEnableClientState(GL_COLOR_ARRAY);
glVertexPointer(2, GL_FLOAT, 0, squareVertices);
glColorPointer(4, GL_UNSIGNED_BYTE, 0, squareColors);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisableClientState(GL_COLOR_ARRAY);
// 座標軸のスケール
float scale = 0.5;
glScalef(scale, scale, scale);
// カメラから見えるようにちょっと移動
glTranslatef(0, 0, 0.1f);
// X軸
glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
glVertexPointer(3, GL_FLOAT, 0, lineX);
glDrawArrays(GL_LINES, 0, 2);
// Y軸
glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
glVertexPointer(3, GL_FLOAT, 0, lineY);
glDrawArrays(GL_LINES, 0, 2);
// Z軸
glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
glVertexPointer(3, GL_FLOAT, 0, lineZ);
glDrawArrays(GL_LINES, 0, 2);
}
// 頂点配列無効
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
// ビュー行列を戻すx
glPopMatrix();
// ダブルバッファリング
glutSwapBuffers();
}
// --------------------------------------------------------------------------
// key(入力されたキー, マウスカーソルのx位置, y位置)
// キーボードの入力時に呼ばれる関数です
// 戻り値: なし
// --------------------------------------------------------------------------
void key(unsigned char key, int x, int y) {
switch (key) {
case 0x1b:
exit(1);
break;
default:
break;
}
}
// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
int main(int argc, char *argv[])
{
// 初期化
if (!ardrone.open()) {
std::cout << "初期化に失敗しました" << std::endl;
return -1;
}
// 画像
cv::Mat frame = ardrone.getImage();
// XMLファイルを開く
std::string filename("camera.xml");
std::fstream file(filename.c_str(), std::ios::in);
// ファイルがない
if (!file.is_open()) {
// 画像バッファ
std::vector<cv::Mat> images;
std::cout << "Press Space key to capture an image" << std::endl;
std::cout << "Press Esc to exit" << std::endl;
// メインループ
while (1) {
// キー入力
int key = cv::waitKey(1);
if (key == 0x1b) break;
// 画像を取得
frame = ardrone.getImage();
// グレースケールに変換
cv::Mat gray;
cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
// チェスボード検出
cv::Size size(PAT_COLS, PAT_ROWS);
std::vector<cv::Point2f> corners;
bool found = cv::findChessboardCorners(gray, size, corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK);
// チェスボードが見つかった
if (found) {
// チェスボードを描画
cv::drawChessboardCorners(frame, size, corners, found);
// スペースキーが押された
if (key == ' ') {
// バッファに追加
images.push_back(gray);
}
}
// 表示
std::ostringstream stream;
stream << "Captured " << images.size() << " image(s).";
cv::putText(frame, stream.str(), cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
cv::imshow("Camera Calibration", frame);
}
// 十分なサンプルがある
if (images.size() > 4) {
cv::Size size(PAT_COLS, PAT_ROWS);
std::vector<std::vector<cv::Point2f>> corners2D;
std::vector<std::vector<cv::Point3f>> corners3D;
for (size_t i = 0; i < images.size(); i++) {
// チェスボード検出
std::vector<cv::Point2f> tmp_corners2D;
bool found = cv::findChessboardCorners(images[i], size, tmp_corners2D);
// チェスボードが見つかった
if (found) {
// コーナーをサブピクセル精度に変換
cv::cornerSubPix(images[i], tmp_corners2D, cvSize(11, 11), cvSize(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, 0.1));
corners2D.push_back(tmp_corners2D);
// 3次元空間座標の設定
const float squareSize = CHESS_SIZE;
std::vector<cv::Point3f> tmp_corners3D;
for (int j = 0; j < size.height; j++) {
for (int k = 0; k < size.width; k++) {
tmp_corners3D.push_back(cv::Point3f((float)(k*squareSize), (float)(j*squareSize), 0.0));
}
}
corners3D.push_back(tmp_corners3D);
}
}
// カメラパラメータの推定
cv::Mat cameraMatrix, distCoeffs;
std::vector<cv::Mat> rvec, tvec;
cv::calibrateCamera(corners3D, corners2D, images[0].size(), cameraMatrix, distCoeffs, rvec, tvec);
std::cout << cameraMatrix << std::endl;
std::cout << distCoeffs << std::endl;
// 保存
cv::FileStorage fs(filename, cv::FileStorage::WRITE);
fs << "intrinsic" << cameraMatrix;
fs << "distortion" << distCoeffs;
}
// ウィンドウの破棄
cv::destroyAllWindows();
}
// XMLファイルを開く
cv::FileStorage rfs(filename, cv::FileStorage::READ);
if (!rfs.isOpened()) {
std::cout << "Failed to open the XML file" << std::endl;
return -1;
}
// カメラパラメータの読み込み
cv::Mat cameraMatrix, distCoeffs;
rfs["intrinsic"] >> cameraMatrix;
rfs["distortion"] >> distCoeffs;
// 歪み補正マップの生成
cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix, frame.size(), CV_32FC1, mapx, mapy);
// カメラパラメータの設定
float fx = cameraMatrix.at<double>(0, 0);
float fy = cameraMatrix.at<double>(1, 1);
float cx = cameraMatrix.at<double>(0, 2);
float cy = cameraMatrix.at<double>(1, 2);
//calibration = CameraCalibration(fx, fy, cx, cy);
calibration = CameraCalibration(fx, fy, frame.cols / 2, frame.rows / 2);
// GLUTの初期化
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(frame.cols, frame.rows);
glutCreateWindow("Mastering OpenCV with Practical Computer Vision Project");
glutDisplayFunc(display);
glutKeyboardFunc(key);
glutIdleFunc(idle);
// 背景をクリア
glClearColor(0.0, 0.0, 1.0, 1.0);
glEnable(GL_DEPTH_TEST);
// メインループ開始
glutMainLoop();
return 0;
}
でもマーカのIDを取得できないから、結局ArUcoでいいじゃんと思ったり...
何か良い方法ないかなあ。
