AR.DroneのZ方向速度の取得

最新版のCV Droneでは、デフォルトで全てのNavdataを送ってもらうように最初のリクエストを変更しています。

// リクエスト送信 (TRUEだと_navdata_demo_tが送られる
sockCommand.sendf("AT*CONFIG=%d,\"general:navdata_demo\",\"FALSE\"\r", seq++);
これまでのCV Droneでは_navdata_demo_t内のvzを返すようになっていたためその値は常にゼロでしたが、今回は_navdata_altitude_t内のaltitude_vzが返ってくるように変更されています。

てかParrotさん最初から入れといてよ

以前作ったデッドレコニングのサンプルもまたいじってみようかな。

いや、むしろmove3D関数にフィードバック実装する方が先か...?
スポンサーサイト



AR.Drone2.0フライトレコーダ情報

先週あたりから、海外フォーラムにてAR.Drone2.0用のフライトレコーダ(USBメモリ+GPSユニット)の情報が出てきました。

ardrone_flight_recorder_page.jpg

フライトレコーダが送ってくるデータはnavdata_gps_tという新しい構造体に格納されるようです。

// From https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/boards/ardrone/at_com.h
typedef double float64_t; //TODO: Fix this nicely, but this is only used here
typedef float float32_t; //TODO: Fix this nicely, but this is only used here

typedef struct _navdata_gps_t {
uint16_t tag; /*!< Navdata block ('option') identifier */
uint16_t size; /*!< set this to the size of this structure */
float64_t lat; /*!< Latitude */
float64_t lon; /*!< Longitude */
float64_t elevation; /*!< Elevation */
float64_t hdop; /*!< hdop */
int32_t data_available; /*!< When there is data available */
uint8_t unk_0[8];
float64_t lat0; /*!< Latitude ??? */
float64_t lon0; /*!< Longitude ??? */
float64_t lat_fuse; /*!< Latitude fused */
float64_t lon_fuse; /*!< Longitude fused */
uint32_t gps_state; /*!< State of the GPS, still need to figure out */
uint8_t unk_1[40];
float64_t vdop; /*!< vdop */
float64_t pdop; /*!< pdop */
float32_t speed; /*!< speed */
uint32_t last_frame_timestamp; /*!< Timestamp from the last frame */
float32_t degree; /*!< Degree */
float32_t degree_mag; /*!< Degree of the magnetic */
uint8_t unk_2[16];
struct{
uint8_t sat;
uint8_t cn0;
} channels[12];
int32_t gps_plugged; /*!< When the gps is plugged */
uint8_t unk_3[108];
float64_t gps_time; /*!< The gps time of week */
uint16_t week; /*!< The gps week */
uint8_t gps_fix; /*!< The gps fix */
uint8_t num_sattelites; /*!< Number of sattelites */
uint8_t unk_4[24];
float64_t ned_vel_c0; /*!< NED velocity */
float64_t ned_vel_c1; /*!< NED velocity */
float64_t ned_vel_c2; /*!< NED velocity */
float64_t pos_accur_c0; /*!< Position accuracy */
float64_t pos_accur_c1; /*!< Position accuracy */
float64_t pos_accur_c2; /*!< Position accuracy */
float32_t speed_acur; /*!< Speed accuracy */
float32_t time_acur; /*!< Time accuracy */
uint8_t unk_5[72];
float32_t temprature;
float32_t pressure;
} __attribute__ ((packed)) navdata_gps_t;
タグは「27」です。

navdata_zimmu3000_tと同じ気がしますが...

ヒャア!もう我慢できねぇ、実装だ!

ファームウェアのバージョンは今回の更新で2.4.1になるようです。

やべ、まだ2.2.9だ。

OpenCV 2.4.6に更新しました

先週のコミットでCV Droneに付属のOpenCVとFFmpegのバージョンをそれぞれ2.4.6、1.2.1に更新しました。

新しい物好きだからね。仕方ないね。

OpenCVの最近の動向としては、
・iOSやAndroidへの移植を促進
・GPU周りの強化 (開発版では10個ものGPUモジュールが追加されています)

と、スマホで画像処理がしたいようです。

CV Droneで大きく変わったところは
・Debugビルドの廃止
・デフォルトで全てのNavdataを送信するように変更
・キャリブレーション用の関数(AT*FTRIM, AT*CALIB)追加
となっております。

FREAK使ってみた

FREAKはOpenCV 2.4.2から導入された特徴量記述子です。
※特徴量検出器ではありません

従来の記述子(SIFTとかSURF)より高速でそこそこロバストらしいです。

前回のサンプルに入れて遊んでみましょう。

#include <opencv2/opencv.hpp>
#include <opencv2/nonfree/nonfree.hpp>

// --------------------------------------------------------------------------
// 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)
{
// SURF/FREAK
cv::SurfFeatureDetector detector; // SURF ※要NON_FREEビルド
cv::FREAK extractor; // <- ここが変わる
↓SURF+FREAKで特徴点抽出
surf_freak.jpg
マンドリル先輩マジイケメンすなぁ


あれ?

何か間違えたかな...?

ちなみにSURFだとこんな感じです。

surf_surf.jpg

んー、FREAKはまだ様子見といったところでしょうか。

サンプルにあったコードを貼り付けただけなので、細かいところは自分でチューニングする必要があるのでしょう。

ORBでホモグラフィー行列推定

SIFTとかSURFでよくあるサンプルをORBで動かしてみました。

↓ここのやつですね。
http://docs.opencv.org/doc/tutorials/features2d/feature_homography/feature_homography.html

十分な対応点が得られない場合や、ホモグラフィー行列がうまく推定できなかった場合のエラー処理が入っていなかったので追加しています。

#include <opencv2/opencv.hpp>

// --------------------------------------------------------------------------
// 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)
{
// ORB
cv::OrbFeatureDetector detector;
cv::OrbDescriptorExtractor extractor;

// 読み込み
cv::Mat image1 = cv::imread("box.png");
cv::Mat image2 = cv::imread("box_in_scene.png");

// 特徴点1
std::vector<cv::KeyPoint> keypointsA;
cv::Mat descriptorsA;
detector.detect(image1, keypointsA);
extractor.compute(image1, keypointsA, descriptorsA);

// 特徴点2
std::vector<cv::KeyPoint> keypointsB;
cv::Mat descriptorsB;
detector.detect(image2, keypointsB);
extractor.compute(image2, keypointsB, descriptorsB);

// マッチング
cv::BFMatcher matcher(cv::NORM_HAMMING, true);
std::vector<cv::DMatch> matches;
matcher.match(descriptorsA, descriptorsB, matches);

// 最小距離
double min_dist = DBL_MAX;
for (int i = 0; i < (int)matches.size(); i++) {
double dist = matches[i].distance;
if (dist < min_dist) min_dist = dist;
}

// 良いペアのみ残す
std::vector<cv::DMatch> good_matches;
for (int i = 0; i < (int)matches.size(); i++) {
if (matches[i].distance < 3.0 * min_dist) good_matches.push_back(matches[i]);
}

// 対応点の表示
cv::Mat img_matches;
cv::drawMatches(image1, keypointsA, image2, keypointsB, good_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

// 十分な対応点がある
if (good_matches.size() > 10) {
std::vector<cv::Point2f> obj, scene;
for (int i = 0; i < (int)good_matches.size(); i++) {
obj.push_back(keypointsA[good_matches[i].queryIdx].pt);
scene.push_back(keypointsB[good_matches[i].trainIdx].pt);
}

// ホモグラフィー行列を計算
cv::Mat H = cv::findHomography(obj, scene, cv::RANSAC);
//std::cout << H << std::endl;

// 行列が空ではない
if (!H.empty()) {
std::vector<cv::Point2d> obj_corners(4), scene_corners(4);
obj_corners[0] = scene_corners[0] = cv::Point2d(0, 0);
obj_corners[1] = scene_corners[1] = cv::Point2d(image1.cols, 0);
obj_corners[2] = scene_corners[2] = cv::Point2d(image1.cols, image1.rows );
obj_corners[3] = scene_corners[3] = cv::Point2d(0, image1.rows);

// ホモグラフィ行列の推定
cv::perspectiveTransform(obj_corners, scene_corners, H);

// 緑の線で囲む (開始点を元画像が左にあるので右にオフセット)
cv::line(img_matches, scene_corners[0] + cv::Point2d(image1.cols, 0), scene_corners[1] + cv::Point2d(image1.cols, 0), cv::Scalar(0, 255, 0), 4);
cv::line(img_matches, scene_corners[1] + cv::Point2d(image1.cols, 0), scene_corners[2] + cv::Point2d(image1.cols, 0), cv::Scalar(0, 255, 0), 4);
cv::line(img_matches, scene_corners[2] + cv::Point2d(image1.cols, 0), scene_corners[3] + cv::Point2d(image1.cols, 0), cv::Scalar(0, 255, 0), 4);
cv::line(img_matches, scene_corners[3] + cv::Point2d(image1.cols, 0), scene_corners[0] + cv::Point2d(image1.cols, 0), cv::Scalar(0, 255, 0), 4);
}
}

// 表示
cv::imshow("camera", img_matches);
cv::waitKey(0);

return 0;
}

結果はこんな感じです。
orb_homography.jpg

うん、いいんじゃない?
プロフィール

puku

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

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

最新記事
最新コメント
最新トラックバック
検索フォーム
カレンダー
06 | 2013/07 | 08
- 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