OpenCVには、カメラの内部パラメータ(ゆがみ等)を求める関数があります。(CalibrateCamera2)
また、3次元空間中の点を画像平面へ投影するProjectPoints2という関数もあり、
今回はこの2つを使って、キャリブレーションパターン(チェスボードのような格子状のもの)の平面上に
垂直な立方体を作ってみました。
出来上がったものは下記のような感じになります。
写真の上では、チェスボードのパターン画像を複数枚取得し、それぞれの位置を記憶します。
記憶した後、CalibrateCamera2を使ってカメラの内部パラメータを求めます。
カメラの内部パラメータは一定なので、1度求めてしまえば下記のように保存、読み込みが可能です。
//保存
cvSave("Intrinsics.xml",intrinsic_matrix);
cvSave("Distortion.xml",distortion_coeffs);
//読み込み
CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");
内部パラメータが求まれば、あとはチェスボードの位置、傾きなどの外部パラメータ(回転ベクトル、推進ベクトル)を毎回求めて、ProjectPoints2に現実空間での座標と内部・外部パラメータを渡してあげれば、
画面上の座標が求められます。
あとは適当に線を引くだけです。
コレとOpenGLを組み合わせれば、パターン上にいろんな物体を描画できるようになると思います。
ただ、パターン検出にかかる時間が100~200msぐらいかかるので、これを何とか短縮できないかな~と考えてます。
今回はオライリーから出ている「詳解 OpenCV」と「ゲーム3D数学」にお世話になりました。
画像処理には行列が必ず出てくるのですが、行列をすっかり忘れていたのでまた最初から学びなおしでした。
今回のソースコードは以下の通りとなります。
////////////////////////////////////// // // OpenCVで遊ぼう! // http://playwithopencv.blogspot.com/ // //CameraCalib.cpp //チェスボードに垂直な立方体を描画します。 // //処理の概要 //1.複数枚のチェスボードの画像を取得し、カメラの内部パラメータを求めます。 //2.求めた内部パラメータを元に、立方体を描画します。 // // // #include "stdafx.h" #include "cv.h" #include "highgui.h" using namespace System; #define IMAGE_NUM (10) /* 記憶させる画像数 */ #define PAT_ROW (7) /* パターンの行数 */ #define PAT_COL (10) /* パターンの列数 */ #define PAT_SIZE (PAT_ROW*PAT_COL) #define ALL_POINTS (IMAGE_NUM*PAT_SIZE) #define CHESS_SIZE (20) /* パターン1マスの1辺サイズ[mm] */ int main(array<System::String ^> ^args) { // Console::WriteLine(L"Hello World"); //return 0; int i, j, k; int corner_count, found; int p_count[IMAGE_NUM]; char presskey; IplImage *src_img; CvSize pattern_size = cvSize (PAT_COL, PAT_ROW); CvPoint3D32f objects[ALL_POINTS]; CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * ALL_POINTS); CvMat object_points; CvMat image_points; CvMat point_counts; CvMat *intrinsic = cvCreateMat (3, 3, CV_32FC1); CvMat *distortion = cvCreateMat (1, 4, CV_32FC1); CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); CvMat *translation = cvCreateMat (1 , 3, CV_32FC1); //立方体生成用 CvMat *srcPoints3D = cvCreateMat (8, 1, CV_32FC3);//元の3次元座標 CvMat *dstPoints2D = cvCreateMat (8, 1, CV_32FC3);//画面に投影したときの2次元座標 //立方体の座標を決める。 int cube_size=5; //1辺の長さをチェスボードのマス5個分に設定 for (i=0;i<8;i++) { switch (i) { case 0: srcPoints3D->data.fl[0] =0; srcPoints3D->data.fl[1] =0; srcPoints3D->data.fl[2] =0; break; case 1: srcPoints3D->data.fl[0+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[1+i*3] =0; srcPoints3D->data.fl[2+i*3] =0; break; case 2: srcPoints3D->data.fl[0+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[1+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[2+i*3] =0; break; case 3: srcPoints3D->data.fl[0+i*3] =0; srcPoints3D->data.fl[1+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[2+i*3] =0; break; //後ろ case 4: srcPoints3D->data.fl[0+i*3] =0; srcPoints3D->data.fl[1+i*3] =0; srcPoints3D->data.fl[2+i*3] =(float)cube_size*CHESS_SIZE; break; case 5: srcPoints3D->data.fl[0+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[1+i*3] =0; srcPoints3D->data.fl[2+i*3] =(float)cube_size*CHESS_SIZE; break; case 6: srcPoints3D->data.fl[0+i*3] =(float)cube_size*CHESS_SIZE;; srcPoints3D->data.fl[1+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[2+i*3] =(float)cube_size*CHESS_SIZE; break; case 7: srcPoints3D->data.fl[0+i*3] =0; srcPoints3D->data.fl[1+i*3] =(float)cube_size*CHESS_SIZE; srcPoints3D->data.fl[2+i*3] =(float)cube_size*CHESS_SIZE; break; default :srcPoints3D->data.fl[0] =0; srcPoints3D->data.fl[1] =0; srcPoints3D->data.fl[2] =0; break; } } //カメラからの取り込み処理 IplImage* cap_img; CvCapture *capture; //開放したり、変更したりするとエラーが起きる capture=cvCreateCameraCapture(0); cap_img=cvQueryFrame(capture); src_img=cvCreateImage(cvGetSize(cap_img),IPL_DEPTH_8U,3); src_img=cvCloneImage(cap_img); IplImage *src_gray = cvCreateImage (cvGetSize (src_img), IPL_DEPTH_8U, 1); //処理時間計測用 double process_time; process_time=(double)cvGetTickCount(); //チェスボードのコーナー検出 int found_num = 0; cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE); while(found_num<IMAGE_NUM) { cap_img=cvQueryFrame(capture); src_img=cvCloneImage(cap_img); found = cvFindChessboardCorners (src_img, pattern_size, &corners[found_num * PAT_SIZE], &corner_count); if (found) { process_time = (double)cvGetTickCount() - process_time; printf( "コーナーを見つけました 処理時間 time = %gms\n", process_time/(cvGetTickFrequency()*1000.)); process_time = (double)cvGetTickCount(); cvCvtColor (src_img, src_gray, CV_BGR2GRAY); cvFindCornerSubPix (src_gray, &corners[found_num * PAT_SIZE], corner_count, cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); cvDrawChessboardCorners (src_img, pattern_size, &corners[found_num * PAT_SIZE], corner_count, found); p_count[found_num] = corner_count; found_num++; } else { //何もしない process_time = (double)cvGetTickCount() - process_time; printf( "コーナーをが見つかりませんでした 処理時間 time = %gms\n", process_time/(cvGetTickFrequency()*1000.)); process_time = (double)cvGetTickCount(); } cvShowImage ("Calibration", src_img); presskey=cvWaitKey (200); if(presskey==27) { break; } } if (found_num != IMAGE_NUM) return -1; cvInitMatHeader (&image_points, ALL_POINTS, 1, CV_32FC2, corners); cvInitMatHeader (&point_counts, IMAGE_NUM, 1, CV_32SC1, p_count); //チェスボードの物理空間での座標設定 //各コーナーが物理空間上ではどの座標になるかを指定する。 //例:チェスボードのマスのサイズが20mmの場合 //コーナー 実際の座標(mm) // X Y X Y // 0 0 = 0 0 // 0 1 = 0 20 // 1 0 = 20 0 for (i = 0; i < IMAGE_NUM; i++) { for (j = 0; j < PAT_ROW; j++) { for (k = 0; k < PAT_COL; k++) { objects[i * PAT_SIZE + j * PAT_COL + k].x =(float) j * CHESS_SIZE; objects[i * PAT_SIZE + j * PAT_COL + k].y =(float) k * CHESS_SIZE; objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0; } } } cvInitMatHeader (&object_points, ALL_POINTS, 3, CV_32FC1, objects); cvSave("object_points.txt",&object_points); cvSave("image_poits.txt",&image_points); cvSave("point_counts.txt",&point_counts); //内部パラメータを求める。 //毎回変わるrotation とtranslationは今回は求めなくてよい cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvGetSize(src_img), intrinsic, distortion,NULL,NULL); cvFree(&corners); //***************************************************** //ここまでで内部パラメータの算出が完了したので、 //算出したパラメータを基に、これ以降撮影する画像に立方体を描画する。 // //チェスボード1枚分 CvPoint3D32f objects_forLoop[PAT_SIZE]; CvPoint2D32f *corners_forLoop =(CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * PAT_SIZE); for (i = 0; i< PAT_ROW; i++) { for (j = 0; j < PAT_COL; j++) { objects_forLoop[i * PAT_COL + j].x =(float) i * CHESS_SIZE; objects_forLoop[i * PAT_COL + j].y =(float) j * CHESS_SIZE; objects_forLoop[i * PAT_COL + j].z = 0.0; } } CvPoint startpoint; CvPoint endpoint; while(1) { cap_img=cvQueryFrame(capture); src_img=cvCloneImage(cap_img); found = cvFindChessboardCorners (src_img, pattern_size, &corners_forLoop[0], &corner_count); if (found) { process_time = (double)cvGetTickCount() - process_time; printf( "コーナーを見つけました 処理時間 time = %gms\n", process_time/(cvGetTickFrequency()*1000.)); process_time = (double)cvGetTickCount(); cvCvtColor (src_img, src_gray, CV_BGR2GRAY); cvFindCornerSubPix (src_gray, &corners_forLoop[0], corner_count, cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03)); // cvDrawChessboardCorners (src_img, pattern_size, &corners_forLoop[0], corner_count, found); //チェスボードの現実空間での座標を設定。 cvInitMatHeader (&image_points, PAT_SIZE, 1, CV_32FC2, corners_forLoop); cvInitMatHeader (&object_points, PAT_SIZE, 3, CV_32FC1, objects_forLoop); //調整用の画像で求めたカメラの内部定数(intrinsticとdistortion)から、rotationとtranslationを求める cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic,distortion,rotation,translation); //求めたものを使用して、現実空間上の座標が画面上だとどの位置に来るかを計算 cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic,distortion,dstPoints2D); // cvSave("srcPoints3D.txt",srcPoints3D); // cvSave("dstPoints2D.txt",dstPoints2D); // cvSave("intrinsic.txt",intrinsic); // cvSave("distortion.txt",distortion); // cvSave("rotation.txt",rotation); // cvSave("translation.txt",translation);//-1.32703932e-003, -1.32703932e-003, -1.32703932e-003 //立方体を描画 //点0から3が立方体の前面、点4から7が立方体の奥面。 //まずは前面と奥面を書く for(i=0;i<2;i++) { for(j=0;j<4;j++) { if(j==3) { startpoint=cvPoint((int)dstPoints2D->data.fl[0+(i*4+j)*3],(int)dstPoints2D->data.fl[1+(i*4+j)*3]); endpoint= cvPoint((int)dstPoints2D->data.fl[0+(i*4+0)*3],(int)dstPoints2D->data.fl[1+(i*4+0)*3]); } else { startpoint=cvPoint((int)dstPoints2D->data.fl[0+(i*4+j)*3], (int)dstPoints2D->data.fl[1+(i*4+j)*3]); endpoint= cvPoint((int)dstPoints2D->data.fl[0+(i*4+j+1)*3],(int)dstPoints2D->data.fl[1+(i*4+j+1)*3]); } cvLine(src_img,startpoint,endpoint,cvScalar(0,255,0),2,8,0); } } //2つの面をつなぐ線を描く //0-4 1-5 2-6 3-7 for(i=0;i<4;i++) { startpoint=cvPoint((int)dstPoints2D->data.fl[0+(i )*3],(int)dstPoints2D->data.fl[1+(i )*3]); endpoint= cvPoint((int)dstPoints2D->data.fl[0+(i+4)*3],(int)dstPoints2D->data.fl[1+(i+4 )*3]); cvLine(src_img,startpoint,endpoint,cvScalar(0,255,0),2,8,0); } } else { //何もしない process_time = (double)cvGetTickCount() - process_time; printf( "コーナーをが見つかりませんでした 処理時間 time = %gms\n", process_time/(cvGetTickFrequency()*1000.)); process_time = (double)cvGetTickCount(); } cvShowImage ("Calibration", src_img); presskey=cvWaitKey (20); if(presskey==27) { break; } } cvReleaseImage(&src_img); cvReleaseImage(&src_gray); cvReleaseMat(&distortion); cvReleaseMat(&intrinsic); cvReleaseMat(&rotation); cvReleaseMat(&translation); cvReleaseMat(&srcPoints3D); cvReleaseMat(&dstPoints2D); cvDestroyWindow ("Calibration"); return 0; }
iPhone(iOS)上でARマーカーを検出するのに、この記事がすごく参考になりました。ありがとうございます!
返信削除