3年位前にSURF特徴量を使ってARっぽいことをやってたんですが，最近見返してみたら直したいところが結構あったので作り直してみました．
ついでに，アクセス解析を見るとsolvePnPRansacについて検索してきている人が多いようなので，知っている範囲でこの関数などについて書いておこうと思います（といってもほぼ本家の英リファレンスのままですが）．
3年前の記事：SURFとSolvePnPRansacで拡張現実

今回はマーカの画像とカメラから得られた現フレームの画像のそれぞれのORB特徴を抽出した後，FlannBasedMatcherでそれらの対応付けを取り，solvePnPRansacでカメラの位置・姿勢を推定しています．

処理結果の動画はこんな感じです．右側がマーカ画像（エアフェスタ浜松で撮った写真）で，左側がカメラから得られた画像です．カラフルな丸・線はORB特徴量の検出された位置とマーカと現フレームにおける対応付けを表しています．推定された位置・姿勢を基に立方体を表示しています．
http://tenteroring.luna.ddns.vc/tenteroring/wp-content/uploads/2016/03/orb_flann_pnpransace.wmv

ソース(VS2013, OpenCV2.4.10で動作確認)

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

// 立方体描画
void draw_cube(cv::Mat& img, const cv::Mat& intrinsic, const cv::Mat& distortion, const cv::Mat& rvec, const cv::Mat& tvec) {
	float half_extent = 0.05f;
	std::vector<cv::Point3f> vertices_3d = {
		cv::Point3f(-half_extent, -half_extent, -half_extent),	cv::Point3f(half_extent, -half_extent, -half_extent),	cv::Point3f(half_extent, half_extent, -half_extent),	cv::Point3f(-half_extent, half_extent, -half_extent),
		cv::Point3f(-half_extent, -half_extent, half_extent),	cv::Point3f(half_extent, -half_extent, half_extent),	cv::Point3f(half_extent, half_extent, half_extent),		cv::Point3f(-half_extent, half_extent, half_extent)
	};

	std::vector<int> indices = {
		0, 1, 1, 2, 2, 3, 3, 0,
		4, 5, 5, 6, 6, 7, 7, 4,
		0, 4, 1, 5, 2, 6, 3, 7
	};

	std::vector<cv::Point2f> vertices_2d(vertices_3d.size());
	cv::projectPoints(vertices_3d, rvec, tvec, intrinsic, distortion, vertices_2d);
	for (int i = 0; i < indices.size(); i += 2) {
		cv::line(img, vertices_2d[indices[i]], vertices_2d[indices[i + 1]], cv::Scalar(0, 255, 0), 2);
	}
}

// 特徴量検出・抽出
std::pair<std::vector<cv::KeyPoint>, cv::Mat> extract_features(const cv::Mat& image, const cv::FeatureDetector& detector, const cv::DescriptorExtractor& extractor) {
	std::pair<std::vector<cv::KeyPoint>, cv::Mat> keypoints_features;
	detector.detect(image, keypoints_features.first);
	extractor.compute(image, keypoints_features.first, keypoints_features.second);
	keypoints_features.second.convertTo(keypoints_features.second, CV_32FC1);
	return keypoints_features;
}

// main
int main(int argc, char** argv) {
	// マーカのスケーリング係数 384pix => 198mm
	const double marker_scale = 198.0 / 384.0 / 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 intrinsic_mat(3, 3, CV_32FC1, intrinsic);
	const cv::Mat distortion_mat(1, 4, CV_32FC1, distortion);

	// 特徴量検出・抽出器
	auto detector = cv::FeatureDetector::create("ORB");
	auto extractor = cv::DescriptorExtractor::create("ORB");

	// マーカ読み込み，特徴量検出・抽出
	cv::Mat marker = cv::imread("marker.jpg", 0);
	auto marker_keypoints_features = extract_features( marker, *detector, *extractor );
	const auto& marker_keypoints = marker_keypoints_features.first;
	const auto& marker_features = marker_keypoints_features.second;

	// マーカの各特徴点の3次元位置を求めておく
	std::vector<cv::Point3f> marker_points(marker_keypoints.size());
	for (int i = 0; i < marker_keypoints.size(); i++) {
		marker_points[i] = cv::Point3f((marker_keypoints[i].pt.x - marker.cols/2) * marker_scale, (marker_keypoints[i].pt.y - marker.rows/2) * marker_scale, 0.0f);
	}

	// flannbased matcherを用意，マーカーの特徴量を追加しておく
	const int match_N = 64;		// マッチングに用いる特徴点数
	cv::FlannBasedMatcher matcher( new cv::flann::KDTreeIndexParams(), new cv::flann::SearchParams() );
	matcher.add(std::vector<cv::Mat> { marker_features });

	cv::VideoCapture cap(0);
	cv::Mat frame, gray_frame;
	// メインループ, ESCが押されるまで
	while (cv::waitKey(1) != 0x1b) {
		cap >> frame;
		cv::cvtColor(frame, gray_frame, CV_BGR2GRAY);

		// 現フレームの特徴量抽出
		auto frame_keypoints_features = extract_features(gray_frame, *detector, *extractor);
		const auto& frame_keypoints = frame_keypoints_features.first;
		const auto& frame_features = frame_keypoints_features.second;
		if (frame_keypoints.size() < match_N) {
			continue;
		}
		
		// マッチング，上位match_N個を抽出
		std::vector<cv::DMatch> matches;
		matcher.match(frame_features, matches);
		std::nth_element(matches.begin(), matches.begin() + match_N, matches.end());
		matches.erase(matches.begin() + match_N, matches.end());

		double sum_distance = 0.0;
		std::vector<cv::Point2f> image_points( matches.size());
		std::vector<cv::Point3f> object_points(matches.size());
		for (int i = 0; i < matches.size(); i++) {
			sum_distance += matches[i].distance;
			image_points[i] = frame_keypoints[matches[i].queryIdx].pt;
			object_points[i] = marker_points[matches[i].trainIdx];
		}

		// 結果描画
		cv::Mat matches_img;
		cv::drawMatches(frame, frame_keypoints, marker, marker_keypoints, matches, matches_img);
		if (sum_distance < 18000.0) {
			cv::Mat rvec, tvec;
			cv::solvePnPRansac(object_points, image_points, intrinsic_mat, distortion_mat, rvec, tvec, false, 256);
			draw_cube(matches_img, intrinsic_mat, distortion_mat, rvec, tvec);
		}

		cv::imshow("match", matches_img);
	}
}

FeatureDetector, DescriptorExtractorの引数を変えればSIFT, SURF, BRIEFなどなど他の特徴量も使用できます．ORB特徴量はそのままだとCV_8Uとして抽出されるようでFlannBasedMatcherに入力することができなかったので，convertToでCV_32Fに変換しています．

solvePnPRansacのuseExtrinsicGuessをtrueにして，前回の推定結果を再利用すてみたりもしたのですが，一度誤った推定をしてしまうとそこに捕らわれてしまう感じがあったので，最終的にはfalseにしました．

今回は簡単に対応付け後の特徴量のL2距離の総和を基に，マーカが画面内に入っているかどうかの判定を行っています．ただしきい値選択がかなりシビアな感じであまり良くない感じだったのでもっといい方法にしたいです．