AR.Droneを動かそう (マーカ有りAR)

AR.Drone使ってるくせにARってほとんどやったことないんですよね。

「AR ToolKit使わないの?」というコメントを以前頂いたり、実際にCV Droneに組み込んだものを見せてもらったりしました。

それで、いざやってみるか!と思ったのですが...ライセンスがGPLだったのであえなく没に。

でも今どき、AR(せめてマーカ有り)ぐらいできないとなーと思い、他を探したらありましたよ。

ArUco

OpenCVベースで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でいいじゃんと思ったり...
何か良い方法ないかなあ。
スポンサーサイト



AR.Droneを動かそう (PID制御)

CV Droneがターゲットにしているのは、趣味の人(私とか)はもちろんですが、
AR.Droneを用いて研究したい人というのもあります。

「なんか画像処理させたいよー」
とか
「目的地まで移動させたいよー」
とか、思い当たる人は少なくないでしょう。

最新のCV Droneのサンプル(sample_tracking.cpp)には前カメラを使って色のついたマーカをゆっくり追いかけるといったものを入れていますが...

コレジャナイ!ようです。まぁ薄々そんな気はしてた

下カメラを使ってマーカをPID制御するには?というのは割と多く寄せられるコメントなので、ここでサンプルをお見せしましょう。


#include "ardrone/ardrone.h"

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

// 初期化
if (!ardrone.open()) {
std::cout << "初期化に失敗しました" << std::endl;
return -1;
}

// 下カメラに切り替え
ardrone.setCamera(1);

// 閾値
int minH = 0, maxH = 179;
int minS = 0, maxS = 255;
int minV = 0, maxV = 255;

// XMLファイルを開く
std::string filename("thresholds.xml");
cv::FileStorage fs(filename, cv::FileStorage::READ);

//ファイルが有れば閾値読み込み
if (fs.isOpened()) {
maxH = fs["H_MAX"];
minH = fs["H_MIN"];
maxS = fs["S_MAX"];
minS = fs["S_MIN"];
maxV = fs["V_MAX"];
minV = fs["V_MIN"];
fs.release();
}

// ウィンドウを生成
cv::namedWindow("binalized");
cv::createTrackbar("H max", "binalized", &maxH, 179);
cv::createTrackbar("H min", "binalized", &minH, 179);
cv::createTrackbar("S max", "binalized", &maxS, 255);
cv::createTrackbar("S min", "binalized", &minS, 255);
cv::createTrackbar("V max", "binalized", &maxV, 255);
cv::createTrackbar("V min", "binalized", &minV, 255);
cv::resizeWindow("binalized", 0, 0);

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

// 画像を取得
cv::Mat image = ardrone.getImage();

// HSVに変換
cv::Mat hsv;
cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);

// 2値化
cv::Mat binalized;
cv::Scalar lower(minH, minS, minV);
cv::Scalar upper(maxH, maxS, maxV);
cv::inRange(hsv, lower, upper, binalized);

// 表示
cv::imshow("binalized", binalized);

// ノイズ除去
cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);
//cv::imshow("morphologyEx", binalized);

// 輪郭を検出
std::vector<std::vector<cv::Point>> contours;
cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

// 一番大きい輪郭を抽出
int contour_index = -1;
double max_area = 0.0;
for (int i = 0; i < (int)contours.size(); i++) {
double area = fabs(cv::contourArea(contours[i]));
if (area > max_area) {
contour_index = i;
max_area = area;
}
}

// マーカ
static cv::Point marker(binalized.cols / 2, binalized.rows / 2);

// マーカが見つかった
if (contour_index >= 0) {
// 重心
cv::Moments moments = cv::moments(contours[contour_index], true);
marker.y = (int)(moments.m01 / moments.m00);
marker.x = (int)(moments.m10 / moments.m00);

// 表示
cv::Rect rect = cv::boundingRect(contours[contour_index]);
cv::rectangle(image, rect, cv::Scalar(0, 255, 0));
//cv::drawContours(image, contours, contour_index, cv::Scalar(0,255,0));
}

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

// キー入力で移動
double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
if (key == CV_VK_UP) vx = 1.0;
if (key == CV_VK_DOWN) vx = -1.0;
if (key == CV_VK_LEFT) vr = 1.0;
if (key == CV_VK_RIGHT) vr = -1.0;
if (key == 'q') vz = 1.0;
if (key == 'a') vz = -1.0;

// マーカ追跡のON/OFF
static int track = 0;
if (key == 't') track = !track;
cv::putText(image, (track) ? "track on" : "track off", cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, (track) ? cv::Scalar(0, 0, 255) : cv::Scalar(0, 255, 0), 1, CV_AA);

// マーカ追跡
if (track) {
// PIDゲイン
const double kp = 0.001;
const double ki = 0.000;
const double kd = 0.000;

// 誤差 (画像座標系-機体座標系の変換のため符号とXYが逆)
double error_x = (binalized.rows / 2 - marker.y);
double error_y = (binalized.cols / 2 - marker.x);

// 時間 [s]
static int64 last_t = 0.0;
double dt = (cv::getTickCount() - last_t) / cv::getTickFrequency();
last_t = cv::getTickCount();

// 積分項
static double integral_x = 0.0, integral_y = 0.0;
if (dt > 0.1) {
// リセット
integral_x = 0.0;
integral_y = 0.0;
}
integral_x += error_x * dt;
integral_y += error_y * dt;

// 微分項
static double previous_error_x = 0.0, previous_error_y = 0.0;
if (dt > 0.1) {
// リセット
previous_error_x = 0.0;
previous_error_y = 0.0;
}
double derivative_x = (error_x - previous_error_x) / dt;
double derivative_y = (error_y - previous_error_y) / dt;
previous_error_x = error_x;
previous_error_y = error_y;

// 操作量
vx = kp * error_x + ki * integral_x + kd * derivative_x;
vy = kp * error_y + ki * integral_y + kd * derivative_y;
vz = 0.0;
vr = 0.0;
std::cout << "(vx, vy)" << "(" << vx << "," << vy << ")" << std::endl;
}

// 移動
ardrone.move3D(vx, vy, vz, vr);

// 表示
cv::imshow("camera", image);
}

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

// 閾値を保存
fs.open(filename, cv::FileStorage::WRITE);
if (fs.isOpened()) {
cv::write(fs, "H_MAX", maxH);
cv::write(fs, "H_MIN", minH);
cv::write(fs, "S_MAX", maxS);
cv::write(fs, "S_MIN", minS);
cv::write(fs, "V_MAX", maxV);
cv::write(fs, "V_MIN", minV);
fs.release();
}

return 0;
}
※注意 サンプルはP制御です(ki=kd=0.0のため)。IゲインとDゲインは自分で調整してください。

時間(Δt)で微分とか積分をコード化できるようになれば、PID制御器の実装なんてチョロいもんです。

ゲインの調整法はというと...

続きを読む

AR.Droneを動かそう(バージョン情報の取得 非WinINet版)

CV Droneは当初Windowsのみサポートしていました。

開発後期でクロスプラットフォーム化するにあたり、それまで使っていたWinINetでのFTP通信が使用できなくなりました。

これはマズい。

何がマズいかというと、バージョン情報の取得方法を考え直さないといけないんですよ。

まぁ、FTPが使えなくてもTCP経由でconfig.iniを取得できますので、なんとかなるといえばそうなのですが...

初期化の順番を考えると、ATコマンド初期化→いろいろ設定→設定情報を取得、というように設定情報の取得は後に回すのが良いかなーと思うんですよね。

TCP通信に関しては自分で作ったTCPクラスがありますからこれを使うとして、AR.DroneのFTP通信ってどうなってるのかを見つつ実装していきましょう。

1. ポート5551を開く
 これは簡単、ただソケット作ってconnectするだけです。
 ポートを開くと何か文字列がやってきます。適当なバッファに突っ込んでおきましょう。
2. ログインする
 USERプロトコルを使って匿名(anonymous)でログインします。
 AR.Droneにはログインパスワードが設定されていないのでPASSは送らなくてもOKです。
3. Passiveモードに切り替え
 ここからがちょっと面倒です。
 PASVを送るとAR.Droneから"227 PASV ok (192,168,1,1,a,b)"といった文字列が送られてきます。
 Passiveモードにおけるデータ転送用のポート番号は「a * 256 + b」で与えられます。
 1.で作ったものとは別のソケットを用意し、上記のポートを開きましょう。
4. ファイル送信のリクエスト
 ポート5551を開いたソケットで"RETR ファイル名"を送りファイルを要求します。
 ファイル名には"version.txt"を設定します"
5. ファイル受信
 ファイルはデータ転送用のポートを開いたソケットに送られます。
 "version.txt"はただのテキストなので、char型のバッファに入れると良いでしょう。
 バッファに入れたら後はsscanfなり何なりでバージョン情報を抽出します。

↓実際のコードはこちらです。

#include <stdio.h>

// TCP通信
#include "tcp.h"

// マクロたち
#define ARDRONE_DEFAULT_ADDR "192.168.1.1" // AR.DroneデフォルトIPアドレス
#define ARDRONE_FTP_PORT (5551) // FTP用ポート

// バージョン情報
struct ARDRONE_VERSION {
int major;
int minor;
int revision;
};

int main(void)
{
TCPSocket socket1, socket2;

// ソケットを開く
if (!socket1.open(ARDRONE_DEFAULT_ADDR, ARDRONE_FTP_PORT)) {
printf("TCPSocket::open(port=%d) failed. (%s, %d)\n", ARDRONE_FTP_PORT, __FILE__, __LINE__);
return 0;
}

// ウェルカムメッセージを受け取る
char buf[1024] = {'\0'};
socket1.receive(buf, sizeof(buf));

// ログイン
socket1.sendf("USER %s\r\n\0", "anonymous");
socket1.receive(buf, sizeof(buf));

// PASVモードに切り替え
int a, b, c, dataport;
socket1.sendf("PASV\r\n\0");
socket1.receive(buf, sizeof(buf));
sscanf(buf, "227 PASV ok (%d,%d,%d,%d,%d,%d)\n", &c, &c, &c, &c, &a, &b);
dataport = a * 256 + b;

// データソケットを開く
if (!socket2.open(ARDRONE_DEFAULT_ADDR, dataport)) {
printf("TCPSocket::open(port=%d) failed. (%s, %d)\n", dataport, __FILE__, __LINE__);
return 0;
}

// ファイル送信リクエスト
socket1.sendf("RETR %s\r\n\0", "version.txt");

// ファイル受信
socket2.receive(buf, sizeof(buf));

// バージョン取得
ARDRONE_VERSION version;
sscanf(buf, "%d.%d.%d", &version.major, &version.minor, &version.revision);
printf("AR.Drone Ver %d.%d.%d\n", version.major, version.minor, version.revision);

// さようなら
socket1.close();
socket2.close();

return 1;
}
tcp.h

WinINetがないと結構手間がかかりますね。タイムアウトの設定も出来ませんし。

さて、当ブログがスタートしてからそろそろ1年になります。

「1年間続ける」ことと「毎週更新する」ことをは決めていましたので、
とりあえず目標達成 といったところでしょうか。フフフ

CV Droneも卒業研究や学会の論文や飛行ロボット大会などでも使われるようになるなど(公式SDKと間違われたりしているとか...マジか)、まぁぼちぼち有名になってきて嬉しい限りです。

Mac で CV Drone

Linuxでビルドできる、ということはMacでもビルドできるということですね。

MacPortsを使えばOpenCVやFFmpegが簡単にインストール出来ます。

$ export PATH=/opt/local/bin:/opt/local/sbin:$PATH
$ export MANPATH=/opt/local/man:$MANPATH
$ sudo port selfupdate
$ sudo port sync
$ sudo port install opencv
$ sudo port install ffmpeg
$ export LIBRARY_PATH=/opt/local/lib:$LIBRARY_PATH
$ export LD_LIBRARY_PATH=/opt/local/lib:$LD_LIBRARY_PATH
$ export C_INCLUDE_PATH=/opt/local/include:$C_INCLUDE_PATH
$ export CPLUS_INCLUDE_PATH=/opt/local/include:$CPLUS_INCLUDE_PATH
$ cd cvdrone-master/build/linux
$ make
$ ./test.a

これは便利♪

cvdrone_on_mac.png


MacでAR.Droneを動かしたい方は是非CV Droneをお試しください。導入はとても簡単です。

【告知】
CV Drone 開発ページではマルチプラットフォーム対応のFTPクライアントを実装してくれる人を
募集しています。「我こそは」という方は是非ご参加ください。私もやります。
私がやりました

AR.Droneを動かそう(フライトアニメーション)

AR.Droneには標準でグルグル回ったりダンスしたりなど、あらかじめ設定されたアニメーションを再生する機能があります。

void ARDrone::setAnimation(int id, int duration)
{
// コマンド送信
sockCommand.sendf("AT*ANIM=%d,%d,%d\r", seq++, id, duration);
}
AT*ANIMというのが該当するコマンドです。durationは再生のタイムアウトですが、デフォルト値(下記サンプル参照)を使うことが推奨されています。

AR.Drone2.0からは「フリップ」が追加されました。さっそくサンプルで遊んでみましょう。
https://github.com/puku0x/cvdrone/blob/master/samples/sample_flight_animation.cpp

#include "ardrone/ardrone.h"

// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description : This is the entry point of the program.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Droneクラス
ARDrone ardrone;

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

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

// 操作説明
printf(" Q - ARDRONE_ANIM_PHI_M30_DEG\n");
printf(" A - ARDRONE_ANIM_PHI_30_DEG\n");
printf(" Z - ARDRONE_ANIM_THETA_M30_DEG\n");
printf(" W - ARDRONE_ANIM_THETA_30_DEG\n");
printf(" S - ARDRONE_ANIM_THETA_20DEG_YAW_200DEG\n");
printf(" X - ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG\n");
printf(" E - ARDRONE_ANIM_TURNAROUND\n");
printf(" D - ARDRONE_ANIM_TURNAROUND_GODOWN\n");
printf(" C - ARDRONE_ANIM_YAW_SHAKE\n");
printf(" R - ARDRONE_ANIM_YAW_DANCE\n");
printf(" F - ARDRONE_ANIM_PHI_DANCE\n");
printf(" V - ARDRONE_ANIM_THETA_DANCE\n");
printf(" T - ARDRONE_ANIM_VZ_DANCE\n");
printf(" G - ARDRONE_ANIM_WAVE\n");
printf(" B - ARDRONE_ANIM_PHI_THETA_MIXED\n");
printf(" Y - ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED\n");
printf(" H - ARDRONE_ANIM_FLIP_AHEAD\n");
printf(" N - ARDRONE_ANIM_FLIP_BEHIND\n");
printf(" U - ARDRONE_ANIM_FLIP_LEFT\n");
printf(" J - ARDRONE_ANIM_FLIP_RIGHT\n");

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

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

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

// Spaceキーで離着陸
if (key == ' ') {
if (ardrone.onGround()) ardrone.takeoff();
else ardrone.landing();
}

// フライトアニメーション
if (key == 'q') ardrone.setAnimation(ARDRONE_ANIM_PHI_M30_DEG, 1000);
if (key == 'a') ardrone.setAnimation(ARDRONE_ANIM_PHI_30_DEG, 1000);
if (key == 'z') ardrone.setAnimation(ARDRONE_ANIM_THETA_M30_DEG, 1000);
if (key == 'w') ardrone.setAnimation(ARDRONE_ANIM_THETA_30_DEG, 1000);
if (key == 's') ardrone.setAnimation(ARDRONE_ANIM_THETA_20DEG_YAW_200DEG, 1000);
if (key == 'x') ardrone.setAnimation(ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG, 1000);
if (key == 'e') ardrone.setAnimation(ARDRONE_ANIM_TURNAROUND, 5000);
if (key == 'd') ardrone.setAnimation(ARDRONE_ANIM_TURNAROUND_GODOWN, 5000);
if (key == 'c') ardrone.setAnimation(ARDRONE_ANIM_YAW_SHAKE, 2000);
if (key == 'r') ardrone.setAnimation(ARDRONE_ANIM_YAW_DANCE, 5000);
if (key == 'f') ardrone.setAnimation(ARDRONE_ANIM_PHI_DANCE, 5000);
if (key == 'v') ardrone.setAnimation(ARDRONE_ANIM_THETA_DANCE, 5000);
if (key == 't') ardrone.setAnimation(ARDRONE_ANIM_VZ_DANCE, 5000);
if (key == 'g') ardrone.setAnimation(ARDRONE_ANIM_WAVE, 5000);
if (key == 'b') ardrone.setAnimation(ARDRONE_ANIM_PHI_THETA_MIXED, 5000);
if (key == 'y') ardrone.setAnimation(ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED, 5000);
if (key == 'h') ardrone.setAnimation(ARDRONE_ANIM_FLIP_AHEAD, 15);
if (key == 'n') ardrone.setAnimation(ARDRONE_ANIM_FLIP_BEHIND, 15);
if (key == 'u') ardrone.setAnimation(ARDRONE_ANIM_FLIP_LEFT, 15);
if (key == 'j') ardrone.setAnimation(ARDRONE_ANIM_FLIP_RIGHT, 15);

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

return 0;
}
ARDRONE_ANIM_VZ_DANCEはバグのせいか全く動きません

また、フリップを実行するとAR.Droneは自動で安全な高さ(3mぐらい)まで上昇するので、天井の低いところでは失敗することがあります。

まぁ、もともとデモ用ですしおすし。
プロフィール

puku

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

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

最新記事
最新コメント
最新トラックバック
検索フォーム
カレンダー
03 | 2021/04 | 05
- - - - 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 -
月別アーカイブ
カテゴリ
スポンサードリンク
RSSリンクの表示
FC2カウンター
リンク
ブロとも申請フォーム

この人とブロともになる

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

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

FC2Blog Ranking

QRコード
QR