左目カメラ画像と右目カメラ画像をBMPファイルに変換するアプリケーションを作成した

”OpenCV 2.4.10 の stereo_match.cpp をやってみた”と”OpenCV 2.4.10 の stereo_match.cpp をやってみた2”でやってみたOpenCVのステレオカメラのキャリブレーションを自分の環境でもやってみたくなって、左目カメラ画像と右目カメラ画像をBMPファイルに変換するアプリケーションを作成した。

（2016/02/26：修正　左目カメラ画像の名前が left_0.bmp になってしまって、”_”が余計だったので、left0.bmp になるようにアプリケーションを修正しました。）

左目カメラ画像と右目カメラ画像をBMPファイルに変換するアプリケーションは、RL_capture_bmp.cpp という名前にした。
引数で左目カメラ画像と右目カメラ画像の名前を指定できる。デフォルトではそれぞれ、left, right という名前で、その後に番号とBMPファイルの拡張子が付く。（left00.bmp, right00.bmp left01.bmp, right01.bmp）
（2016/03/22： -n オプションを追加）

// -l : left bmp file name
// -r : right bmp file name
// -n : Start File Number
// -h : help

g++ でコンパイルを行った。
g++ RL_capture_bmp.cpp -o RL_capture_bmp

./RL_capture_bmp で起動した。
コマンドは2つで w で左目カメラ画像と右目カメラ画像をBMPファイルに保存する。q で Exit する。


left00.bmp, right00.bmp left01.bmp, right01.bmp　left02.bmp, right02.bmp ができているのが分かる。


left00.bmp を表示してみた。


right00.bmp を表示してみた。


やはり、細かいドットが出ているがステレオカメラのキャリブレーションは大丈夫だろうか？ダメならば、ここをデバックする必要があるかも知れない？

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

//
// RL_capture_bmp.cpp
// 2016/02/24
// 2016/03/22 : -n : Start File Number
//
// This software converts the left and right of the camera image to BMP file.
// -l : left bmp file name
// -r : right bmp file name
// -n : Start File Number
// -h : help
//

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

#include "bmpheader.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)

int WriteBMPfile(FILE *fbmp, volatile unsigned int *frame_buffer, BMP24FORMAT **bmp_data);

int main(int argc, char *argv[]){
    int opt;
    int c, help_flag=0;
    char left_bmp_fn[256] = "left";
    char right_bmp_fn[256] = "right";
    unsigned char  attr[1024];
    unsigned long  phys_addr;
    volatile unsigned int *Leye_addr, *Reye_addr;
    int i, j;
    int file_no = 0;
    FILE *fbmp;
    BMP24FORMAT **bmp_data; // 24 bits Date of BMP files (SVGA_HORIZONTAL_PIXELS * SVGA_VERTICAL_LINES)

    while ((opt=getopt(argc, argv, "l:r:n:h")) != -1){
        switch (opt){
            case 'l':
                strcpy(left_bmp_fn, optarg);
                break;
            case 'r':
                strcpy(right_bmp_fn, optarg);
                break;
            case 'n':
                file_no = atoi(optarg);
                break;
            case 'h':
                help_flag = 1;
                break;
        }
    }
   
    if (help_flag == 1){ // help
        printf("Usage : RL_capture_bmp [-l <left bmp file name>] [-r <right bmp file name>] [-n <Start File Number>] [-h]\n");
    }

    // udmabuf0
    int fdf = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The chache 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);

    // allocated the memory for bmp file
    if ((bmp_data=(BMP24FORMAT **)malloc(sizeof(BMP24FORMAT *)*SVGA_VERTICAL_LINES)) == NULL){
        fprintf(stderr, "Can not allocate memory of the first dimension of SVGA_VERTICAL_LINES of bmp_data\n");
        exit(1);
    }
    for (i=0; i<SVGA_VERTICAL_LINES; i++){
        if ((bmp_data[i]=(BMP24FORMAT *)malloc(sizeof(BMP24FORMAT) * SVGA_HORIZONTAL_PIXELS)) == NULL){
            fprintf(stderr, "Can not allocate %d th memory of the first dimension of bmp_data\n", i);
            exit(1);
        }
    }
   
    // assigned the left and right eys's frame buffer
    Leye_addr = frame_buffer; // The Left Camera Image
    Reye_addr = (volatile unsigned int *)((unsigned)frame_buffer+SVGA_3_PICTURES+0x8); // The Right Camera Image

    char lbmp_file[256];
    char rbmp_file[256];

    // w - writed the left and right eye's bmp files.  q - exit.
    c = getc(stdin);
    while(c != 'q'){
        switch ((char)c) {
            case 'w' : // w - writed the left and right eye's bmp files.
                // writed the left and right eys's frame buffer
                sprintf(lbmp_file, "left%d.bmp", file_no);
                if ((fbmp=fopen(lbmp_file, "wb")) == NULL){
                    fprintf(stderr, "Cannot open %s in binary mode\n", lbmp_file);
                    exit(1);
                }
                WriteBMPfile(fbmp, Leye_addr, bmp_data);
                fclose(fbmp);

                sprintf(rbmp_file, "right%d.bmp", file_no);
                if ((fbmp=fopen(rbmp_file, "wb")) == NULL){
                    fprintf(stderr, "Cannot open %s in binary mode\n", rbmp_file);
                    exit(1);
                }
                WriteBMPfile(fbmp, Reye_addr, bmp_data);
                fclose(fbmp);
                
                printf("file No. = %d\n", file_no);

                file_no++;
                break;
        }
        c = getc(stdin);
    }

    for(i=0; i<SVGA_VERTICAL_LINES; i++){
        free(bmp_data[i]);
    }
    free(bmp_data);
    munmap((void *)frame_buffer, (SVGA_3_PICTURES+XGA_3_PICTURES+SVGA_ALL_DISP_ADDRESS));
    close(fdf);
    
    return(0);
}

int WriteBMPfile(FILE *fbmp, volatile unsigned *frame_buffer, BMP24FORMAT **bmp_data){
    BITMAPFILEHEADER bmpfh; // file header for a bmp file
    BITMAPINFOHEADER bmpih; // INFO header for BMP file

    // Copy the camera color data of the bmp_data (data of BMP when its starts from lower left)
    for (int i=0; i<SVGA_VERTICAL_LINES; i++){
        for (int j=0; j<SVGA_HORIZONTAL_PIXELS; j++){
            bmp_data[(SVGA_VERTICAL_LINES-1)-i][j].red = (frame_buffer[i*SVGA_HORIZONTAL_PIXELS+j]>>16)&0xff;
            bmp_data[(SVGA_VERTICAL_LINES-1)-i][j].green = (frame_buffer[i*SVGA_HORIZONTAL_PIXELS+j]>>8)&0xff;
            bmp_data[(SVGA_VERTICAL_LINES-1)-i][j].blue = (frame_buffer[i*SVGA_HORIZONTAL_PIXELS+j])&0xff;
        }
    }

    // Assign a value to the file header of the BMP file
    bmpfh.bfType = 0x4d42;
    bmpfh.bfSize = SVGA_HORIZONTAL_PIXELS*SVGA_VERTICAL_LINES*3+54;
    bmpfh.bfReserved1 = 0;
    bmpfh.bfReserved2 = 0;
    bmpfh.bfOffBits = 0x36;

    // Assign a value to the INFO header of the BMP file
    bmpih.biSize = 0x28;
    bmpih.biWidth = SVGA_HORIZONTAL_PIXELS;
    bmpih.biHeight = SVGA_VERTICAL_LINES;
    bmpih.biPlanes = 0x1;
    bmpih.biBitCount = 24;
    bmpih.biCompression = 0;
    bmpih.biSizeImage = 0;
    bmpih.biXPixPerMeter = 3779;
    bmpih.biYPixPerMeter = 3779;
    bmpih.biClrUsed = 0;
    bmpih.biClrImporant = 0;

    // Writing of BMP file header
    fwrite(&bmpfh.bfType, sizeof(char), 2, fbmp);
    fwrite(&bmpfh.bfSize, sizeof(long), 1, fbmp);
    fwrite(&bmpfh.bfReserved1, sizeof(short), 1, fbmp);
    fwrite(&bmpfh.bfReserved2, sizeof(short), 1, fbmp);
    fwrite(&bmpfh.bfOffBits, sizeof(long), 1, fbmp);

    // Writing of BMP INFO header
    fwrite(&bmpih, sizeof(BITMAPINFOHEADER), 1, fbmp);

    // Writing of lbmp_data anr rbmp_data
    for (int i=0; i<SVGA_VERTICAL_LINES; i++) {
        for (int j=0; j<SVGA_HORIZONTAL_PIXELS; j++) {
            fputc((int)bmp_data[i][j].blue, fbmp);
            fputc((int)bmp_data[i][j].green, fbmp);
            fputc((int)bmp_data[i][j].red, fbmp);
        }
    }
}