Zybotのカメラによる白線追従走行

Zybot の白線検出、追従走行のアプリケーション・ソフトウェアを作って、白線追従走行させてみました。
その様子です。




まだまだ、ON-OFF 制御で走行もふらつき気味ですが、一応、白線追従走行はできるようになりました。
ただ、直角に曲がるのは難しいですね。後ろの車輪をサーボモーターで向きを変えるか？前輪を左右逆転するとかしないといけないのでしょうか？
もっと車輪の間隔を狭めた方が小回り効きそうです。トレッドを短くするということですね。
ロボットカーは画像処理の要素だけでなく、車の要素が入ってくるので、制御が難しいですね。。。
なお、現在のZybot は、後車輪の自在キャスターが回りすぎてしまうので、クリップを改造して回り止めを付けています。


現在の制御用のアプリケーション・ソフトウェアを貼っておきます。

// wl_tracking.cpp
// 2016/09/02 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <string.h>

#include "xpwm.h"
#include "xmotor_monitor.h"

#define DIR_LEFT_NORMAL        1
#define DIR_LEFT_REVERSE    0
#define DIR_RIGHT_NORMAL    0
#define DIR_RIGHT_REVERSE    1

#define PIXEL_NUM_OF_BYTES    4
#define SVGA_HORIZONTAL_PIXELS  800
#define SVGA_VERTICAL_LINES     600
#define SVGA_ALL_DISP_ADDRESS   (SVGA_HORIZONTAL_PIXELS * SVGA_VERTICAL_LINES * PIXEL_NUM_OF_BYTES)

#define GABOR_DETECT_LINE        590
#define GABOR_DETECT_LINE_ADDR    (SVGA_HORIZONTAL_PIXELS * GABOR_DETECT_LINE * PIXEL_NUM_OF_BYTES)
#define GABOR_THRESHOLD            5
#define DIST_THRESHOLD            30

#define LEFT_GABOR_EDGE_OVERFLOW    0
#define RIGHT_GABOR_EDGE_OVERFLOW    (SVGA_HORIZONTAL_PIXELS/2)

#define DEBUG
//#define MOTOR_OFF
#define MEMCPY

// Gobor filter
//
volatile unsigned *gabor_fil_on(int &fd4){
    int fd2, fd3;
    volatile unsigned *axis_switch_0, *axis_switch_1;
    volatile unsigned *gabor_filter_lh_0;
    int gabor_cntrl;

   // axis_switch_0 (UIO2)
    fd2 = open("/dev/uio2", O_RDWR); // axis_switch_0 interface AXI4 Lite Slave
    if (fd2 < 1){
        fprintf(stderr, "/dev/uio2 (axis_switch_0) open error\n");
        exit(-1);
    }
    axis_switch_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd2, 0);
    if (!axis_switch_0){
        fprintf(stderr, "axis_switch_0 mmap error\n");
        exit(-1);
    }
    
    // axis_switch_1 (UIO3)
    fd3 = open("/dev/uio3", O_RDWR); // axis_switch_1 interface AXI4 Lite Slave
    if (fd3 < 1){
        fprintf(stderr, "/dev/uio3 (axis_switch_1) open error\n");
        exit(-1);
    }
    axis_switch_1 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd3, 0);
    if (!axis_switch_1){
        fprintf(stderr, "axis_switch_1 mmap error\n");
        exit(-1);
    }
    
    // gabor_filter_lh_0 (UIo14)
    fd4 = open("/dev/uio14", O_RDWR); // gabor_filter_lh_0 interface AXI4 Lite Slave
    if (fd4 < 1){
        fprintf(stderr, "/dev/uio14 (gabor_filter_lh_0) open error\n");
        exit(-1);
    }
    gabor_filter_lh_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd4, 0);
    if (!gabor_filter_lh_0){
        fprintf(stderr, "lap_filter_axis_0 mmap error\n");
        exit(-1);
    }
      
    // axis_switch_1, 1to2 ,Select M01_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    axis_switch_1[16] = 0x80000000; // 0x40 = 0x80000000; disable
    axis_switch_1[17] = 0x80000000; // 0x44 = 0x80000000; disable
    axis_switch_1[18] = 0; // 0x48 = 0;
    axis_switch_1[0] = 0x2; // 0x0 = 2; Commit registers
    
    // gabor filter AXIS Start
    gabor_filter_lh_0[6] = 0; // left parameter
    gabor_cntrl = gabor_filter_lh_0[0] & 0x80; // Auto Restart bit
    gabor_filter_lh_0[0] = gabor_cntrl | 0x01; // Start bit set
    gabor_filter_lh_0[0] = 0x80; // Auto Restart bit set
    
    // axis_switch_0, 2to1, Select S01_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    axis_switch_0[16] = 0x2; // 0x40 = 0x2;
    axis_switch_0[0] = 0x2; // 0x0 = 2; Commit registers

    munmap((void *)axis_switch_0, 0x10000);
    munmap((void *)axis_switch_1, 0x10000);
    
    close(fd2);
    close(fd3);
    
    return(gabor_filter_lh_0);
}

int search_gabor_edge(unsigned int start_addr, unsigned int number, int threshold){
    volatile int *imgaddr, *endaddr;
    int i;
    
    imgaddr = (volatile int *)start_addr;
    
    for (i=0; i<number; i++){
        int c=imgaddr[i] & 0xff;
        //printf("%d\n",c);
        if (c >= threshold){
            break;
        }
    }
    return(i); 
}

// Motor
//
void motor_settings(XPwm *motorLp, XPwm *motorRp){
    XPwm_DisableAutoRestart(motorLp);
    while(!XPwm_IsIdle(motorLp)) ;
    XPwm_Start(motorLp);
    XPwm_EnableAutoRestart(motorLp);
    
     XPwm_DisableAutoRestart(motorRp);
    while(!XPwm_IsIdle(motorRp)) ;
    XPwm_Start(motorRp);
    XPwm_EnableAutoRestart(motorRp);
}

void Stopped_Zybot(XPwm *motorLp, XPwm *motorRp){
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_sw_late_V(motorRp, 0);
}

void motor_initialize(XPwm &motorL, XPwm &motorR, XMotor_monitor &mmL, XMotor_monitor &mmR){
    XPwm *motorLp, *motorRp;
    XMotor_monitor *mmLp, *mmRp;
    
    motorLp = &motorL;
    motorRp = &motorR;
    mmLp = &mmL;
    mmRp = &mmR;
    
    // Initialization of motor
    if (XPwm_Initialize(motorLp, "pwm_0") != XST_SUCCESS){
        fprintf(stderr,"pwm_0 (Left) open error\n");
        exit(-1);
    }
    if (XPwm_Initialize(motorRp, "pwm_1") != XST_SUCCESS){
        fprintf(stderr,"pwm_1 (Right) open error\n");
        exit(-1);
    }
    
    
    // Initialization of motor monitor
    if (XMotor_monitor_Initialize(mmLp, "motor_monitor_0") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_0 (Left) open error\n");
        exit(-1);
    }
    if (XMotor_monitor_Initialize(mmRp, "motor_monitor_1") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_1 (Right) open error\n");
        exit(-1);
    }

    // The Motors is rotated in the forward direction.
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_dir_V(motorLp, 1);

    XPwm_Set_sw_late_V(motorRp, 0);
     XPwm_Set_dir_V(motorRp, 0);

    motor_settings(motorLp, motorRp);
}


int main(){
    int fd4;
    volatile unsigned *gabor_filter_lh_0;
    XPwm motorL, motorR;
    XMotor_monitor mmL, mmR;
    unsigned char  attr[1024];
    unsigned long  phys_addr;
      int left_wl_edge, right_wl_edge;

    // Gabor filter Initialize
    gabor_filter_lh_0 = gabor_fil_on(fd4);

    // Motor Initialize
    motor_initialize(motorL, motorR, mmL, mmR);
    
    // udmabuf0
    int fdf = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The cache is disabled. 
    if (fdf == -1){
        fprintf(stderr, "/dev/udmabuf0 open error\n");
        exit(-1);
    }
    volatile unsigned *frame_buffer = (volatile unsigned *)mmap(NULL, 3*SVGA_ALL_DISP_ADDRESS, PROT_READ|PROT_WRITE, MAP_SHARED, fdf, 0);
    if (!frame_buffer){
        fprintf(stderr, "frame_buffer mmap error\n");
        exit(-1);
    }

    // phys_addr of udmabuf0
    int fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
    if (fdp == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open error\n");
        exit(-1);
    }
    read(fdp, attr, 1024);
    sscanf((const char *)attr, "%lx", &phys_addr);  
    close(fdp);
    printf("phys_addr = %x\n", (unsigned int)phys_addr);
    
    // main loop
    printf("White line Tracking start. \n");
    while(1){
        // Gabor filter for left white line 
        gabor_filter_lh_0[6] = 0; // left parameter

        usleep(250000); // Wait 67 ms
        left_wl_edge = SVGA_HORIZONTAL_PIXELS/2 - search_gabor_edge(
            (unsigned int)frame_buffer+GABOR_DETECT_LINE_ADDR, SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);
#ifdef MEMCPY
        memcpy((unsigned int *)((unsigned int)frame_buffer+SVGA_ALL_DISP_ADDRESS), (unsigned int *)frame_buffer,
            SVGA_ALL_DISP_ADDRESS);
#endif

        // Gabor filter for right white line
        gabor_filter_lh_0[6] = 1; // right parameter
        usleep(250000); // Wait 67 ms
        right_wl_edge = search_gabor_edge(
            (unsigned int)frame_buffer+GABOR_DETECT_LINE_ADDR+(SVGA_HORIZONTAL_PIXELS/2)*PIXEL_NUM_OF_BYTES,
            SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);
#ifdef MEMCPY
        memcpy((unsigned int *)((unsigned int)frame_buffer+2*SVGA_ALL_DISP_ADDRESS), (unsigned int *)frame_buffer,
            SVGA_ALL_DISP_ADDRESS);
#endif

#ifdef DEBUG
        printf("left_wl_edge = %d, right_wl_edge = %d\n", left_wl_edge, right_wl_edge);
#endif

        if (left_wl_edge == LEFT_GABOR_EDGE_OVERFLOW){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 5);
            XPwm_Set_sw_late_V(&motorR, 35);
#endif
#ifdef DEBUG
            printf("Left gabor edge is overflow\n");
#endif
        } else if (right_wl_edge == RIGHT_GABOR_EDGE_OVERFLOW){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 35);
            XPwm_Set_sw_late_V(&motorR, 5);
#endif
#ifdef DEBUG
            printf("Right gabar edge is overflow\n");
#endif
        } else if ((right_wl_edge - left_wl_edge) > DIST_THRESHOLD){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 25);
            XPwm_Set_sw_late_V(&motorR, 15);
#endif
#ifdef DEBUG
            printf("Right turn\n");
#endif
        } else if ((right_wl_edge - left_wl_edge) < -DIST_THRESHOLD){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 15);
            XPwm_Set_sw_late_V(&motorR, 25);
#endif
#ifdef DEBUG
            printf("Left turn\n");
#endif
        } else if (abs(right_wl_edge - left_wl_edge) <= DIST_THRESHOLD){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 20);
            XPwm_Set_sw_late_V(&motorR, 20);
#endif
#ifdef DEBUG
            printf("Go straight\n");
#endif
        }
    }
}

なぜか、ガボール・フィルタを設定してから、67 ms 待つだけでは画像が完全に処理しきれていないことがあったので、待ち時間を延ばしました。なんででしょうか？
左右のガボール・フィルタ画像を別のフレームバッファに割り当ててDMA転送すれば、もっと速くできそうです。
カメラのfps を上げることも考えてみようと思います。

Zybot による白線間の自動走行3（実機テスト）

”Zybot による白線間の自動走行2（Gabor fillter の修正、Vivado HLS）”の続き。

前回は、左白線検出、右白線検出を画像フレームごとに交互に繰り返すガボール・フィルタをIP 化した。今回は、それをVivado 2016.2 のIP として、回路に加えた。
具体的には、今回のガボール・フィルタIP をZYBO_0_162_3 フォルダのVivado プロジェクトの既存のガボール・フィルタIP と交換して論理合成、インプリメント、ビットストリームの生成を行った。ZYBO_0_162_4 フォルダとした。（”Gabor FilterをZYBO_0_162_2 へ実装してみた”を参照のこと）
これをビットストリームまで生成して、SDKでBOOT.bin を作成して、SDカードに書き込んでZYBO でブートした。
AXI VDMA のフレームバッファを 4 にして、ガボール・フィルタの左白線検出、右白線検出交互検出にしてみたが、左白線検出のフレームだけを表示しても、交互に表示されてしまう。

次に、ガボール・フィルタを 3 回フレームを処理するように書き換えて、2回と同様にやってみたが、交互に表示されるときと、右だけになる場合がある。

この方法でやる場合には、左白線検出、右白線検出の同期をとる必要があるようだ。

とりあえずは、最初の画像 1 フレームだけのガボール・フィルタに戻して、ソフトウェアで白線検出を試みることにしようと思う。

Zybot による白線間の自動走行2（Gabor fillter の修正、Vivado HLS）

”Zybot による白線間の自動走行1（Gabor fillter の修正、C ソースコード）”の続き。

前回は左白線検出と右白線検出を交互にできるように修正したガボール・フィルタのソースコードを貼った。今回は、Vivado HLS 2016.2 での結果を書いておく。

まずは、Vivado HLS 2016.2 でのプロジェクトを示す。


次に、C シミュレーションを行った。なお、画面サイズは640 x 480 に変更してある。

Compiling ../../../Gabor_filter_lh_2_tb.cpp in debug mode
Compiling ../../../Gabor_filter_lh_2.cpp in debug mode
Generating csim.exe

outs
ERROR HW and SW results mismatch i = 557, j = 73, HW = 00171717, SW = 00141414
ERROR HW and SW results mismatch i = 419, j = 88, HW = 00171717, SW = 00141414
ERROR HW and SW results mismatch i = 419, j = 92, HW = 00131313, SW = 00101010
ERROR HW and SW results mismatch i = 405, j = 100, HW = 003e3e3e, SW = 003b3b3b
ERROR HW and SW results mismatch i = 426, j = 106, HW = 00121212, SW = 000f0f0f
ERROR HW and SW results mismatch i = 486, j = 119, HW = 00171717, SW = 00141414
ERROR HW and SW results mismatch i = 235, j = 120, HW = 001b1b1b, SW = 00181818
ERROR HW and SW results mismatch i = 437, j = 120, HW = 00e1e1e1, SW = 00dedede
ERROR HW and SW results mismatch i = 441, j = 120, HW = 00141414, SW = 00111111
ERROR HW and SW results mismatch i = 235, j = 121, HW = 00141414, SW = 00111111
ERROR HW and SW results mismatch i = 425, j = 124, HW = 00a9a9a9, SW = 00a6a6a6
ERROR HW and SW results mismatch i = 425, j = 125, HW = 00e1e1e1, SW = 00dedede
ERROR HW and SW results mismatch i = 252, j = 132, HW = 004e4e4e, SW = 004b4b4b
ERROR HW and SW results mismatch i = 254, j = 133, HW = 00d9d9d9, SW = 00d6d6d6
ERROR HW and SW results mismatch i = 296, j = 145, HW = 00161616, SW = 00131313
ERROR HW and SW results mismatch i = 296, j = 146, HW = 00181818, SW = 00151515
ERROR HW and SW results mismatch i = 135, j = 156, HW = 00dcdcdc, SW = 00d9d9d9
ERROR HW and SW results mismatch i = 137, j = 156, HW = 003f3f3f, SW = 003c3c3c
ERROR HW and SW results mismatch i = 139, j = 157, HW = 00040404, SW = 00010101
ERROR HW and SW results mismatch i = 491, j = 163, HW = 00efefef, SW = 00ececec
ERROR HW and SW results mismatch i = 495, j = 163, HW = 00f3f3f3, SW = 00f0f0f0
Success HW and SW results match

WARNING: Hls::stream 'hls::stream >.2' contains leftover data, which may result in RTL simulation hanging.
WARNING: Hls::stream 'hls::stream >.1' contains leftover data, which may result in RTL simulation hanging.
INFO: [SIM 1] CSim done with 0 errors.

結構、固定小数点演算と浮動小数点演算の二乗誤差が 9 以上の項が出ているが、9 で収まっているようだ。

次に、C コードの合成を行った。


まずは、Timing のTarget をなぜ 10 ns ではなく、7 ns にしているかだが、10 ns で C コードを合成して、IP 化を行って、Vivado でIP を回路に加えてインプリメントすると、このガボール・フィルタのIP 内でクリティカルパスが発生して、100 MHｚで動作しないからだ。クリティカルパスの遅延は、16 ns 程度になってしまった。、Timing のTarget を 7 ns にすると、Vivado でも 100 MHz で動作できるようだ。これは、Vivado HLS のクリティカルパスの遅延計算が甘いのかもしれない？

次に、Detail のLoop 2 を見ると、Latency が 614410 となっていた。画像の 640 x 480 x 2 画面 = 614400 なので、10 クロック余計なだけであるので、1クロックで 1 ピクセル処理ができていると言える。

C/RTL 協調シミュレーションを行った。


Latency は 614454 クロックで、ここでも、1クロックで 1 ピクセル処理ができていると言えると思う。

C/RTL 協調シミュレーション波形を示す。


ins_TVALID, ins_TREADY, outs_TVALID, outs_TREADY もずっと 1 で、1クロックで 1 ピクセル処理ができている。

画像を 800 x 600 に戻して、もう一度、C コードの合成を行ってから、IP 化を行った。

Zybot による白線間の自動走行1（Gabor fillter の修正、C ソースコード）

”Zybot のカメラ画像でGabor Filterのパラメータを取得した3（Zybotでテスト）”の最後で、書いた様に、Gabor fillter を修正した。書いた文章を引用する。

次は、今のガボールフィルタのままだと、右白線、左白線とCPUで設定する必要があるから、CPUで左右を設定してから、ガボール・フィルタ処理結果を取得する必要がある。これはフレームレートが遅くなってしまう。
そこで、左右白線の2フレーム1セットでガボールフィルタ処理を行うことにする。左右は当然異なるフレームバッファにDMAする。その為にVDMAを4フレーム対応にして左右左右のフレームバッファにライトする。
これで左右白線用ガボールフィルタはカメラのフレームレートが15 fps だから、半分の 7.5 fps でデータが取得できる。
このエッジをとりあえずはCPUで探索して、どの位置にいるかを確認することにしよう。

という訳で、Gabor_filter_lh をGabor_filter_lh_2 フォルダにコピーした。
今回は、左白線検出、右白線検出を交互に行うので、どのような C ソースコードにしようか？と思ったが、一番確実なのは、左白線検出パラメータと右白線検出パラメータで 2 回ガボール・フィルタをかければよいだろうということになった。
入力引数の RorL のビット幅を 1 bit から 2 bit に増やして「L_R_WEIGHT」というモードを増やした。
Gabor_filter_lh_2() 関数は、2回ガボール・フィルタを行うように変更した。「L_R_WEIGHT」を RorL 引数にセットすると、1回目は左白線検出、2回目は右白線検出を行う。
今回はC ソースコードを貼っておこう。
まずは、Gabor_filter_lh_2.h から貼っておく。

// Gabor_filter_lh_2.h
// 2016/07/24
// 2016/07/25 : 右白線検出用のGabor Filterの重みを追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
// 2016/08/29 : 1回目左白線検出、2回目右白線検出のモードを追加
//

#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__

#define HORIZONTAL_PIXEL_WIDTH    800
#define VERTICAL_PIXEL_WIDTH    600

//#define HORIZONTAL_PIXEL_WIDTH    640 // for Simulation
//#define VERTICAL_PIXEL_WIDTH    480

//#define HORIZONTAL_PIXEL_WIDTH    64
//#define VERTICAL_PIXEL_WIDTH    48

#define ALL_PIXEL_VALUE    (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define ARRAY_SIZE    9

#define RIGHT_WEIGHT    1
#define LEFT_WEIGHT        0
#define L_R_WEIGHT        2

const int gabor_weight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用＋右白線検出用
    {
        {0,-3,-10,-16,-7,7,10,5,1},
        {-3,-15,-27,-11,32,50,29,5,-2},
        {-11,-24,-5,73,135,95,16,-17,-10},
        {-11,4,85,187,160,14,-72,-52,-14},
        {4,51,135,137,-18,-159,-136,-45,-2},
        {16,50,59,-39,-179,-185,-73,3,13},
        {10,12,-25,-104,-131,-60,15,27,11},
        {1,-7,-31,-48,-24,18,29,14,3},
        {-1,-5,-9,-4,10,16,10,2,-1}
    },
    {
        {1,5,7,1,-12,-17,-10,-3,0},
        {1,11,33,45,21,-16,-27,-15,-4},
        {-8,-5,35,107,131,69,-2,-21,-11},
        {-17,-47,-51,40,169,187,93,13,-7},
        {-8,-54,-134,-147,-18,123,130,58,11},
        {9,-6,-82,-185,-187,-65,36,44,17},
        {11,24,12,-55,-125,-112,-43,1,7},
        {3,14,30,23,-13,-41,-33,-12,-1},
        {0,2,9,17,14,1,-6,-5,-2}
    }
};
const float gabor_fweight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用＋右白線検出用(float)
    {
        {0.001282,-0.009914,-0.04062,-0.060586,-0.027574,0.026072,0.038427,0.018191,0.003056},
        {-0.012155,-0.057448,-0.104645,-0.042953,0.123263,0.197238,0.114451,0.020448,-0.007239},
        {-0.042252,-0.093065,-0.018911,0.285374,0.525746,0.372687,0.060734,-0.064748,-0.040465},
        {-0.042261,0.015673,0.332798,0.728763,0.625046,0.053591,-0.283076,-0.203293,-0.05608},
        {0.017342,0.198305,0.52554,0.535526,-0.069756,-0.622839,-0.531089,-0.177366,-0.006367},
        {0.060866,0.19708,0.231032,-0.154219,-0.699885,-0.721808,-0.286707,0.013004,0.049249},
        {0.038379,0.04877,-0.098477,-0.404993,-0.510165,-0.233566,0.057894,0.104366,0.041887},
        {0.0047,-0.0278,-0.121277,-0.187262,-0.093276,0.070512,0.113857,0.055799,0.009976},
        {-0.003798,-0.01885,-0.035607,-0.01709,0.037692,0.064268,0.038606,0.007536,-0.002133}
    },
    {
        {0.005562,0.018882,0.028293,0.004499,-0.044995,-0.064838,-0.039469,-0.009822,0.000815},
        {0.002294,0.04108,0.127023,0.175094,0.083025,-0.063755,-0.106402,-0.057798,-0.01406},
        {-0.031269,-0.021096,0.135641,0.417286,0.512467,0.269946,-0.008354,-0.082091,-0.041357},
        {-0.066348,-0.184919,-0.197802,0.15614,0.65976,0.728616,0.361674,0.052074,-0.027152},
        {-0.031146,-0.211178,-0.523777,-0.573856,-0.069756,0.480311,0.506451,0.225223,0.041031},
        {0.035552,-0.023892,-0.320104,-0.723563,-0.728735,-0.253689,0.1391,0.170625,0.067723},
        {0.04216,0.094939,0.047511,-0.216623,-0.488075,-0.437898,-0.168739,0.003336,0.027009},
        {0.012112,0.056596,0.115239,0.090332,-0.05076,-0.158403,-0.127847,-0.046375,-0.004918},
        {-0.00168,0.007437,0.036985,0.067021,0.053689,0.004977,-0.02365,-0.018248,-0.005928}
    }
};

#endif

次に、Gabor_fiter_lh_2.cpp を貼っておく。

// Gabor_fiter_lh_2.cpp
// 2016/07/23 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
// 2016/08/29 : 1回目左白線検出、2回目右白線検出のモードを追加
//

#include <stdio.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh_2.h"

int conv_rgb2y(int rgb);

int Gabor_filter_lh_2(hls::stream<ap_axis<32,1,1,1> >& ins,
        hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<2> & RorL){
#pragma HLS INTERFACE s_axilite port=RorL
#pragma HLS INTERFACE axis port=ins
#pragma HLS INTERFACE axis port=outs
#pragma HLS INTERFACE s_axilite port=return

    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (int k=0; k<2; k++){
        for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
            for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
#pragma HLS PIPELINE II=1
                if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                    ins >> pix;    // AXI4-Stream からの入力

                mbuf.shift_left();    // mbuf の列を1ビット左シフト
                for(i=ARRAY_SIZE-2; i>=0; --i){
                    mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
                }
                gray_pix = conv_rgb2y(pix.data);
                mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

                // LineBuffer の更新
                linebuf.shift_down(x);
                linebuf.insert_bottom(gray_pix, x);

                // 使用する配列を決定する
                int ano;
                switch (RorL){
                case LEFT_WEIGHT :
                    ano = LEFT_WEIGHT;
                    break;
                case RIGHT_WEIGHT :
                    ano = RIGHT_WEIGHT;
                    break;
                case L_R_WEIGHT :
                    if (k == 0)
                        ano = LEFT_WEIGHT;
                    else
                        ano = RIGHT_WEIGHT;
                    break;
                default :
                    ano = LEFT_WEIGHT;
                    break;
                }

                // Gabor filter の演算
                for (j=0, val=0; j<ARRAY_SIZE-1; j++){
                    for (i=0; i<ARRAY_SIZE-1; i++){
                        val += gabor_weight[ano][j][i] * mbuf(ARRAY_SIZE-1-j,i);
                    }
                }
                val = val/256; // 256倍してあるので、1/256して戻す
                if (val<0)
                    //val = -val; // 絶対値
                    val = 0; // マイナスの値を0に丸める
                else if (val>255)
                    val = 255;

                // Gabor filter・データの書き込み
                gabor.data = (val<<16)+(val<<8)+val;
                // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
                if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                    gabor.data = 0;

                if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                    gabor.user = 1;
                else
                    gabor.user = 0;

                if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                    gabor.last = 1;
                else
                    gabor.last = 0;

                outs << gabor;    // AXI4-Stream へ出力
             }
         }
    }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

最後に、Gabor_filter_lh_2_tb.cpp を貼っておく。

// Gabor_filter_lh_2_tb.cpp
// 2016/07/24 by marsee
// 2016/07/25 : 右白線検出用のGabor Filterを追加して、右左の白線を指定するRorL 引数を追加
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ap_int.h>
#include <hls_stream.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <ap_axi_sdata.h>
#include <hls_video.h>

#include "Gabor_filter_lh_2.h"
#include "bmp_header.h"

int Gabor_filter_lh_2(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<2> & RorL);

int conv_rgb2y_soft(int rgb);
int Gabor_filter_lh_2_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<2> & RorL);

#define CLOCK_PERIOD 10
#define RIGHT_OR_LEFT   L_R_WEIGHT
#define BMP_FILE_NAME   "road_1.bmp"

int main()
{
    using namespace std;

    hls::stream<ap_axis<32,1,1,1> > ins;
    hls::stream<ap_axis<32,1,1,1> > ins_soft;
    hls::stream<ap_axis<32,1,1,1> > outs;
    hls::stream<ap_axis<32,1,1,1> > outs_soft;
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> vals;
    ap_axis<32,1,1,1> vals_soft;

    int m_seq = 1; // M系列の値
    int i, k;
    int xor_shift;

    BITMAPFILEHEADER bmpfhr; // BMPファイルのファイルヘッダ(for Read)
    BITMAPINFOHEADER bmpihr; // BMPファイルのINFOヘッダ(for Read)
    FILE *fbmpr, *fbmpw, *fbmpwf;
    int *rd_bmp, *hw_gabor, *sw_gabor;
    int blue, green, red;
    ap_uint<2> r_l;
    char fhname[100];
    char fsname[100];

    if ((fbmpr = fopen(BMP_FILE_NAME, "rb")) == NULL){ // test.bmp をオープン
        fprintf(stderr, "Can't open test.bmp by binary read mode\n");
        exit(1);
    }
    // bmpヘッダの読み出し
    fread(&bmpfhr.bfType, sizeof(char), 2, fbmpr);
    fread(&bmpfhr.bfSize, sizeof(long), 1, fbmpr);
    fread(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpr);
    fread(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpr);
    fread(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpr);

    // ピクセルを入れるメモリをアロケートする
    if ((rd_bmp =(int *)malloc(sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }
    if ((hw_gabor =(int *)malloc(2 * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }
    if ((sw_gabor =(int *)malloc(2 * sizeof(int) * (bmpihr.biWidth * bmpihr.biHeight))) == NULL){
        fprintf(stderr, "Can't allocate hw_gabor memory\n");
        exit(1);
    }

    // rd_bmp にBMPのピクセルを代入。その際に、行を逆転する必要がある
    for (int y=0; y<bmpihr.biHeight; y++){
        for (int x=0; x<bmpihr.biWidth; x++){
            blue = fgetc(fbmpr);
            green = fgetc(fbmpr);
            red = fgetc(fbmpr);
            rd_bmp[((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] = (blue & 0xff) | ((green & 0xff)<<8) | ((red & 0xff)<<16);
        }
    }
    fclose(fbmpr);

    // ins に入力データを用意する
    for(int i=0; i<5; i++){    //　dummy data
        pix.user = 0;
        pix.data = i;
        ins << pix;
    }

    // 2画面分を入力する
    for(int k=0; k<2; k++){
        for(int j=0; j < bmpihr.biHeight; j++){
            for(i=0; i < bmpihr.biWidth; i++){
                pix.data = (ap_int<32>)rd_bmp[(j*bmpihr.biWidth)+i];

                if (j==0 && i==0)    // 最初のデータの時に TUSER を 1 にする
                    pix.user = 1;
                else
                    pix.user = 0;

                if (i == bmpihr.biWidth-1) // 行の最後でTLASTをアサートする
                    pix.last = 1;
                else
                    pix.last = 0;

                ins << pix;
                ins_soft << pix;
            }
        }
    }

    r_l = (ap_uint<2>)RIGHT_OR_LEFT;
    Gabor_filter_lh_2(ins, outs, r_l);
    Gabor_filter_lh_2_soft(ins_soft, outs_soft, r_l);

    // ハードウェアとソフトウェアのラプラシアン・フィルタの値のチェック
    cout << endl;
    cout << "outs" << endl;
    for(k=0; k<2; k++){
        for(int j=0; j < bmpihr.biHeight; j++){
            for(i=0; i < bmpihr.biWidth; i++){
                outs >> vals;
                outs_soft >> vals_soft;
                ap_int<32> val = vals.data;
                ap_int<32> val_soft = vals_soft.data;

                hw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+(j*bmpihr.biWidth)+i] = (int)val;
                sw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+(j*bmpihr.biWidth)+i] = (int)val_soft;

                //printf("k=%d, j=%d, i=%d\n",k,j,i);
                if ((double)pow((double)(val&0xff)-(val_soft&0xff),(double)2) > 4){ // 2乗誤差が4よりも大きい
                    printf("ERROR HW and SW results mismatch i = %ld, j = %ld, HW = %08x, SW = %08x\n", i, j, (int)val, (int)val_soft);
                    //return(1);
                }
                //if (vals.last)
                    //cout << "AXI-Stream is end" << endl;
            }
        }
    }
    cout << "Success HW and SW results match" << endl;
    cout << endl;

    // ハードウェアのラプラシアンフィルタの結果を temp_gabor0.bmp, temp_gabor1.bmp へ出力する
    for(k=0; k<2; k++){
        if(k == 0){
            if ((fbmpw=fopen("temp_gabor0.bmp", "wb")) == NULL){
                fprintf(stderr, "Can't open temp_gabor0.bmp by binary write mode\n");
                exit(1);
            }
        } else {
            if ((fbmpw=fopen("temp_gabor1.bmp", "wb")) == NULL){
                fprintf(stderr, "Can't open temp_gabor1.bmp by binary write mode\n");
                exit(1);
            }
        }

        // BMPファイルヘッダの書き込み
        fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpw);
        fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpw);
        fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpw);
        fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpw);
        // RGB データの書き込み、逆順にする
        for (int y=0; y<bmpihr.biHeight; y++){
            for (int x=0; x<bmpihr.biWidth; x++){
                blue = hw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
                green = (hw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
                red = (hw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

                fputc(blue, fbmpw);
                fputc(green, fbmpw);
                fputc(red, fbmpw);
            }
        }
        fclose(fbmpw);
    }

    // ソフトウェアのラプラシアンフィルタの結果を temp_gabor_float0.bmp, temp_gabor_float1.bmp へ出力する
    for(k=0; k<2; k++){
        if (k == 0){
            if ((fbmpwf=fopen("temp_gabor_float0.bmp", "wb")) == NULL){
                fprintf(stderr, "Can't open temp_gabor_float0.bmp by binary write mode\n");
                exit(1);
            }
        } else {
            if ((fbmpwf=fopen("temp_gabor_float1.bmp", "wb")) == NULL){
                fprintf(stderr, "Can't open temp_gabor_float1.bmp by binary write mode\n");
                exit(1);
            }
        }

        // BMPファイルヘッダの書き込み
        fwrite(&bmpfhr.bfType, sizeof(char), 2, fbmpwf);
        fwrite(&bmpfhr.bfSize, sizeof(long), 1, fbmpwf);
        fwrite(&bmpfhr.bfReserved1, sizeof(short), 1, fbmpwf);
        fwrite(&bmpfhr.bfReserved2, sizeof(short), 1, fbmpwf);
        fwrite(&bmpfhr.bfOffBits, sizeof(long), 1, fbmpwf);
        fwrite(&bmpihr, sizeof(BITMAPINFOHEADER), 1, fbmpwf);
        // RGB データの書き込み、逆順にする
        for (int y=0; y<bmpihr.biHeight; y++){
            for (int x=0; x<bmpihr.biWidth; x++){
                blue = sw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] & 0xff;
                green = (sw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x] >> 8) & 0xff;
                red = (sw_gabor[bmpihr.biWidth*bmpihr.biHeight*k+((bmpihr.biHeight-1)-y)*bmpihr.biWidth+x]>>16) & 0xff;

                fputc(blue, fbmpwf);
                fputc(green, fbmpwf);
                fputc(red, fbmpwf);
            }
        }
        fclose(fbmpwf);
    }

    free(rd_bmp);
    free(hw_gabor);

    return 0;
}

int Gabor_filter_lh_2_soft(hls::stream<ap_axis<32,1,1,1> >& ins, hls::stream<ap_axis<32,1,1,1> >& outs, ap_uint<2> & RorL){
    ap_axis<32,1,1,1> pix;
    ap_axis<32,1,1,1> gabor;

    hls::LineBuffer<ARRAY_SIZE-1, HORIZONTAL_PIXEL_WIDTH, int> linebuf;
    hls::Window<ARRAY_SIZE, ARRAY_SIZE, int> mbuf;

    int gray_pix, val, i, j, x, y;
    float valf;

    do {    // user が 1になった時にフレームがスタートする
        ins >> pix;
    } while(pix.user == 0);

    for (int k=0; k<2; k++){
        for (y=0; y<VERTICAL_PIXEL_WIDTH; y++){
            for (x=0; x<HORIZONTAL_PIXEL_WIDTH; x++){
                if (!(x==0 && y==0))    // 最初の入力はすでに入力されている
                    ins >> pix;    // AXI4-Stream からの入力

                mbuf.shift_left();    // mbuf の列を1ビット左シフト
                for(i=ARRAY_SIZE-2; i>=0; --i){
                    mbuf.insert(linebuf(i,x), i+1, ARRAY_SIZE-1);
                }
                gray_pix = conv_rgb2y_soft(pix.data);
                mbuf.insert(gray_pix, 0, ARRAY_SIZE-1);

                // LineBuffer の更新
                linebuf.shift_down(x);
                linebuf.insert_bottom(gray_pix, x);

                // 使用する配列を決定する
                int ano;
                switch (RorL){
                case LEFT_WEIGHT :
                    ano = LEFT_WEIGHT;
                    break;
                case RIGHT_WEIGHT :
                    ano = RIGHT_WEIGHT;
                    break;
                case L_R_WEIGHT :
                    if (k == 0)
                        ano = LEFT_WEIGHT;
                    else
                        ano = RIGHT_WEIGHT;
                    break;
                default :
                    ano = LEFT_WEIGHT;
                    break;
                }

                // Gabor filter の演算
                for (j=0, valf=0; j<ARRAY_SIZE-1; j++){
                    for (i=0; i<ARRAY_SIZE-1; i++){
                        valf += gabor_fweight[ano][j][i] * (float)mbuf(ARRAY_SIZE-1-j,i);
                    }
                }

                val = (int)valf;
                if (val<0)
                    //val = -val; // 絶対値
                    val = 0; // マイナスの値を0に丸める
                else if (val>255)
                    val = 255;

                // Gabor filter・データの書き込み
                gabor.data = (val<<16)+(val<<8)+val;
                // 最初のARRAY_SIZE-1行とその他の行の最初のARRAY_SIZE-1列は無効データなので0とする
                if (x<(ARRAY_SIZE-1) || y<(ARRAY_SIZE-1))
                    gabor.data = 0;

                if (x==0 && y==0) // 最初のデータでは、TUSERをアサートする
                    gabor.user = 1;
                else
                    gabor.user = 0;

                if (x == (HORIZONTAL_PIXEL_WIDTH-1))    // 行の最後で TLAST をアサートする
                    gabor.last = 1;
                else
                    gabor.last = 0;

                outs << gabor;    // AXI4-Stream へ出力
             }
         }
    }
     return(0);
}

// RGBからYへの変換
// RGBのフォーマットは、{8'd0, R(8bits), G(8bits), B(8bits)}, 1pixel = 32bits
// 輝度信号Yのみに変換する。変換式は、Y =  0.299R + 0.587G + 0.114B
// "YUVフォーマット及び YUV<->RGB変換"を参考にした。http://vision.kuee.kyoto-u.ac.jp/~hiroaki/firewire/yuv.html
//　2013/09/27 : float を止めて、すべてint にした
int conv_rgb2y_soft(int rgb){
    int r, g, b, y_f;
    int y;

    b = rgb & 0xff;
    g = (rgb>>8) & 0xff;
    r = (rgb>>16) & 0xff;

    y_f = 77*r + 150*g + 29*b; //y_f = 0.299*r + 0.587*g + 0.114*b;の係数に256倍した
    y = y_f >> 8; // 256で割る

    return(y);
}

Zybot のカメラ画像でGabor Filterのパラメータを取得した3（Zybotでテスト）

”Zybot のカメラ画像でGabor Filterのパラメータを取得した2（gabor_filter_lh プロジェクト）”の続き。

前回はパラメータを取り直したガボール・フィルタのIP 化を行った。今回は、ガボール・フィルタIP をZYBO_0_162_3 フォルダのVivado プロジェクトの既存のガボール・フィルタIP と交換して論理合成、インプリメント、ビットストリームの生成を行った。（”Gabor FilterをZYBO_0_162_2 へ実装してみた”を参照のこと）

ハードウェアをエクスポートしてSDK を立ち上げ、BOOT イメージを作成して、SDカードに書いてZybot に挿入して、ガボール・フィルタ画像を取得した。

左白線用ガボール・フィルタの画像を示す。


右白線用ガボール・フィルタの画像を示す。


左白線と右白線は分離できていると思う。以前よりも多少エッジが弱くなった気がするが。。。

次は、今のガボールフィルタのままだと、右白線、左白線とCPUで設定する必要があるから、CPUで左右を設定してから、ガボール・フィルタ処理結果を取得する必要がある。これはフレームレートが遅くなってしまう。
そこで、左右白線の2フレーム1セットでガボールフィルタ処理を行うことにする。左右は当然異なるフレームバッファにDMAする。その為にVDMAを4フレーム対応にして左右左右のフレームバッファにライトする。
これで左右白線用ガボールフィルタはカメラのフレームレートが15 fps だから、半分の 7.5 fps でデータが取得できる。
このエッジをとりあえずはCPUで探索して、どの位置にいるかを確認することにしよう。

Zybot のカメラ画像でGabor Filterのパラメータを取得した2（gabor_filter_lh プロジェクト）

”Zybot のカメラ画像でGabor Filterのパラメータを取得した”の続き。

前回は左白線検出用、右白線検出用のGabor Filterのパラメータを取得し、加工した。今回はその加工したパラメータをVivado HLS のgabor_filter_lh プロジェクトに入れてみた。

”Zybot のカメラ画像でGabor Filterのパラメータを取得した”で、Gabor Filterのパラメータを256 倍して、整数に直した。これをハードウェア化用のパラメータとした。元の小数のGabor fillter のパラメータはテストベンチで整数演算との誤差比較用に使用する。（gabor_filter_lh については、”ガボール・フィルタ (Gabor Filter) による白線検出10（hls::LineBuffer と hls::Window を使用7）”辺りを参照のこと）


そのパラメータは、Gabor_filter_lh.h に入力した。Gabor_filter_lh.h を示す。

// Gabor_filter_lh.h
// 2016/07/24
// 2016/07/25 : 右白線検出用のGabor Filterの重みを追加
// 2016/07/27 : 右白線検出用配列と左白線検出用配列を統合
//

#ifndef __Gabor_filter_lh_H__
#define __Gabor_filter_lh_H__

//#define HORIZONTAL_PIXEL_WIDTH    800
//#define VERTICAL_PIXEL_WIDTH    600

#define HORIZONTAL_PIXEL_WIDTH    640 // for Simulation
#define VERTICAL_PIXEL_WIDTH    480

//#define HORIZONTAL_PIXEL_WIDTH    64
//#define VERTICAL_PIXEL_WIDTH    48

#define ALL_PIXEL_VALUE    (HORIZONTAL_PIXEL_WIDTH*VERTICAL_PIXEL_WIDTH)

#define ARRAY_SIZE    9
#define RIGHT_WEIGHT    1
#define LEFT_WEIGHT        0

const int gabor_weight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用＋右白線検出用
    {
        {0,-3,-10,-16,-7,7,10,5,1},
        {-3,-15,-27,-11,32,50,29,5,-2},
        {-11,-24,-5,73,135,95,16,-17,-10},
        {-11,4,85,187,160,14,-72,-52,-14},
        {4,51,135,137,-18,-159,-136,-45,-2},
        {16,50,59,-39,-179,-185,-73,3,13},
        {10,12,-25,-104,-131,-60,15,27,11},
        {1,-7,-31,-48,-24,18,29,14,3},
        {-1,-5,-9,-4,10,16,10,2,-1}
    },
    {
        {1,5,7,1,-12,-17,-10,-3,0},
        {1,11,33,45,21,-16,-27,-15,-4},
        {-8,-5,35,107,131,69,-2,-21,-11},
        {-17,-47,-51,40,169,187,93,13,-7},
        {-8,-54,-134,-147,-18,123,130,58,11},
        {9,-6,-82,-185,-187,-65,36,44,17},
        {11,24,12,-55,-125,-112,-43,1,7},
        {3,14,30,23,-13,-41,-33,-12,-1},
        {0,2,9,17,14,1,-6,-5,-2}
    }
};
const float gabor_fweight[2][ARRAY_SIZE][ARRAY_SIZE] = { // 左白線検出用＋右白線検出用(float)
    {
        {0.001282,-0.009914,-0.04062,-0.060586,-0.027574,0.026072,0.038427,0.018191,0.003056},
        {-0.012155,-0.057448,-0.104645,-0.042953,0.123263,0.197238,0.114451,0.020448,-0.007239},
        {-0.042252,-0.093065,-0.018911,0.285374,0.525746,0.372687,0.060734,-0.064748,-0.040465},
        {-0.042261,0.015673,0.332798,0.728763,0.625046,0.053591,-0.283076,-0.203293,-0.05608},
        {0.017342,0.198305,0.52554,0.535526,-0.069756,-0.622839,-0.531089,-0.177366,-0.006367},
        {0.060866,0.19708,0.231032,-0.154219,-0.699885,-0.721808,-0.286707,0.013004,0.049249},
        {0.038379,0.04877,-0.098477,-0.404993,-0.510165,-0.233566,0.057894,0.104366,0.041887},
        {0.0047,-0.0278,-0.121277,-0.187262,-0.093276,0.070512,0.113857,0.055799,0.009976},
        {-0.003798,-0.01885,-0.035607,-0.01709,0.037692,0.064268,0.038606,0.007536,-0.002133}
    },
    {
        {0.005562,0.018882,0.028293,0.004499,-0.044995,-0.064838,-0.039469,-0.009822,0.000815},
        {0.002294,0.04108,0.127023,0.175094,0.083025,-0.063755,-0.106402,-0.057798,-0.01406},
        {-0.031269,-0.021096,0.135641,0.417286,0.512467,0.269946,-0.008354,-0.082091,-0.041357},
        {-0.066348,-0.184919,-0.197802,0.15614,0.65976,0.728616,0.361674,0.052074,-0.027152},
        {-0.031146,-0.211178,-0.523777,-0.573856,-0.069756,0.480311,0.506451,0.225223,0.041031},
        {0.035552,-0.023892,-0.320104,-0.723563,-0.728735,-0.253689,0.1391,0.170625,0.067723},
        {0.04216,0.094939,0.047511,-0.216623,-0.488075,-0.437898,-0.168739,0.003336,0.027009},
        {0.012112,0.056596,0.115239,0.090332,-0.05076,-0.158403,-0.127847,-0.046375,-0.004918},
        {-0.00168,0.007437,0.036985,0.067021,0.053689,0.004977,-0.02365,-0.018248,-0.005928}
    }
};

#endif

これを使用してC シミュレーションを行った。
まずは、普通の道の写真の road_1.bmp でやってみた。まずは、road_1.bmp を示す。


まずは、左白線用ガボール・フィルタを適用するようにテストベンチを修正して、C シミュレーションを行った。
このときに整数演算のガボール・フィルタ結果と浮動小数点演算のガボール・フィルタ結果の二乗誤差が9 を超える値はなかった。
左白線用ガボール・フィルタで、整数演算の結果を示す。


次に、右白線用のガボール・フィルタを適用するようにテストベンチを修正して、C シミュレーションを行った。
このときに整数演算のガボール・フィルタ結果と浮動小数点演算のガボール・フィルタ結果の二乗誤差が9 を超える値は21 個発生した。例を示す。

ERROR HW and SW results mismatch i = 556, j = 73, HW = 00171717, SW = 00141414
ERROR HW and SW results mismatch i = 418, j = 88, HW = 00171717, SW = 00141414
ERROR HW and SW results mismatch i = 418, j = 92, HW = 00131313, SW = 00101010
ERROR HW and SW results mismatch i = 404, j = 100, HW = 003e3e3e, SW = 003b3b3b
ERROR HW and SW results mismatch i = 425, j = 106, HW = 00121212, SW = 000f0f0f

右白線用ガボール・フィルタで、整数演算の結果を示す


先に普通の道の写真を今回パラメータを修正したガボール・フィルタ結果を示したが、今度はパラメータの検出用に使用した画像を使用して、今回のガボール・フィルタをかけてみた。


最初に左白線用ガボール・フィルタの結果を示す。


今回も左白線用ガボール・フィルタでの整数演算のガボール・フィルタ結果と浮動小数点演算のガボール・フィルタ結果の二乗誤差が9 を超える値はなかった。

次に、右白線用のガボール・フィルタ結果を示す。


このときに整数演算のガボール・フィルタ結果と浮動小数点演算のガボール・フィルタ結果の二乗誤差が9 を超える値は20 個発生した。

C コードの合成を行った。結果を示す。


引き続き、IP 化を行った。

Zybot のカメラ画像でGabor Filterのパラメータを取得した

”Zybot のカメラ画像をBMPファイルに変換するアプリケーションを作成した”で、Zybot のカメラ画像を取得してBMPファイルに変換するアプリケーション・ソフトウェアを作成した。今回は、このアプリケーションで取得したZybot のカメラ画像からGabor Filterのパラメータを取得する。

”ガボール・フィルタ (Gabor Filter) による白線検出2”のVivado HLS プロジェクトを使用して、扱う画像をZybot のカメラ画像に変更した。

まずは、左白線用Gabor Filterパラメータは、 sigma = 4, Lambda = 85, Psi = 94, Theta = 50 が良さそうだ。


次に、右白線用Gabor Filterパラメータは、 sigma = 4, Lambda = 85, Psi = 94, Theta = 125 が良さそうだ。


これらのパラメータでの、Gabor Filterの9 x 9 のカーネルの値を示す。

左白線用Gabor Filterパラメータ sigma = 4, Lambda = 85, Psi = 94, Theta = 50
0.001282,-0.009914,-0.040620,-0.060586,-0.027574,0.026072,0.038427,0.018191,0.003056
-0.012155,-0.057448,-0.104645,-0.042953,0.123263,0.197238,0.114451,0.020448,-0.007239
-0.042252,-0.093065,-0.018911,0.285374,0.525746,0.372687,0.060734,-0.064748,-0.040465
-0.042261,0.015673,0.332798,0.728763,0.625046,0.053591,-0.283076,-0.203293,-0.056080
0.017342,0.198305,0.525540,0.535526,-0.069756,-0.622839,-0.531089,-0.177366,-0.006367
0.060866,0.197080,0.231032,-0.154219,-0.699885,-0.721808,-0.286707,0.013004,0.049249
0.038379,0.048770,-0.098477,-0.404993,-0.510165,-0.233566,0.057894,0.104366,0.041887
0.004700,-0.027800,-0.121277,-0.187262,-0.093276,0.070512,0.113857,0.055799,0.009976
-0.003798,-0.018850,-0.035607,-0.017090,0.037692,0.064268,0.038606,0.007536,-0.002133

右白線用Gabor Filterパラメータ sigma = 4, Lambda = 85, Psi = 94, Theta = 125
0.005562,0.018882,0.028293,0.004499,-0.044995,-0.064838,-0.039469,-0.009822,0.000815
0.002294,0.041080,0.127023,0.175094,0.083025,-0.063755,-0.106402,-0.057798,-0.014060
-0.031269,-0.021096,0.135641,0.417286,0.512467,0.269946,-0.008354,-0.082091,-0.041357
-0.066348,-0.184919,-0.197802,0.156140,0.659760,0.728616,0.361674,0.052074,-0.027152
-0.031146,-0.211178,-0.523777,-0.573856,-0.069756,0.480311,0.506451,0.225223,0.041031
0.035552,-0.023892,-0.320104,-0.723563,-0.728735,-0.253689,0.139100,0.170625,0.067723
0.042160,0.094939,0.047511,-0.216623,-0.488075,-0.437898,-0.168739,0.003336,0.027009
0.012112,0.056596,0.115239,0.090332,-0.050760,-0.158403,-0.127847,-0.046375,-0.004918
-0.001680,0.007437,0.036985,0.067021,0.053689,0.004977,-0.023650,-0.018248,-0.005928

次に、”ガボール・フィルタ (Gabor Filter) による白線検出3（HLSビデオライブラリ）”を参考にして、int で扱える形にしてみた。

カーネルのパラメータを256倍して、四捨五入を行った。
左白線検出用パラメータを示す。


この値は256倍してあるので、カーネルを画像データにかけた後で、1/256倍する必要がある。

次に右白線検出用パラメータを示す。