追記：バージョンアップ？しました．
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 );
}