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

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

orb_flann_pnpransac

solvePnPRansacの詳細の説明

そもそもPnP問題とはPerspective n-Pointの略で,n個の点集合のそれぞれの点について3次元座標とそれをカメラ画像上に投影した時の2次元座標の対応が与えられたときに,カメラの位置・姿勢を求める問題です.基本的にはLevenberg-Marquardt法などで投影後二乗誤差を最小化するような位置・姿勢を求めます.OpenCVにはこれを解くsolvePnPというそのままな名前の関数がありますが,最小二乗問題の常で間違った対応付けが含まれているとそれに引っ張られて正しい解が得られなくなります.

そこで間違った対応付けは無視してPnP問題を解くためにsolvePnP関数にRANSACを適用したのがsolvePnPRansacです.関数の引数などの詳細は以下のような感じです.

void solvePnPRansac(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int iterationsCount=100, float reprojectionError=8.0, int minInliersCount=100, OutputArray inliers=noArray(), int flags=ITERATIVE )

この関数は与えられた3次元点集合のobjectPointsと,それに対応する画像上での2次元点集合のimagePointsからカメラの位置・姿勢を推定します.推定される姿勢はobjectPointsを画像上に投影した点集合とimagePointsの二乗誤差を最小化するようなものになります.RANSACを用いることでsolvePnP関数と比べて外れ値に頑強な推定が可能です.

引数
objectPoints
N個の点の3次元座標.cv::Matで与える場合にはCV_32FC1の3xNかNx3,CV_32FC3の1xNかNx1で与える.std::vector<cv::Point3f>として与えてもよい.
imagePoints N個の点の画像上での2次元座標.cv::MatならCV_32FC1の2xNかNx2,CV_32FC2の1xNかNx1で与える.std::vector<cv::Point2f>として与えてもよい.
cameraMatrix カメラの内部パラメータ行列
distCoeff カメラのレンズ歪みパラメータ.歪み無しと仮定するなら空行列(cv::Mat())を与えればよい.
rvec 推定されたカメラの回転ベクトルが出力される
tvec 推定されたカメラの併進ベクトルが出力される
useExtrinsicGuess trueであれば,初期値として入力されたrvec, tvecの値を使用し,そこから推定を行う.
iterationCount RANSACの最大反復回数.多いほど推定結果が高精度になり得るが,処理時間も伸びる.
reprojectionError RANSACで用いられるインライアしきい値.投影後の誤差がこれ以下の点はインライアとして扱われる.小さいほど外れ値を除きやすいが大量の点が必要になる.
minInliersCount この点数以上のインライアが得られれば計算を終了する.
inliers 最終推定結果においてインライアとなった点集合のインデクス.
flags PnP問題を解く手法.solvePnPと共通.CV_ITERATIVEならLevenberg-Marquardt法を使って投影誤差を最小化する.CV_P3PならGaoらによる最適化手法,CV_EPNPならNoguerらによる最適化手法を用いる.



FlannBasedMatcherの詳細の説明

OpenCVには特徴量の対応付けのためのDescriptorMatcherというインタフェースが用意されています.これを継承したクラスにはBFMatcherとFlannBasedMatcherがあります.BFMatcherは総当たりで最も良い対応付けを求めるので点数に応じて処理が非常に重くなります.FlannBasedMatcherはFlann(近似最近傍探索ライブラリ)を使ったマッチングを提供します.FlannBasedMatcherにはBFMatcherに比べて非常に効率的なマッチング手法が用意されているうえに,細かなパラメータ調整も可能なため,基本的にはBGMatcherよりもこちらを使った方がよいです.

FlannBasedMatcherはコンストラクタで探索手法・パラメータを指定します.

FlannBasedMatcher(const Ptr<flann::IndexParams>& indexParams=new flann::KDTreeIndexParams(), const Ptr<flann::SearchParams>& searchParams=new flann::SearchParams() );

コンストラクタの引数
indexParams Flannの探索手法を選択します.LinearIndexParams()では線形探索を行います.点数に比例した処理時間がかかりますが,オーバーヘッドが少ないので点数が少ない場合には他の手法よりも高速になる可能性があります.KDTreeIndexParamsではkd-reeを使った探索を行います.KMeansIndexParamsでは階層的K-meansを使った探索を行います.CompositeIndexParamsではkd-treeとK-meansを組み合わせた手法が使われます.LshIndexParamsではlocality sensitive hashingを使った探索が行われます.この手法は特殊なハッシュ関数を使うことで非常に効率的な探索を行うことができますが,得られる結果は最適なものとなることは保証されておらず,あくまで近似解となります.AutotunedIndexParamsでは引数に探索精度,インデックス構築時間,メモリ効率などを入力することで適切な手法を選択して探索が行われます.
searchParams このパラメータの引数に木を再帰的に探索する回数を指定します.大きな引数を与えれば正確な探索解が得られますが,長い処理時間がかかります.AutotunedIndexParamsを選択していた場合には適切なパラメータが自動的に使用され,この引数の値は無視されます.

個人的にはデータ点数が非常に小さい場合にはLinearIndexParams,ほどほどの時にはKDTreeIndexParams,非常に多い場合にはLshIndexParamsとかを使うと良いように思います.ただ,実際にはパフォーマンスを解析を行ったうえで一番いいものを選ぶべきだとは思います.

FlannBasedMatcherは事前にaddメソッドで学習データを登録しておいて,後でmatchメソッドにクエリデータを渡してマッチングを行います.今回の場合であればマーカ画像は固定なのではじめのaddメソッド時にFlannのインデックスが作成されて後は,それを使った効率的なマッチングが利用できます.


処理結果の動画はこんな感じです.右側がマーカ画像(エアフェスタ浜松で撮った写真)で,左側がカメラから得られた画像です.カラフルな丸・線は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距離の総和を基に,マーカが画面内に入っているかどうかの判定を行っています.ただしきい値選択がかなりシビアな感じであまり良くない感じだったのでもっといい方法にしたいです.

1件のコメント

  1. […] 追記:バージョンアップ?しました. […]

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です