FC2カウンター FPGAの部屋 OpenCV 2.4.10 の stereo_calib.cpp を自分のカメラ画像でやってみた4(stereo_match_cam.cpp の作成)

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

FPGAの部屋

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

OpenCV 2.4.10 の stereo_calib.cpp を自分のカメラ画像でやってみた4(stereo_match_cam.cpp の作成)

OpenCV 2.4.10 の stereo_calib.cpp を自分のカメラ画像でやってみた3”の続き。

前回で、”OpenCV 2.4.10 の stereo_calib.cpp を自分のカメラ画像でやってみた”が上手く行った。でも、これは、右目カメラ画像と左目カメラ画像をBMPファイル変換して、そのBMPファイルに対して stereo_match を起動して、disparity を計測した。これだとBMPファイルにする手間があるので、右目カメラ画像と左目カメラ画像をバッファしているフレームバッファから直接、画像を取得して、stereo_match するようにソフトウェアを書き換えた。これを stereo_match_cam.cpp とした。

stereo_match.cpp には、Copyright が書いてあるので、stereo_match_cam.cpp の全文をブログに書くことはせずに変更点だけを載せようと思う。
まずは、インクルード文や、define 文などを追加した。

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

using namespace cv;

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

static void print_help()
{


次に、argc < 3 の時に help を表示する機能を削除して、”-h”オプションでヘルプを表示する機能を追加した。

    /* if(argc < 3)
    {
        print_help();
        return 0;
    } */


    forint i = 1; i < argc; i++ )
    {
        if ( argv[i][0] == '-' && argv[i][1] == 'h')
        {
            print_help();
            return 0;
        }
    }


次に、udmabuf の処理を追加した。

    StereoBM bm;
    StereoSGBM sgbm;
    StereoVar var;

    // Start by marsee
    unsigned char  attr[1024];
    unsigned long  phys_addr;

    // 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, 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 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);
    // End by marsee

    forint i = 1; i < argc; i++ )
    {


img1_filename と img2_filename が無い時の処理を削除した。

    /*if( !img1_filename || !img2_filename )
    {
        printf("Command-line parameter error: both left and right images must be specified\n");
        return -1;
    } */


左と右目のカメラのフレームバッファの画像をMat にコピーした。なお、この方法については、”OpenCVでピクセルにアクセスする方法4つ”の”その1. ポインタを取得してピクセルの値を取得・設定"を参照させて頂いた。
次に、フレームバッファの画像は 800 x 600 ピクセルなので、640 x 480 ピクセルにリサイズした。800 x 600 の画像でやってみたのだが、left , right のポップアップ・ウインドウに画像が表示されなかった。

    int color_mode = alg == STEREO_BM ? 0 : -1;
    //Mat img1 = imread(img1_filename, color_mode);
    //Mat img2 = imread(img2_filename, color_mode);

    // Start by marsee
    Mat imgL_t(Size(800600), CV_8UC3);
    Mat imgR_t(Size(800600), CV_8UC3);

    volatile unsigned int *Leye_addr = frame_buffer; // The Left Camera Image
    volatile unsigned int *Reye_addr = (volatile unsigned int *)((unsigned)frame_buffer+SVGA_3_PICTURES+0x8); // The Right Camera Image

    // Copy "Left Camera Image" to Mat
    for (int y=0; y<imgL_t.rows; y++){
        Vec3b* ptr = imgL_t.ptr<Vec3b>(y);
        for (int x=0; x<imgL_t.cols; x++){
            int blue = Leye_addr[y*imgL_t.cols+x] & 0xff;
            int green = (Leye_addr[y*imgL_t.cols+x] & 0xff00)>>8;
            int red = (Leye_addr[y*imgL_t.cols+x] & 0xff0000)>>16;
            ptr[x] = Vec3b(blue, green, red);
        }
    }
    // Copy "Right Camera Image" to Mat
    for (int y=0; y<imgR_t.rows; y++){
        Vec3b* ptr = imgR_t.ptr<Vec3b>(y);
        for (int x=0; x<imgR_t.cols; x++){
            int blue = Reye_addr[y*imgR_t.cols+x] & 0xff;
            int green = (Reye_addr[y*imgR_t.cols+x] & 0xff00)>>8;
            int red = (Reye_addr[y*imgR_t.cols+x] & 0xff0000)>>16;
            ptr[x] = Vec3b(blue, green, red);
        }
    }

    Mat img1(Size(640480), CV_8UC3);
    Mat img2(Size(640480), CV_8UC3);

    resize(imgL_t, img1, img1.size(), 00, INTER_CUBIC);
    resize(imgR_t, img2, img2.size(), 00, INTER_CUBIC);
    // End by marsee

    if( scale != 1.f )
    {


変更してから、g++_opencv でコンパイルした。(opencv 用のg++ コンパイルコマンドを作ってある)
./stereo_match_cam -i intrinsics.yml -e extrinsics.yml
コマンドで起動した。
stereo_calib_86_160330.png

left, right, disparity ウインドウが開いた。
stereo_calib_87_160330.jpg

上手く行った。これでソフトウェアでは、上手く行ったので、ハードウェアでのアクセラレーション方法を考えよう。
  1. 2016年03月31日 03:17 |
  2. OpenCV
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


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

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