プログラミングに関する私的メモ（現在：OpenGL・OpenCV・ARToolKit・Kinect・Unity）

カメラキャリブレーション編集する

最終更新： pafuhana1213 pafuhana1213 2012年07月13日(金) 14:19:58履歴

自分用・OpenCV2.3用

//プロジェクトのプロパティ⇒C/C++⇒全般　の追加のインクルードディレクトリに
// 『C:\OpenCV2.3\include』を追加のこと
#include "opencv2\\opencv.hpp"

#include <stdio.h>
#include <string.h>
#include <time.h>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <iomanip>

#ifdef _DEBUG
//Debugモードの場合
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_core231d.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_imgproc231d.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_highgui231d.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_calib3d231d.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_features2d231d.lib")
#else
//Releaseモードの場合
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_core231.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_imgcccccccproc231.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_highgui231.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_calib3231d.lib")
#pragma comment(lib,"C:\\OpenCV2.3\\lib\\opencv_features2d231.lib")
#endif

//using namespace cv;
using namespace std;

namespace
{
	static const string WINDOWNAME_SOURCE               = "Source Image";
	static const string SAVEIMAGE_BASENAME		         = "source";
	static const string SAVEIMAGE_EXT                   = ".jpg";

	static const string CAMERAPARAM_XMLPATH             = "CAM_PARAM_MAT.xml";    // カメラのパラメータ情報出力パス
	static const string XMLTAG_CAMERAMAT                = "CameraMatrix";       // カメラ行列のタグ
	static const string XMLTAG_DISTCOEFFS               = "DistortCoeffs";      // 歪み係数のタグ

	enum eCheck{
		CHESS,
		CIRCLES
	};

	static eCheck checkPattern = CHESS; 
}


int InitCamera( cv::VideoCapture* in_cap,int in_width, int in_height )
{
	//取込サイズの設定
	in_cap->set(CV_CAP_PROP_FRAME_WIDTH, in_width);
	in_cap->set(CV_CAP_PROP_FRAME_HEIGHT, in_height);
	// カメラがオープンできたかの確認
	if(!in_cap->isOpened()) return -1;

	return 0;
}

int CaptureBoardImage( cv::VideoCapture* in_cap, int* io_imgNum )
{
	cv::Mat			frame;
	*in_cap >> frame;  // キャプチャ

	cv::imshow("Capture", frame); // 表示

	//キー入力
	int key = cv::waitKey(30);

	// c:キャプチャ
	if( key == 'c'){
		//　キャプチャ画像名決定
		stringstream	stream;
		stream << SAVEIMAGE_BASENAME <<  *io_imgNum << SAVEIMAGE_EXT;
		string  fileName = stream.str();
		//　キャプチャ画像出力
		cv::imwrite( fileName, frame );
		cout << "Save marker image:" << fileName << endl;
		(*io_imgNum)++;

	}
	// q:キャプチャ終了
	else if( key == 'q' ){
		cout << "Capture End"<< endl;
		return 1;
	}
	// esc:プログラム終了
	else if( key == '\x1b' ){
		return -1;
	}

	return 0;
}

void ReadBoardImage( int* in_imgNum, vector<cv::Mat>* io_checkerImgs )
{
	for( int i = 0; i < (*in_imgNum); i++ ){
		stringstream	stream;
		stream << SAVEIMAGE_BASENAME <<  i << SAVEIMAGE_EXT;
		string  fileName = stream.str();
		io_checkerImgs->push_back( cv::imread( fileName ) );
		cout << "Load marker image: " << fileName << endl;
	}
}


int CalcBoardCorners( int* in_imgNum, vector<cv::Mat> io_checkerImgs, const cv::Size* in_patternSize, vector<vector<cv::Point2f>>* io_imagePoints)
{
	int flag =0;

	cv::namedWindow( WINDOWNAME_SOURCE, CV_WINDOW_AUTOSIZE );

	for( int i = 0; i < (*in_imgNum); i++ ){
		vector<cv::Point2f> tmpImgPoints;
		if( checkPattern == CHESS ){
			if( findChessboardCorners( io_checkerImgs[i], (*in_patternSize), tmpImgPoints,
				CV_CALIB_CB_ADAPTIVE_THRESH + CV_CALIB_CB_NORMALIZE_IMAGE + CV_CALIB_CB_FAST_CHECK ) ){
					printf("*********");
					io_imagePoints->push_back(tmpImgPoints);
					cout << " Success: " << i << endl;
					flag++;
					// 検出点を描画する
					cv::drawChessboardCorners( io_checkerImgs[i], (*in_patternSize), ( cv::Mat )( tmpImgPoints ), true );
					cv::imshow( WINDOWNAME_SOURCE, io_checkerImgs[i] );
					cv::waitKey( 0 );
			}
			else{
				cout << "Fail: " << i << endl;
			}
		}else if( checkPattern == CIRCLES ){
			if(findCirclesGrid(io_checkerImgs[i], (*in_patternSize), tmpImgPoints)){
				printf("*********");
				io_imagePoints->push_back(tmpImgPoints);
				cout << " Success: " << i << endl;
				flag++;
				// 検出点を描画する
				cv::drawChessboardCorners( io_checkerImgs[i], (*in_patternSize), ( cv::Mat )( tmpImgPoints ), true );
				cv::imshow( WINDOWNAME_SOURCE, io_checkerImgs[i] );
				cv::waitKey( 0 );
			}
			else{
				cout << "Fail: " << i << endl;
			}
		}
	}

	if(flag == 0) return -1;
	else return 0;
}

void CalcWorldPoints( vector<vector<cv::Point2f>>* in_imagePoints, const cv::Size* in_patternSize, 
	vector<vector<cv::Point3f>>* io_worldPoints, int* in_checkSize )
{
	// 利用するチェッカーパターンの数
	const size_t numberOfCheckerPatterns = in_imagePoints->size();

	(*io_worldPoints).resize(numberOfCheckerPatterns);
	for( size_t imgIdx = 0; imgIdx < numberOfCheckerPatterns; imgIdx++ )
	{
		for( int areaCnt = 0 ; areaCnt < (*in_patternSize).area(); areaCnt++ )
		{
			(*io_worldPoints)[imgIdx].push_back( cv::Point3f(
				static_cast<float>( areaCnt % (*in_patternSize).width * (*in_checkSize) ),
				static_cast<float>( areaCnt / (*in_patternSize).width * (*in_checkSize) ),
				0.0f ) );
		}
	}
}

int main(int argc, char* argv[])
{

	int imgNum = 0;
	int camId = 0, camWidth = 640, camHeight = 480;
	int checkSize = 24, checkPointX = 10, checkPointY = 7;
	int pattern = CHESS;
	if(pattern == CHESS)	checkPattern = CHESS;
	else if(pattern == CIRCLES) checkPattern = CIRCLES;

	//カメラの初期化（カメラの選択）
	cv::VideoCapture cap(camId);
	if( -1 == InitCamera( &cap, camWidth, camHeight ) ){
		cout << "ERROR:カメラ初期化" << endl;
		return -1;
	}
	cout << "Initialize Camera" << endl;
	cout << "Key c:capture q:finish ESC:program end" << endl;
	//ウィンドウの表示
	cv::namedWindow("Capture", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);

	// キャリブレーション用画像キャプチャ
	while(1) {
		int flag = CaptureBoardImage( &cap, &imgNum );
		if( flag == 1 )		break;
		if( flag == -1 )	return 0;
	}
	//後始末
	cvDestroyWindow ("Capture");

	/***********************************************************************/

	vector<cv::Mat>				checkerImgs;
	const cv::Size				patternSize( checkPointX, checkPointY );
	vector<vector<cv::Point2f>> imagePoints;
	vector<vector<cv::Point3f>> worldPoints;

	// キャプチャ(マーカ)画像読み込み
	ReadBoardImage( &imgNum, &checkerImgs );

	// チェックパターンの交点座標を求め，imagePointsに格納する
	if( -1 == CalcBoardCorners( &imgNum, checkerImgs, &patternSize, &imagePoints )){
		return 0;
	}

	// チェッカーパターンの交点の世界座標を決定する
	CalcWorldPoints( &imagePoints, &patternSize, &worldPoints, &checkSize );


	/***********************************************************************/

	// カメラパラメータ行列
	cv::Mat         cameraMatrix;       // 内部パラメータ行列
	cv::Mat         distCoeffs;         // レンズ歪み行列
	vector<cv::Mat> rotationVectors;    // 画像ごとに得られる回転ベクトル
	vector<cv::Mat> translationVectors; // 画像ごとに得られる平行移動ベクトル

	cout << "Start Calibration " << endl;

	// XMLファイルから前回の結果を取得する
	cv::FileStorage rfs(CAMERAPARAM_XMLPATH, cv::FileStorage::READ);
	bool bExistCameraParam = rfs.isOpened();
	if (bExistCameraParam){
		rfs[XMLTAG_CAMERAMAT] >> cameraMatrix;
		rfs[XMLTAG_DISTCOEFFS] >> distCoeffs;
		rfs.release();

		// 前回のカメラパラメータを初期値に再推定
		// 必要に応じて CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6  を flag に指定してください
		cv::calibrateCamera( worldPoints, imagePoints, checkerImgs[0].size(),
			cameraMatrix, distCoeffs, rotationVectors, translationVectors, CV_CALIB_USE_INTRINSIC_GUESS );
	}
	else{
		// 初回キャリブレーション
		cv::calibrateCamera( worldPoints, imagePoints, checkerImgs[0].size(),
			cameraMatrix, distCoeffs, rotationVectors, translationVectors );
	}

	cout << "Finish Calibration" << endl;

	/***********************************************************************/

	//カメラ内部パラメータの計算
	double apertureWidth = 0, apertureHeight = 0 ;	// センサの物理的な幅・高さ
	double fovx = 0, fovy = 0;						// 出力される水平（垂直）軸にそった画角（度単位）
	double focalLength = 0;							// mm単位で表されたレンズの焦点距離
	cv::Point2d principalPoint = (0,0);				// ピクセル単位で表されたレンズの焦点距離
	double aspectRatio = 0;							// アスペクト比

	cv::calibrationMatrixValues( cameraMatrix, checkerImgs[0].size(), apertureWidth, apertureHeight,
		fovx, fovy, focalLength, principalPoint, aspectRatio );

	cout << "Calc Camera Param" << endl;

	// XMLファイルへ結果を出力する
	cout << "Start Output Xml File" << endl;

	cv::FileStorage wfs(CAMERAPARAM_XMLPATH, cv::FileStorage::WRITE);
	wfs << XMLTAG_CAMERAMAT << cameraMatrix;
	wfs << XMLTAG_DISTCOEFFS << distCoeffs;

	wfs << "aperture_Width" << apertureWidth;
	wfs << "aperture_Height" << apertureHeight;
	wfs << "fov_x" << fovx;
	wfs << "fov_y" << fovy;
	wfs << "focal_Length" << focalLength;
	wfs << "principal_Point" << principalPoint;
	wfs << "aspect_Ratio" << aspectRatio;
	wfs.release();

	cout << "Finish Output Xml File" << endl;

	return 0;
}
カメラキャリブレーション - 私的なプログラミングメモ 先頭へ

このページへのコメント

1Dlz8c Great, thanks for sharing this post. Awesome.

0
Posted by stunning seo guys 2014年02月04日(火) 22:13:38 返信

TS9x6Y A big thank you for your blog.Thanks Again. Awesome.

0
Posted by stunning seo guys 2014年01月22日(水) 03:58:20 返信

gH382I Thanks-a-mundo for the article post. Great.

0
Posted by nice site here 2014年01月18日(土) 04:42:08 返信

r0YvzU Thanks so much for the blog article.Thanks Again. Cool.

0
Posted by check it out 2013年12月31日(火) 12:12:43 返信

Q0BrBS Great, thanks for sharing this blog post.Really thank you! Fantastic.

0
Posted by watch for this 2013年12月20日(金) 07:52:33 返信

コメントをかく


「http://」を含む投稿は禁止されています。

利用規約をご確認のうえご記入下さい
 

最近更新したページ

最新コメント

QRコード

アクセス解析中

管理人/副管理人のみ編集できます