AR.Droneでパノラマ画像

OpenCVのパノラマ画像生成機能(Stitching)を使ってみましょう。

最小限のコードはこんな感じです。

// 画像セット
std::vector<cv::Mat> images;
images.puch_back(何かの画像1);
images.puch_back(何かの画像2);
:
images.puch_back(何かの画像n);

// パノラマ画像生成
cv::Mat result;
cv::Stitcher stitcher = cv::Stitcher::createDefault();
stitcher.stitch(images, result);
AR.Droneへ応用するときに毎ループpush_backするとパノラマ画像の生成に時間がかかる(というか画像が多すぎるとメモリ不足になる)ので注意しましょう。

#include "ardrone/ardrone.h"
#include <opencv2/stitching/stitcher.hpp>

#define KEY_DOWN(key) (GetAsyncKeyState(key) & 0x8000)
#define KEY_PUSH(key) (GetAsyncKeyState(key) & 0x0001)

// --------------------------------------------------------------------------
// 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;
}

// 画像保存用
std::vector<cv::Mat> snapshots;

// 直前の画像
cv::Mat last = cv::Mat(ardrone.getImage(), true);

// ORB
cv::OrbFeatureDetector detector;
cv::OrbDescriptorExtractor extractor;

// メインループ
while (!GetAsyncKeyState(VK_ESCAPE)) {
// 更新
if (!ardrone.update()) break;

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

// 特徴点の抽出
cv::Mat descriptorsA, descriptorsB;
std::vector<cv::KeyPoint> keypointsA, keypointsB;
detector.detect(last, keypointsA);
detector.detect(image, keypointsB);
extractor.compute(last, keypointsA, descriptorsA);
extractor.compute(image, keypointsB, descriptorsB);

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

// マッチ数
int count = 0;
for (int i = 0; i < (int)matches.size(); i++) {
if (matches[i].queryIdx == matches[i].trainIdx) count++; // これ合ってんのか?
}

// マッチ数が0なら保存
if (count == 0) {
image.copyTo(last);
cv::Ptr<cv::Mat> tmp(new cv::Mat());
image.copyTo(*tmp);
snapshots.push_back(*tmp);
}

// 表示
cv::Mat matchImage;
cv::drawMatches(last, keypointsA, image, keypointsB, matches, matchImage, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
cv::imshow("camera", matchImage);
cv::waitKey(1);
}

// ここからStiching
cv::Mat result;
cv::Stitcher stitcher = cv::Stitcher::createDefault();
printf("Stitching images...\n");
if (stitcher.stitch(snapshots, result) == cv::Stitcher::OK) {
cv::imshow("result", result);
cv::imwrite("result.jpg", result);
cvWaitKey(0);
}

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

return 0;
}
本サンプルではORB特徴量を抽出し、前フレームと比較することでシーンの切り替わりを検出しています。
58行目のやつです。でもこんなので本当にいいのかな?

result_stitching.jpg

CV Droneのサンプルにも収録されています。
https://github.com/puku0x/cvdrone/blob/master/src/samples/sample_stitching.cpp
スポンサーサイト



pthreadを使ってみよう

マルチスレッド大好きな私ですが、pthread(POSIX Threads)は扱ったことがありませんでした。

にわかでスンマセン。

「Linuxに移植したい」との声を受けて、最新のCV Droneには従来の_beginthreadex()による実装からpthreadを用いたものに変更しています。といってもFTPの部分がまだWinINet依存ですが。

スレッドの生成はこんな感じです。

#include <pthread.h>

// 生成
pthread_t thread;
pthread_create(&thread, NULL, loop, NULL);
こちらはスレッド関数です。

// スレッドの処理
void* loop(void *arg)
{
while (1) {
// 何かの処理
// :
pthread_testcancel(); // 外部から終了させるときに必要
}
return NULL;
}
それにしてもなんで関数ポインタのポインタを渡すんでしょう?

外部からスレッドを終了させる仕組みがあるのがいいですね。

// 外部から終了
pthread_cancel(thread);
pthread_join(thread, NULL);
Win32APIでもTerminateThread()で同じことができますが、危険なのであまり使いたくありません。
volatileのフラグを排他制御しながらアレコレするのも面倒ですしね。

AR.Droneを動かそう(設定情報の取得)

久しぶりのAR.Droneネタです。

AR.Droneが使うポートは以下の5つです。
・5551: FTP用
・5554: ナビゲーションデータ
・5555: ビデオ
・5556: ATコマンド
・5559: 設定

AR.Droneの現在の設定を取得するにはTCPポート5559に接続します。

しかし、ただ接続しただけでは何も来ません。
ナビゲーションデータと同様にリクエストを送る必要があります。

マニュアルには"AT*CTRL=seq,4,0\r"を送れば良いと書かれてありますが、
実際のところはその前にコマンドバッファをクリアしなければなりません。

// コンフィグ送信のリクエストを送る
sockCommand.sendf("AT*CTRL=%d,5,0\r", seq++); // クリア
sockCommand.sendf("AT*CTRL=%d,4,0\r", seq++); // リクエスト

// 受信 (TCP:5559)
char buf[4096];
int size = sockConfig.receive((void*)&buf, sizeof(buf));
これに気づくまでにどれだけかかったか…。

cv::Matxのススメ

cv::Matを使うと行列の演算を簡単に行えるので便利と言えば便利なのですが、
初期化や要素のアクセスはちょっと面倒ですよね。

// 2x3 の行列
cv: cv::Mat mat23 = (cv::Mat_<double>(2,3) << 1, 2, 3, 4, 5, 6);

// 要素へのアクセス
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
std::cout << mat23.at<double>(i,j) << std::endl;
}
}
単純な行列を扱うだけなら、cv::Matxを使うと良いでしょう。

初期化は以下の2通りあります。

// 2x3 の行列
cv::Matx23d mat23(1, 2, 3,
4, 5, 6);

// 2x3 の行列
cv::Matx23d mat23;
mat23 << 1, 2, 3,
4, 5, 6;
要素へのアクセスはMatlabのように()を用います。

// 要素へのアクセス
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
std::cout << mat23(i,j) << std::endl;
}
}
簡単なのでオススメです。

5点アルゴリズム使ってみた (2)

※5点アルゴリズムはOpenCV2.4.5では削除されています。2.4.9を使いましょう。

求めた基礎行列からカメラの姿勢を得るにはcv::decomposeEssentialMat()かcv::recoverPose()を使います。

この関数もまだドキュメントに載っていません。Willow Garageさん はよ!
載りました http://docs.opencv.org/trunk/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#findessentialmat


cv::Matx33d R;
cv::Matx31d t;
cv::recoverPose(E, ptsA, ptsB, R, t, focal, pp);
勘で使っていきましょう。

とりあえず何枚か用意した画像をマッチングして、
5point_match.jpg

十分な対応が得られた場合のみ姿勢を計算する、というように作ります。

#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
//#pragma comment (lib, "pcl_common_release.lib")
//#pragma comment (lib, "pcl_visualization_release.lib")
//#pragma comment (lib, "vtkFiltering.lib")
//#pragma comment (lib, "vtkCommon.lib")
//#pragma comment (lib, "vtksys.lib")

// --------------------------------------------------------------------------
// 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)
{
// キャリブレーションデータ
cv::Mat cameraMatrix, distCoeffs;
cv::FileStorage fs("camera.xml", CV_STORAGE_READ);
fs["intrinsic"] >> cameraMatrix;
fs["distortion"] >> distCoeffs;

// 読み込み
std::vector<cv::Mat> images;
for (int i = 0; i < 15; i++) {
char name[256];
sprintf(name, "%d.bmp", i);
cv::Mat img;
cv::undistort(cv::imread(name), img, cameraMatrix, distCoeffs);
images.push_back(img);
}

// ORB
cv::OrbFeatureDetector detector;
cv::OrbDescriptorExtractor extractor;

// 初期カメラ姿勢
cv::Matx33d R_AB;
cv::Matx31d t_AB;
R_AB << 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0;
t_AB << 0.0, 0.0, 0.0;

// 3Dビューア
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(0, 0, 0);
viewer.addCoordinateSystem(2.0);
viewer.initCameraParameters();

for (int i = 1; i < (int)images.size(); i++) {
// 特徴点検出
cv::Mat descriptorsA, descriptorsB;
std::vector<cv::KeyPoint> keypointsA, keypointsB;
detector.detect(images[i-1], keypointsA);
detector.detect(images[i], keypointsB);
extractor.compute(images[i-1], keypointsA, descriptorsA);
extractor.compute(images[i], keypointsB, descriptorsB);

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

// 最大・最小距離
double min_dist = DBL_MAX;
for (int j = 0; j < (int)matches.size(); j++) {
double dist = matches[j].distance;
if (dist < min_dist) min_dist = (dist < 1.0) ? 1.0 : dist;
}

// 良いペアのみ残す
double cutoff = 5.0 * min_dist;
std::set<int> existing_trainIdx;
std::vector<cv::DMatch> matches_good;
for (int j = 0; j < (int)matches.size(); j++) {
if (matches[j].trainIdx <= 0) matches[j].trainIdx = matches[j].imgIdx;
if (matches[j].distance > 0.0 && matches[j].distance < cutoff) {
if (existing_trainIdx.find(matches[j].trainIdx) == existing_trainIdx.end() && matches[j].trainIdx >= 0 && matches[j].trainIdx < (int)keypointsB.size()) {
matches_good.push_back(matches[j]);
existing_trainIdx.insert(matches[j].trainIdx);
}
}
}

// 表示
cv::Mat matchImage;
cv::drawMatches(images[i-1], keypointsA, images[i], keypointsB, matches_good, matchImage, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
cv::imshow("match", matchImage);
cv::waitKey(1);

// 5ペア以上は必要
if (matches_good.size() > 100) {
// ステレオペア
std::vector<cv::Point2f>ptsA, ptsB;
for (int j = 0; j < (int)matches_good.size(); j++) {
ptsA.push_back(keypointsA[matches_good[j].queryIdx].pt);
ptsB.push_back(keypointsB[matches_good[j].trainIdx].pt);
}

// 焦点距離とレンズ主点
double fovx, fovy, focal, pasp;
cv::Point2d pp;
cv::calibrationMatrixValues(cameraMatrix, cv::Size(imgA.cols, imgA.rows), 0.0, 0.0, fovx, fovy, focal, pp, pasp);

// 5点アルゴリズムで基礎行列を計算
cv::Matx33d E = cv::findEssentialMat(ptsA, ptsB, focal, pp);

// カメラ姿勢
cv::Matx33d R;
cv::Matx31d t;
cv::recoverPose(E, ptsA, ptsB, R, t, focal, pp);

// カメラ移動
Eigen::Affine3f view;
Eigen::Matrix4f _t;
R_AB = R_AB * R.t();
t_AB = t_AB - R_AB * t;
_t << R_AB(0,0), R_AB(0,1), R_AB(0,2), t_AB(0,0),
R_AB(1,0), R_AB(1,1), R_AB(1,2), t_AB(1,0),
R_AB(2,0), R_AB(2,1), R_AB(2,2), t_AB(2,0);
view = _t;
viewer.addCoordinateSystem(1.0, view);
}
}

// 表示
viewer.spin();
return 0;
}
今回はデバッグ用にPCL(Point Cloud Library)で表示しています。

5point_pose.jpg
カメラを右に動かしたときの結果(軸があってないかも?)

うん?
プロフィール

puku

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

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

最新記事
最新コメント
最新トラックバック
検索フォーム
カレンダー
03 | 2013/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カウンター
リンク
ブロとも申請フォーム

この人とブロともになる

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

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

FC2Blog Ranking

QRコード
QR