2011/12/26

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

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

白線情報を元にした自己位置推定法では、白線をいかに多く検出できるかが鍵となります。

メジャーなのはスキャンライン(走査線)を使った方法です。
最新のプログラムでは、スキャンラインは近・中・遠距離の3段構成でそれぞれ32本、64本、128本配置されています。多段構成にすることで、手前の白線だけでなく、その奥の白線も検出することが出来ます。

では、ソースコードを見ながら白線検出アルゴリズムを説明していきます。

cvThresholdで白色を抽出し、前回検出したフィールド領域との論理積をとります。白色は輝度が高いのでYに閾値を設定すればすぐに抽出できます。白線以外のものを検出しないようにマスクをかけるとなお良いでしょう。

スキャンラインの総数は以下の式で求められます。
コードにするとこんな感じです。

const int LAYER_NUM = 3;
const int LINE_NUM = 32;
const int TOTAL_LINE_NUM = (LINE_NUM*((1<<LAYER_NUM)-1));

i番目のスキャンラインの開始・終了点は以下の式で求められます。
求めた式を元に、画像の中心からスキャンラインを伸ばし、白線を検出します。

void Vision::detectLines(Data &data)
{
    const int minLen = 35;
    const int maxLen = 110;

    // フィールドとの論理積
    cvAnd(images.cvWhite, images.cvField, images.cvLines);

    // ここから白線検出
    for (int i = 0; i < LAYER_NUM; i++) {
        // スキャンライン数
        int lineNum = LINE_NUM * (1<<i);
        // 角度の変化
        double theta_k = (2.0 * M_PI) / lineNum;
        // 始点・終点
        int start = (int)(minLen + (i+0) * (maxLen - minLen) / LAYER_NUM);
        int end   = (int)(minLen + (i+1) * (maxLen - minLen) / LAYER_NUM);
        // スキャンラインを走査
        for (int j = 0; j < lineNum; j++) {
            // i層j番目のスキャンライン
            int k = LINE_NUM * ((1<<i)-1) + j;
            // スキャンライン角度
            double theta = -(j * theta_k + M_PI/2.0);
            // 初期化
            lines[k].distance = -1.0;
            lines[k].angle = 0.0;
            // 始めから終わりまで
            for (int l = start; l < end; l++) {
                CvPoint scanline;
                scanline.x = (int)(l * cos(theta) + images.center.x);
                scanline.y = (int)(l * sin(theta) + images.center.y);
                uchar val = CV_IMAGE_ELEM(images.cvLines, uchar, scanline.y, scanline.x);
                // 白線が見つかった
                if (val > 0) {
                    // 検出した白線を表示
                    lines[k].angle = CV_IMAGE_ELEM(images.cvAngle, float, scanline.y, scanline.x);
                    lines[k].distance = CV_IMAGE_ELEM(images.cvDistance, float, scanline.y, scanline.x);
                    break;
                }
            }
        }
    }
}
次回はパーティクルフィルタを用いた自己位置推定アルゴリズムを詳しく解説していこうと思います。

2011/12/23

進捗

こんにちは、Noguchiです。

今日は今(12/14現在)の活動内容を簡単に紹介します。


まず、ハード班です。

Shimizuはバッテリー部の改良です。
以前より、バッテリーの故障が目立っていました。
調べていくうちに様々な問題点が浮き彫りになってきており、現在改良中です。
この件については、年明けにでもブログ1回分取って、詳細説明いたします。

Tsutsumiはロボットカバーの改良とキッカー部分のリフター付けです。
カバーは以前に比べて、取り外しがしやすいものへと変更いたしました。
リフター部に関しては、キッカー部分に付けています。
図の○の箇所になります。

これを付けることにより、ボールと蹴ったときに浮いたボール、俗に言うループボールを
放つことが出来るようになりました。
しかしながらこれを付けたことにより、ドリブル等の別の問題点が発生すると話し合いにより、
判断し、更なる改良が必要だと考えております。


次にソフト班です。

InoueはPKの改良を行いました。
人間の行うPKとは異なり、ロボットはセンターサークルより動き始め、30秒以内に
ボールを蹴るというのが、ロボカップサッカー中型リーグにおけるルールです。
ルールに基づき、プログラムを新しいものへ変更いたしました。
ゴール確率もある程度期待できるモノになっております。
今後はインプレイ中のシュート条件の改良を行って行く予定です。

Noguchiは経路計画の再考をしております。
現在、机上で検討しております。
検討終了後に、プログラムへ移行する予定です。
こちらに関しても、詳しい事が言える段階で説明いたします。

Takenakaはゴールキーパーのプログラムを新バージョンへ移植しております。



--

先日の清掃の続きを行いました。

問題の倉庫でしたが、棚や道具の配置替えをし、必要用途や頻度に
合わせて棚の整理をしただけで、きれいになりました。

残りは、いろいろな場所に転がっていたネジの分類分けと雑用です。
地味な作業ではありますが、大事なことです。


"塵も積もれば山となる"ですね。

2011/12/19

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

こんにちは、Shinpukuです。

今回はフィールド領域の検出を行います。

これはフィールド外の物体を誤って検出しないようにするためです。

それではアルゴリズムを紹介していきます。

まず、閾値処理により緑色を抽出します。
次にノイズ除去のため膨張・収縮を行います。
一番大きい領域のみ残します。
cvConvexHull2関数を用いて欠けた領域の補間を行います。
これでフィールド領域を検出することが出来ました。

黄色や白色との論理積を取ることでボールや白線を検出することができます。 

コードは以下の通りです。

void Vision::detectField(Data &data)
{
    double max = 0;
    CvSeq *contour = NULL, *maxContour = NULL;
    CvMemStorage *contourStorage = cvCreateMemStorage();
    // 膨張・縮小
    cvMorphologyEx(images.cvGreen, images.cvField, NULL, NULL, CV_MOP_CLOSE);
    // 中心円を描く
    const int radius = MIN(IMAGE_HALF_WIDTH, IMAGE_HALF_HEIGHT) * 0.3;
    cvCircle(images.cvField, images.center, radius, cvScalarAll(255), -1);
    // 輪郭を抽出
    cvFindContours(images.cvField, contourStorage, &contour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    // 一番大きい領域を求める
    while (contour) {
        double size = fabs(cvContourArea(contour, CV_WHOLE_SEQ));
        if (size > max) {
            maxContour = contour;
            max = size;
        }
        contour = contour->h_next;
    }
    // 初期化
    cvZero(images.cvField);
    // 見つかった
    if (maxContour) {
        // 凸包を求める
        CvSeq *hull = cvConvexHull2(maxContour);
        // 凸包を構成する点
        CvPoint *pts = new CvPoint[hull->total];
        for(int i = 0; i < hull->total; ++i) pts[i] = **(CvPoint**)cvGetSeqElem(hull, i);
        // フィールド領域を描画
        cvFillConvexPoly(images.cvField, pts, hull->total, cvScalarAll(255));
        cvAnd(images.cvField, images.cvMask, images.cvField);
        // メモリ解放
        delete [] pts;
    }
    // メモリ解放
    cvReleaseMemStorage(&contourStorage);
}
次回は白線を検出してみようと思います。

2011/12/14

御挨拶と年末大掃除

お初に目にかかります。
2012年度ソフト班・HP担当になりましたNoguchiです。
今後、Hibikino-Musashiの日々の活動や技術的なお話ができればと思っております。
文章能力に欠ける私でありますが、温かく見守っていただけると幸いです。

--

先代より代替わりが行われ、早2ヶ月が経つでしょうか。

先日、年末ということもあり、部屋の大掃除を行いました。

片付けの最中です ( 掃除前の画像は取り忘れました… )



表だけでこれだけのゴミが出ました。
予想していたよりは意外と少なかったですね。






問題だった工具類の整理も我らがハード班Shimizuのおかげでこんなにきれいになりました。



数時間かけ、はきれいになりました。は…。

はい、まだ倉庫には手を付けていません。
たぶん表とは比べ物にならない大物が待ち構えていると思います。

はぁ~、まだまだ先が思いやられます。

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

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

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