git.maemo.org Git - opencv/blob - otherlibs/highgui/cvcap_dc1394.cpp

   1 /* This is the contributed code:
   2 Firewire and video4linux camera support for highgui
   3
   4 2003-03-12  Magnus Lundin
   5 lundin@mlu.mine.nu
   6
   7 THIS EXEPERIMENTAL CODE
   8 Tested on 2.4.19 with 1394, video1394, v4l, dc1394 and raw1394 support
   9
  10 This set of files adds support for firevre and usb cameras.
  11 First it tries to install a firewire camera,
  12 if that fails it tries a v4l/USB camera
  13
  14 It has been tested with the motempl sample program
  15
  16 INSTALLATION
  17 Install OpenCV
  18 Install v4l
  19 Install dc1394 raw1394 - coriander should work with your camera
  20     Backup highgui folder
  21     Copy new files
  22     cd into highgui folder
  23     make clean  (cvcap.cpp must be rebuilt)
  24     make
  25     make install
  26
  27
  28 The build is controlled by the following entries in the highgui Makefile:
  29
  30 libhighgui_la_LIBADD = -L/usr/X11R6/lib -lXm -lMrm -lUil -lpng  -ljpeg -lz -ltiff -lavcodec -lraw1394 -ldc1394_control
  31 DEFS = -DHAVE_CONFIG_H -DHAVE_DC1394 HAVE_CAMV4L
  32
  33
  34 Now it should be possible to use highgui camera functions, works for me.
  35
  36
  37 THINGS TO DO
  38 Better ways to select 1394 or v4l camera
  39 Better support for videosize
  40 Format7
  41
  42 Comments and changes welcome
  43 /Magnus
  44
  45 2005-10-19 Roman Stanchak
  46 rstanchak@yahoo.com
  47
  48 Support added for setting MODE and other DC1394 properties.  Also added CONVERT_RGB flag
  49 which indicates whether or not color conversion is performed in cvRetrieveFrame.  The default
  50 for CONVERT_RGB=1 for backward compatibility.
  51
  52 Tested with 2.6.12 with libdc1394-1.0.0, libraw1394-0.10.1 using a Point Grey Flea
  53
  54 */
  55
  56
  57 /*M///////////////////////////////////////////////////////////////////////////////////////
  58 //
  59 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
  60 //
  61 //  By downloading, copying, installing or using the software you agree to this license.
  62 //  If you do not agree to this license, do not download, install,
  63 //  copy or use the software.
  64 //
  65 //
  66 //                        Intel License Agreement
  67 //                For Open Source Computer Vision Library
  68 //
  69 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  70 // Third party copyrights are property of their respective owners.
  71 //
  72 // Redistribution and use in source and binary forms, with or without modification,
  73 // are permitted provided that the following conditions are met:
  74 //
  75 //   * Redistribution's of source code must retain the above copyright notice,
  76 //     this list of conditions and the following disclaimer.
  77 //
  78 //   * Redistribution's in binary form must reproduce the above copyright notice,
  79 //     this list of conditions and the following disclaimer in the documentation
  80 //     and/or other materials provided with the distribution.
  81 //
  82 //   * The name of Intel Corporation may not be used to endorse or promote products
  83 //     derived from this software without specific prior written permission.
  84 //
  85 // This software is provided by the copyright holders and contributors "as is" and
  86 // any express or implied warranties, including, but not limited to, the implied
  87 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  88 // In no event shall the Intel Corporation or contributors be liable for any direct,
  89 // indirect, incidental, special, exemplary, or consequential damages
  90 // (including, but not limited to, procurement of substitute goods or services;
  91 // loss of use, data, or profits; or business interruption) however caused
  92 // and on any theory of liability, whether in contract, strict liability,
  93 // or tort (including negligence or otherwise) arising in any way out of
  94 // the use of this software, even if advised of the possibility of such damage.
  95 //
  96 //M*/
  97
  98 #include "_highgui.h"
  99
 100 #if !defined WIN32 && defined HAVE_DC1394
 101
 102 #include <unistd.h>
 103 #include <stdint.h>
 104 #include <libraw1394/raw1394.h>
 105 #include <libdc1394/dc1394_control.h>
 106
 107 #ifdef NDEBUG
 108 #define CV_WARN(message)
 109 #else
 110 #define CV_WARN(message) fprintf(stderr, "warning: %s (%s:%d)\n", message, __FILE__, __LINE__)
 111 #endif
 112
 113 #define CV_DC1394_CALL(expr)                                                  \
 114 if((expr)<0){                                                                 \
 115         OPENCV_ERROR(CV_StsInternal, "", "libdc1394 function call returned < 0"); \
 116 }
 117
 118 #define  DELAY              50000
 119
 120 // bpp for 16-bits cameras... this value works for PtGrey DragonFly...
 121 #define MONO16_BPP 8
 122
 123 /* should be in pixelformat */
 124 static void uyv2bgr(const unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);
 125 static void uyvy2bgr(const unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);
 126 static void uyyvyy2bgr(const unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);
 127 static void y2bgr(const unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);
 128 static void y162bgr(const unsigned char *src, unsigned char *dest, unsigned long long int NumPixels, int bits);
 129 static void rgb482bgr(const unsigned char *src8, unsigned char *dest, unsigned long long int NumPixels, int bits);
 130
 131 static char * videodev[4]={
 132   "/dev/video1394/0",
 133   "/dev/video1394/1",
 134   "/dev/video1394/2",
 135   "/dev/video1394/3"
 136 };
 137
 138 typedef struct CvCaptureCAM_DC1394
 139 {
 140     raw1394handle_t handle;
 141     nodeid_t  camera_node;
 142     dc1394_cameracapture* camera;
 143     int format;
 144     int mode;
 145     int color_mode;
 146     int frame_rate;
 147     char * device_name;
 148     IplImage frame;
 149         int convert;
 150         int buffer_is_writeable;  // indicates whether frame.imageData is allocated by OpenCV or DC1394
 151 }
 152 CvCaptureCAM_DC1394;
 153
 154 static void icvCloseCAM_DC1394( CvCaptureCAM_DC1394* capture );
 155
 156 static int icvGrabFrameCAM_DC1394( CvCaptureCAM_DC1394* capture );
 157 static IplImage* icvRetrieveFrameCAM_DC1394( CvCaptureCAM_DC1394* capture );
 158
 159 static double icvGetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id );
 160 static int    icvSetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id, double value );
 161
 162 // utility functions
 163 static int    icvFormatSupportedCAM_DC1394(int format, quadlet_t formats);
 164 static int    icvModeSupportedCAM_DC1394(int format, int mode, quadlet_t modes);
 165 static int    icvColorMode( int mode );
 166 static unsigned int icvGetBestFrameRate( CvCaptureCAM_DC1394 * capture, int format, int mode);
 167 static int    icvResizeFrame(CvCaptureCAM_DC1394 * capture);
 168
 169 /***********************   Implementations  ***************************************/
 170 #define MAX_PORTS 3
 171 #define MAX_CAMERAS 8
 172 #define NUM_BUFFERS 8
 173 struct raw1394_portinfo ports[MAX_PORTS];
 174 static raw1394handle_t handles[MAX_PORTS];
 175 static int camCount[MAX_PORTS];
 176 static int numPorts = -1;
 177 static int numCameras = 0;
 178 static nodeid_t *camera_nodes;
 179 struct camnode {dc1394_cameracapture cam;int portnum;} cameras[MAX_CAMERAS];
 180
 181 static const int preferred_modes[]
 182 = {
 183     // uncomment the following line to test a particular mode:
 184     //FORMAT_VGA_NONCOMPRESSED, MODE_640x480_MONO16, 0,
 185     FORMAT_SVGA_NONCOMPRESSED_2,
 186     MODE_1600x1200_RGB, MODE_1600x1200_YUV422, MODE_1280x960_RGB, MODE_1280x960_YUV422,
 187     MODE_1600x1200_MONO, MODE_1280x960_MONO, MODE_1600x1200_MONO16, MODE_1280x960_MONO16,
 188     FORMAT_SVGA_NONCOMPRESSED_1,
 189     MODE_1024x768_RGB, MODE_1024x768_YUV422, MODE_800x600_RGB, MODE_800x600_YUV422,
 190     MODE_1024x768_MONO, MODE_800x600_MONO, MODE_1024x768_MONO16, MODE_800x600_MONO16,
 191     FORMAT_VGA_NONCOMPRESSED,
 192    MODE_640x480_RGB, MODE_640x480_YUV422, MODE_640x480_YUV411, MODE_320x240_YUV422,
 193     MODE_160x120_YUV444, MODE_640x480_MONO, MODE_640x480_MONO16,
 194     FORMAT_SCALABLE_IMAGE_SIZE,
 195     MODE_FORMAT7_0, MODE_FORMAT7_1, MODE_FORMAT7_2, MODE_FORMAT7_3,
 196     MODE_FORMAT7_4, MODE_FORMAT7_5, MODE_FORMAT7_6, MODE_FORMAT7_7,
 197         0
 198 };
 199
 200 void icvInitCapture_DC1394(){
 201         int p;
 202
 203         raw1394handle_t raw_handle = raw1394_new_handle();
 204         if( raw_handle == 0 ) {
 205                 numPorts = 0;
 206                 return;
 207         }
 208         numPorts = raw1394_get_port_info(raw_handle, ports, MAX_PORTS);
 209         raw1394_destroy_handle(raw_handle);
 210         for (p = 0; p < numPorts; p++) {
 211                 handles[p] = dc1394_create_handle(p);
 212                 if (handles[p]==NULL) {  numPorts=-1; return; /*ERROR_CLEANUP_EXIT*/   }
 213
 214                 /* get the camera nodes and describe them as we find them */
 215                 camera_nodes = dc1394_get_camera_nodes(handles[p], &camCount[p], 0);
 216                 for (int i=0;i<camCount[p];i++) {
 217                         cameras[numCameras].cam.node = camera_nodes[i];
 218                         cameras[numCameras].portnum = p;
 219                         dc1394_stop_iso_transmission(handles[p], camera_nodes[i]);
 220                         numCameras++;
 221                 }
 222         }
 223 };
 224
 225 static CvCaptureCAM_DC1394 * icvCaptureFromCAM_DC1394 (int index)
 226 {
 227         quadlet_t modes[8], formats;
 228         int i;
 229
 230         if (numPorts<0) icvInitCapture_DC1394();
 231         if (numPorts==0)
 232                 return 0;     /* No i1394 ports found */
 233         if (numCameras<1)
 234                 return 0;
 235         if (index>=numCameras)
 236                 return 0;
 237         if (index<0)
 238                 return 0;
 239
 240         CvCaptureCAM_DC1394 * pcap = (CvCaptureCAM_DC1394*)cvAlloc(sizeof(*pcap));
 241
 242         /* Select a port and camera */
 243         pcap->device_name = videodev[cameras[index].portnum];
 244         pcap->handle = handles[cameras[index].portnum];
 245         pcap->camera = &cameras[index].cam;
 246
 247         // get supported formats
 248         if (dc1394_query_supported_formats(pcap->handle, pcap->camera->node, &formats)<0) {
 249                 fprintf(stderr,"%s:%d: Could not query supported formats\n",__FILE__,__LINE__);
 250                 formats=0x0;
 251         }
 252         for (i=0; i < NUM_FORMATS; i++) {
 253                 modes[i]=0;
 254                 if (icvFormatSupportedCAM_DC1394(i+FORMAT_MIN, formats)){
 255                         if (dc1394_query_supported_modes(pcap->handle, pcap->camera->node, i+FORMAT_MIN, &modes[i])<0) {
 256                                 fprintf(stderr,"%s:%d: Could not query Format%d modes\n",__FILE__,__LINE__,i);
 257                         }
 258                 }
 259         }
 260
 261         pcap->format = 0;
 262         pcap->mode = 0;
 263         pcap->color_mode = 0;
 264         pcap->frame_rate = 0;
 265
 266         int format_idx = -1;
 267
 268         // scan the list of preferred modes, and find a supported one
 269         for(i=0; (pcap->mode == 0) && (preferred_modes[i] != 0); i++) {
 270                 if((preferred_modes[i] >= FORMAT_MIN) && (preferred_modes[i] <= FORMAT_MAX)) {
 271                         pcap->format = preferred_modes[i];
 272                         format_idx = preferred_modes[i] - FORMAT_MIN;
 273                         continue;
 274                 }
 275                 assert(format_idx != -1);
 276                 if ( ! icvFormatSupportedCAM_DC1394(pcap->format, formats) )
 277                         continue;
 278                 if ( icvModeSupportedCAM_DC1394(pcap->format, preferred_modes[i], modes[format_idx]) ){
 279                         pcap->mode = preferred_modes[i];
 280                 }
 281         }
 282         if (pcap->mode == 0) {
 283                 fprintf(stderr,"%s:%d: Could not find a supported mode for this camera\n",__FILE__,__LINE__);
 284                 goto ERROR;
 285         }
 286
 287         pcap->color_mode = icvColorMode( pcap->mode );
 288         if( pcap->color_mode == -1){
 289                 fprintf(stderr,"%s:%d: ERROR: BPP is Unsupported!!\n",__FILE__,__LINE__);
 290                 goto ERROR;
 291         }
 292
 293         // set frame rate to optimal value given format and mode
 294         pcap->frame_rate = icvGetBestFrameRate(pcap, pcap->format, pcap->mode);
 295
 296         if (pcap->format!=FORMAT_SCALABLE_IMAGE_SIZE) { // everything except Format 7
 297                 if (dc1394_dma_setup_capture(pcap->handle, pcap->camera->node, index+1 /*channel*/,
 298                                         pcap->format, pcap->mode, SPEED_400,
 299                                         pcap->frame_rate, NUM_BUFFERS,
 300 #ifdef HAVE_DC1394_095
 301                                         0 /*do_extra_buffering*/,
 302 #endif
 303                                         1 /*DROP_FRAMES*/,
 304                                         pcap->device_name, pcap->camera) != DC1394_SUCCESS) {
 305                         fprintf(stderr,"%s:%d: Failed to setup DMA capture with VIDEO1394\n",__FILE__,__LINE__);
 306                         goto ERROR;
 307                 }
 308         }
 309         else {
 310                 if(dc1394_dma_setup_format7_capture(pcap->handle,pcap->camera->node,index+1 /*channel*/,
 311                                         pcap->mode, SPEED_400, QUERY_FROM_CAMERA,
 312                                         (unsigned int)QUERY_FROM_CAMERA, (unsigned int)QUERY_FROM_CAMERA,
 313                                         (unsigned int)QUERY_FROM_CAMERA, (unsigned int)QUERY_FROM_CAMERA,
 314                                         NUM_BUFFERS,
 315 #ifdef HAVE_DC1394_095
 316                                         0 /*do_extra_buffering*/,
 317 #endif
 318                                         1 /*DROP_FRAMES*/,
 319                                         pcap->device_name, pcap->camera) != DC1394_SUCCESS) {
 320                         fprintf(stderr,"%s:%d: Failed to setup DMA capture with VIDEO1394\n",__FILE__,__LINE__);
 321                         goto ERROR;
 322                 }
 323         }
 324
 325         if (dc1394_start_iso_transmission(pcap->handle, pcap->camera->node)!=DC1394_SUCCESS) {
 326                 fprintf(stderr,"%s:%d: Could not start ISO transmission\n",__FILE__,__LINE__);
 327                 goto ERROR;
 328         }
 329
 330         usleep(DELAY);
 331
 332         dc1394bool_t status;
 333         if (dc1394_get_iso_status(pcap->handle, pcap->camera->node, &status)!=DC1394_SUCCESS) {
 334                 fprintf(stderr,"%s:%d: Could get ISO status",__FILE__,__LINE__);
 335                 goto ERROR;
 336         }
 337         if (status==DC1394_FALSE) {
 338                 fprintf(stderr,"%s:%d: ISO transmission refuses to start",__FILE__,__LINE__);
 339                 goto ERROR;
 340         }
 341
 342         // convert camera image to RGB by default
 343         pcap->convert=1;
 344
 345         // no image data allocated yet
 346         pcap->buffer_is_writeable = 0;
 347
 348         memset(&(pcap->frame), 0, sizeof(IplImage));
 349         icvResizeFrame( pcap );
 350         return pcap;
 351
 352 ERROR:
 353         return 0;
 354 };
 355
 356 static void icvCloseCAM_DC1394( CvCaptureCAM_DC1394* capture ){
 357         dc1394_stop_iso_transmission(capture->handle, capture->camera->node);
 358         dc1394_dma_unlisten (capture->handle, capture->camera);
 359         /* Deallocate space for RGBA data */
 360         if(capture->convert){
 361                 cvFree(&capture->frame.imageData);
 362         }
 363 }
 364
 365 static int icvGrabFrameCAM_DC1394( CvCaptureCAM_DC1394* capture ){
 366         // TODO: should this function wait until the next frame is available or return
 367         // immediately ?
 368         float waiting = 0;
 369         do{
 370                 int result = dc1394_dma_single_capture_poll(capture->camera);
 371                 if(result==DC1394_SUCCESS){
 372                         return 1;
 373                 }
 374                 else if(result==DC1394_NO_FRAME){
 375                         usleep(1000000/120);  //sleep for at least a 1/2 of the frame rate
 376                         waiting += 1.0/120.0;
 377                 }
 378                 else{
 379                         printf("dc1394_dma_single_capture_poll failed\n");
 380                         return 0;
 381                 }
 382         } while(waiting<2);
 383         printf("dc1394_dma_single_capture_poll timed out\n");
 384         return 0;
 385 }
 386
 387 static IplImage* icvRetrieveFrameCAM_DC1394( CvCaptureCAM_DC1394* capture ){
 388         if(capture->camera->capture_buffer )
 389         {
 390                 if(capture->convert){
 391                         /* Convert to RGBA */
 392                         unsigned char * src = (unsigned char *)capture->camera->capture_buffer;
 393                         unsigned char * dst = (unsigned char *)capture->frame.imageData;
 394                         switch (capture->color_mode) {
 395                                 case COLOR_FORMAT7_RGB8:
 396                                         printf("icvRetrieveFrame convert RGB to BGR\n");
 397                                         /* Convert RGB to BGR */
 398                                         for (int i=0;i<capture->frame.imageSize;i+=6) {
 399                                                 dst[i]   = src[i+2];
 400                                                 dst[i+1] = src[i+1];
 401                                                 dst[i+2] = src[i];
 402                                                 dst[i+3] = src[i+5];
 403                                                 dst[i+4] = src[i+4];
 404                                                 dst[i+5] = src[i+3];
 405                                         }
 406                                         break;
 407                                 case COLOR_FORMAT7_YUV422:
 408                                         //printf("icvRetrieveFrame convert YUV422 to BGR %d\n");
 409                                         uyvy2bgr(src,
 410                                                         dst,
 411                                                         capture->camera->frame_width * capture->camera->frame_height);
 412                                         break;
 413                                 case COLOR_FORMAT7_MONO8:
 414                                         //printf("icvRetrieveFrame convert MONO8 to BGR %d\n");
 415                                         y2bgr(src,
 416                                                         dst,
 417                                                         capture->camera->frame_width * capture->camera->frame_height);
 418                                         break;
 419                                 case COLOR_FORMAT7_YUV411:
 420                                         //printf("icvRetrieveFrame convert YUV411 to BGR %d\n");
 421                                         uyyvyy2bgr(src,
 422                                                         dst,
 423                                                         capture->camera->frame_width * capture->camera->frame_height);
 424                                         break;
 425                                 case COLOR_FORMAT7_YUV444:
 426                                         //printf("icvRetrieveFrame convert YUV444 to BGR %d\n");
 427                                         uyv2bgr(src,
 428                                                         dst,
 429                                                         capture->camera->frame_width * capture->camera->frame_height);
 430                                         break;
 431                                 case COLOR_FORMAT7_MONO16:
 432                                         //printf("icvRetrieveFrame convert MONO16 to BGR %d\n");
 433                                         y162bgr(src,
 434                                                         dst,
 435                                                         capture->camera->frame_width * capture->camera->frame_height, MONO16_BPP);
 436                                         break;
 437                                 case COLOR_FORMAT7_RGB16:
 438                                         //printf("icvRetrieveFrame convert RGB16 to BGR %d\n");
 439                                         rgb482bgr(src,
 440                                                         dst,
 441                                                         capture->camera->frame_width * capture->camera->frame_height, MONO16_BPP);
 442                                         break;
 443                                 default:
 444                                         fprintf(stderr,"%s:%d: Unsupported color mode %d\n",__FILE__,__LINE__,capture->color_mode);
 445                                         return 0;
 446                         } /* switch (capture->mode) */
 447                 }
 448                 else{
 449                         // return raw data
 450                         capture->frame.imageData = (char *) capture->camera->capture_buffer;
 451                         capture->frame.imageDataOrigin = (char *) capture->camera->capture_buffer;
 452                 }
 453
 454                 // TODO: if convert=0, we are not actually done with the buffer
 455                 // but this seems to work anyway.
 456                 dc1394_dma_done_with_buffer(capture->camera);
 457
 458                 return &capture->frame;
 459         }
 460         return 0;
 461 };
 462
 463 static double icvGetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id ){
 464         int index=-1;
 465         switch ( property_id ) {
 466                 case CV_CAP_PROP_CONVERT_RGB:
 467                         return capture->convert;
 468                 case CV_CAP_PROP_MODE:
 469                         return capture->mode;
 470                 case CV_CAP_PROP_FORMAT:
 471                         return capture->format;
 472                 case CV_CAP_PROP_FPS:
 473                         CV_DC1394_CALL(dc1394_get_video_framerate(capture->handle, capture->camera->node,
 474                                         (unsigned int *) &capture->camera->frame_rate));
 475                         switch(capture->camera->frame_rate) {
 476                                 case FRAMERATE_1_875:
 477                                         return 1.875;
 478                                 case FRAMERATE_3_75:
 479                                         return 3.75;
 480                                 case FRAMERATE_7_5:
 481                                         return 7.5;
 482                                 case FRAMERATE_15:
 483                                         return 15.;
 484                                 case FRAMERATE_30:
 485                                         return 30.;
 486                                 case FRAMERATE_60:
 487                                         return 60;
 488 #if NUM_FRAMERATES > 6
 489                                 case FRAMERATE_120:
 490                                         return 120;
 491 #endif
 492 #if NUM_FRAMERATES > 7
 493                                 case FRAMERATE_240:
 494                                         return 240;
 495 #endif
 496                         }
 497         default:
 498                         index = property_id;  // did they pass in a LIBDC1394 feature flag?
 499                         break;
 500         }
 501         if(index>=FEATURE_MIN && index<=FEATURE_MAX){
 502                 dc1394bool_t has_feature;
 503                 CV_DC1394_CALL( dc1394_is_feature_present(capture->handle, capture->camera->node,
 504                                                                       index, &has_feature));
 505                 if(!has_feature){
 506                         CV_WARN("Feature is not supported by this camera");
 507                 }
 508                 else{
 509                         unsigned int value;
 510                         dc1394_get_feature_value(capture->handle, capture->camera->node, index, &value);
 511                         return (double) value;
 512                 }
 513         }
 514
 515         return 0;
 516 };
 517
 518 // resize capture->frame appropriately depending on camera and capture settings
 519 static int icvResizeFrame(CvCaptureCAM_DC1394 * capture){
 520         if(capture->convert){
 521                 // resize if sizes are different, formats are different
 522                 // or conversion option has changed
 523                 if(capture->camera->frame_width != capture->frame.width ||
 524                    capture->camera->frame_height != capture->frame.height ||
 525                    capture->frame.depth != 8 ||
 526                    capture->frame.nChannels != 3 ||
 527                    capture->frame.imageData == NULL ||
 528                    capture->buffer_is_writeable == 0)
 529                 {
 530                         if(capture->frame.imageData && capture->buffer_is_writeable){
 531                                 cvReleaseData( &(capture->frame));
 532                         }
 533                         cvInitImageHeader( &capture->frame, cvSize( capture->camera->frame_width,
 534                                                                                 capture->camera->frame_height ),
 535                                                                 IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
 536                         cvCreateData( &(capture->frame) );
 537                         capture->buffer_is_writeable = 1;
 538                 }
 539
 540         }
 541         else {
 542                 // free image data if allocated by opencv
 543                 if(capture->buffer_is_writeable){
 544                         cvReleaseData(&(capture->frame));
 545                 }
 546
 547                 // figure out number of channels and bpp
 548                 int bpp = 8;
 549                 int nch = 3;
 550                 int width = capture->camera->frame_width;
 551                 int height = capture->camera->frame_height;
 552                 double code = CV_FOURCC('B','G','R',0);
 553                 switch(capture->color_mode){
 554                 case COLOR_FORMAT7_YUV422:
 555                         nch = 2;
 556                         code = CV_FOURCC('Y','4','2','2');
 557                         break;
 558                 case COLOR_FORMAT7_MONO8:
 559                         code = CV_FOURCC('Y',0,0,0);
 560                         nch = 1;
 561                         break;
 562                 case COLOR_FORMAT7_YUV411:
 563                         code = CV_FOURCC('Y','4','1','1');
 564                         width *= 2;
 565                         nch = 3;  //yy[u/v]
 566                         break;
 567                 case COLOR_FORMAT7_YUV444:
 568                         code = CV_FOURCC('Y','U','V',0);
 569                         nch = 3;
 570                         break;
 571                 case COLOR_FORMAT7_MONO16:
 572                         code = CV_FOURCC('Y',0,0,0);
 573                         bpp = IPL_DEPTH_16S;
 574                         nch = 1;
 575                         break;
 576                 case COLOR_FORMAT7_RGB16:
 577                         bpp = IPL_DEPTH_16S;
 578                         nch = 3;
 579                         break;
 580                 default:
 581                         break;
 582                 }
 583                 // reset image header
 584                 cvInitImageHeader( &capture->frame,cvSize( width, height ), bpp, nch, IPL_ORIGIN_TL, 4 );
 585                 //assert(capture->frame.imageSize == capture->camera->quadlets_per_frame*4);
 586                 capture->buffer_is_writeable = 0;
 587         }
 588         return 1;
 589 }
 590
 591 // Toggle setting about whether or not RGB color conversion is to be performed
 592 // Allocates/Initializes capture->frame appropriately
 593 int icvSetConvertRGB(CvCaptureCAM_DC1394 * capture, int convert){
 594         if(convert==capture->convert){
 595                 // no action necessary
 596                 return 1;
 597         }
 598         capture->convert = convert;
 599         return icvResizeFrame( capture );
 600 }
 601
 602 // given desired format, mode, and modes bitmask from camera, determine if format and mode are supported
 603 static int
 604 icvFormatSupportedCAM_DC1394(int format, quadlet_t formats){
 605         // formats is a bitmask whose higher order bits indicate whether format is supported
 606         int shift = 31 - (format - FORMAT_MIN);
 607         int mask = 1 << shift;
 608         return (formats & mask) != 0;
 609 }
 610
 611 // analyze modes bitmask from camera to determine if desired format and mode are supported
 612 static int
 613 icvModeSupportedCAM_DC1394(int format, int mode, quadlet_t modes){
 614         // modes is a bitmask whose higher order bits indicate whether mode is supported
 615         int format_idx = format - FORMAT_MIN;
 616         int mode_format_min = MODE_FORMAT0_MIN + 32*format_idx;
 617         int shift = 31 - (mode - mode_format_min);
 618         int mask = 0x1 << shift;
 619         return (modes & mask) != 0;
 620 }
 621
 622 // Setup camera to use given dc1394 mode
 623 static int
 624 icvSetModeCAM_DC1394( CvCaptureCAM_DC1394 * capture, int mode ){
 625         quadlet_t modes, formats;
 626         //printf("<icvSetModeCAM_DC1394>\n");
 627
 628         // figure out corrent format for this mode
 629         int format = (mode - MODE_FORMAT0_MIN) / 32 + FORMAT_MIN;
 630
 631         // get supported formats
 632         if (dc1394_query_supported_formats(capture->handle, capture->camera->node, &formats)<0) {
 633                 fprintf(stderr,"%s:%d: Could not query supported formats\n",__FILE__,__LINE__);
 634                 return 0;
 635         }
 636
 637         // is format for requested mode supported ?
 638         if(icvFormatSupportedCAM_DC1394(format, formats)==0){
 639                 return 0;
 640         }
 641
 642         // get supported modes for requested format
 643         if (dc1394_query_supported_modes(capture->handle, capture->camera->node, format, &modes)<0){
 644                 fprintf(stderr,"%s:%d: Could not query supported modes for format %d\n",__FILE__,__LINE__, capture->format);
 645                 return 0;
 646         }
 647
 648         // is requested mode supported ?
 649         if(! icvModeSupportedCAM_DC1394(format, mode, modes) ){
 650                 return 0;
 651         }
 652
 653         int color_mode = icvColorMode( mode );
 654
 655         if(color_mode == -1){
 656                 return 0;
 657         }
 658
 659         int frame_rate = icvGetBestFrameRate(capture, format, mode);
 660
 661         dc1394_dma_unlisten(capture->handle, capture->camera);
 662         if (dc1394_dma_setup_capture(capture->handle, capture->camera->node, capture->camera->channel /*channel*/,
 663                                 format, mode, SPEED_400,
 664                                 frame_rate, NUM_BUFFERS,
 665 #ifdef HAVE_DC1394_095
 666                                 0 /*do_extra_buffering*/,
 667 #endif
 668                                 1 /*DROP_FRAMES*/,
 669                                 capture->device_name, capture->camera) != DC1394_SUCCESS) {
 670                 fprintf(stderr,"%s:%d: Failed to setup DMA capture with VIDEO1394\n",__FILE__,__LINE__);
 671                 return 0;
 672         }
 673         dc1394_start_iso_transmission(capture->handle, capture->camera->node);
 674
 675         capture->frame_rate = frame_rate;
 676         capture->format = format;
 677         capture->mode = mode;
 678         capture->color_mode = color_mode;
 679
 680         // now fix image size to match new mode
 681         icvResizeFrame( capture );
 682         return 1;
 683 }
 684
 685 // query camera for supported frame rates and select fastest for given format and mode
 686 static unsigned int icvGetBestFrameRate( CvCaptureCAM_DC1394 * capture, int format, int mode  ){
 687         quadlet_t framerates;
 688         if (dc1394_query_supported_framerates(capture->handle, capture->camera->node,
 689                                 format, mode, &framerates)!=DC1394_SUCCESS)
 690         {
 691                 fprintf(stderr,"%s:%d: Could not query supported framerates\n",__FILE__,__LINE__);
 692                 framerates = 0;
 693         }
 694
 695         for (int f=FRAMERATE_MAX; f>=FRAMERATE_MIN; f--) {
 696                 if (framerates & (0x1<< (31-(f-FRAMERATE_MIN)))) {
 697                         return f;
 698                 }
 699         }
 700         return 0;
 701 }
 702
 703 static int
 704 icvSetFrameRateCAM_DC1394( CvCaptureCAM_DC1394 * capture, double value ){
 705         unsigned int fps=15;
 706         if(capture->format == FORMAT_SCALABLE_IMAGE_SIZE)
 707                 return 0; /* format 7 has no fixed framerates */
 708         if (value==-1){
 709                 fps=icvGetBestFrameRate( capture, capture->format, capture->mode );
 710         }
 711         else if (value==1.875)
 712                 fps=FRAMERATE_1_875;
 713         else if (value==3.75)
 714                 fps=FRAMERATE_3_75;
 715         else if (value==7.5)
 716                 fps=FRAMERATE_7_5;
 717         else if (value==15)
 718                 fps=FRAMERATE_15;
 719         else if (value==30)
 720                 fps=FRAMERATE_30;
 721         else if (value==60)
 722                 fps=FRAMERATE_60;
 723 #if NUM_FRAMERATES > 6
 724         else if (value==120)
 725                 fps=FRAMERATE_120;
 726 #endif
 727 #if NUM_FRAMERATES > 7
 728         else if (value==240)
 729                 fps=FRAMERATE_240;
 730 #endif
 731         dc1394_set_video_framerate(capture->handle, capture->camera->node,fps);
 732         dc1394_get_video_framerate(capture->handle, capture->camera->node,
 733                                         (unsigned int *) &capture->camera->frame_rate);
 734
 735         return fps==(unsigned int) capture->camera->frame_rate;
 736 }
 737
 738 // for given mode return color format
 739 static int
 740 icvColorMode( int mode ){
 741         switch(mode) {
 742         case MODE_160x120_YUV444:
 743                 return COLOR_FORMAT7_YUV444;
 744         case MODE_320x240_YUV422:
 745         case MODE_640x480_YUV422:
 746         case MODE_800x600_YUV422:
 747         case MODE_1024x768_YUV422:
 748         case MODE_1280x960_YUV422:
 749         case MODE_1600x1200_YUV422:
 750                 return COLOR_FORMAT7_YUV422;
 751         case MODE_640x480_YUV411:
 752                 return COLOR_FORMAT7_YUV411;
 753         case MODE_640x480_RGB:
 754         case MODE_800x600_RGB:
 755         case MODE_1024x768_RGB:
 756         case MODE_1280x960_RGB:
 757         case MODE_1600x1200_RGB:
 758                 return COLOR_FORMAT7_RGB8;
 759         case MODE_640x480_MONO:
 760         case MODE_800x600_MONO:
 761         case MODE_1024x768_MONO:
 762         case MODE_1280x960_MONO:
 763         case MODE_1600x1200_MONO:
 764                 return COLOR_FORMAT7_MONO8;
 765         case MODE_640x480_MONO16:
 766         case MODE_800x600_MONO16:
 767         case MODE_1024x768_MONO16:
 768         case MODE_1280x960_MONO16:
 769         case MODE_1600x1200_MONO16:
 770                 return COLOR_FORMAT7_MONO16;
 771         case MODE_FORMAT7_0:
 772         case MODE_FORMAT7_1:
 773         case MODE_FORMAT7_2:
 774         case MODE_FORMAT7_3:
 775         case MODE_FORMAT7_4:
 776         case MODE_FORMAT7_5:
 777         case MODE_FORMAT7_6:
 778         case MODE_FORMAT7_7:
 779                 fprintf(stderr,"%s:%d: Format7 not yet supported\n",__FILE__,__LINE__);
 780         default:
 781                 break;
 782         }
 783         return -1;
 784 }
 785
 786 // function to set camera properties using dc1394 feature enum
 787 // val == -1 indicates to set this property to 'auto'
 788 static int
 789 icvSetFeatureCAM_DC1394( CvCaptureCAM_DC1394* capture, int feature_id, int val){
 790         dc1394bool_t isOn = DC1394_FALSE;
 791                 dc1394bool_t hasAutoCapability = DC1394_FALSE;
 792                 dc1394bool_t isAutoOn = DC1394_FALSE;
 793                 unsigned int nval;
 794                 unsigned int minval,maxval;
 795
 796                 // Turn the feature on if it is OFF
 797                 if( dc1394_is_feature_on(capture->handle, capture->camera->node, feature_id, &isOn)
 798                                 == DC1394_FAILURE ) {
 799                         return 0;
 800                 }
 801                 if( isOn == DC1394_FALSE ) {
 802                 // try to turn it on.
 803                 if( dc1394_feature_on_off(capture->handle, capture->camera->node, feature_id, 1) == DC1394_FAILURE ) {
 804                     fprintf(stderr, "error turning feature %d on!\n", feature_id);
 805                     return 0;
 806                 }
 807                 }
 808
 809                 // Check if the feature supports auto mode
 810                 dc1394_has_auto_mode(capture->handle, capture->camera->node, feature_id, &hasAutoCapability);
 811                 if( hasAutoCapability ) {
 812
 813                         // now check if the auto is on.
 814                         if( dc1394_is_feature_auto(capture->handle, capture->camera->node, feature_id, &isAutoOn ) == DC1394_FAILURE ) {
 815                                 fprintf(stderr, "error determining if feature %d has auto on!\n", index);
 816                                 return 0;
 817                         }
 818                 }
 819                 // Caller requested auto mode, but cannot support it
 820                 else if(val==-1){
 821             fprintf(stderr, "feature %d does not support auto mode\n", feature_id);
 822             return 0;
 823                 }
 824
 825                 if(val==-1){
 826             // if the auto mode isn't enabled, enable it
 827             if( isAutoOn == DC1394_FALSE ) {
 828                 if(dc1394_auto_on_off(capture->handle, capture->camera->node, feature_id, 1) == DC1394_FAILURE ) {
 829                     fprintf(stderr, "error turning feature %d auto ON!\n", feature_id);
 830                     return 0;
 831                 }
 832             }
 833                         return 1;
 834         }
 835
 836                 // ELSE turn OFF auto and adjust feature manually
 837                 if( isAutoOn == DC1394_TRUE ) {
 838                         if(dc1394_auto_on_off(capture->handle, capture->camera->node, feature_id, 0) == DC1394_FAILURE ) {
 839                                 fprintf(stderr, "error turning feature %d auto OFF!\n", feature_id);
 840                                 return 0;
 841                         }
 842                 }
 843
 844                 // Clamp val to within feature range
 845                 CV_DC1394_CALL( dc1394_get_min_value(capture->handle, capture->camera->node, feature_id, &minval));
 846                 CV_DC1394_CALL( dc1394_get_max_value(capture->handle, capture->camera->node, feature_id, &maxval));
 847                 val = MIN(maxval, MAX(val, minval));
 848
 849
 850                 if (dc1394_set_feature_value(capture->handle, capture->camera->node, feature_id, val) ==
 851                                 DC1394_FAILURE){
 852                         fprintf(stderr, "error setting feature value\n");
 853                         return 0;
 854                 }
 855                 if (dc1394_get_feature_value(capture->handle, capture->camera->node, feature_id, &nval) ==
 856                                 DC1394_FAILURE){
 857                         fprintf(stderr, "error setting feature value\n");
 858                         return 0;
 859                 }
 860                 return nval==(unsigned int)val;
 861
 862 }
 863
 864 // cvSetCaptureProperty callback function implementation
 865 static int
 866 icvSetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id, double value ){
 867         int index=-1;
 868         switch ( property_id ) {
 869                 case CV_CAP_PROP_CONVERT_RGB:
 870                         return icvSetConvertRGB( capture, value != 0 );
 871                 case CV_CAP_PROP_MODE:
 872                         return icvSetModeCAM_DC1394( capture, (int) value );
 873                 case CV_CAP_PROP_FPS:
 874                         return icvSetFrameRateCAM_DC1394( capture, value );
 875                 case CV_CAP_PROP_BRIGHTNESS:
 876                         index = FEATURE_BRIGHTNESS;
 877                         break;
 878                 case CV_CAP_PROP_CONTRAST:
 879                         index = FEATURE_GAMMA;
 880                         break;
 881                 case CV_CAP_PROP_SATURATION:
 882                         index = FEATURE_SATURATION;
 883                         break;
 884                 case CV_CAP_PROP_HUE:
 885                         index = FEATURE_HUE;
 886                         break;
 887                 case CV_CAP_PROP_GAIN:
 888                         index = FEATURE_GAIN;
 889                         break;
 890                 default:
 891                         index = property_id;  // did they pass in a LIBDC1394 feature flag?
 892                         break;
 893         }
 894         if(index>=FEATURE_MIN && index<=FEATURE_MAX){
 895                 return icvSetFeatureCAM_DC1394(capture, index, (int) value);
 896         }
 897         return 0;
 898 };
 899
 900 /**********************************************************************
 901  *
 902  *  CONVERSION FUNCTIONS TO RGB 24bpp
 903  *
 904  **********************************************************************/
 905
 906 /* color conversion functions from Bart Nabbe. *//* corrected by Damien: bad coeficients in YUV2RGB */
 907 #define YUV2RGB(y, u, v, r, g, b)\
 908         r = y + ((v*1436) >> 10);\
 909 g = y - ((u*352 + v*731) >> 10);\
 910 b = y + ((u*1814) >> 10);\
 911 r = r < 0 ? 0 : r;\
 912 g = g < 0 ? 0 : g;\
 913 b = b < 0 ? 0 : b;\
 914 r = r > 255 ? 255 : r;\
 915 g = g > 255 ? 255 : g;\
 916 b = b > 255 ? 255 : b
 917
 918         static void
 919 uyv2bgr(const unsigned char *src, unsigned char *dest,
 920                 unsigned long long int NumPixels)
 921 {
 922         register int i = NumPixels + (NumPixels << 1) - 1;
 923         register int j = NumPixels + (NumPixels << 1) - 1;
 924         register int y, u, v;
 925         register int r, g, b;
 926
 927         while (i > 0) {
 928                 v = src[i--] - 128;
 929                 y = src[i--];
 930                 u = src[i--] - 128;
 931                 YUV2RGB(y, u, v, r, g, b);
 932                 dest[j--] = r;
 933                 dest[j--] = g;
 934                 dest[j--] = b;
 935         }
 936 }
 937
 938         static void
 939 uyvy2bgr(const unsigned char *src, unsigned char *dest,
 940                 unsigned long long int NumPixels)
 941 {
 942         register int i = (NumPixels << 1) - 1;
 943         register int j = NumPixels + (NumPixels << 1) - 1;
 944         register int y0, y1, u, v;
 945         register int r, g, b;
 946
 947         while (i > 0) {
 948                 y1 = src[i--];
 949                 v = src[i--] - 128;
 950                 y0 = src[i--];
 951                 u = src[i--] - 128;
 952                 YUV2RGB(y1, u, v, r, g, b);
 953                 dest[j--] = r;
 954                 dest[j--] = g;
 955                 dest[j--] = b;
 956                 YUV2RGB(y0, u, v, r, g, b);
 957                 dest[j--] = r;
 958                 dest[j--] = g;
 959                 dest[j--] = b;
 960         }
 961 }
 962
 963
 964         static void
 965 uyyvyy2bgr(const unsigned char *src, unsigned char *dest,
 966                 unsigned long long int NumPixels)
 967 {
 968         register int i = NumPixels + (NumPixels >> 1) - 1;
 969         register int j = NumPixels + (NumPixels << 1) - 1;
 970         register int y0, y1, y2, y3, u, v;
 971         register int r, g, b;
 972
 973         while (i > 0) {
 974                 y3 = src[i--];
 975                 y2 = src[i--];
 976                 v = src[i--] - 128;
 977                 y1 = src[i--];
 978                 y0 = src[i--];
 979                 u = src[i--] - 128;
 980                 YUV2RGB(y3, u, v, r, g, b);
 981                 dest[j--] = r;
 982                 dest[j--] = g;
 983                 dest[j--] = b;
 984                 YUV2RGB(y2, u, v, r, g, b);
 985                 dest[j--] = r;
 986                 dest[j--] = g;
 987                 dest[j--] = b;
 988                 YUV2RGB(y1, u, v, r, g, b);
 989                 dest[j--] = r;
 990                 dest[j--] = g;
 991                 dest[j--] = b;
 992                 YUV2RGB(y0, u, v, r, g, b);
 993                 dest[j--] = r;
 994                 dest[j--] = g;
 995                 dest[j--] = b;
 996         }
 997 }
 998
 999         static void
1000 y2bgr(const unsigned char *src, unsigned char *dest,
1001                 unsigned long long int NumPixels)
1002 {
1003         register int i = NumPixels - 1;
1004         register int j = NumPixels + (NumPixels << 1) - 1;
1005         register int y;
1006
1007         while (i > 0) {
1008                 y = src[i--];
1009                 dest[j--] = y;
1010                 dest[j--] = y;
1011                 dest[j--] = y;
1012         }
1013 }
1014
1015         static void
1016 y162bgr(const unsigned char *src, unsigned char *dest,
1017                 unsigned long long int NumPixels, int bits)
1018 {
1019         register int i = (NumPixels << 1) - 1;
1020         register int j = NumPixels + (NumPixels << 1) - 1;
1021         register int y;
1022
1023         while (i > 0) {
1024                 y = src[i--];
1025                 y = (y + (src[i--] << 8)) >> (bits - 8);
1026                 dest[j--] = y;
1027                 dest[j--] = y;
1028                 dest[j--] = y;
1029         }
1030 }
1031
1032 // this one was in coriander but didn't take bits into account
1033         static void
1034 rgb482bgr(const unsigned char *src, unsigned char *dest,
1035                 unsigned long long int NumPixels, int bits)
1036 {
1037         register int i = (NumPixels << 1) - 1;
1038         register int j = NumPixels + (NumPixels << 1) - 1;
1039         register int y;
1040
1041         while (i > 0) {
1042                 y = src[i--];
1043                 dest[j-2] = (y + (src[i--] << 8)) >> (bits - 8);
1044                 j--;
1045                 y = src[i--];
1046                 dest[j] = (y + (src[i--] << 8)) >> (bits - 8);
1047                 j--;
1048                 y = src[i--];
1049                 dest[j+2] = (y + (src[i--] << 8)) >> (bits - 8);
1050                 j--;
1051         }
1052 }
1053
1054
1055 class CvCaptureCAM_DC1394_CPP : public CvCapture
1056 {
1057 public:
1058     CvCaptureCAM_DC1394_CPP() { captureDC1394 = 0; }
1059     virtual ~CvCaptureCAM_DC1394_CPP() { close(); }
1060
1061     virtual bool open( int index );
1062     virtual void close();
1063
1064     virtual double getProperty(int);
1065     virtual bool setProperty(int, double);
1066     virtual bool grabFrame();
1067     virtual IplImage* retrieveFrame();
1068 protected:
1069
1070     CvCaptureCAM_DC1394* captureDC1394;
1071 };
1072
1073 bool CvCaptureCAM_DC1394_CPP::open( int index )
1074 {
1075     close();
1076     captureDC1394 = icvCaptureFromCAM_DC1394(index);
1077     return captureDC1394 != 0;
1078 }
1079
1080 void CvCaptureCAM_DC1394_CPP::close()
1081 {
1082     if( captureDC1394 )
1083     {
1084         icvCloseCAM_DC1394( captureDC1394 );
1085         cvFree( &captureDC1394 );
1086     }
1087 }
1088
1089 bool CvCaptureCAM_DC1394_CPP::grabFrame()
1090 {
1091     return captureDC1394 ? icvGrabFrameCAM_DC1394( captureDC1394 ) != 0 : false;
1092 }
1093
1094 IplImage* CvCaptureCAM_DC1394_CPP::retrieveFrame()
1095 {
1096     return captureDC1394 ? (IplImage*)icvRetrieveFrameCAM_DC1394( captureDC1394 ) : 0;
1097 }
1098
1099 double CvCaptureCAM_DC1394_CPP::getProperty( int propId )
1100 {
1101     return captureDC1394 ? icvGetPropertyCAM_DC1394( captureDC1394, propId ) : 0;
1102 }
1103
1104 bool CvCaptureCAM_DC1394_CPP::setProperty( int propId, double value )
1105 {
1106     return captureDC1394 ? icvSetPropertyCAM_DC1394( captureDC1394, propId, value ) != 0 : false;
1107 }
1108
1109 CvCapture* cvCreateCameraCapture_DC1394( int index )
1110 {
1111     CvCaptureCAM_DC1394_CPP* capture = new CvCaptureCAM_DC1394_CPP;
1112
1113     if( capture->open( index ))
1114         return capture;
1115
1116     delete capture;
1117     return 0;
1118 }
1119
1120 #endif