vcs.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     CvCaptureVTable* vtable;
 141     raw1394handle_t handle;
 142     nodeid_t  camera_node;
 143     dc1394_cameracapture* camera;
 144     int format;
 145     int mode;
 146     int color_mode;
 147     int frame_rate;
 148     char * device_name;
 149     IplImage frame;
 150         int convert;
 151         int buffer_is_writeable;  // indicates whether frame.imageData is allocated by OpenCV or DC1394
 152 }
 153 CvCaptureCAM_DC1394;
 154
 155 static void icvCloseCAM_DC1394( CvCaptureCAM_DC1394* capture );
 156
 157 static int icvGrabFrameCAM_DC1394( CvCaptureCAM_DC1394* capture );
 158 static IplImage* icvRetrieveFrameCAM_DC1394( CvCaptureCAM_DC1394* capture );
 159
 160 static double icvGetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id );
 161 static int    icvSetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id, double value );
 162
 163 // utility functions
 164 static int    icvFormatSupportedCAM_DC1394(int format, quadlet_t formats);
 165 static int    icvModeSupportedCAM_DC1394(int format, int mode, quadlet_t modes);
 166 static int    icvColorMode( int mode );
 167 static unsigned int icvGetBestFrameRate( CvCaptureCAM_DC1394 * capture, int format, int mode);
 168 static int    icvResizeFrame(CvCaptureCAM_DC1394 * capture);
 169
 170 /***********************   Implementations  ***************************************/
 171 #define MAX_PORTS 3
 172 #define MAX_CAMERAS 8
 173 #define NUM_BUFFERS 8
 174 struct raw1394_portinfo ports[MAX_PORTS];
 175 static raw1394handle_t handles[MAX_PORTS];
 176 static int camCount[MAX_PORTS];
 177 static int numPorts = -1;
 178 static int numCameras = 0;
 179 static nodeid_t *camera_nodes;
 180 struct camnode {dc1394_cameracapture cam;int portnum;} cameras[MAX_CAMERAS];
 181
 182 static const int preferred_modes[]
 183 = {
 184     // uncomment the following line to test a particular mode:
 185     //FORMAT_VGA_NONCOMPRESSED, MODE_640x480_MONO16, 0,
 186     FORMAT_SVGA_NONCOMPRESSED_2,
 187     MODE_1600x1200_RGB, MODE_1600x1200_YUV422, MODE_1280x960_RGB, MODE_1280x960_YUV422,
 188     MODE_1600x1200_MONO, MODE_1280x960_MONO, MODE_1600x1200_MONO16, MODE_1280x960_MONO16,
 189     FORMAT_SVGA_NONCOMPRESSED_1,
 190     MODE_1024x768_RGB, MODE_1024x768_YUV422, MODE_800x600_RGB, MODE_800x600_YUV422,
 191     MODE_1024x768_MONO, MODE_800x600_MONO, MODE_1024x768_MONO16, MODE_800x600_MONO16,
 192     FORMAT_VGA_NONCOMPRESSED,
 193    MODE_640x480_RGB, MODE_640x480_YUV422, MODE_640x480_YUV411, MODE_320x240_YUV422,
 194     MODE_160x120_YUV444, MODE_640x480_MONO, MODE_640x480_MONO16,
 195     FORMAT_SCALABLE_IMAGE_SIZE,
 196     MODE_FORMAT7_0, MODE_FORMAT7_1, MODE_FORMAT7_2, MODE_FORMAT7_3,
 197     MODE_FORMAT7_4, MODE_FORMAT7_5, MODE_FORMAT7_6, MODE_FORMAT7_7,
 198         0
 199 };
 200
 201 static CvCaptureVTable captureCAM_DC1394_vtable =
 202 {
 203         6,
 204         (CvCaptureCloseFunc)icvCloseCAM_DC1394,
 205         (CvCaptureGrabFrameFunc)icvGrabFrameCAM_DC1394,
 206         (CvCaptureRetrieveFrameFunc)icvRetrieveFrameCAM_DC1394,
 207         (CvCaptureGetPropertyFunc)icvGetPropertyCAM_DC1394,
 208         (CvCaptureSetPropertyFunc)icvSetPropertyCAM_DC1394,
 209         (CvCaptureGetDescriptionFunc)0
 210 };
 211
 212 void icvInitCapture_DC1394(){
 213         int p;
 214
 215         raw1394handle_t raw_handle = raw1394_new_handle();
 216         if( raw_handle == 0 ) {
 217                 numPorts = 0;
 218                 return;
 219         }
 220         numPorts = raw1394_get_port_info(raw_handle, ports, MAX_PORTS);
 221         raw1394_destroy_handle(raw_handle);
 222         for (p = 0; p < numPorts; p++) {
 223                 handles[p] = dc1394_create_handle(p);
 224                 if (handles[p]==NULL) {  numPorts=-1; return; /*ERROR_CLEANUP_EXIT*/   }
 225
 226                 /* get the camera nodes and describe them as we find them */
 227                 camera_nodes = dc1394_get_camera_nodes(handles[p], &camCount[p], 0);
 228                 for (int i=0;i<camCount[p];i++) {
 229                         cameras[numCameras].cam.node = camera_nodes[i];
 230                         cameras[numCameras].portnum = p;
 231                         dc1394_stop_iso_transmission(handles[p], camera_nodes[i]);
 232                         numCameras++;
 233                 }
 234         }
 235 };
 236
 237 CvCapture * cvCaptureFromCAM_DC1394 (int index)
 238 {
 239         quadlet_t modes[8], formats;
 240         int i;
 241
 242         if (numPorts<0) icvInitCapture_DC1394();
 243         if (numPorts==0)
 244                 return 0;     /* No i1394 ports found */
 245         if (numCameras<1)
 246                 return 0;
 247         if (index>=numCameras)
 248                 return 0;
 249         if (index<0)
 250                 return 0;
 251
 252         CvCaptureCAM_DC1394 * pcap = (CvCaptureCAM_DC1394*)cvAlloc(sizeof(CvCaptureCAM_DC1394));
 253         pcap->vtable = &captureCAM_DC1394_vtable;
 254
 255         /* Select a port and camera */
 256         pcap->device_name = videodev[cameras[index].portnum];
 257         pcap->handle = handles[cameras[index].portnum];
 258         pcap->camera = &cameras[index].cam;
 259
 260         // get supported formats
 261         if (dc1394_query_supported_formats(pcap->handle, pcap->camera->node, &formats)<0) {
 262                 fprintf(stderr,"%s:%d: Could not query supported formats\n",__FILE__,__LINE__);
 263                 formats=0x0;
 264         }
 265         for (i=0; i < NUM_FORMATS; i++) {
 266                 modes[i]=0;
 267                 if (icvFormatSupportedCAM_DC1394(i+FORMAT_MIN, formats)){
 268                         if (dc1394_query_supported_modes(pcap->handle, pcap->camera->node, i+FORMAT_MIN, &modes[i])<0) {
 269                                 fprintf(stderr,"%s:%d: Could not query Format%d modes\n",__FILE__,__LINE__,i);
 270                         }
 271                 }
 272         }
 273
 274         pcap->format = 0;
 275         pcap->mode = 0;
 276         pcap->color_mode = 0;
 277         pcap->frame_rate = 0;
 278
 279         int format_idx = -1;
 280
 281         // scan the list of preferred modes, and find a supported one
 282         for(i=0; (pcap->mode == 0) && (preferred_modes[i] != 0); i++) {
 283                 if((preferred_modes[i] >= FORMAT_MIN) && (preferred_modes[i] <= FORMAT_MAX)) {
 284                         pcap->format = preferred_modes[i];
 285                         format_idx = preferred_modes[i] - FORMAT_MIN;
 286                         continue;
 287                 }
 288                 assert(format_idx != -1);
 289                 if ( ! icvFormatSupportedCAM_DC1394(pcap->format, formats) )
 290                         continue;
 291                 if ( icvModeSupportedCAM_DC1394(pcap->format, preferred_modes[i], modes[format_idx]) ){
 292                         pcap->mode = preferred_modes[i];
 293                 }
 294         }
 295         if (pcap->mode == 0) {
 296                 fprintf(stderr,"%s:%d: Could not find a supported mode for this camera\n",__FILE__,__LINE__);
 297                 goto ERROR;
 298         }
 299
 300         pcap->color_mode = icvColorMode( pcap->mode );
 301         if( pcap->color_mode == -1){
 302                 fprintf(stderr,"%s:%d: ERROR: BPP is Unsupported!!\n",__FILE__,__LINE__);
 303                 goto ERROR;
 304         }
 305
 306         // set frame rate to optimal value given format and mode
 307         pcap->frame_rate = icvGetBestFrameRate(pcap, pcap->format, pcap->mode);
 308
 309         if (pcap->format!=FORMAT_SCALABLE_IMAGE_SIZE) { // everything except Format 7
 310                 if (dc1394_dma_setup_capture(pcap->handle, pcap->camera->node, index+1 /*channel*/,
 311                                         pcap->format, pcap->mode, SPEED_400,
 312                                         pcap->frame_rate, NUM_BUFFERS,
 313 #ifdef HAVE_DC1394_095
 314                                         0 /*do_extra_buffering*/,
 315 #endif
 316                                         1 /*DROP_FRAMES*/,
 317                                         pcap->device_name, pcap->camera) != DC1394_SUCCESS) {
 318                         fprintf(stderr,"%s:%d: Failed to setup DMA capture with VIDEO1394\n",__FILE__,__LINE__);
 319                         goto ERROR;
 320                 }
 321         }
 322         else {
 323                 if(dc1394_dma_setup_format7_capture(pcap->handle,pcap->camera->node,index+1 /*channel*/,
 324                                         pcap->mode, SPEED_400, QUERY_FROM_CAMERA,
 325                                         (unsigned int)QUERY_FROM_CAMERA, (unsigned int)QUERY_FROM_CAMERA,
 326                                         (unsigned int)QUERY_FROM_CAMERA, (unsigned int)QUERY_FROM_CAMERA,
 327                                         NUM_BUFFERS,
 328 #ifdef HAVE_DC1394_095
 329                                         0 /*do_extra_buffering*/,
 330 #endif
 331                                         1 /*DROP_FRAMES*/,
 332                                         pcap->device_name, pcap->camera) != DC1394_SUCCESS) {
 333                         fprintf(stderr,"%s:%d: Failed to setup DMA capture with VIDEO1394\n",__FILE__,__LINE__);
 334                         goto ERROR;
 335                 }
 336         }
 337
 338         if (dc1394_start_iso_transmission(pcap->handle, pcap->camera->node)!=DC1394_SUCCESS) {
 339                 fprintf(stderr,"%s:%d: Could not start ISO transmission\n",__FILE__,__LINE__);
 340                 goto ERROR;
 341         }
 342
 343         usleep(DELAY);
 344
 345         dc1394bool_t status;
 346         if (dc1394_get_iso_status(pcap->handle, pcap->camera->node, &status)!=DC1394_SUCCESS) {
 347                 fprintf(stderr,"%s:%d: Could get ISO status",__FILE__,__LINE__);
 348                 goto ERROR;
 349         }
 350         if (status==DC1394_FALSE) {
 351                 fprintf(stderr,"%s:%d: ISO transmission refuses to start",__FILE__,__LINE__);
 352                 goto ERROR;
 353         }
 354
 355         // convert camera image to RGB by default
 356         pcap->convert=1;
 357
 358         // no image data allocated yet
 359         pcap->buffer_is_writeable = 0;
 360
 361         memset(&(pcap->frame), 0, sizeof(IplImage));
 362         icvResizeFrame( pcap );
 363         return (CvCapture *)pcap;
 364
 365 ERROR:
 366         return 0;
 367 };
 368
 369 static void icvCloseCAM_DC1394( CvCaptureCAM_DC1394* capture ){
 370         dc1394_stop_iso_transmission(capture->handle, capture->camera->node);
 371         dc1394_dma_unlisten (capture->handle, capture->camera);
 372         /* Deallocate space for RGBA data */
 373         if(capture->convert){
 374                 cvFree(&capture->frame.imageData);
 375         }
 376 }
 377
 378 static int icvGrabFrameCAM_DC1394( CvCaptureCAM_DC1394* capture ){
 379         // TODO: should this function wait until the next frame is available or return
 380         // immediately ?
 381         float waiting = 0;
 382         do{
 383                 int result = dc1394_dma_single_capture_poll(capture->camera);
 384                 if(result==DC1394_SUCCESS){
 385                         return 1;
 386                 }
 387                 else if(result==DC1394_NO_FRAME){
 388                         usleep(1000000/120);  //sleep for at least a 1/2 of the frame rate
 389                         waiting += 1.0/120.0;
 390                 }
 391                 else{
 392                         printf("dc1394_dma_single_capture_poll failed\n");
 393                         return 0;
 394                 }
 395         } while(waiting<2);
 396         printf("dc1394_dma_single_capture_poll timed out\n");
 397         return 0;
 398 }
 399
 400 static IplImage* icvRetrieveFrameCAM_DC1394( CvCaptureCAM_DC1394* capture ){
 401         if(capture->camera->capture_buffer )
 402         {
 403                 if(capture->convert){
 404                         /* Convert to RGBA */
 405                         unsigned char * src = (unsigned char *)capture->camera->capture_buffer;
 406                         unsigned char * dst = (unsigned char *)capture->frame.imageData;
 407                         switch (capture->color_mode) {
 408                                 case COLOR_FORMAT7_RGB8:
 409                                         printf("icvRetrieveFrame convert RGB to BGR\n");
 410                                         /* Convert RGB to BGR */
 411                                         for (int i=0;i<capture->frame.imageSize;i+=6) {
 412                                                 dst[i]   = src[i+2];
 413                                                 dst[i+1] = src[i+1];
 414                                                 dst[i+2] = src[i];
 415                                                 dst[i+3] = src[i+5];
 416                                                 dst[i+4] = src[i+4];
 417                                                 dst[i+5] = src[i+3];
 418                                         }
 419                                         break;
 420                                 case COLOR_FORMAT7_YUV422:
 421                                         //printf("icvRetrieveFrame convert YUV422 to BGR %d\n");
 422                                         uyvy2bgr(src,
 423                                                         dst,
 424                                                         capture->camera->frame_width * capture->camera->frame_height);
 425                                         break;
 426                                 case COLOR_FORMAT7_MONO8:
 427                                         //printf("icvRetrieveFrame convert MONO8 to BGR %d\n");
 428                                         y2bgr(src,
 429                                                         dst,
 430                                                         capture->camera->frame_width * capture->camera->frame_height);
 431                                         break;
 432                                 case COLOR_FORMAT7_YUV411:
 433                                         //printf("icvRetrieveFrame convert YUV411 to BGR %d\n");
 434                                         uyyvyy2bgr(src,
 435                                                         dst,
 436                                                         capture->camera->frame_width * capture->camera->frame_height);
 437                                         break;
 438                                 case COLOR_FORMAT7_YUV444:
 439                                         //printf("icvRetrieveFrame convert YUV444 to BGR %d\n");
 440                                         uyv2bgr(src,
 441                                                         dst,
 442                                                         capture->camera->frame_width * capture->camera->frame_height);
 443                                         break;
 444                                 case COLOR_FORMAT7_MONO16:
 445                                         //printf("icvRetrieveFrame convert MONO16 to BGR %d\n");
 446                                         y162bgr(src,
 447                                                         dst,
 448                                                         capture->camera->frame_width * capture->camera->frame_height, MONO16_BPP);
 449                                         break;
 450                                 case COLOR_FORMAT7_RGB16:
 451                                         //printf("icvRetrieveFrame convert RGB16 to BGR %d\n");
 452                                         rgb482bgr(src,
 453                                                         dst,
 454                                                         capture->camera->frame_width * capture->camera->frame_height, MONO16_BPP);
 455                                         break;
 456                                 default:
 457                                         fprintf(stderr,"%s:%d: Unsupported color mode %d\n",__FILE__,__LINE__,capture->color_mode);
 458                                         return 0;
 459                         } /* switch (capture->mode) */
 460                 }
 461                 else{
 462                         // return raw data
 463                         capture->frame.imageData = (char *) capture->camera->capture_buffer;
 464                         capture->frame.imageDataOrigin = (char *) capture->camera->capture_buffer;
 465                 }
 466
 467                 // TODO: if convert=0, we are not actually done with the buffer
 468                 // but this seems to work anyway.
 469                 dc1394_dma_done_with_buffer(capture->camera);
 470
 471                 return &capture->frame;
 472         }
 473         return 0;
 474 };
 475
 476 static double icvGetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id ){
 477         int index=-1;
 478         switch ( property_id ) {
 479                 case CV_CAP_PROP_CONVERT_RGB:
 480                         return capture->convert;
 481                 case CV_CAP_PROP_MODE:
 482                         return capture->mode;
 483                 case CV_CAP_PROP_FORMAT:
 484                         return capture->format;
 485                 case CV_CAP_PROP_FPS:
 486                         CV_DC1394_CALL(dc1394_get_video_framerate(capture->handle, capture->camera->node,
 487                                         (unsigned int *) &capture->camera->frame_rate));
 488                         switch(capture->camera->frame_rate) {
 489                                 case FRAMERATE_1_875:
 490                                         return 1.875;
 491                                 case FRAMERATE_3_75:
 492                                         return 3.75;
 493                                 case FRAMERATE_7_5:
 494                                         return 7.5;
 495                                 case FRAMERATE_15:
 496                                         return 15.;
 497                                 case FRAMERATE_30:
 498                                         return 30.;
 499                                 case FRAMERATE_60:
 500                                         return 60;
 501 #if NUM_FRAMERATES > 6
 502                                 case FRAMERATE_120:
 503                                         return 120;
 504 #endif
 505 #if NUM_FRAMERATES > 7
 506                                 case FRAMERATE_240:
 507                                         return 240;
 508 #endif
 509                         }
 510         default:
 511                         index = property_id;  // did they pass in a LIBDC1394 feature flag?
 512                         break;
 513         }
 514         if(index>=FEATURE_MIN && index<=FEATURE_MAX){
 515                 dc1394bool_t has_feature;
 516                 CV_DC1394_CALL( dc1394_is_feature_present(capture->handle, capture->camera->node,
 517                                                                       index, &has_feature));
 518                 if(!has_feature){
 519                         CV_WARN("Feature is not supported by this camera");
 520                 }
 521                 else{
 522                         unsigned int value;
 523                         dc1394_get_feature_value(capture->handle, capture->camera->node, index, &value);
 524                         return (double) value;
 525                 }
 526         }
 527
 528         return 0;
 529 };
 530
 531 // resize capture->frame appropriately depending on camera and capture settings
 532 static int icvResizeFrame(CvCaptureCAM_DC1394 * capture){
 533         if(capture->convert){
 534                 // resize if sizes are different, formats are different
 535                 // or conversion option has changed
 536                 if(capture->camera->frame_width != capture->frame.width ||
 537                    capture->camera->frame_height != capture->frame.height ||
 538                    capture->frame.depth != 8 ||
 539                    capture->frame.nChannels != 3 ||
 540                    capture->frame.imageData == NULL ||
 541                    capture->buffer_is_writeable == 0)
 542                 {
 543                         if(capture->frame.imageData && capture->buffer_is_writeable){
 544                                 cvReleaseData( &(capture->frame));
 545                         }
 546                         cvInitImageHeader( &capture->frame, cvSize( capture->camera->frame_width,
 547                                                                                 capture->camera->frame_height ),
 548                                                                 IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
 549                         cvCreateData( &(capture->frame) );
 550                         capture->buffer_is_writeable = 1;
 551                 }
 552
 553         }
 554         else {
 555                 // free image data if allocated by opencv
 556                 if(capture->buffer_is_writeable){
 557                         cvReleaseData(&(capture->frame));
 558                 }
 559
 560                 // figure out number of channels and bpp
 561                 int bpp = 8;
 562                 int nch = 3;
 563                 int width = capture->camera->frame_width;
 564                 int height = capture->camera->frame_height;
 565                 double code = CV_FOURCC('B','G','R',0);
 566                 switch(capture->color_mode){
 567                 case COLOR_FORMAT7_YUV422:
 568                         nch = 2;
 569                         code = CV_FOURCC('Y','4','2','2');
 570                         break;
 571                 case COLOR_FORMAT7_MONO8:
 572                         code = CV_FOURCC('Y',0,0,0);
 573                         nch = 1;
 574                         break;
 575                 case COLOR_FORMAT7_YUV411:
 576                         code = CV_FOURCC('Y','4','1','1');
 577                         width *= 2;
 578                         nch = 3;  //yy[u/v]
 579                         break;
 580                 case COLOR_FORMAT7_YUV444:
 581                         code = CV_FOURCC('Y','U','V',0);
 582                         nch = 3;
 583                         break;
 584                 case COLOR_FORMAT7_MONO16:
 585                         code = CV_FOURCC('Y',0,0,0);
 586                         bpp = IPL_DEPTH_16S;
 587                         nch = 1;
 588                         break;
 589                 case COLOR_FORMAT7_RGB16:
 590                         bpp = IPL_DEPTH_16S;
 591                         nch = 3;
 592                         break;
 593                 default:
 594                         break;
 595                 }
 596                 // reset image header
 597                 cvInitImageHeader( &capture->frame,cvSize( width, height ), bpp, nch, IPL_ORIGIN_TL, 4 );
 598                 //assert(capture->frame.imageSize == capture->camera->quadlets_per_frame*4);
 599                 capture->buffer_is_writeable = 0;
 600         }
 601         return 1;
 602 }
 603
 604 // Toggle setting about whether or not RGB color conversion is to be performed
 605 // Allocates/Initializes capture->frame appropriately
 606 int icvSetConvertRGB(CvCaptureCAM_DC1394 * capture, int convert){
 607         if(convert==capture->convert){
 608                 // no action necessary
 609                 return 1;
 610         }
 611         capture->convert = convert;
 612         return icvResizeFrame( capture );
 613 }
 614
 615 // given desired format, mode, and modes bitmask from camera, determine if format and mode are supported
 616 static int
 617 icvFormatSupportedCAM_DC1394(int format, quadlet_t formats){
 618         // formats is a bitmask whose higher order bits indicate whether format is supported
 619         int shift = 31 - (format - FORMAT_MIN);
 620         int mask = 1 << shift;
 621         return (formats & mask) != 0;
 622 }
 623
 624 // analyze modes bitmask from camera to determine if desired format and mode are supported
 625 static int
 626 icvModeSupportedCAM_DC1394(int format, int mode, quadlet_t modes){
 627         // modes is a bitmask whose higher order bits indicate whether mode is supported
 628         int format_idx = format - FORMAT_MIN;
 629         int mode_format_min = MODE_FORMAT0_MIN + 32*format_idx;
 630         int shift = 31 - (mode - mode_format_min);
 631         int mask = 0x1 << shift;
 632         return (modes & mask) != 0;
 633 }
 634
 635 // Setup camera to use given dc1394 mode
 636 static int
 637 icvSetModeCAM_DC1394( CvCaptureCAM_DC1394 * capture, int mode ){
 638         quadlet_t modes, formats;
 639         //printf("<icvSetModeCAM_DC1394>\n");
 640
 641         // figure out corrent format for this mode
 642         int format = (mode - MODE_FORMAT0_MIN) / 32 + FORMAT_MIN;
 643
 644         // get supported formats
 645         if (dc1394_query_supported_formats(capture->handle, capture->camera->node, &formats)<0) {
 646                 fprintf(stderr,"%s:%d: Could not query supported formats\n",__FILE__,__LINE__);
 647                 return 0;
 648         }
 649
 650         // is format for requested mode supported ?
 651         if(icvFormatSupportedCAM_DC1394(format, formats)==0){
 652                 return 0;
 653         }
 654
 655         // get supported modes for requested format
 656         if (dc1394_query_supported_modes(capture->handle, capture->camera->node, format, &modes)<0){
 657                 fprintf(stderr,"%s:%d: Could not query supported modes for format %d\n",__FILE__,__LINE__, capture->format);
 658                 return 0;
 659         }
 660
 661         // is requested mode supported ?
 662         if(! icvModeSupportedCAM_DC1394(format, mode, modes) ){
 663                 return 0;
 664         }
 665
 666         int color_mode = icvColorMode( mode );
 667
 668         if(color_mode == -1){
 669                 return 0;
 670         }
 671
 672         int frame_rate = icvGetBestFrameRate(capture, format, mode);
 673
 674         dc1394_dma_unlisten(capture->handle, capture->camera);
 675         if (dc1394_dma_setup_capture(capture->handle, capture->camera->node, capture->camera->channel /*channel*/,
 676                                 format, mode, SPEED_400,
 677                                 frame_rate, NUM_BUFFERS,
 678 #ifdef HAVE_DC1394_095
 679                                 0 /*do_extra_buffering*/,
 680 #endif
 681                                 1 /*DROP_FRAMES*/,
 682                                 capture->device_name, capture->camera) != DC1394_SUCCESS) {
 683                 fprintf(stderr,"%s:%d: Failed to setup DMA capture with VIDEO1394\n",__FILE__,__LINE__);
 684                 return 0;
 685         }
 686         dc1394_start_iso_transmission(capture->handle, capture->camera->node);
 687
 688         capture->frame_rate = frame_rate;
 689         capture->format = format;
 690         capture->mode = mode;
 691         capture->color_mode = color_mode;
 692
 693         // now fix image size to match new mode
 694         icvResizeFrame( capture );
 695         return 1;
 696 }
 697
 698 // query camera for supported frame rates and select fastest for given format and mode
 699 static unsigned int icvGetBestFrameRate( CvCaptureCAM_DC1394 * capture, int format, int mode  ){
 700         quadlet_t framerates;
 701         if (dc1394_query_supported_framerates(capture->handle, capture->camera->node,
 702                                 format, mode, &framerates)!=DC1394_SUCCESS)
 703         {
 704                 fprintf(stderr,"%s:%d: Could not query supported framerates\n",__FILE__,__LINE__);
 705                 framerates = 0;
 706         }
 707
 708         for (int f=FRAMERATE_MAX; f>=FRAMERATE_MIN; f--) {
 709                 if (framerates & (0x1<< (31-(f-FRAMERATE_MIN)))) {
 710                         return f;
 711                 }
 712         }
 713         return 0;
 714 }
 715
 716 static int
 717 icvSetFrameRateCAM_DC1394( CvCaptureCAM_DC1394 * capture, double value ){
 718         unsigned int fps=15;
 719         if(capture->format == FORMAT_SCALABLE_IMAGE_SIZE)
 720                 return 0; /* format 7 has no fixed framerates */
 721         if (value==-1){
 722                 fps=icvGetBestFrameRate( capture, capture->format, capture->mode );
 723         }
 724         else if (value==1.875)
 725                 fps=FRAMERATE_1_875;
 726         else if (value==3.75)
 727                 fps=FRAMERATE_3_75;
 728         else if (value==7.5)
 729                 fps=FRAMERATE_7_5;
 730         else if (value==15)
 731                 fps=FRAMERATE_15;
 732         else if (value==30)
 733                 fps=FRAMERATE_30;
 734         else if (value==60)
 735                 fps=FRAMERATE_60;
 736 #if NUM_FRAMERATES > 6
 737         else if (value==120)
 738                 fps=FRAMERATE_120;
 739 #endif
 740 #if NUM_FRAMERATES > 7
 741         else if (value==240)
 742                 fps=FRAMERATE_240;
 743 #endif
 744         dc1394_set_video_framerate(capture->handle, capture->camera->node,fps);
 745         dc1394_get_video_framerate(capture->handle, capture->camera->node,
 746                                         (unsigned int *) &capture->camera->frame_rate);
 747
 748         return fps==(unsigned int) capture->camera->frame_rate;
 749 }
 750
 751 // for given mode return color format
 752 static int
 753 icvColorMode( int mode ){
 754         switch(mode) {
 755         case MODE_160x120_YUV444:
 756                 return COLOR_FORMAT7_YUV444;
 757         case MODE_320x240_YUV422:
 758         case MODE_640x480_YUV422:
 759         case MODE_800x600_YUV422:
 760         case MODE_1024x768_YUV422:
 761         case MODE_1280x960_YUV422:
 762         case MODE_1600x1200_YUV422:
 763                 return COLOR_FORMAT7_YUV422;
 764         case MODE_640x480_YUV411:
 765                 return COLOR_FORMAT7_YUV411;
 766         case MODE_640x480_RGB:
 767         case MODE_800x600_RGB:
 768         case MODE_1024x768_RGB:
 769         case MODE_1280x960_RGB:
 770         case MODE_1600x1200_RGB:
 771                 return COLOR_FORMAT7_RGB8;
 772         case MODE_640x480_MONO:
 773         case MODE_800x600_MONO:
 774         case MODE_1024x768_MONO:
 775         case MODE_1280x960_MONO:
 776         case MODE_1600x1200_MONO:
 777                 return COLOR_FORMAT7_MONO8;
 778         case MODE_640x480_MONO16:
 779         case MODE_800x600_MONO16:
 780         case MODE_1024x768_MONO16:
 781         case MODE_1280x960_MONO16:
 782         case MODE_1600x1200_MONO16:
 783                 return COLOR_FORMAT7_MONO16;
 784         case MODE_FORMAT7_0:
 785         case MODE_FORMAT7_1:
 786         case MODE_FORMAT7_2:
 787         case MODE_FORMAT7_3:
 788         case MODE_FORMAT7_4:
 789         case MODE_FORMAT7_5:
 790         case MODE_FORMAT7_6:
 791         case MODE_FORMAT7_7:
 792                 fprintf(stderr,"%s:%d: Format7 not yet supported\n",__FILE__,__LINE__);
 793         default:
 794                 break;
 795         }
 796         return -1;
 797 }
 798
 799 // function to set camera properties using dc1394 feature enum
 800 // val == -1 indicates to set this property to 'auto'
 801 static int
 802 icvSetFeatureCAM_DC1394( CvCaptureCAM_DC1394* capture, int feature_id, int val){
 803         dc1394bool_t isOn = DC1394_FALSE;
 804                 dc1394bool_t hasAutoCapability = DC1394_FALSE;
 805                 dc1394bool_t isAutoOn = DC1394_FALSE;
 806                 unsigned int nval;
 807                 unsigned int minval,maxval;
 808
 809                 // Turn the feature on if it is OFF
 810                 if( dc1394_is_feature_on(capture->handle, capture->camera->node, feature_id, &isOn)
 811                                 == DC1394_FAILURE ) {
 812                         return 0;
 813                 }
 814                 if( isOn == DC1394_FALSE ) {
 815                 // try to turn it on.
 816                 if( dc1394_feature_on_off(capture->handle, capture->camera->node, feature_id, 1) == DC1394_FAILURE ) {
 817                     fprintf(stderr, "error turning feature %d on!\n", feature_id);
 818                     return 0;
 819                 }
 820                 }
 821
 822                 // Check if the feature supports auto mode
 823                 dc1394_has_auto_mode(capture->handle, capture->camera->node, feature_id, &hasAutoCapability);
 824                 if( hasAutoCapability ) {
 825
 826                         // now check if the auto is on.
 827                         if( dc1394_is_feature_auto(capture->handle, capture->camera->node, feature_id, &isAutoOn ) == DC1394_FAILURE ) {
 828                                 fprintf(stderr, "error determining if feature %d has auto on!\n", index);
 829                                 return 0;
 830                         }
 831                 }
 832                 // Caller requested auto mode, but cannot support it
 833                 else if(val==-1){
 834             fprintf(stderr, "feature %d does not support auto mode\n", feature_id);
 835             return 0;
 836                 }
 837
 838                 if(val==-1){
 839             // if the auto mode isn't enabled, enable it
 840             if( isAutoOn == DC1394_FALSE ) {
 841                 if(dc1394_auto_on_off(capture->handle, capture->camera->node, feature_id, 1) == DC1394_FAILURE ) {
 842                     fprintf(stderr, "error turning feature %d auto ON!\n", feature_id);
 843                     return 0;
 844                 }
 845             }
 846                         return 1;
 847         }
 848
 849                 // ELSE turn OFF auto and adjust feature manually
 850                 if( isAutoOn == DC1394_TRUE ) {
 851                         if(dc1394_auto_on_off(capture->handle, capture->camera->node, feature_id, 0) == DC1394_FAILURE ) {
 852                                 fprintf(stderr, "error turning feature %d auto OFF!\n", feature_id);
 853                                 return 0;
 854                         }
 855                 }
 856
 857                 // Clamp val to within feature range
 858                 CV_DC1394_CALL( dc1394_get_min_value(capture->handle, capture->camera->node, feature_id, &minval));
 859                 CV_DC1394_CALL( dc1394_get_max_value(capture->handle, capture->camera->node, feature_id, &maxval));
 860                 val = MIN(maxval, MAX(val, minval));
 861
 862
 863                 if (dc1394_set_feature_value(capture->handle, capture->camera->node, feature_id, val) ==
 864                                 DC1394_FAILURE){
 865                         fprintf(stderr, "error setting feature value\n");
 866                         return 0;
 867                 }
 868                 if (dc1394_get_feature_value(capture->handle, capture->camera->node, feature_id, &nval) ==
 869                                 DC1394_FAILURE){
 870                         fprintf(stderr, "error setting feature value\n");
 871                         return 0;
 872                 }
 873                 return nval==(unsigned int)val;
 874
 875 }
 876
 877 // cvSetCaptureProperty callback function implementation
 878 static int
 879 icvSetPropertyCAM_DC1394( CvCaptureCAM_DC1394* capture, int property_id, double value ){
 880         int index=-1;
 881         switch ( property_id ) {
 882                 case CV_CAP_PROP_CONVERT_RGB:
 883                         return icvSetConvertRGB( capture, value != 0 );
 884                 case CV_CAP_PROP_MODE:
 885                         return icvSetModeCAM_DC1394( capture, (int) value );
 886                 case CV_CAP_PROP_FPS:
 887                         return icvSetFrameRateCAM_DC1394( capture, value );
 888                 case CV_CAP_PROP_BRIGHTNESS:
 889                         index = FEATURE_BRIGHTNESS;
 890                         break;
 891                 case CV_CAP_PROP_CONTRAST:
 892                         index = FEATURE_GAMMA;
 893                         break;
 894                 case CV_CAP_PROP_SATURATION:
 895                         index = FEATURE_SATURATION;
 896                         break;
 897                 case CV_CAP_PROP_HUE:
 898                         index = FEATURE_HUE;
 899                         break;
 900                 case CV_CAP_PROP_GAIN:
 901                         index = FEATURE_GAIN;
 902                         break;
 903                 default:
 904                         index = property_id;  // did they pass in a LIBDC1394 feature flag?
 905                         break;
 906         }
 907         if(index>=FEATURE_MIN && index<=FEATURE_MAX){
 908                 return icvSetFeatureCAM_DC1394(capture, index, (int) value);
 909         }
 910         return 0;
 911 };
 912
 913 /**********************************************************************
 914  *
 915  *  CONVERSION FUNCTIONS TO RGB 24bpp
 916  *
 917  **********************************************************************/
 918
 919 /* color conversion functions from Bart Nabbe. *//* corrected by Damien: bad coeficients in YUV2RGB */
 920 #define YUV2RGB(y, u, v, r, g, b)\
 921         r = y + ((v*1436) >> 10);\
 922 g = y - ((u*352 + v*731) >> 10);\
 923 b = y + ((u*1814) >> 10);\
 924 r = r < 0 ? 0 : r;\
 925 g = g < 0 ? 0 : g;\
 926 b = b < 0 ? 0 : b;\
 927 r = r > 255 ? 255 : r;\
 928 g = g > 255 ? 255 : g;\
 929 b = b > 255 ? 255 : b
 930
 931         static void
 932 uyv2bgr(const unsigned char *src, unsigned char *dest,
 933                 unsigned long long int NumPixels)
 934 {
 935         register int i = NumPixels + (NumPixels << 1) - 1;
 936         register int j = NumPixels + (NumPixels << 1) - 1;
 937         register int y, u, v;
 938         register int r, g, b;
 939
 940         while (i > 0) {
 941                 v = src[i--] - 128;
 942                 y = src[i--];
 943                 u = src[i--] - 128;
 944                 YUV2RGB(y, u, v, r, g, b);
 945                 dest[j--] = r;
 946                 dest[j--] = g;
 947                 dest[j--] = b;
 948         }
 949 }
 950
 951         static void
 952 uyvy2bgr(const unsigned char *src, unsigned char *dest,
 953                 unsigned long long int NumPixels)
 954 {
 955         register int i = (NumPixels << 1) - 1;
 956         register int j = NumPixels + (NumPixels << 1) - 1;
 957         register int y0, y1, u, v;
 958         register int r, g, b;
 959
 960         while (i > 0) {
 961                 y1 = src[i--];
 962                 v = src[i--] - 128;
 963                 y0 = src[i--];
 964                 u = src[i--] - 128;
 965                 YUV2RGB(y1, u, v, r, g, b);
 966                 dest[j--] = r;
 967                 dest[j--] = g;
 968                 dest[j--] = b;
 969                 YUV2RGB(y0, u, v, r, g, b);
 970                 dest[j--] = r;
 971                 dest[j--] = g;
 972                 dest[j--] = b;
 973         }
 974 }
 975
 976
 977         static void
 978 uyyvyy2bgr(const unsigned char *src, unsigned char *dest,
 979                 unsigned long long int NumPixels)
 980 {
 981         register int i = NumPixels + (NumPixels >> 1) - 1;
 982         register int j = NumPixels + (NumPixels << 1) - 1;
 983         register int y0, y1, y2, y3, u, v;
 984         register int r, g, b;
 985
 986         while (i > 0) {
 987                 y3 = src[i--];
 988                 y2 = src[i--];
 989                 v = src[i--] - 128;
 990                 y1 = src[i--];
 991                 y0 = src[i--];
 992                 u = src[i--] - 128;
 993                 YUV2RGB(y3, u, v, r, g, b);
 994                 dest[j--] = r;
 995                 dest[j--] = g;
 996                 dest[j--] = b;
 997                 YUV2RGB(y2, u, v, r, g, b);
 998                 dest[j--] = r;
 999                 dest[j--] = g;
1000                 dest[j--] = b;
1001                 YUV2RGB(y1, u, v, r, g, b);
1002                 dest[j--] = r;
1003                 dest[j--] = g;
1004                 dest[j--] = b;
1005                 YUV2RGB(y0, u, v, r, g, b);
1006                 dest[j--] = r;
1007                 dest[j--] = g;
1008                 dest[j--] = b;
1009         }
1010 }
1011
1012         static void
1013 y2bgr(const unsigned char *src, unsigned char *dest,
1014                 unsigned long long int NumPixels)
1015 {
1016         register int i = NumPixels - 1;
1017         register int j = NumPixels + (NumPixels << 1) - 1;
1018         register int y;
1019
1020         while (i > 0) {
1021                 y = src[i--];
1022                 dest[j--] = y;
1023                 dest[j--] = y;
1024                 dest[j--] = y;
1025         }
1026 }
1027
1028         static void
1029 y162bgr(const unsigned char *src, unsigned char *dest,
1030                 unsigned long long int NumPixels, int bits)
1031 {
1032         register int i = (NumPixels << 1) - 1;
1033         register int j = NumPixels + (NumPixels << 1) - 1;
1034         register int y;
1035
1036         while (i > 0) {
1037                 y = src[i--];
1038                 y = (y + (src[i--] << 8)) >> (bits - 8);
1039                 dest[j--] = y;
1040                 dest[j--] = y;
1041                 dest[j--] = y;
1042         }
1043 }
1044
1045 // this one was in coriander but didn't take bits into account
1046         static void
1047 rgb482bgr(const unsigned char *src, unsigned char *dest,
1048                 unsigned long long int NumPixels, int bits)
1049 {
1050         register int i = (NumPixels << 1) - 1;
1051         register int j = NumPixels + (NumPixels << 1) - 1;
1052         register int y;
1053
1054         while (i > 0) {
1055                 y = src[i--];
1056                 dest[j-2] = (y + (src[i--] << 8)) >> (bits - 8);
1057                 j--;
1058                 y = src[i--];
1059                 dest[j] = (y + (src[i--] << 8)) >> (bits - 8);
1060                 j--;
1061                 y = src[i--];
1062                 dest[j+2] = (y + (src[i--] << 8)) >> (bits - 8);
1063                 j--;
1064         }
1065 }
1066
1067 #endif