「ソースコード」タグアーカイブ

CamShift

ちょっとCamShiftを使った物体追跡プログラムを書いてみました．

CamShiftはMeanShiftTrackingを改良したもので，物体の大きさや姿勢の変化も追跡することができます．
触った感じだと，CamShiftの結果はRotatedRectで帰ってくるのに対して入力には通常のRectを入れなければならないので，若干扱いづらい部分がありした．RotatedRectに外接する矩形をそのまま次の初期探索領域にすると，回転によって領域が大きくなりすぎがちだったため，今回は外接する矩形を少し小さくしたものを次の初期探索領域としています．
あと，ヒストグラムにも割りと敏感なので注意してやる必要がありそうです．

ソース(VS2013, OpenCV2.4.8)

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

/********************************
 * 初期矩形指定用マウスコールバック
********************************/
void onmouse(int event, int x, int y, int flags, void* userdata) {
	static bool lbutton_is_down = false;
	cv::Rect* rect = (cv::Rect*)userdata;

	switch (event) {
	case CV_EVENT_LBUTTONDOWN:
		lbutton_is_down = true;
		rect->x = x;
		rect->y = y;
		break;
	case CV_EVENT_LBUTTONUP:
		lbutton_is_down = false;
		break;
	case CV_EVENT_MOUSEMOVE:
		if (lbutton_is_down) {
			rect->width = x - rect->x;
			rect->height = y - rect->y;
		}
		break;
	}
}

/********************************
* 初期矩形設定
********************************/
cv::Rect getInitRect(const cv::Mat& frame) {
	cv::Rect init_rect(0, 0, 100, 100);
	cv::namedWindow("frame");
	cv::setMouseCallback("frame", onmouse, &init_rect);

	while (cv::waitKey(100) != ' ') {
		cv::Mat canvas = frame.clone();
		cv::rectangle(canvas, init_rect, cv::Scalar(0, 255, 0), 2);
		cv::imshow("frame", canvas);
	}
	return init_rect;
}

/********************************
 * main
********************************/
int main(int argc, char** argv) {
	cv::VideoCapture cap("mov03.mov");
	if (!cap.isOpened()) {
		std::cerr << "failed to open input file" << std::endl;
		std::cin.ignore(1);
		return 0;
	}

	cv::Mat frame, hist;
	cap >> frame;
	cv::Rect rect = getInitRect(frame);	// 初期探索矩形

	// ターゲットのヒストグラム生成
	int hist_size[] = { 64, 64, 64 };
	float range[] = { 0, 256 };
	const float* ranges[] = { range, range, range };
	int channels[] = { 0, 1, 2 };
	cv::Mat roi(frame, rect);
	cv::calcHist(&roi, 1, channels, cv::Mat(), hist, 3, hist_size, ranges);

	// メインループ, escが押されるまで
	while (cv::waitKey(66) != 0x1b) {
		cap >> frame;
		if (!frame.data) {
			continue;
		}

		// ヒストグラムのバックプロジェクション計算
		cv::Mat back_proj;
		cv::calcBackProject(&frame, 1, channels, hist, back_proj, ranges);
		// camshift
		cv::TermCriteria term_crit(cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, 1);
		cv::RotatedRect rotated_rect = cv::CamShift(back_proj, rect, term_crit);

		// RotatedRectから次の探索矩形，boundingboxを少し小さくしたもの
		rect = rotated_rect.boundingRect();
		rect.x += rect.width / 8;
		rect.y += rect.height / 8;
		rect.width *= (6.0 / 8.0);
		rect.height *= (6.0 / 8.0);

		// RotatedRectから頂点座標
		std::vector<cv::Point2f> pts(4);
		rotated_rect.points(&pts[0]);
		std::vector<cv::Point> ptsi(4);
		std::copy(pts.begin(), pts.end(), ptsi.begin());

		// 描画
		cv::rectangle(frame, rect, cv::Scalar(255, 0, 0), 2);
		cv::polylines(frame, ptsi, true, cv::Scalar(0, 255, 0), 2);
		cv::imshow("frame", frame);
	}
}

矩形検出

コメントを残す

ちょっとやってみたい事があるので，その準備として画像中の矩形検出処理を書いてみました．処理手順は単純に

画像を２値化
輪郭線検出
輪郭線を近似して，四角形になっているか判定

のような感じです．
四角形かどうかの判定は4点かつ，面積が一定以上かどうかで判定しています．
追加で凸形になっているかを調べるともっといいかもしれません．

緑が検出された輪郭線．青がその中で矩形っぽいもの．

2014/03/20 ソースちょっと修正

/***************************
* 矩形検出
* @author : tetro
***************************/
#include <vector>
#include <sstream>
#include <iostream>
#include <opencv2/opencv.hpp>

/***************************
* main
***************************/
int main(){
	cv::VideoCapture cap(0);
	// esc を押すまで
	while (cv::waitKey(5) != 0x1b){
		cv::Mat frame;
		cap >> frame;
		// キャプチャできていなければ処理を飛ばす
		if (!frame.data){
			continue;
		}

		// 2値化
		cv::Mat grayImage, binImage;
		cv::cvtColor(frame, grayImage, CV_BGR2GRAY);
		cv::threshold(grayImage, binImage, 128.0, 255.0, CV_THRESH_OTSU);
		cv::imshow("bin", binImage);

		// 輪郭抽出
		std::vector< std::vector< cv::Point > > contours;
		cv::findContours(binImage, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
		// 検出された輪郭線の描画
		for (auto contour = contours.begin(); contour != contours.end(); contour++){
			cv::polylines(frame, *contour, true, cv::Scalar(0, 255, 0), 2);
		}

		// 輪郭が四角形かの判定
		for (auto contour = contours.begin(); contour != contours.end(); contour++){
			// 輪郭を直線近似
			std::vector< cv::Point > approx;
			cv::approxPolyDP(cv::Mat(*contour), approx, 50.0, true);
			// 近似が4線かつ面積が一定以上なら四角形
			double area = cv::contourArea(approx);
			if (approx.size() == 4 && area > 1000.0){
				cv::polylines(frame, approx, true, cv::Scalar(255, 0, 0), 2);
				std::stringstream sst;
				sst << "area : " << area;
				cv::putText(frame, sst.str(), approx[0], CV_FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 128, 0));
			}
		}
		cv::imshow("frame", frame);
	}
}

SURFとSolvePnPRansacで拡張現実

コメントを残す

追記：バージョンアップ？しました．

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 );
}