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

次回はパーティクルフィルタを用いた自己位置推定アルゴリズムを詳しく解説していこうと思います。