プログラミング | tetro

3連休，割りと暇だったので前からやってみたかったSURFを使ったナチュラルマーカARを試してみました．

マーカに使ったのは毎日プレイしているvitaの空き箱．

結果はこんな感じ．緑色の丸はSURF特徴点．
　

プログラムはほぼOpenCVで書きました．
処理は以下のとおり．
1. マーカ，動画の両方のSURF特徴量を抽出
2. BruteForceMatcherでマッチング
3. マッチング結果の上位20個を使ってsolvePnPRansac
4. 得られた外部パラメータを使って立方体を描画

3番目のsolvePnPRansacが肝で，OpenCV2.4くらいから追加された機能なのですが，RANSACを使って外部パラメータの推定を行なってくれます．
古いsolvePnP関数だと，最小二乗誤差となる解を探すだけなのでエラーデータに弱かったりするのですが，solvePnPRansacならかなりロバストにパラメータを求めてくれます．
今回，マッチング情報が割りと不正確なのでRANSACを使わないとうまく推定できませんでした．

半分くらいしか写ってなくても大丈夫

今回，これを試すためだけにわざわざwebカメラを買ったのでうまく動いてよかったです．
ただ，マーカが写っていなくても描画しているのでその辺の修正をしないといけないですね．
また今度OpenGLと組み合わせてもうちょっとリッチな画面にしてみようかと思います．

どうでもいいですけど，SURFって名前かっこいいですよね．
エースコンバット3を思い出しちゃいます．

追記 : ソースを整理したので公開(VS2010, OpenCV2.4.3)

/**************************************************
 * SURFでARもどき
 * @author : tetro
**************************************************/
#include <vector>
#include <utility>
#include <iostream>
#include <algorithm>

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

// グローバル関数
void DrawCube( cv::Mat& img, const cv::Mat& rotation, const cv::Mat& translation, const cv::Mat& intrinsic, const cv::Mat& distortion );
std::pair<std::vector<cv::KeyPoint>, cv::Mat> DetectAndExtractFeatures( const cv::Mat& img, cv::FeatureDetector& detector, cv::DescriptorExtractor& extractor );
std::pair<cv::Mat, cv::Mat> CalcRotationAndTranslation( const cv::Mat& intrinsic, const cv::Mat& distortion, const std::vector<cv::KeyPoint>& queryKeypoints, const std::vector<cv::KeyPoint>& resultKeypoints, const std::vector<cv::DMatch>& matches, const cv::Size& markerSize, const double markerScale );

using namespace std;

/****************************************************
 * main
 *
****************************************************/
int main(){
	// マーカのスケーリング係数 180.6pix => 98mm
	const double markerScale = 98.0 / 180.6 / 1000.0;

	// 内部パラメータ，歪み係数
	float intrinsic[] = { 7.89546997e+002, 0.0f, 3.46134979e+002, 0.0f, 7.91432861e+002, 2.53656479e+002, 0.0f, 0.0f, 1.0f };
	float distortion[] = { -2.07489878e-002, 9.17263553e-002, -1.40290568e-003, 1.13266008e-002 };
	const cv::Mat intrinsicMat( 3, 3, CV_32FC1, intrinsic );
	const cv::Mat distortionMat( 1, 4, CV_32FC1, distortion );

	// SURF
	cv::SurfFeatureDetector detector( 1000 );
	cv::SurfDescriptorExtractor extractor;

	// マーカの特徴量を計算
	cv::Mat markerImg = cv::imread( "vita.png" ), markerGrayImg;
	cv::cvtColor( markerImg, markerGrayImg, CV_BGR2GRAY );
	pair<vector<cv::KeyPoint>, cv::Mat> markerFeatures = DetectAndExtractFeatures( markerGrayImg, detector, extractor );

	for( auto itr = begin(markerFeatures.first); itr != end(markerFeatures.first); itr++ ){
		cv::circle( markerImg, itr->pt, itr->size * 0.2, cv::Scalar(0,255,0) );
	}
	cv::imshow( "marker", markerImg );

	// キャプチャ画像に対して処理ループ
	cv::VideoCapture capture(0);
	while( cv::waitKey(1) != 0x1b ){
		cv::Mat frameImg, frameGrayImg;
		capture >> frameImg;
		cv::cvtColor( frameImg, frameGrayImg, CV_BGR2GRAY );
		// キャプチャ画像の特徴量を計算
		pair<vector<cv::KeyPoint>, cv::Mat> frameFeatures = DetectAndExtractFeatures( frameGrayImg, detector, extractor );
		for( auto itr = begin(frameFeatures.first); itr != end(frameFeatures.first); itr++ ){
			cv::circle( frameImg, itr->pt, itr->size * 0.2, cv::Scalar(0,255,0) );
		}
		if( frameFeatures.first.empty() ){
			continue;
		}

		// マッチング
		std::vector<cv::DMatch> matches;
		cv::BruteForceMatcher<cv::L2<float>> matcher;
		matcher.match( markerFeatures.second, frameFeatures.second, matches );
		// 上位20個を採用
		nth_element( begin(matches), begin(matches) + 20, end(matches) );
		matches.erase( begin(matches) + 21, end(matches)  );
		cv::Mat matchedImg;
		cv::drawMatches( markerImg, markerFeatures.first, frameImg, frameFeatures.first, matches, matchedImg );

		// 外部パラメータ計算
		pair<cv::Mat, cv::Mat> rotTrans = CalcRotationAndTranslation( intrinsicMat, distortionMat, markerFeatures.first, frameFeatures.first, matches, markerImg.size(), markerScale );
		DrawCube( frameImg, rotTrans.first, rotTrans.second, intrinsicMat, distortionMat );

		cv::imshow( "frame", frameImg );
		cv::imshow( "matched", matchedImg );
	}
}

/****************************************************
 * CreateCubeVertices
 * 立方体の頂点を作る
****************************************************/
std::vector<cv::Point3f>& CubeVertices(){
	const float halfExtents = 0.05;
	static vector<cv::Point3f> cube3d;
	if( cube3d.empty() ){
		cube3d.push_back( cv::Point3f(-halfExtents,-halfExtents,0.0f) );
		cube3d.push_back( cv::Point3f(halfExtents,-halfExtents,0.0f) );
		cube3d.push_back( cv::Point3f(halfExtents,halfExtents,0.0f) );
		cube3d.push_back( cv::Point3f(-halfExtents,halfExtents,0.0f) );
		cube3d.push_back( cv::Point3f(-halfExtents,-halfExtents,-halfExtents*2.0f) );
		cube3d.push_back( cv::Point3f(halfExtents,-halfExtents,-halfExtents*2.0f) );
		cube3d.push_back( cv::Point3f(halfExtents,halfExtents,-halfExtents*2.0f) );
		cube3d.push_back( cv::Point3f(-halfExtents,halfExtents,-halfExtents*2.0f) );
	}

	return move( cube3d );
}

/****************************************************
 * DrawCube
 * 立方体を描画する
****************************************************/
void DrawCube( cv::Mat& img, const cv::Mat& rotation, const cv::Mat& translation, const cv::Mat& intrinsic, const cv::Mat& distortion ){
	vector<cv::Point3f> cube3d = CubeVertices();
	vector<cv::Point2f> cube2d;
	cv::projectPoints( cube3d, rotation, translation, intrinsic, distortion, cube2d );

	for( auto p2d = begin(cube2d); p2d != end(cube2d); p2d++ ){
		cv::circle( img, *p2d, 5, cv::Scalar(0,0,255), 2 );
	}
	cv::line( img, cube2d[0], cube2d[1], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[1], cube2d[2], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[2], cube2d[3], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[3], cube2d[0], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[4], cube2d[5], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[5], cube2d[6], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[6], cube2d[7], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[7], cube2d[4], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[0], cube2d[4], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[1], cube2d[5], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[2], cube2d[6], cv::Scalar(255,0,0), 2 );
	cv::line( img, cube2d[3], cube2d[7], cv::Scalar(255,0,0), 2 );
}

/****************************************************
 * DetectAndExtractFeatures
 * 特徴量を検出，記述子を抽出
****************************************************/
std::pair<std::vector<cv::KeyPoint>, cv::Mat> DetectAndExtractFeatures( const cv::Mat& img, cv::FeatureDetector& detector, cv::DescriptorExtractor& extractor ){

	vector<cv::KeyPoint> keypoints;
	detector.detect( img, keypoints );
	cv::Mat descriptor;
	extractor.compute( img, keypoints, descriptor );

	return make_pair( move(keypoints), move(descriptor) );
}

/****************************************************
 * CalcRotationAndTranslation
 * 外部パラメータを計算する
****************************************************/
std::pair<cv::Mat, cv::Mat> CalcRotationAndTranslation( const cv::Mat& intrinsic, const cv::Mat& distortion, const std::vector<cv::KeyPoint>& queryKeypoints, const std::vector<cv::KeyPoint>& resultKeypoints, const std::vector<cv::DMatch>& matches, const cv::Size& markerSize, const double markerScale ){

	vector<cv::Point3f> objectPoints;
	vector<cv::Point2f> imagePoints;
	objectPoints.reserve( matches.size() );
	imagePoints.reserve( matches.size() );
	for( auto match = begin(matches); match != end(matches); match++ ){
		cv::Point2f querypoint = queryKeypoints[match->queryIdx].pt;
		cv::Point2f trainpoint = resultKeypoints[match->trainIdx].pt;
		objectPoints.push_back( cv::Point3f( (querypoint.x - markerSize.width/2) * markerScale, (querypoint.y - markerSize.height/2) * markerScale, 0.0f ) );
		imagePoints.push_back( trainpoint );
	}

	cv::Mat rotationVec, translationVec;
	cv::solvePnPRansac( objectPoints, imagePoints, intrinsic, distortion, rotationVec, translationVec );

	return make_pair( rotationVec, translationVec );
}

車のナンバープレートとかの4つの数字に四則演算を行なって10を作るっていうゲームがありますよね．名前は”Make 10″っていうらしいです．

で，この間，話の流れでどのくらいの割合のナンバープレートで10が作れるのだろうかという話題になりました．経験論では半分以上は作れるだろう，という人が多かったのですが実際はどうなのかはっきりとはわかりませんでした．
ちょっと気になったので”Make 10″を解くプログラムを書いてみました．
makeTenSrc.zip

大した計算量にはならなさそうだったので，総当りでごり押しする方向で作りました．
また，分数も扱えるようにしたかったのでboost::rationalクラスを使用してみました．

numbers が問題となる数字列で，solution が解答です．
解答はポーランド的な記法ですが，コーディング上の事情で一番右の2つの数字が，左辺と右辺で逆になってます．
(例えば numbers : 1919 の solution : * 9 + 1 / 9 1 は (((1/9) + 1 ) * 9) と解いている)
直そうかと思いましたが，面倒だったので放置です．

それで結局，何割のナンバープレートで10が作れるかというと…

… 7680 / 9999 !!
だいたい77%，思ってたよりだいぶ大きな値です．びっくりしました．

ざっと見た感じでは間違った解答はなかったと思いますが，もしかしたら間違いがあるかもしれません．
需要があるかわかりませんが下が解答一覧です．
makeTenSolution.zip
解答に間違いがありましたらコメントかなんかで連絡ください．

tetro

「プログラミング」カテゴリーアーカイブ

boostインストール

SURFとSolvePnPRansacで拡張現実

エンディアン変換

make 10