2010/05/22

OpenCVのカメラキャリブレーションとARもどき

前回の続きです。

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

2010/05/19

マーカーの検出プログラム(ARToolKit風)

最近、マーカーの認識の実験をしてました。
マーカーを認識して、そのマーカーボードに垂直な線を出せるようになりました。

速度は遅いですが、ARToolKit風のことをOpenCVのみでやってみたいと思います。

ひとまず、写真1枚で行った実験画像。
2ピクセルぐらいの緑色の線がZ軸になります。
カメラのキャリブレーションが済んでないので、若干ずれてはいますが。




ではまた。

2010/05/11

MacOSXでOpenCV2.0を使う(Mac Ports)

以前、MacでOpenCVを使おうと、Mac PortsでOpenCVをインストールしたら、
バージョンが1.0だったので放置してました。(2.0が使いたかったのです)

久しぶりにOpenCVのwikiを見たら、Portsに2.0がチェックインされたようなので、早速インストールしてみました。
http://opencv.willowgarage.com/wiki/Mac_OS_X_OpenCV_Port

ターミナルを開いて、下記のコマンドを入れるだけです。
sudo port selfupdate
sudo port install opencv

ネットを巡回しつつ、2時間ぐらい放っておいたら、自動的にインストールが完了してました。

らくちんすぎる!
ありがとう Portsの中の人。


また、XCodeでOpenCVを使ったプログラムを作るときの手順は下記の通りです。
(Wikiの内容を翻訳してみました)

1.XCodeのプロジェクトを、「Command Line Utility -> Standard」テンプレートで作成します。
2.プロジェクトの名前を付けます。
3.「グループとファイル」の、プロジェクト名を右クリックして、「情報を見る」を選択します。
4.「ビルド」タブを選んで、構成を「すべての構成」にします。
5.「アーキテクチャ」の、「有効なアーキテクチャ」から、すべてのPPCを削除します。残るのはi386と x86_64になると思います。
6.下の方にスクロールしていき、「検索パス」の「ヘッダ検索パス」に、 /opt/local/include/opencvを追加します。
7.プロジェクト情報を閉じます。
8.「グループとファイル」のところで、グループを新規に追加して、名前をOpenCVにします。
9.OpenCVグループフォルダを右クリックし、「追加」->「既存のフレームワーク」
10.Finderの画面が出たら、 / を押します。押すとプロンプトが出るので、 /opt/local/libを入力します。
11.libcxcore.dylib、libcvaux.dylib、libcv.dylib、libhighgui.dylib、libml.dylib を選びます。
(私の環境ではlibcxcore.4.dylib等を選びました。)
12.「コピーする」のチェックを外して、「追加」を押します。

あとはいつもと同じようにコーディングします。

//MacでOpenCVを使ってみるテスト

#include 
#include "cv.h"
#include "highgui.h"

int main (int argc, const char * argv[]) {
    IplImage * hoge;
 hoge= cvLoadImage("/Users/NEKO_1.jpg",1);

 //ウィンドウを作成して画像を表示
 cvNamedWindow("W",1);
 cvShowImage("W",hoge);
 
 //10秒間待機する
 cvWaitKey(10000);
 
 //終わったら後片付け
 cvReleaseImage(&hoge);
 cvDestroyWindow("W");
 
 return 0;
}

Windowsのときより設定項目が若干多いですが、慣れれば気にならなくなるのかなぁ。。。

2010/05/08

OpenCV 新しい表記方法

OpenCVのリファレンスを見ていたら、OpenCV2.0からは新しいインターフェースが
実装されたとの事です。
http://opencv.jp/opencv-2svn/cpp/

サンプルコードを見ると、メモリの開放とかも自動的に行ってくれるように
なったようです。 こりゃ楽チンだ~

今後はこっちの表記を使っていきます。

//
//カメラからの入力画像をもとにエッジ抽出を行う。
//

#include "cv.h"
#include "highgui.h"

using namespace cv;

int main(int, char**)
{
    VideoCapture cap(0); // カメラを適当に選ぶ
    if(!cap.isOpened())  // カメラが動作してるかチェック
        return -1;

    Mat edges;
    namedWindow("edges",1);
    for(;;)
    {
        Mat frame;
        cap >> frame; // カメラからフレームに転送
        cvtColor(frame, edges, CV_BGR2GRAY);
        GaussianBlur(edges, edges, Size(7,7), 1.5, 1.5);
        Canny(edges, edges, 0, 30, 3);
        imshow("edges", edges);
        if(waitKey(30) >= 0) break;
    }
    return 0;
}

2010/05/01

OpenCVで定点観測システム

OpenCVでWebカメラからキャプチャ出来る、ということでちょっと思いついたのがコレです。
被写体はペットでも盆栽でも作物でも何でもよいです。

定点監視システム
目的:とあるモノを定期的に撮影し、保存する。

必要なもの:
1.Webサーバー(外部に公開する場合)
FTPに対応していて、画像がアップロードできるWebサーバーなら何でもよいです。

2.Webカメラ
PCに接続できるカメラであれば特に問題ないと思います。

3.キャプチャ用PC
キャプチャは頻繁に行わないので(1時間に1回とか)、一昔前のノートPCでも大丈夫だと思います。

仕組み:

・OpenCVでcvCreateCameraCapture()を使用してキャプチャし、1フレームだけcvSaveImageで保存する。 保存後、アプリケーションは自動的に終了する。

・下記のような内容のバッチファイルを作成し、タスクを利用してノートPC上で定期的に実行する。
 -OpenCVでキャプチャするアプリケーションを実行
 -保存されたファイルをFTPコマンドを使用してWebサーバにアップロード
タスクの実行間隔時間を調整すれば、1日単位でも1分単位でも定点監視が出来るでしょう。


以上で簡単な定点観測システムが出来ると思います。

一番単純なパターンだと、同じファイル名で保存してそのファイルをアップロードする形になるので、ディスク上には常に最新のデータのみ残る形となります。

過去の記録を残したい場合は、c++でGetLocalTimeしたものをsprintfで整形してファイル名にするか、バッチファイル内でcopyコマンドを使用してファイル名に日付や時間を入れるといいと思います。