firewirecameras.cpp

来自「一个语言识别引擎」· C++ 代码 · 共 1,029 行 · 第 1/3 页

                }

            for(w = 2; w < out_width-2; w++)
                {
                    x = 3 * w;
                    n = index + x;
                    temp_buff[n+0] = (unsigned char) row[x+0];
                    temp_buff[n+1] = (unsigned char) row[x+1];
                    temp_buff[n+2] = (unsigned char) row[x+2];
                }
        }  

    delete [] row;

    ////////////
  
    int * col;
    col = new int[out_height*3];
    for(index=0; index<out_height*3; index++) col[index]=0;

    for(w = 2; w < out_width-2; w++)
        {
            x = 3*w;
            for(h = 2; h < out_height-2; h++)
                {
                    y = 3*h;
                    n = ((2*h)*s) + x;
                    col[y+0] = (temp_buff[(n-(2*s))+0] +(temp_buff[(n-s)+0]*4) + (temp_buff[n+0]*6) + 
                                (temp_buff[(n+s)+0]*4) + temp_buff[(n+(2*s))+0])/16;
                    col[y+1] = (temp_buff[(n-(2*s))+1] + (temp_buff[(n-s)+1]*4) + (temp_buff[n+1]*6) + 
                                (temp_buff[(n+s)+1]*4) + temp_buff[(n+(2*s))+1])/16;
                    col[y+2] = (temp_buff[(n-(2*s))+2] + (temp_buff[(n-s)+2]*4) + (temp_buff[n+2]*6) + 
                                (temp_buff[(n+s)+2]*4) + temp_buff[(n+(2*s))+2])/16;
                }
 
            for(h = 2; h < out_height-2; h++)
                {
                    y = 3*h;
                    n = (h*out_width*3)+x;
                    buff_out[n+0] = (unsigned char) col[y+0];
                    buff_out[n+1] = (unsigned char) col[y+1];
                    buff_out[n+2] = (unsigned char) col[y+2];
                }
        }
  
    delete [] col;
}


void FWCameras::capture_color_image_3x3(unsigned char * & buff_out, int camera_num)
{
    if(!cam_num_ok(camera_num)) return;
    static unsigned char temp_buff [320*240*3];
    if(PRINTF_ON) printf("capture_a_frame\n");
    capture_a_frame(camera_num);
 
    if(PRINTF_ON) printf("capture_a_frame completed!\n");

    int frame_width, frame_height;
    frame_width = cameras[camera_num].capture.frame_width;
    frame_height = cameras[camera_num].capture.frame_height;
    unsigned char * buffer;
    buffer = (unsigned char *) (cameras[camera_num].capture.capture_buffer);

    if(PRINTF_ON) printf("frame_height = %d, frame_width = %d \n", frame_height, frame_width);
  
    int dh, dw;
    dh = 2; dw = 2;
   
    int tw,th; th = frame_height/dh; tw = frame_width/dw;
    //int sz = th*tw*3;

    //int r_int, 
    int g_int, b_int;
    //unsigned char r,g,b;
    int h,w;

    int index;
    int temp_width, temp_height, temp_size; 
    temp_width = 320; temp_height = 240; temp_size = temp_width * temp_height * 3;
    //set to 0
    for(index=0; index<temp_size; index++) temp_buff[index] = 0;

    for(h = 2; h < (frame_height-1); h=h+dh)
        {
            for(w = 2; w < (frame_width-1); w=w+dw)
                {
                    index = ( (w/dw) + ((h/dh)*tw) ) * 3; 
	  
                    temp_buff[index] = buffer[(h*frame_width)+w];
	
                    g_int = (  buffer[(h*frame_width)+(w-1)] +   buffer[(h*frame_width)+(w+1)] + 
                               buffer[((h-1)*frame_width)+w] +   buffer[((h+1)*frame_width)+w])/4;
                    temp_buff[index+1] = (unsigned char) g_int;

                    b_int = (  buffer[((h-1)*frame_width)+(w-1)] +   buffer[((h+1)*frame_width)+(w+1)] + 
                               buffer[((h-1)*frame_width)+(w+1)] +   buffer[((h+1)*frame_width)+(w-1)])/4;
                    temp_buff[index+2] = (unsigned char) b_int;
                }
        }

    release_capture_buffer(&(cameras[camera_num].capture), camera_num);

    /////////////////////////////////////////////
    //perform a 3x3 separable convolution and downsample by 2

    int out_width, out_height, out_size;
    out_width = 160; out_height = 120; out_size = out_width * out_height * 3;

    for(index=0; index < out_size; index++) buff_out[index] = 0;
  
    int * row;
    row = new int [out_width*3];
    for(index=0; index < out_width*3; index++) row[index]=0;

    int n; int x; int y;
    int s = (temp_width*3);
 
    for(h = 0; h < temp_height; h++)
        {
            index = h*(temp_width*3);
            for(w = 1; w < out_width-1; w++)
                {	  
                    x = 3 * w;
                    n = index + (2*x);
                    row[x+0] = (temp_buff[n-3] + (temp_buff[n]*2) + temp_buff[n+3])/4;
                    row[x+1] = (temp_buff[n-2] + (temp_buff[n+1]*2) + temp_buff[n+4])/4;
                    row[x+2] = (temp_buff[n-1] + (temp_buff[n+2]*2) + temp_buff[n+5])/4;
                }

            for(w = 1; w < out_width-1; w++)
                {
                    x = 3 * w;
                    n = index + x;
                    temp_buff[n+0] = (unsigned char) row[x+0];
                    temp_buff[n+1] = (unsigned char) row[x+1];
                    temp_buff[n+2] = (unsigned char) row[x+2];
                }
        }  

    delete [] row;

    ////////////
  
    int * col;
    col = new int [out_height*3];
    for(index=0; index<out_height*3; index++) col[index]=0;

    for(w = 1; w < out_width-1; w++)
        {
            x = 3*w;
            for(h = 1; h < out_height-1; h++)
                {
                    y = 3*h;
                    n = ((2*h)*s) + x;
                    col[y+0] = (temp_buff[(n-s)+0] + (temp_buff[n+0]*2) + temp_buff[(n+s)+0])/4;
                    col[y+1] = (temp_buff[(n-s)+1] + (temp_buff[n+1]*2) + temp_buff[(n+s)+1])/4;
                    col[y+2] = (temp_buff[(n-s)+2] + (temp_buff[n+2]*2) + temp_buff[(n+s)+2])/4;
                }
      
            for(h = 1; h < out_height-1; h++)
                {
                    y = 3*h;
                    n = (h*out_width*3)+x;
                    buff_out[n+0] = (unsigned char) col[y+0];
                    buff_out[n+1] = (unsigned char) col[y+1];
                    buff_out[n+2] = (unsigned char) col[y+2];
                }
        }
  
    delete [] col;  
}

#if 0
void FWCameras::capture_mono_image(YARPImageOf<YarpPixelMono> &grabbed_image_out, int camera_num)
{
    if(!cam_num_ok(camera_num)) return;
    capture_a_frame(camera_num);
    int frame_width, frame_height;
    frame_width = cameras[camera_num].capture.frame_width;
    frame_height = cameras[camera_num].capture.frame_height;
    unsigned char * buffer;
    buffer = (unsigned char *) (cameras[camera_num].capture.capture_buffer);

    if(PRINTF_ON) printf("frame_height = %d, frame_width = %d \n", frame_height, frame_width);
  
    int dh, dw;
    dh = 4;
    dw = 4;

    grabbed_image_out.Resize(frame_width/dw, frame_height/dh);
  
    int r_int, g_int, b_int, t_int;
    unsigned char r,g,b;
    int h,w;

    for(h = 2; h < (frame_height-1); h=h+dh)
        {
            for(w = 2; w < (frame_width-1); w=w+dw)
                {
                    r = buffer[(h*frame_width)+w];
                    g_int = (buffer[(h*frame_width)+(w-1)] + buffer[(h*frame_width)+(w+1)] + 
                             buffer[((h-1)*frame_width)+w] + buffer[((h+1)*frame_width)+w])/4;
                    g = (unsigned char) g_int;
                    b_int = (buffer[((h-1)*frame_width)+(w-1)] + buffer[((h+1)*frame_width)+(w+1)] + 
                             buffer[((h-1)*frame_width)+(w+1)] + buffer[((h+1)*frame_width)+(w-1)])/4;
                    b = (unsigned char) b_int;
                    t_int = (r+g+b)/3;
	  
                    grabbed_image_out.Pixel(w/dw,h/dh) = (unsigned char) t_int; 
                }
        }

    release_capture_buffer(&(cameras[camera_num].capture), camera_num);
}
#endif

int FWCameras::getX(int camera_num)
{
    int buf_w, buf_h, buf_length;

    if(!cam_num_ok(camera_num)) return 0;

    switch(capture_size)
        {
        case _320x240:
            buf_w=320; buf_h=240;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120_5x5:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120_3x3:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        default:
            buf_w=320; buf_h=240;
            buf_length = buf_w*buf_h*3;
            break;
        }

    return buf_w;
}

int FWCameras::getY(int camera_num)
{
    int buf_w, buf_h, buf_length;
    if(!cam_num_ok(camera_num)) return 0;

    switch(capture_size)
        {
        case _320x240:
            buf_w=320; buf_h=240;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120_5x5:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120_3x3:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        default:
            buf_w=320; buf_h=240;
            buf_length = buf_w*buf_h*3;
            break;
        }

    return buf_h;
}

int FWCameras::getBufferLength(int camera_num)
{
    int buf_w, buf_h, buf_length;
    if(!cam_num_ok(camera_num)) return(false);

    switch(capture_size)
        {
        case _320x240:
            buf_w=320; buf_h=240;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120_5x5:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        case _160x120_3x3:
            buf_w=160; buf_h=120;
            buf_length = buf_w*buf_h*3;
            break;
        default:
            buf_w=320; buf_h=240;
            buf_length = buf_w*buf_h*3;
            break;
        }

    return buf_length;
}

bool FWCameras::init_cameras(bool dma_on_in)
{
    init_variables(dma_on_in);
    bool init_ok = init_firewire();
    if(init_ok) 
        {
            //int num_camera=GetNumberOfCameras();
            fprintf(stderr, "Found %d cameras\n", num_cameras);
            for(int cam_num=0; cam_num<num_cameras; cam_num++) 
                {
                    fw_ready[cam_num] = start_firewire(cam_num);
                    if(cam_num_ok(cam_num))
                        {
                            if (PRINTF_ON)
                                {
                                    printf("camera info for camera #%d\n", cam_num);
                                    get_camera_info(cam_num);
                                }
                        } 
                    else
                        {
                            printf("\ncamera %d will not be usable\n", cam_num);
                            init_ok = false;
                        }
                }
        }   
    return init_ok;
}