2011/12/12

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

こんにちは、Shinpukuです。

今回は色抽出についてお話しします。

カメラから得た画像の中には、ボールや白線といった私たちが欲しい情報が含まれています。
具体的には黄色(オレンジ)、緑、白、黒がそれに当たります。

色抽出の前にカメラ画像の色空間をRGBからYUV(YCbCr)に変換します。
// RGB->YUV色変換
cvCvtColor(images.cvCamera, images.cvCameraYUV, CV_RGB2YCrCb);
cvSplit(images.cvCameraYUV, images.cvY, images.cvU, images.cvV, NULL);

YUVは明るさの変化に強く、ロボカップで用いられている色を簡単に表現することができます。
カメラからYUV422で出力されるのですがRGB->HSV変換の名残や僕の勉強不足のせいでこうなっています。

YUVの各チャンネルで閾値を設定して画像を2値化します。

OpenCVで代表的なものはcvThreshold、cvInRangeS、cvLUTですが、私たちのプログラムでは速度を優先してcvThresholdを用いています。
// 色抽出
cvThreshold(images.cvY, images.cvYellow, thresholds.maxYellowY, 0, CV_THRESH_TOZERO_INV);
cvThreshold(images.cvYellow, images.cvYellow, thresholds.minYellowY, 255, CV_THRESH_BINARY);
cvThreshold(images.cvU, images.cvBuf, thresholds.maxYellowU, 0, CV_THRESH_TOZERO_INV);
cvThreshold(images.cvBuf, images.cvBuf, thresholds.minYellowU, 255, CV_THRESH_BINARY);
cvAnd(images.cvYellow, images.cvBuf, images.cvYellow);
cvThreshold(images.cvV, images.cvBuf, thresholds.maxYellowV, 0, CV_THRESH_TOZERO_INV);
cvThreshold(images.cvBuf, images.cvBuf, thresholds.minYellowV, 255, CV_THRESH_BINARY);
cvAnd(images.cvYellow, images.cvBuf, images.cvYellow);

cvLUTは3ch→1chへの変換が出来ず、なぜかcvThresholdよりも遅いため使っていません。

cvInRangeSを使うと低速ですが、綺麗なコードになります。
// 色抽出
CvScalar lower = cvScalar(thresholds.minYellowY, thresholds.minYellowU, thresholds.minYellowV);
CvScalar upper = cvScalar(thresholds.maxYellowY, thresholds.maxYellowU, thresholds.maxYellowV);
cvInRangeS(images.cvCameraYUV, lower, upper, images.cvYellow);

色抽出の結果は以下のようになります。

2011/12/05

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

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

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

2011/12/01

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

お久しぶりです。

Hibikino-MusashiメインプログラマーShinpukuです。

今年はイランオープンにジャパンオープン、世界大会、秋季大会といろいろあって大変なとても充実した一年でした。

中型リーグの交流会もあり、ロボットのプログラムについて熱心な意見交換が行われていました。このブログを見てプログラムの参考にしているという話も聞き、他のチームの役に立ててもらって僕も嬉しい限りです。

さて、今回は質問の多かったビジョンについてお話ししようと思います。

僕たちのロボットには全方位カメラが搭載されてあり、これから画像を取得します。


カメラはSONY製のDFW-VL500です。ドライバは標準のものではなく、CMUのドライバを使っています。

取得したカメラ画像をビジョンで扱いやすいように加工します。
・BGR→RGB変換
・反転(鏡に映している為)
・カメラの取り付け角度に応じて回転
・240×240にトリミング

コードはこのようになります。

int Camera::getImage(IplImage *image)
{
    const int img_w = image->width, img_h = image->height; 
    const int cam_w = CAMERA_WIDTH, cam_h = CAMERA_HEIGHT;
    const int tmp_w = MAX(cam_w, cam_h), tmp_h = tmp_w;
    // 画像取得
    int camError = theCamera.AcquireImageEx(FALSE, NULL);
    // カメラの接続チェック
    if (camError != CAM_SUCCESS) {
        if (camError == CAM_ERROR_NOT_INITIALIZED) printf("Camera is not initialized.\n");
        printf("Some camera trouble.\n");
        return 0;
    }
    // 作業用イメージ
    IplImage *rawCamera = cvCreateImage(cvSize(cam_w, cam_h), IPL_DEPTH_8U, 3);
    IplImage *tmpCamera = cvCreateImage(cvSize(tmp_w, tmp_h), IPL_DEPTH_8U, 3);
    // 画像取得
    theCamera.getRGB((uchar*)rawCamera->imageData, rawCamera->imageSize);
    cvConvertImage(rawCamera, rawCamera, CV_CVTIMG_SWAP_RB);
    // コピー
    cvZero(tmpCamera);
    cvSetImageROI(tmpCamera, cvRect((tmp_w-cam_w)/2, (tmp_h-cam_h)/2, cam_w, cam_h));
    cvCopy(rawCamera, tmpCamera);
    cvResetImageROI(tmpCamera);
    cvReleaseImage(&rawCamera);
    // 反転(鏡に映しているため)
    cvFlip(tmpCamera, tmpCamera);
    // 回転
    IplImage *clnCamera = cvCloneImage(tmpCamera);
    CvMat *rotation = cvCreateMat(2, 3, CV_32FC1);
    CvPoint2D32f center = cvPoint2D32f(tmp_w/2, tmp_h/2);
    cv2DRotationMatrix(center, offset.angle, 1.0, rotation);
    cvWarpAffine(clnCamera, tmpCamera, rotation);
    cvReleaseMat(&rotation);
    cvReleaseImage(&clnCamera);
    // ROIで切り抜き
    CvRect ROI = cvRect((tmp_w-img_w)/2 + offset.x, (tmp_h-img_h)/2 + offset.y, img_w, img_h);
    if (ROI.x < 0) ROI.x = 0;
    if (ROI.y < 0) ROI.y = 0;
    if (ROI.x > (tmp_w - img_w)) ROI.x = tmp_w - img_w; 
    if (ROI.y > (tmp_h - img_h)) ROI.y = tmp_h - img_h; 
    cvSetImageROI(tmpCamera, ROI);
    cvCopy(tmpCamera, image);
    cvReleaseImage(&tmpCamera);
    return 1;
}

最終的に出力される画像はこのようになります。画像の上下左右はロボットの前後左右と対応しています。
面倒なことをしていますが、こうすることでこの後の処理がとてもわかりやすくなります。

ビジョンではいろいろな処理をしていますが、今回はこの辺で。