ただ，一つ問題になるのはスケールの推定です．原理的に単眼のvisual SLAM/odometryにおいては画像情報だけから環境のスケールを推定することができません．例えば，大きな箱を遠くから見ているのと，小さな箱を近く見ている場合には同じような絵が得られてしまうため大きさの区別がつきません．そのため，ARにおいて物体の大きさを指定して特定の場所に置くというのは少し難しい話になります（大きさ10cmの立方体をカメラから100cm前に置く！みたいなことを正確に行うのは難しい）．今回はカメラの動きに非常に単純なアドホックな過程を置いてスケールの推定を行ってみます．
DSO本体に続いて，DSOをラップするdso_rosパッケージをインストールします．基本的にはDSO本体のパスを指定してパッケージをcloneするだけですが，デフォルトだとcatkinが使えないのでブランチを切り替えておきましょう．
流れとしては 1. DSO本体インストール, 2. ROSパッケージインストール, 3. visual odometryを使ったAR, 4. 環境のpoint cloudを使ったインタラクションAR という感じになります．基本的な部分は省いていくので，ROSのインストールとかDirect visual odometryってなんぞやとかは書きません．
環境は Ubuntu 16.04 + ROS kinetic でPCはThinkpad25，カメラはそのへんに落ちてたwebカメラです．Direct系の手法はローリングシャッターに弱いのですが，それでもまあまあ動いてしまったりします．でも，グローバルシャッターカメラを持っている人はそっちを使うようにしましょう(そんな人はこんなブログ見ないか)．
追記：バージョンアップ？しました．
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 );
}