追記：バージョンアップ？しました．
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)
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 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 | /************************************************** * 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 );} |