OpenCVで作るビジョンプログラム(2)

お疲れさまです。Shinpukuです。

本題に入る前に、私たちのプログラムに用いられている座標系についてお話ししようと思います。

ロボットの基本的な座標系は、フィールドの中心を原点とした右手座標系です。プログラム中では「絶対座標系」と呼ばれます。

 

これとは別に、ロボットを原点とした座標系は「相対座標系」と呼ばれます。

カメラから見える物体の位置は相対座標系となります。画像中の座標から実際の座標に変換するために、私たちは「距離・角度マップ」というものを作っています。

左の画像は距離、右の画像は角度を表しています。

距離はExcel等を使って、ピクセル距離から実距離に変換する式を求めます。
角度は簡単で、反時計回りを正として角度（+π～-π）を指定してきます。

プログラム中では、距離・角度マップはOpenCVで扱いやすいようにIplImageとして保存します。例えば、距離マップは32bitの1チャンネル画像として生成します。

#define IMAGE_HEIGHT  240

#define IMAGE_WIDTH   240

CvSize size = cvSize(IMAGE_WIDTH, IMAGE_HEIGHT);

IplImage *cvDistance = cvCreateImage(size, IPL_DEPTH_32F, 1);

マップの保存は、

// ピクセル距離と実距離（サンプル）
double pixel[] = {0,  30,  37,  43,  48,  53,  57,  61,  76,  85,  91,  96,  99, 103, 105, 106};
double distance[] = {0.0, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0};
// 5次多項式近似で求めた係数
double x[] = {-0.000984, 0.058100, -0.003357, 0.000089, -0.000001, 0.000000};
// 画像中心

CvPoint center = cvPoint(cvDistance->width/2, cvDistance->height/2);

for (int i = 0; i; cvDistance->height; i++) {

    for (int j = 0; j; cvDistance->width; j++) {
        // 画像中心から(i,j)までの距離
        int r = hypot(i - center.x, j - center.y);
        // pixel_distanceを近似関数に入力したときの距離
        double dist = x[5]*pow5(r) + x[4]*pow4(r) + x[3]*pow3(r) + x[2]*pow2(r) + x[1]*r + x[0];
        // 書き出し

        fwrite((float*)&dist, sizeof(float), 1, file);

    }

}

このように、バイナリでもASCIIでもいいので何かのファイルに保存します。
こうすれば毎回計算しなくて済みますね。

マップを読み込むときは、

for (int i = 0; i; cvDistance->height; i++) {
    for (int j = 0; j; cvDistance->width; j++) {
        fread((float*)cvDistance->imageData + i * cvDistance->width + j, sizeof(float), 1, file);
    }
}

IplImage構造体をマップのデータで埋めます。

結局何がしたいのかというと、画像中で見えている物体をこのように、

対応する距離と角度を一発でバシッと決めてやりたいのです。

// 距離・角度
double distance = CV_IMAGE_ELEM(cvDistance, float, img_y, img_x);
double angle = CV_IMAGE_ELEM(cvAngle, float, img_y, img_x);

CV_IMAGE_ELEMマクロを使えばこんなに簡単！

前回と同様、ちょっとした工夫ですが、かなり役に立ちます。

最後に、自己位置情報を用いると、

// 絶対座標系に変換
pos.x = data.position.x + distance * cos(angle + data.position.angle);

pos.y = data.position.y + distance * sin(angle + data.position.angle);

このように相対座標系から絶対座標系へ変換できます。

次回は色抽出をしてみましょう。