デッドレコニング(Dead-reckoning)とは、エンコーダやセンサの情報から機体の相対的な位置・姿勢を推定することをいいます。
AR.Droneには加速度センサやジャイロスコープといったセンサが搭載されてあり、ユーザはAR.Droneが推定した速度・姿勢を用いることができます。
Navdataのvx、vy、vzはAR.Drone自身からみた速度なので回転行列を使ってワールド座標に変換します。
※上から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;
}
動かした結果がこちらです。
本当は1mぐらい上がったのですが、こちらのAR.Drone1.0はvzがゼロになるという呪いにかかっているため、z位置が0.2mまでという結果になっています。
