FC2カウンター FPGAの部屋 ステレオカメラによる距離測定テスト10(左右カメラの視差の測定)

FPGAやCPLDの話題やFPGA用のツールの話題などです。 マニアックです。 日記も書きます。

FPGAの部屋

FPGAの部屋の有用と思われるコンテンツのまとめサイトを作りました。Xilinx ISEの初心者の方には、FPGAリテラシーおよびチュートリアルのページをお勧めいたします。

ステレオカメラによる距離測定テスト10(左右カメラの視差の測定)

ステレオカメラによる距離測定テスト9(アプリケーションの作製2)”の続き。

左右カメラの視差の測定を行うアプリケーションを作っていたのだが、取りあえず完成した。左目カメラ画像に右目カメラ画像の一部を表示するアプリケーションだ。右目カメラ画像の一部を表示するエリアは、左目カメラ画像の輝度を 1/2 にして、そこに輝度を 1/2 にした右目カメラ画像を足し合わせる。デフォルトでは、右目カメラ画像の縦方向真ん中で、横方向左端の 160 ピクセル X 120 行を切り取って、左目カメラ画像に表示する。
アプリケーションのファイル名はRL_alpha_blend.c だ。オプションは5つあって、

c : 右目カメラ画像の横方向の幅を指定する(初期値: 160 ピクセル)
r : 右目カメラ画像の縦方向の幅を指定する(初期値: 120 行)
d : 右目カメラ画像の横方向の切り取り位置(初期値: 0 ピクセル)
s : 右目カメラ画像の縦方向の切り取り位置(初期値: 0 ピクセル)

となっている。

右目カメラ画像を出力するために、ZYBO_0_2 で ./udmabuf_insmod, ./cam_disp_vdma を起動して、HDMI 端子に右目カメラ画像を出力した。

左目カメラ画像を出すためにには、StereoCamTest で ./udmabuf_insmod, ./StereoCam_Alt_Disp を実行した。
左目カメラ画像を示す。
StereoCamTest_74_160214.jpg

左目カメラ画像用ZYBO に転送されてきた右目カメラ画像を示す。
StereoCamTest_75_160214.jpg

右目カメラ画像用、左目カメラ画像用ZYBO の様子を示す。
StereoCamTest_73_160214.jpg

次に、StereoCamTest の左目カメラ画像用ZYBO で、./RL_alpha_blend でアプリケーションを立ち上げた。
StereoCamTest_70_160214.png

左目カメラ画像に右目カメラ画像の一部が表示されているのが分かる。ピンクの枠で囲った部分が右目カメラ画像の一部を傘な合わせた部分だ。
StereoCamTest_76_160214.jpg

右目カメラ画像の一部を RL_alpha_blead アプリケーション上でコマンドを入力して位置を合わせる。コマンドを紹介する。

j : 左へ 1 ピクセル移動
k : 右へ 1 ピクセル移動
h : 左へ 10 ピクセル移動
l (Lの小文字): 右へ 10 ピクセル移動
g : 左へ 30 ピクセル移動
; : 右へ 30 ピクセル移動
i : 上へ 1 ピクセル移動
u : 上へ 10 ピクセル移動
m : 下へ 1 ピクセル移動
n : 下へ 10 ピクセル移動


右目カメラ画像を移動させる途中の画面を示す。
StereoCamTest_71_160214.png

StereoCamTest_77_160214.jpg

最初に比べて、右目カメラ画像の一部が移動しているのがわかると思う。

右目カメラ画像の一部を左目カメラ画像と一致された状態を示す。
StereoCamTest_72_160214.png

StereoCamTest_78_160214.jpg

右目カメラ画像の一部を 43 ピクセル移動させると左目カメラ画像に一致した。なお、ZYBO からの一致させたオブジェクトまでの距離は 2.3 m だった。

最後に、RL_alpha_blend.c を貼っておく。

//
// RL_alpha_blend.c  by marsee
// 2016/02/06
//
// 800 pixels x 600 lines x 4 bytes x 3 pictures + 1024 pixels x 768 lines x 4 bytes x 3 pictures +
//   800 pixels x 600 lines x 4 bytes(alpha blend area for Right camera and Left camera)  = 17117184
//
// When you press the j key, right camera image is moved one pixel to the left.
// When you press the k key, right camera image is moved one pixel to the right.
// When you press the h key, right camera image is moved ten pixels to the left.
// When you press the l key, right camera image is moved ten pixels to the right.
// When you press the i key, right camera image is moved one pixel to the up.
// When you press the m key, right camera image is moved one pixel to the down.
//
// Usage : RL_alpha_blend [-c 80] [-r 60] [-h]
// -c and -r indicates the area to be cut out of the left edge of the right-eye camera image.
//

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

#define PIXEL_NUM_OF_BYTES    4
#define NUMBER_OF_WRITE_FRAMES    3 // Note: If not at least 3 or more, the image is not displayed in succession.

#define XGA_HORIZONTAL_PIXELS   1024
#define XGA_VERTICAL_LINES       768
#define XGA_ALL_DISP_ADDRESS    (XGA_HORIZONTAL_PIXELS * XGA_VERTICAL_LINES * PIXEL_NUM_OF_BYTES)
#define XGA_3_PICTURES          (XGA_ALL_DISP_ADDRESS * NUMBER_OF_WRITE_FRAMES) 

#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 SVGA_3_PICTURES         (SVGA_ALL_DISP_ADDRESS * NUMBER_OF_WRITE_FRAMES)

#define DEFAULT_RECTANGLE_PIXELS    160
#define DEFAULT_RECTANGLE_LINES     120

int half_bright(int pixel);
int alpha_blend(int pixel1, int pixel2);

int main(int argc, char *argv[]){
    int column_width = DEFAULT_RECTANGLE_PIXELS;
    int row_width = DEFAULT_RECTANGLE_LINES;
    int opt, c;
    int help_flag = 0;
    int fdf, fdp, fd6;
    volatile unsigned *frame_buffer;
    volatile unsigned *bmdc_axi_lites0;
    unsigned char  attr[1024];
    unsigned long  phys_addr;
    volatile unsigned *Leye_addr, *alpha_addr;
    int i, j;
    volatile unsigned *Reye_start_addr, *Reye_stride;
    volatile unsigned *abdisp_start_addr, *abdisp_addr;
    volatile unsigned *s, *t;
    unsigned int   buf_size;
    int offset_row=0, offset_column=0;
    int reye_img_offset_row=0, reye_img_offset_column=0;
    
    while ((opt=getopt(argc, argv, "c:r:d:s:h")) != -1){
        switch (opt){
            case 'c':
                column_width = atoi(optarg);
                break;
            case 'r':
                row_width = atoi(optarg);
                break;
            case 'd':
                reye_img_offset_column = atoi(optarg);
                break;
            case 's':
                reye_img_offset_row = atoi(optarg);
                break;
            case 'h':
                help_flag = 1;
                break;
        }
    }

    printf("column_width = %d, row_width = %dn", column_width, row_width);
    printf("reye_img_offset_column = %d, reye_img_offset_row = %dn", reye_img_offset_column, reye_img_offset_row);
    
    if (help_flag == 1){ // help
        printf("Usage : RL_alpha_blend [-c 160] [-r 120] [-d 0] [-s 0] [-h]n");
        printf("-c: column_width, -r: row_widthn");
        printf("-d: column of right eys's camere image offset, -s: row of right eys's camere image offsetn");
        printf("-c and -r indicates the area to be cut out of the left edge of the right-eye camera image.n");
        printf("n");
        printf("Key bindn");
        printf("j: left 1 pixelnk: right 1 pixelnh: left 10 pixelsnl: right 10 pixelsn");
        printf("g: left 30 pixelsn';': right 30 pixelsn");
        printf("i: up 1 pixelnu: up 10 pixelsnm: down 1 pixelnn: down 10 pixelsn");
        return(0);
    }

    // Bitmap Display Controller 0 AXI4 Lite Slave (UIO6)
    fd6 = open("/dev/uio6", O_RDWR); // bitmap_display_controller 0 axi4 lite
    if (fd6 < 1){
        fprintf(stderr, "/dev/uio6 (bitmap_disp_cntrler_axi_master_0) open errorn");
        exit(-1);
    }
    bmdc_axi_lites0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd6, 0);
    if (!bmdc_axi_lites0){
        fprintf(stderr, "bmdc_axi_lites0 mmap errorn");
        exit(-1);
    }

    // udmabuf0
    fdf = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The chache is disabled. 
    if (fdf == -1){
        fprintf(stderr, "/dev/udmabuf0 open errorn");
        exit(-1);
    }
    frame_buffer = (volatile unsigned *)mmap(NULL, SVGA_3_PICTURES+XGA_3_PICTURES+SVGA_ALL_DISP_ADDRESS, PROT_READ|PROT_WRITE, MAP_SHARED, fdf, 0);
    if (!frame_buffer){
        fprintf(stderr, "frame_buffer mmap errorn");
        exit(-1);
    }

    // phys_addr of udmabuf0
    fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
    if (fdp == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open errorn");
        exit(-1);
    }
    read(fdp, attr, 1024);
    sscanf(attr, "%lx", &phys_addr);  
    close(fdp);
    printf("phys_addr = %xn", (int)phys_addr);

    // buf_size of udmabuf0
    fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/size", O_RDONLY);
    if (fdp == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/size open errorn");
        exit(-1);
    }
    read(fdp, attr, 1024);
    sscanf(attr, "%d", &buf_size);  
    close(fdp);
    printf("buf_size = %dn", (int)buf_size);
    
    // Display the left-eye camera image
    Leye_addr = frame_buffer;
    alpha_addr = (volatile unsigned *)((unsigned)frame_buffer+SVGA_3_PICTURES+XGA_3_PICTURES);

    s = alpha_addr;
    t = Leye_addr;
    for (i=0; i<SVGA_VERTICAL_LINES; i++){
        for (j=0; j<SVGA_HORIZONTAL_PIXELS; j++){
            *s =  *t;
            s++;
            t++;
        }
    }
    bmdc_axi_lites0[0] = (unsigned)phys_addr+SVGA_3_PICTURES+XGA_3_PICTURES;
    
    // Display the right eye camera image, taken in a rectangular
    Reye_start_addr = (volatile unsigned *)((unsigned)frame_buffer + SVGA_3_PICTURES + (SVGA_HORIZONTAL_PIXELS * (SVGA_VERTICAL_LINES - row_width)/2 * PIXEL_NUM_OF_BYTES)+0x8+reye_img_offset_column+(SVGA_HORIZONTAL_PIXELS * reye_img_offset_row * PIXEL_NUM_OF_BYTES)); // left edge + offset
    Reye_stride = (volatile unsigned *)((SVGA_HORIZONTAL_PIXELS - column_width)*PIXEL_NUM_OF_BYTES);

    abdisp_start_addr = (volatile unsigned *)((unsigned)frame_buffer + SVGA_3_PICTURES + XGA_3_PICTURES+ (SVGA_HORIZONTAL_PIXELS * (SVGA_VERTICAL_LINES - row_width)/2 * PIXEL_NUM_OF_BYTES));

    abdisp_addr = abdisp_start_addr;
    s = abdisp_addr;
    t = Reye_start_addr;
    
    for (i=0; i<row_width; i++){
        for (j=0; j<column_width; j++){
            *s = alpha_blend(*s, *t);
            s++;
            t++;
        }
        s = (volatile unsigned *)((unsigned)s+ (unsigned)Reye_stride);
        t = (volatile unsigned *)((unsigned)t + (unsigned)Reye_stride);
    }
    
    printf("offset_row = %d pixels, offset_column = %d linesn", offset_row, offset_column);
    
    // j, k, h, l, i, m , q key
    while(c != 'q'){
        c = getc(stdin);
        switch ((char)c) {
            case 'j' :  // left 1 pixel
                abdisp_addr--;
                offset_row--;
                break;
            case 'k' :  // right 1 pixel
                abdisp_addr++;
                offset_row++;
                break;
            case 'h' :  // left 10 pixels
                abdisp_addr -= 10;
                offset_row -= 10;
                break;
            case 'l' :  // right 10 pixels
                abdisp_addr += 10;
                offset_row += 10;
                break;
            case 'g' : // left 30 pixels
                abdisp_addr -= 30;
                offset_row -= 30;
                break;          
            case ';' :  // right 30 pixels
                abdisp_addr += 30;
                offset_row += 30;
                break;
            case 'i' :  // up 1 pixel
                abdisp_addr -= SVGA_HORIZONTAL_PIXELS;
                offset_column--;
                break;
            case 'u' :  // up 10 pixel
                abdisp_addr -= (SVGA_HORIZONTAL_PIXELS*10);
                offset_column -= 10;
                break;
            case 'm' :  // down 1 pixel
                abdisp_addr += SVGA_HORIZONTAL_PIXELS;
                offset_column++;
                break;
            case 'n' :  // down 10 pixel
                abdisp_addr += (SVGA_HORIZONTAL_PIXELS*10);
                offset_column += 10;
                break;
        }

        // Display the left-eye camera image
        s = alpha_addr;
        t = Leye_addr;
        for (i=0; i<SVGA_VERTICAL_LINES; i++){
            for (j=0; j<SVGA_HORIZONTAL_PIXELS; j++){
                *s =  *t;
                s++;
                t++;
            }
        }

        // Display the right eye camera image, taken in a rectangular
        s = abdisp_addr;
        t = Reye_start_addr;
        for (i=0; i<row_width; i++){
            for (j=0; j<column_width; j++){
                *s = alpha_blend(*s, *t);
                s++;
                t++;
            }
            s = (volatile unsigned *)((unsigned)s+ (unsigned)Reye_stride);
            t = (volatile unsigned *)((unsigned)t + (unsigned)Reye_stride);
        }
        if (c != 'n')
            printf("offset_row = %d pixels, offset_column = %d linesn", offset_row, offset_column);
    }
    munmap((void *)bmdc_axi_lites0, 0x10000);
    munmap((void *)frame_buffer, (SVGA_3_PICTURES+XGA_3_PICTURES+SVGA_ALL_DISP_ADDRESS));
    close(fd6);
    close(fdf);
    
    return(0);
}

int half_bright(int pixel){
    int hpixel = 0;

    hpixel = (pixel & 0xff)/2;
    hpixel |= (pixel & 0xff00)/2;
    hpixel |= (pixel & 0xff0000)/2;

    return(hpixel);
}

int alpha_blend(int pixel1, int pixel2){
    int r, g, b;

    b = (pixel1 & 0xff)/2 + (pixel2 & 0xff)/2;
    g = ((pixel1 & 0xff00)/2 + (pixel2 & 0xff00)/2) & 0xff00;
    r = ((pixel1 & 0xff0000)/2 + (pixel2 & 0xff0000)/2) & 0xff0000;
    return(b+g+r);
}

  1. 2016年02月14日 06:06 |
  2. ステレオカメラによる画像解析
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


管理者にだけ表示を許可する

トラックバック URL
http://marsee101.blog19.fc2.com/tb.php/3396-7be30064
この記事にトラックバックする(FC2ブログユーザー)