カメラキャリブレーション

■ カメラキャリブレーション

サンプル

カメラキャリブレーション cvCalibrateCamera2, cvFindExtrinsicCameraParams2

Zhangの手法を用いてカメラのキャリブレーションを行い，結果をファイルに保存する

サンプルコード

#include <stdio.h>
#include <cv.h>
#include <highgui.h>

#define IMAGE_NUM  (25)		/* 画像数 */
#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 (24.0)	/* パターン1マスの1辺サイズ[mm] */

int
main (int argc, char *argv[])
{
  int i, j, k;
  int corner_count, found;
  int p_count[IMAGE_NUM];
  IplImage *src_img[IMAGE_NUM];
  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 *rotation = cvCreateMat (1, 3, CV_32FC1);
  CvMat *translation = cvCreateMat (1, 3, CV_32FC1);
  CvMat *distortion = cvCreateMat (1, 4, CV_32FC1);

  // (1)キャリブレーション画像の読み込み
  for (i = 0; i < IMAGE_NUM; i++) {
    char buf[32];
    sprintf (buf, "calib_img/%02d.png", i);
    if ((src_img[i] = cvLoadImage (buf, CV_LOAD_IMAGE_COLOR)) == NULL) {
      fprintf (stderr, "cannot load image file : %s\n", buf);
    }
  }

  // (2)3次元空間座標の設定
  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 = j * CHESS_SIZE;
	objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;
	objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;
      }
    }
  }
  cvInitMatHeader (&object_points, ALL_POINTS, 3, CV_32FC1, objects);

  // (3)チェスボード（キャリブレーションパターン）のコーナー検出
  int found_num = 0;
  cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE);
  for (i = 0; i < IMAGE_NUM; i++) {
    found = cvFindChessboardCorners (src_img[i], pattern_size, &corners[i * PAT_SIZE], &corner_count);
    fprintf (stderr, "%02d...", i);
    if (found) {
      fprintf (stderr, "ok\n");
      found_num++;
    }
    else {
      fprintf (stderr, "fail\n");
    }
    // (4)コーナー位置をサブピクセル精度に修正，描画
    IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1);
    cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY);
    cvFindCornerSubPix (src_gray, &corners[i * PAT_SIZE], corner_count,
			cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
    cvDrawChessboardCorners (src_img[i], pattern_size, &corners[i * PAT_SIZE], corner_count, found);
    p_count[i] = corner_count;
    cvShowImage ("Calibration", src_img[i]);
    cvWaitKey (0);
  }
  cvDestroyWindow ("Calibration");

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

  // (5)内部パラメータ，歪み係数の推定
  cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion);

  // (6)外部パラメータの推定
  CvMat sub_image_points, sub_object_points;
  int base = 0;
  cvGetRows (&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);
  cvGetRows (&object_points, &sub_object_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);
  cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);

  // (7)XMLファイルへの書き出し
  CvFileStorage *fs;
  fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE);
  cvWrite (fs, "intrinsic", intrinsic);
  cvWrite (fs, "rotation", rotation);
  cvWrite (fs, "translation", translation);
  cvWrite (fs, "distortion", distortion);
  cvReleaseFileStorage (&fs);

  for (i = 0; i < IMAGE_NUM; i++) {
    cvReleaseImage (&src_img[i]);
  }

  return 0;
}

// (1)キャリブレーション画像の読み込み
キャリブレーションに必要な画像列を読み込む．ここでは，calib_imgディレクトリ以下に あらかじめ用意した，"00.png"〜"24.png" という名前の画像ファイルを利用する． ここは，画像ファイルを置いたディレクトリやファイル名に合わせて適宜変更すること．
このように，このプログラムはカメラから直接画像を取得するものではないことに注意する． 本プログラムでは，キャリブレーションを行いたいカメラでキャリブレーション用のチェスパターン画像を前もって撮影しておく必要がある． zhangのキャリブレーションに必要な画像枚数（ビューの数）は決まっていないが， 一般的には，ブレなどのない適切な画像を数十枚程度用意すると十分な精度が得られる． 利用する画像数やパターンの行数，列数などの値はプログラムの最初で定義されているので， これらのパラメータも各自の環境に合わせて適宜変更すること．

// (2)3次元空間座標の設定
今回は，1マスの長さが24.0[mm]，内側のコーナー数が7×10のチェスボードパターンを利用した （実際のチェスボードパターンファイル:PDF）． 後述する理由により，コーナー数は，奇数×偶数，あるいは偶数×奇数の組み合わせを用いるのが望ましい． xおよびyの値は，パターンのマスのサイズにあわせて値を代入する． ここで設定される軸方向は，カメラの外部パラメータに影響を与える． また，zはすべて0または1に設定する必要がある．

// (3)チェスボード（キャリブレーションパターン）のコーナー検出
関数cvFindChessboardCorners()により，チェスボードのコーナーを検出する． コーナーは左下から右上に向けて検出され，第2引数で設定されたサイズのコーナーが全て検出された場合には，0以外の値を返す．
ちなみに，本プログラムとはあまり関係ないが，2枚以上のキャリブレーションパターンを認識させたい場合は， （縦横の矩形数が）異なるタイプのキャリブレーションパターンを用意する， あるいは，一つのパターンを検出する度にそこを塗りつぶして再探索する，などの手法が考えられる．
OpenCVの関数cvFindChessboardCorners()が検出するコーナーの順番は，パターンの行と列の定義に依存する． 以下の画像チェスパターンを，サンプルプログラムのように7×10（7行10列）と定義すると， 検出コーナーの最初，つまりキャリブレーション結果のワールド座標原点は，画像の（A)の場所になる． また，同様の画像を10×7（10行7列）と定義すると，原点は（B）となる． 関数cvFindChessboardCorners()は，チェスパターンのコーナーを検出するために矩形の検出を行うが， 矩形（黒色）に斜めに隣接する矩形の数が1つのものがあれば（図中の左上と右上）， その矩形が含むコーナーの最初の点にする．それが存在しない場合は，斜め隣接矩形数が2つのものを用いる． コーナー数が，奇数×偶数，あるいは偶数×奇数の組み合わせのチェスパターンならば， その最初のコーナーが一意に決定されるが，そうでない場合は，コーナーが一意に決定されず， チェスパターンの画像によっては検出されるコーナーの順番が異なってしまう可能性がある．

// (4)コーナー位置をサブピクセル精度に修正，描画
関数cvFindChessboardCorners()においても，0.5[pixel]単位でコーナーが検出されるが， さらに検出されたコーナーをサブピクセル精度で修正する． また，それらのコーナーを画像上に描画し，実際に表示する． すべてのコーナーが正しく検出された場合は内部で定義された7色で描画され，そうでない場合はすべてのコーナーが赤色で描画される． このサンプルコードでは，全ての画像（25枚）でコーナーが正しく検出されない場合は，プログラムが終了する．

// (5)内部パラメータ，歪み係数の推定
関数cvCalibrateCamera2()により，カメラの内部パラメータ，および歪み係数を推定する． 内部パラメータは，以下の式のAにあたる．

// (6)外部パラメータの推定
カメラの外部パラメータを推定する．これは，上述の式の[R|t]にあたる． つまり，三次元のあるワールド座標系原点から，カメラ座標系への変換を表すパラメータである． ワールド座標系原点からの回転および並進を表す外部パラメータを決定するためには，基準となるワールド座標系原点が必要となる． ここでは，base=0として0番目の画像（"00.png"）を指定しており，この画像 に写るパターンの3次元座標系（(2)で与えられた）における外部パラメータが推定される． この際のワールド座標系の原点は，(3)の項で説明したように決定される． このサンプルプログラムでは，実行結果例にある画像の赤色の行の最初のコーナーが原点となる．

// (7)XMLファイルへの書き出し
求められた，内部パラメータ，外部パラメータ，歪み係数をファイルに書き出す． ここでは，拡張子によりXML形式を指定しているが，YAML形式で出力することも可能である．

実行結果例

歪み補正 cvUndistort2

キャリブレーションデータを利用して，歪みを補正する

サンプルコード

#include <cv.h>
#include <highgui.h>

int
main (int argc, char *argv[])
{
  IplImage *src_img, *dst_img;
  CvMat *intrinsic, *distortion;
  CvFileStorage *fs;
  CvFileNode *param;

  // (1)補正対象となる画像の読み込み
  if (argc < 2 || (src_img = cvLoadImage (argv[1], CV_LOAD_IMAGE_COLOR)) == 0)
    return -1;
  dst_img = cvCloneImage (src_img);

  // (2)パラメータファイルの読み込み
  fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_READ);
  param = cvGetFileNodeByName (fs, NULL, "intrinsic");
  intrinsic = (CvMat *) cvRead (fs, param);
  param = cvGetFileNodeByName (fs, NULL, "distortion");
  distortion = (CvMat *) cvRead (fs, param);
  cvReleaseFileStorage (&fs);

  // (3)歪み補正
  cvUndistort2 (src_img, dst_img, intrinsic, distortion);

  // (4)画像を表示，キーが押されたときに終了
  cvNamedWindow ("Distortion", CV_WINDOW_AUTOSIZE);
  cvShowImage ("Distortion", src_img);
  cvNamedWindow ("UnDistortion", CV_WINDOW_AUTOSIZE);
  cvShowImage ("UnDistortion", dst_img);
  cvWaitKey (0);

  cvDestroyWindow ("Distortion");
  cvDestroyWindow ("UnDistortion");
  cvReleaseImage (&src_img);
  cvReleaseImage (&dst_img);
  cvReleaseMat (&intrinsic);
  cvReleaseMat (&distortion);

  return 0;
}

// (1)補正対象となる画像の読み込み
補正を行う画像を読み込む．もちろん，補正対象となる画像を撮影したカメラは，キャリブレーション済みであるとする．

// (2)パラメータファイルの読み込み
キャリブレーション結果を格納したパラメータファイルを読み込む． 歪み補正に必要なパラメータは，内部パラメータ，および歪み係数だけなので，これをファイルから読み込む．

// (3)歪み補正
関数cvUndisotort2()により，入力画像の歪み補正をおこなう．

// (4)画像を表示，キーが押されたときに終了
歪み補正前画像と歪み補正後画像を表示し，なにかキーが押されるまで待つ．

実行結果例

マップを利用した歪み補正 cvInitUndistortMap

キャリブレーションデータを利用してマップを作成し，歪みを補正する

サンプルコード

#include <cv.h>
#include <highgui.h>

int
main (int argc, char *argv[])
{
  IplImage *src_img, *dst_img;
  CvMat *intrinsic, *distortion;
  CvFileStorage *fs;
  CvFileNode *param;
  IplImage *mapx = cvCreateImage (cvSize (640, 480), IPL_DEPTH_32F, 1);
  IplImage *mapy = cvCreateImage (cvSize (640, 480), IPL_DEPTH_32F, 1);

  if (argc < 2 || (src_img = cvLoadImage (argv[1], CV_LOAD_IMAGE_COLOR)) == 0)
    return -1;
  dst_img = cvCloneImage (src_img);

  fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_READ);
  param = cvGetFileNodeByName (fs, NULL, "intrinsic");
  intrinsic = (CvMat *) cvRead (fs, param);
  param = cvGetFileNodeByName (fs, NULL, "distortion");
  distortion = (CvMat *) cvRead (fs, param);
  cvReleaseFileStorage (&fs);

  // (1)歪み補正のためのマップ初期化
  cvInitUndistortMap (intrinsic, distortion, mapx, mapy);
  // (2)歪み補正
  cvRemap (src_img, dst_img, mapx, mapy);

  cvNamedWindow ("Distortion", CV_WINDOW_AUTOSIZE);
  cvShowImage ("Distortion", src_img);
  cvNamedWindow ("UnDistortion", CV_WINDOW_AUTOSIZE);
  cvShowImage ("UnDistortion", dst_img);
  cvWaitKey (0);

  cvDestroyWindow ("Distortion");
  cvDestroyWindow ("UnDistortion");
  cvReleaseImage (&src_img);
  cvReleaseImage (&dst_img);
  cvReleaseImage (&mapx);
  cvReleaseImage (&mapy);
  cvReleaseMat (&intrinsic);
  cvReleaseMat (&distortion);

  return 0;
}

// (1)歪み補正のためのマップ初期化
関数cvInitUndistortMap()を用いて，歪みを補正するためのマップ mapx，mapy の初期化を行う．

// (2)歪み補正
作成されたマップを用いて，画像の幾何変換（cvRemap）を行う． 関数cvUndistort2()を利用する場合と比べて，計算速度が向上する． しかし，一般的な幾何変換を行う関数を利用しているためか，以前のバージョンで実装されていた歪み補正関数に比べてパフォーマンスが劣る． もちろん，以前の関数 cvUnDistortInit，cvUnDistort も cvcompat.h 内で宣言されているが， 将来的に廃止される予定なので利用は推奨されない （またコメントによれば，これらの関数は，quite hackerish implementations である）． 単純に歪み補正の速度だけを求めるならば， Yahoo!Groupsの投稿（Speeding up undistort）にもあるように，自分でLUTを作成すれば良い． その場合は，画像の補完を別途行う必要がある．

実行結果例