modules/highgui/src/cap_dc1394_v2.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "precomp.hpp"
  43
  44 #ifdef HAVE_DC1394_2
  45
  46 #include <unistd.h>
  47 #include <stdint.h>
  48 #ifdef WIN32
  49   // On Windows, we have no sys/select.h, but we need to pick up
  50   // select() which is in winsock2.
  51   #ifndef __SYS_SELECT_H__
  52     #define __SYS_SELECT_H__ 1
  53     #include <winsock2.h>
  54   #endif
  55 #else
  56   #include <sys/select.h>
  57 #endif /*WIN32*/
  58 #include <dc1394/dc1394.h>
  59 #include <stdlib.h>
  60 #include <string.h>
  61
  62 static dc1394error_t adaptBufferStereoLocal(dc1394video_frame_t *in, dc1394video_frame_t *out)
  63 {
  64     uint32_t bpp;
  65
  66     // buffer position is not changed. Size is boubled in Y
  67     out->size[0] = in->size[0];
  68     out->size[1] = in->size[1] * 2;
  69     out->position[0] = in->position[0];
  70     out->position[1] = in->position[1];
  71
  72     // color coding is set to mono8 or raw8.
  73     switch (in->color_coding)
  74     {
  75     case DC1394_COLOR_CODING_RAW16:
  76         out->color_coding = DC1394_COLOR_CODING_RAW8;
  77         break;
  78     case DC1394_COLOR_CODING_MONO16:
  79     case DC1394_COLOR_CODING_YUV422:
  80         out->color_coding = DC1394_COLOR_CODING_MONO8;
  81         break;
  82     default:
  83         return DC1394_INVALID_COLOR_CODING;
  84     }
  85
  86     // keep the color filter value in all cases. if the format is not raw it will not be further used anyway
  87     out->color_filter = in->color_filter;
  88
  89     // the output YUV byte order must be already set if the buffer is YUV422 at the output
  90     // if the output is not YUV we don't care about this field.
  91     // Hence nothing to do.
  92     // we always convert to 8bits (at this point) we can safely set this value to 8.
  93     out->data_depth = 8;
  94
  95     // don't know what to do with stride... >>>> TODO: STRIDE SHOULD BE TAKEN INTO ACCOUNT... <<<<
  96     // out->stride=??
  97     // the video mode should not change. Color coding and other stuff can be accessed in specific fields of this struct
  98     out->video_mode = in->video_mode;
  99
 100     // padding is kept:
 101     out->padding_bytes = in->padding_bytes;
 102
 103     // image bytes changes:    >>>> TODO: STRIDE SHOULD BE TAKEN INTO ACCOUNT... <<<<
 104     dc1394_get_color_coding_bit_size(out->color_coding, &bpp);
 105     out->image_bytes = (out->size[0] * out->size[1] * bpp) / 8;
 106
 107     // total is image_bytes + padding_bytes
 108     out->total_bytes = out->image_bytes + out->padding_bytes;
 109
 110     // bytes-per-packet and packets_per_frame are internal data that can be kept as is.
 111     out->packet_size  = in->packet_size;
 112     out->packets_per_frame = in->packets_per_frame;
 113
 114     // timestamp, frame_behind, id and camera are copied too:
 115     out->timestamp = in->timestamp;
 116     out->frames_behind = in->frames_behind;
 117     out->camera = in->camera;
 118     out->id = in->id;
 119
 120     // verify memory allocation:
 121     if (out->total_bytes > out->allocated_image_bytes)
 122     {
 123         free(out->image);
 124         out->image = (uint8_t*)malloc(out->total_bytes * sizeof(uint8_t));
 125         out->allocated_image_bytes = out->total_bytes;
 126     }
 127
 128     // Copy padding bytes:
 129     memcpy(&(out->image[out->image_bytes]), &(in->image[in->image_bytes]), out->padding_bytes);
 130     out->little_endian = DC1394_FALSE; // not used before 1.32 is out.
 131     out->data_in_padding = DC1394_FALSE; // not used before 1.32 is out.
 132     return DC1394_SUCCESS;
 133 }
 134
 135 static dc1394error_t dc1394_deinterlace_stereo_frames_fixed(dc1394video_frame_t *in,
 136     dc1394video_frame_t *out, dc1394stereo_method_t method)
 137 {
 138     if((in->color_coding == DC1394_COLOR_CODING_RAW16) ||
 139        (in->color_coding == DC1394_COLOR_CODING_MONO16) ||
 140        (in->color_coding == DC1394_COLOR_CODING_YUV422))
 141     {
 142         switch (method)
 143         {
 144
 145         case DC1394_STEREO_METHOD_INTERLACED:
 146             adaptBufferStereoLocal(in, out);
 147 //FIXED by AB:
 148 //          dc1394_deinterlace_stereo(in->image, out->image, in->size[0], in->size[1]);
 149             dc1394_deinterlace_stereo(in->image, out->image, out->size[0], out->size[1]);
 150             break;
 151
 152         case DC1394_STEREO_METHOD_FIELD:
 153             adaptBufferStereoLocal(in, out);
 154             memcpy(out->image, in->image, out->image_bytes);
 155             break;
 156         }
 157
 158         return DC1394_INVALID_STEREO_METHOD;
 159     }
 160     else
 161         return DC1394_FUNCTION_NOT_SUPPORTED;
 162 }
 163
 164 static uint32_t getControlRegister(dc1394camera_t *camera, uint64_t offset)
 165 {
 166     uint32_t value = 0;
 167     dc1394error_t err = dc1394_get_control_register(camera, offset, &value);
 168
 169     assert(err == DC1394_SUCCESS);
 170     return err == DC1394_SUCCESS ? value : 0xffffffff;
 171 }
 172
 173 struct CvDC1394
 174 {
 175     CvDC1394();
 176     ~CvDC1394();
 177
 178     dc1394_t* dc;
 179     fd_set camFds;
 180 };
 181
 182 CvDC1394::CvDC1394()
 183 {
 184     dc = dc1394_new();
 185     FD_ZERO(&camFds);
 186 }
 187
 188 CvDC1394::~CvDC1394()
 189 {
 190     if (dc)
 191         dc1394_free(dc);
 192     dc = 0;
 193 }
 194
 195 static CvDC1394 dc1394;
 196
 197 class CvCaptureCAM_DC1394_v2_CPP : public CvCapture
 198 {
 199 public:
 200     static int dc1394properties[CV_CAP_PROP_MAX_DC1394];
 201     CvCaptureCAM_DC1394_v2_CPP();
 202     virtual ~CvCaptureCAM_DC1394_v2_CPP()
 203     {
 204         close();
 205     }
 206
 207     virtual bool open(int index);
 208     virtual void close();
 209
 210     virtual double getProperty(int);
 211     virtual bool setProperty(int, double);
 212     virtual bool grabFrame();
 213     virtual IplImage* retrieveFrame(int);
 214     virtual int getCaptureDomain() { return CV_CAP_DC1394; } // Return the type of the capture object: CV_CAP_VFW, etc...
 215
 216
 217 protected:
 218     virtual bool startCapture();
 219     virtual bool getVidereCalibrationInfo( char* buf, int bufSize );
 220     virtual bool initVidereRectifyMaps( const char* info, IplImage* ml[2], IplImage* mr[2] );
 221
 222     uint64_t guid;
 223     dc1394camera_t* dcCam;
 224     int isoSpeed;
 225     int videoMode;
 226     int frameWidth, frameHeight;
 227     double fps;
 228     int nDMABufs;
 229     bool started;
 230     int userMode;
 231
 232     enum { VIDERE = 0x5505 };
 233
 234     int cameraId;
 235     bool colorStereo;
 236     dc1394bayer_method_t bayer;
 237     dc1394color_filter_t bayerFilter;
 238
 239     enum { NIMG = 2 };
 240     IplImage *img[NIMG];
 241     dc1394video_frame_t* frameC;
 242     int nimages;
 243
 244     bool rectify;
 245     bool init_rectify;
 246     IplImage *maps[NIMG][2];
 247     dc1394featureset_t feature_set;
 248 };
 249 //mapping CV_CAP_PROP_ to DC1394_FEATUREs
 250 int CvCaptureCAM_DC1394_v2_CPP::dc1394properties[CV_CAP_PROP_MAX_DC1394] = {
 251 -1, //no corresponding feature for CV_CAP_PROP_POS_MSEC
 252 -1,-1,-1,-1,
 253 DC1394_FEATURE_FRAME_RATE, //CV_CAP_PROP_FPS - fps can be set for format 7 only!
 254 -1,-1,-1,-1,
 255 DC1394_FEATURE_BRIGHTNESS, //CV_CAP_PROP_BRIGHTNESS 10
 256 -1,
 257 DC1394_FEATURE_SATURATION, //CV_CAP_PROP_SATURATION
 258 DC1394_FEATURE_HUE,
 259 DC1394_FEATURE_GAIN,
 260 DC1394_FEATURE_SHUTTER, //CV_CAP_PROP_EXPOSURE
 261 -1, //CV_CAP_PROP_CONVERT_RGB
 262 DC1394_FEATURE_WHITE_BALANCE, //corresponds to CV_CAP_PROP_WHITE_BALANCE_BLUE_U and CV_CAP_PROP_WHITE_BALANCE_RED_V, see set function to check these props are set
 263 -1,-1,
 264 DC1394_FEATURE_SHARPNESS, //20
 265 DC1394_FEATURE_EXPOSURE, //CV_CAP_PROP_AUTO_EXPOSURE - this is auto exposure according to the IIDC standard
 266 DC1394_FEATURE_GAMMA, //CV_CAP_PROP_GAMMA
 267 DC1394_FEATURE_TEMPERATURE, //CV_CAP_PROP_TEMPERATURE
 268 DC1394_FEATURE_TRIGGER, //CV_CAP_PROP_TRIGGER
 269 DC1394_FEATURE_TRIGGER_DELAY, //CV_CAP_PROP_TRIGGER_DELAY
 270 DC1394_FEATURE_WHITE_BALANCE, //CV_CAP_PROP_WHITE_BALANCE_RED_V
 271 DC1394_FEATURE_ZOOM, //CV_CAP_PROP_ZOOM
 272 DC1394_FEATURE_FOCUS, //CV_CAP_PROP_FOCUS
 273 -1 //CV_CAP_PROP_GUID
 274 };
 275 CvCaptureCAM_DC1394_v2_CPP::CvCaptureCAM_DC1394_v2_CPP()
 276 {
 277     guid = 0;
 278     dcCam = 0;
 279     isoSpeed = 400;
 280     fps = 15;
 281     nDMABufs = 8;
 282     started = false;
 283     cameraId = 0;
 284     colorStereo = false;
 285     bayer = DC1394_BAYER_METHOD_BILINEAR;
 286     bayerFilter = DC1394_COLOR_FILTER_GRBG;
 287     frameWidth = 640;
 288     frameHeight = 480;
 289
 290     for (int i = 0; i < NIMG; i++)
 291         img[i] = maps[i][0] = maps[i][1] = 0;
 292     frameC = 0;
 293     nimages = 1;
 294     rectify = false;
 295     userMode = -1;
 296 }
 297
 298
 299 bool CvCaptureCAM_DC1394_v2_CPP::startCapture()
 300 {
 301     int i;
 302     int code = 0;
 303     if (!dcCam)
 304         return false;
 305     if (isoSpeed > 0)
 306     {
 307         code = dc1394_video_set_iso_speed(dcCam,
 308                                           isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
 309                                           isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
 310                                           isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
 311                                           isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
 312                                           isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
 313                                           DC1394_ISO_SPEED_3200);
 314     }
 315
 316     // should a specific mode be used
 317     if (userMode >= 0)
 318
 319     {
 320         dc1394video_mode_t wantedMode;
 321         dc1394video_modes_t videoModes;
 322         dc1394_video_get_supported_modes(dcCam, &videoModes);
 323
 324         //set mode from number, for example the second supported mode, i.e userMode = 1
 325
 326         if (userMode < (int)videoModes.num)
 327         {
 328             wantedMode = videoModes.modes[userMode];
 329         }
 330
 331         //set modes directly from DC134 constants (from dc1394video_mode_t)
 332         else if ((userMode >= DC1394_VIDEO_MODE_MIN) && (userMode <= DC1394_VIDEO_MODE_MAX ))
 333         {
 334             //search for wanted mode, to check if camera supports it
 335             int j = 0;
 336             while ((j< (int)videoModes.num) && videoModes.modes[j]!=userMode)
 337             {
 338                 j++;
 339             }
 340
 341             if ((int)videoModes.modes[j]==userMode)
 342             {
 343                 wantedMode = videoModes.modes[j];
 344             }
 345             else
 346             {
 347                 userMode = -1;  // wanted mode not supported, search for best mode
 348             }
 349         }
 350         else
 351         {
 352             userMode = -1;      // wanted mode not supported, search for best mode
 353         }
 354         //if userMode is available: set it and update size
 355         if (userMode != -1)
 356         {
 357             code = dc1394_video_set_mode(dcCam, wantedMode);
 358             uint32_t width, height;
 359             dc1394_get_image_size_from_video_mode(dcCam, wantedMode, &width, &height);
 360             frameWidth  = (int)width;
 361             frameHeight = (int)height;
 362         }
 363     }
 364
 365     if (userMode == -1 && (frameWidth > 0 || frameHeight > 0))
 366     {
 367         dc1394video_mode_t bestMode = (dc1394video_mode_t) - 1;
 368         dc1394video_modes_t videoModes;
 369         dc1394_video_get_supported_modes(dcCam, &videoModes);
 370         for (i = 0; i < (int)videoModes.num; i++)
 371         {
 372             dc1394video_mode_t mode = videoModes.modes[i];
 373             if (mode >= DC1394_VIDEO_MODE_FORMAT7_MIN && mode <= DC1394_VIDEO_MODE_FORMAT7_MAX)
 374                 continue;
 375             int pref = -1;
 376             dc1394color_coding_t colorCoding;
 377             dc1394_get_color_coding_from_video_mode(dcCam, mode, &colorCoding);
 378
 379             uint32_t width, height;
 380             dc1394_get_image_size_from_video_mode(dcCam, mode, &width, &height);
 381             if ((int)width == frameWidth || (int)height == frameHeight)
 382             {
 383                 if (colorCoding == DC1394_COLOR_CODING_RGB8 ||
 384                         colorCoding == DC1394_COLOR_CODING_RAW8)
 385                 {
 386                     bestMode = mode;
 387                     break;
 388                 }
 389
 390                 if (colorCoding == DC1394_COLOR_CODING_YUV411 ||
 391                         colorCoding == DC1394_COLOR_CODING_YUV422 ||
 392                         (colorCoding == DC1394_COLOR_CODING_YUV444 &&
 393                         pref < 1))
 394                 {
 395                     bestMode = mode;
 396                     pref = 1;
 397                     break;
 398                 }
 399
 400                 if (colorCoding == DC1394_COLOR_CODING_MONO8)
 401                 {
 402                     bestMode = mode;
 403                     pref = 0;
 404                 }
 405             }
 406         }
 407         if ((int)bestMode >= 0)
 408             code = dc1394_video_set_mode(dcCam, bestMode);
 409     }
 410
 411     if (fps > 0)
 412     {
 413         dc1394video_mode_t mode;
 414         dc1394framerates_t framerates;
 415         double minDiff = DBL_MAX;
 416         dc1394framerate_t bestFps = (dc1394framerate_t) - 1;
 417
 418         dc1394_video_get_mode(dcCam, &mode);
 419         dc1394_video_get_supported_framerates(dcCam, mode, &framerates);
 420
 421         for (i = 0; i < (int)framerates.num; i++)
 422         {
 423             dc1394framerate_t ifps = framerates.framerates[i];
 424             double fps1 = (1 << (ifps - DC1394_FRAMERATE_1_875)) * 1.875;
 425             double diff = fabs(fps1 - fps);
 426             if (diff < minDiff)
 427             {
 428                 minDiff = diff;
 429                 bestFps = ifps;
 430             }
 431         }
 432         if ((int)bestFps >= 0)
 433             code = dc1394_video_set_framerate(dcCam, bestFps);
 434     }
 435
 436     if (cameraId == VIDERE)
 437     {
 438         bayerFilter = DC1394_COLOR_FILTER_GBRG;
 439         nimages = 2;
 440         uint32_t value = 0;
 441         dc1394_get_control_register(dcCam, 0x50c, &value);
 442         colorStereo = (value & 0x80000000) != 0;
 443     }
 444
 445     code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
 446     if (code >= 0)
 447     {
 448         FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
 449         dc1394_video_set_transmission(dcCam, DC1394_ON);
 450         if (cameraId == VIDERE)
 451         {
 452             enum { PROC_MODE_OFF, PROC_MODE_NONE, PROC_MODE_TEST, PROC_MODE_RECTIFIED, PROC_MODE_DISPARITY, PROC_MODE_DISPARITY_RAW };
 453             int procMode = PROC_MODE_RECTIFIED;
 454             usleep(100000);
 455             uint32_t qval1 = 0x08000000 | (0x90 << 16) | ((procMode & 0x7) << 16);
 456             uint32_t qval2 = 0x08000000 | (0x9C << 16);
 457             dc1394_set_control_register(dcCam, 0xFF000, qval1);
 458             dc1394_set_control_register(dcCam, 0xFF000, qval2);
 459         }
 460         started = true;
 461     }
 462
 463     return code >= 0;
 464 }
 465
 466 bool CvCaptureCAM_DC1394_v2_CPP::open(int index)
 467 {
 468     bool result = false;
 469     dc1394camera_list_t* cameraList = 0;
 470     dc1394error_t err;
 471
 472     close();
 473
 474     if (!dc1394.dc)
 475         goto _exit_;
 476
 477     err = dc1394_camera_enumerate(dc1394.dc, &cameraList);
 478     if (err < 0 || !cameraList || (unsigned)index >= (unsigned)cameraList->num)
 479         goto _exit_;
 480
 481     guid = cameraList->ids[index].guid;
 482     dcCam = dc1394_camera_new(dc1394.dc, guid);
 483     if (!dcCam)
 484         goto _exit_;
 485
 486     cameraId = dcCam->vendor_id;
 487     //get all features
 488     if (dc1394_feature_get_all(dcCam,&feature_set) == DC1394_SUCCESS)
 489         result = true;
 490     else
 491         result = false;
 492
 493 _exit_:
 494     if (cameraList)
 495         dc1394_camera_free_list(cameraList);
 496
 497     return result;
 498 }
 499
 500 void CvCaptureCAM_DC1394_v2_CPP::close()
 501 {
 502     if (dcCam)
 503     {
 504         // check for fileno valid before using
 505         int fileno=dc1394_capture_get_fileno(dcCam);
 506
 507         if (fileno>=0 && FD_ISSET(fileno, &dc1394.camFds))
 508             FD_CLR(fileno, &dc1394.camFds);
 509         dc1394_video_set_transmission(dcCam, DC1394_OFF);
 510         dc1394_capture_stop(dcCam);
 511         dc1394_camera_free(dcCam);
 512         dcCam = 0;
 513         started = false;
 514     }
 515
 516     for (int i = 0; i < NIMG; i++)
 517     {
 518         cvReleaseImage(&img[i]);
 519         cvReleaseImage(&maps[i][0]);
 520         cvReleaseImage(&maps[i][1]);
 521     }
 522     if (frameC)
 523     {
 524         if (frameC->image)
 525             free(frameC->image);
 526         free(frameC);
 527         frameC = 0;
 528     }
 529 }
 530
 531
 532 bool CvCaptureCAM_DC1394_v2_CPP::grabFrame()
 533 {
 534     dc1394capture_policy_t policy = DC1394_CAPTURE_POLICY_WAIT;
 535     bool code = false, isColor;
 536     dc1394video_frame_t *dcFrame = 0, *fs = 0;
 537     int i, nch;
 538
 539     if (!dcCam || (!started && !startCapture()))
 540         return false;
 541
 542     dc1394_capture_dequeue(dcCam, policy, &dcFrame);
 543
 544     if (!dcFrame)
 545         return false;
 546
 547     if (/*dcFrame->frames_behind > 1 ||*/ dc1394_capture_is_frame_corrupt(dcCam, dcFrame) == DC1394_TRUE)
 548     {
 549         goto _exit_;
 550     }
 551
 552     isColor = dcFrame->color_coding != DC1394_COLOR_CODING_MONO8 &&
 553               dcFrame->color_coding != DC1394_COLOR_CODING_MONO16 &&
 554               dcFrame->color_coding != DC1394_COLOR_CODING_MONO16S;
 555
 556     if (nimages == 2)
 557     {
 558         fs = (dc1394video_frame_t*)calloc(1, sizeof(*fs));
 559
 560         //dc1394_deinterlace_stereo_frames(dcFrame, fs, DC1394_STEREO_METHOD_INTERLACED);
 561         dc1394_deinterlace_stereo_frames_fixed(dcFrame, fs, DC1394_STEREO_METHOD_INTERLACED);
 562
 563         dc1394_capture_enqueue(dcCam, dcFrame); // release the captured frame as soon as possible
 564         dcFrame = 0;
 565         if (!fs->image)
 566             goto _exit_;
 567         isColor = colorStereo;
 568     }
 569     nch = isColor ? 3 : 1;
 570
 571     for (i = 0; i < nimages; i++)
 572     {
 573         IplImage fhdr;
 574         dc1394video_frame_t f = fs ? *fs : *dcFrame, *fc = &f;
 575         f.size[1] /= nimages;
 576         f.image += f.size[0] * f.size[1] * i; // TODO: make it more universal
 577         if (isColor)
 578         {
 579             if (!frameC)
 580                 frameC = (dc1394video_frame_t*)calloc(1, sizeof(*frameC));
 581             frameC->color_coding = nch == 3 ? DC1394_COLOR_CODING_RGB8 : DC1394_COLOR_CODING_MONO8;
 582             if (nimages == 1)
 583             {
 584                 dc1394_convert_frames(&f, frameC);
 585                 dc1394_capture_enqueue(dcCam, dcFrame);
 586                 dcFrame = 0;
 587             }
 588             else
 589             {
 590                 f.color_filter = bayerFilter;
 591                 dc1394_debayer_frames(&f, frameC, bayer);
 592             }
 593             fc = frameC;
 594         }
 595         if (!img[i])
 596             img[i] = cvCreateImage(cvSize(fc->size[0], fc->size[1]), 8, nch);
 597         cvInitImageHeader(&fhdr, cvSize(fc->size[0], fc->size[1]), 8, nch);
 598         cvSetData(&fhdr, fc->image, fc->size[0]*nch);
 599
 600     // Swap R&B channels:
 601     if (nch==3)
 602         cvConvertImage(&fhdr,&fhdr,CV_CVTIMG_SWAP_RB);
 603
 604         if( rectify && cameraId == VIDERE && nimages == 2 )
 605         {
 606             if( !maps[0][0] || maps[0][0]->width != img[i]->width || maps[0][0]->height != img[i]->height )
 607             {
 608                 CvSize size = cvGetSize(img[i]);
 609                 cvReleaseImage(&maps[0][0]);
 610                 cvReleaseImage(&maps[0][1]);
 611                 cvReleaseImage(&maps[1][0]);
 612                 cvReleaseImage(&maps[1][1]);
 613                 maps[0][0] = cvCreateImage(size, IPL_DEPTH_16S, 2);
 614                 maps[0][1] = cvCreateImage(size, IPL_DEPTH_16S, 1);
 615                 maps[1][0] = cvCreateImage(size, IPL_DEPTH_16S, 2);
 616                 maps[1][1] = cvCreateImage(size, IPL_DEPTH_16S, 1);
 617                 char buf[4*4096];
 618                 if( getVidereCalibrationInfo( buf, (int)sizeof(buf) ) &&
 619                     initVidereRectifyMaps( buf, maps[0], maps[1] ))
 620                     ;
 621                 else
 622                     rectify = false;
 623             }
 624             cvRemap(&fhdr, img[i], maps[i][0], maps[i][1]);
 625         }
 626         else
 627             cvCopy(&fhdr, img[i]);
 628     }
 629
 630     code = true;
 631
 632 _exit_:
 633     if (dcFrame)
 634         dc1394_capture_enqueue(dcCam, dcFrame);
 635     if (fs)
 636     {
 637         if (fs->image)
 638             free(fs->image);
 639         free(fs);
 640     }
 641
 642     return code;
 643 }
 644
 645 IplImage* CvCaptureCAM_DC1394_v2_CPP::retrieveFrame(int idx)
 646 {
 647     return 0 <= idx && idx < nimages ? img[idx] : 0;
 648 }
 649
 650 double CvCaptureCAM_DC1394_v2_CPP::getProperty(int propId)
 651 {
 652     switch (propId)
 653     {
 654     case CV_CAP_PROP_FRAME_WIDTH:
 655         return frameWidth ? frameWidth : frameHeight*4 / 3;
 656     case CV_CAP_PROP_FRAME_HEIGHT:
 657         return frameHeight ? frameHeight : frameWidth*3 / 4;
 658     case CV_CAP_PROP_FPS:
 659         return fps;
 660     case CV_CAP_PROP_RECTIFICATION:
 661         return rectify ? 1 : 0;
 662     case CV_CAP_PROP_WHITE_BALANCE_BLUE_U:
 663         if (dc1394_feature_whitebalance_get_value(dcCam,
 664                                                   &feature_set.feature[DC1394_FEATURE_WHITE_BALANCE-DC1394_FEATURE_MIN].BU_value,
 665                                                   &feature_set.feature[DC1394_FEATURE_WHITE_BALANCE-DC1394_FEATURE_MIN].RV_value) == DC1394_SUCCESS)
 666         return feature_set.feature[DC1394_FEATURE_WHITE_BALANCE-DC1394_FEATURE_MIN].BU_value;
 667         break;
 668     case CV_CAP_PROP_WHITE_BALANCE_RED_V:
 669         if (dc1394_feature_whitebalance_get_value(dcCam,
 670                                                   &feature_set.feature[DC1394_FEATURE_WHITE_BALANCE-DC1394_FEATURE_MIN].BU_value,
 671                                                   &feature_set.feature[DC1394_FEATURE_WHITE_BALANCE-DC1394_FEATURE_MIN].RV_value) == DC1394_SUCCESS)
 672         return feature_set.feature[DC1394_FEATURE_WHITE_BALANCE-DC1394_FEATURE_MIN].RV_value;
 673         break;
 674     case CV_CAP_PROP_GUID:
 675         //the least 32 bits are enough to identify the camera
 676         return (double) (guid & 0x00000000FFFFFFFF);
 677         break;
 678     case CV_CAP_PROP_MODE:
 679         return (double) userMode;
 680         break;
 681     case CV_CAP_PROP_ISO_SPEED:
 682         return (double) isoSpeed;
 683     default:
 684         if (propId<CV_CAP_PROP_MAX_DC1394 && dc1394properties[propId]!=-1
 685             && dcCam)
 686             //&& feature_set.feature[dc1394properties[propId]-DC1394_FEATURE_MIN].on_off_capable)
 687             if (dc1394_feature_get_value(dcCam,(dc1394feature_t)dc1394properties[propId],
 688                 &feature_set.feature[dc1394properties[propId]-DC1394_FEATURE_MIN].value) == DC1394_SUCCESS)
 689               return feature_set.feature[dc1394properties[propId]-DC1394_FEATURE_MIN].value;
 690     }
 691     return -1; // the value of the feature can be 0, so returning 0 as an error is wrong
 692 }
 693
 694 bool CvCaptureCAM_DC1394_v2_CPP::setProperty(int propId, double value)
 695 {
 696     switch (propId)
 697     {
 698     case CV_CAP_PROP_FRAME_WIDTH:
 699         if(started)
 700             return false;
 701         frameWidth = cvRound(value);
 702         frameHeight = 0;
 703         break;
 704     case CV_CAP_PROP_FRAME_HEIGHT:
 705         if(started)
 706             return false;
 707         frameWidth = 0;
 708         frameHeight = cvRound(value);
 709         break;
 710     case CV_CAP_PROP_FPS:
 711         if(started)
 712             return false;
 713         fps = value;
 714         break;
 715     case CV_CAP_PROP_RECTIFICATION:
 716         if( cameraId != VIDERE )
 717             return false;
 718         rectify = fabs(value) > FLT_EPSILON;
 719         break;
 720     case CV_CAP_PROP_MODE:
 721         if(started)
 722           return false;
 723         userMode = cvRound(value);
 724         break;
 725     case CV_CAP_PROP_ISO_SPEED:
 726         if(started)
 727           return false;
 728         isoSpeed = cvRound(value);
 729         break;
 730         //The code below is based on coriander, callbacks.c:795, refer to case RANGE_MENU_MAN :
 731          default:
 732              if (propId<CV_CAP_PROP_MAX_DC1394 && dc1394properties[propId]!=-1
 733                  && dcCam)
 734              {
 735                  //get the corresponding feature from property-id
 736                  dc1394feature_info_t *act_feature = &feature_set.feature[dc1394properties[propId]-DC1394_FEATURE_MIN];
 737
 738                  if (cvRound(value) == CV_CAP_PROP_DC1394_OFF)
 739                  {
 740                      if (  (act_feature->on_off_capable)
 741                            && (dc1394_feature_set_power(dcCam, act_feature->id, DC1394_OFF) == DC1394_SUCCESS))
 742                      {
 743                          act_feature->is_on=DC1394_OFF;
 744                          return true;
 745                      }
 746                      return false;
 747                  }
 748                  //try to turn the feature ON, feature can be ON and at the same time it can be not capable to change state to OFF
 749                  if ( (act_feature->is_on == DC1394_OFF) && (act_feature->on_off_capable == DC1394_TRUE))
 750                  {
 751                      if (dc1394_feature_set_power(dcCam, act_feature->id, DC1394_ON) == DC1394_SUCCESS)
 752                        feature_set.feature[dc1394properties[propId]-DC1394_FEATURE_MIN].is_on=DC1394_ON;
 753                  }
 754                  //turn off absolute mode - the actual value will be stored in the value field,
 755                  //otherwise it would be stored into CSR (control and status register) absolute value
 756                  if (act_feature->absolute_capable
 757                      && dc1394_feature_set_absolute_control(dcCam, act_feature->id, DC1394_OFF) !=DC1394_SUCCESS)
 758                      return false;
 759                  else
 760                      act_feature->abs_control=DC1394_OFF;
 761                  //set AUTO
 762                  if (cvRound(value) == CV_CAP_PROP_DC1394_MODE_AUTO)
 763                  {
 764                      if (dc1394_feature_set_mode(dcCam, act_feature->id, DC1394_FEATURE_MODE_AUTO)!=DC1394_SUCCESS)
 765                          return false;
 766                      act_feature->current_mode=DC1394_FEATURE_MODE_AUTO;
 767                      return true;
 768                  }
 769                  //set ONE PUSH
 770                  if (cvRound(value) == CV_CAP_PROP_DC1394_MODE_ONE_PUSH_AUTO)
 771                  {
 772                      //have to set to manual first, otherwise one push will be ignored (AVT  manual 4.3.0 p. 115)
 773                      if (dc1394_feature_set_mode(dcCam, act_feature->id, DC1394_FEATURE_MODE_ONE_PUSH_AUTO)!=DC1394_SUCCESS)
 774                          return false;
 775                      //will change to
 776                      act_feature->current_mode=DC1394_FEATURE_MODE_ONE_PUSH_AUTO;
 777                      return true;
 778                  }
 779                  //set the feature to MANUAL mode,
 780                  if (dc1394_feature_set_mode(dcCam, act_feature->id, DC1394_FEATURE_MODE_MANUAL)!=DC1394_SUCCESS)
 781                      return false;
 782                  else
 783                      act_feature->current_mode=DC1394_FEATURE_MODE_MANUAL;
 784                  // if property is one of the white balance features treat it in different way
 785                  if (propId == CV_CAP_PROP_WHITE_BALANCE_BLUE_U)
 786                  {
 787                      if (dc1394_feature_whitebalance_set_value(dcCam,cvRound(value), act_feature->RV_value)!=DC1394_SUCCESS)
 788                          return false;
 789                      else
 790                      {
 791                          act_feature->BU_value = cvRound(value);
 792                          return true;
 793                      }
 794                  }
 795                  if (propId == CV_CAP_PROP_WHITE_BALANCE_RED_V)
 796                  {
 797                      if (dc1394_feature_whitebalance_set_value(dcCam, act_feature->BU_value, cvRound(value))!=DC1394_SUCCESS)
 798                          return false;
 799                      else
 800                      {
 801                          act_feature->RV_value = cvRound(value);
 802                          return true;
 803                      }
 804                  }
 805
 806                  //first: check boundaries
 807                  if (value < act_feature->min)
 808                  {
 809                      value = act_feature->min;
 810                  }
 811                  else if (value > act_feature->max)
 812                  {
 813                      value = act_feature->max;
 814                  }
 815
 816                  if (dc1394_feature_set_value(dcCam, act_feature->id, cvRound(value)) == DC1394_SUCCESS)
 817                  {
 818                      act_feature->value = value;
 819                      return true;
 820                  }
 821              }
 822              return false;
 823     }
 824     return true;
 825 }
 826
 827
 828 bool CvCaptureCAM_DC1394_v2_CPP::getVidereCalibrationInfo( char* buf, int bufSize )
 829 {
 830     int pos;
 831
 832     for( pos = 0; pos < bufSize - 4; pos += 4 )
 833     {
 834         uint32_t quad = getControlRegister(dcCam, 0xF0800 + pos);
 835         if( quad == 0 || quad == 0xffffffff )
 836             break;
 837         buf[pos] = (uchar)(quad >> 24);
 838         buf[pos+1] = (uchar)(quad >> 16);
 839         buf[pos+2] = (uchar)(quad >> 8);
 840         buf[pos+3] = (uchar)(quad);
 841     }
 842
 843     if( pos == 0 )
 844         return false;
 845
 846     buf[pos] = '\0';
 847     return true;
 848 }
 849
 850
 851 bool CvCaptureCAM_DC1394_v2_CPP::initVidereRectifyMaps( const char* info,
 852     IplImage* ml[2], IplImage* mr[2] )
 853 {
 854     float identity_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 855     CvMat l_rect = cvMat(3, 3, CV_32F, identity_data), r_rect = l_rect;
 856     float l_intrinsic_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 857     float r_intrinsic_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 858     CvMat l_intrinsic = cvMat(3, 3, CV_32F, l_intrinsic_data);
 859     CvMat r_intrinsic = cvMat(3, 3, CV_32F, r_intrinsic_data);
 860     float l_distortion_data[] = {0,0,0,0,0}, r_distortion_data[] = {0,0,0,0,0};
 861     CvMat l_distortion = cvMat(1, 5, CV_32F, l_distortion_data);
 862     CvMat r_distortion = cvMat(1, 5, CV_32F, r_distortion_data);
 863     IplImage* mx = cvCreateImage(cvGetSize(ml[0]), IPL_DEPTH_32F, 1);
 864     IplImage* my = cvCreateImage(cvGetSize(ml[0]), IPL_DEPTH_32F, 1);
 865     int k, j;
 866
 867     for( k = 0; k < 2; k++ )
 868     {
 869         const char* section_name = k == 0 ? "[left_camera]" : "[right_camera]";
 870         static const char* param_names[] = { "f ", "fy", "Cx", "Cy" "kappa1", "kappa2", "tau1", "tau2", "kappa3", 0 };
 871         const char* section_start = strstr( info, section_name );
 872         CvMat* intrinsic = k == 0 ? &l_intrinsic : &r_intrinsic;
 873         CvMat* distortion = k == 0 ? &l_distortion : &r_distortion;
 874         CvMat* rectification = k == 0 ? &l_rect : &r_rect;
 875         IplImage** dst = k == 0 ? ml : mr;
 876         if( !section_start )
 877             break;
 878         section_start += strlen(section_name);
 879         for( j = 0; param_names[j] != 0; j++ )
 880         {
 881             const char* param_value_start = strstr(section_start, param_names[j]);
 882             float val=0;
 883             if(!param_value_start)
 884                 break;
 885             sscanf(param_value_start + strlen(param_names[j]), "%f", &val);
 886             if( j < 4 )
 887                 intrinsic->data.fl[j == 0 ? 0 : j == 1 ? 4 : j == 2 ? 2 : 5] = val;
 888             else
 889                 distortion->data.fl[j - 4] = val;
 890         }
 891         if( param_names[j] != 0 )
 892             break;
 893
 894         // some sanity check for the principal point
 895         if( fabs(mx->width*0.5 - intrinsic->data.fl[2]) > mx->width*0.1 ||
 896             fabs(my->height*0.5 - intrinsic->data.fl[5]) > my->height*0.1 )
 897         {
 898             cvScale( &intrinsic, &intrinsic, 0.5 ); // try the corrected intrinsic matrix for 2x lower resolution
 899             if( fabs(mx->width*0.5 - intrinsic->data.fl[2]) > mx->width*0.05 ||
 900                 fabs(my->height*0.5 - intrinsic->data.fl[5]) > my->height*0.05 )
 901                 cvScale( &intrinsic, &intrinsic, 2 ); // revert it back if the new variant is not much better
 902             intrinsic->data.fl[8] = 1;
 903         }
 904
 905         cvInitUndistortRectifyMap( intrinsic, distortion,
 906                     rectification, intrinsic, mx, my );
 907         cvConvertMaps( mx, my, dst[0], dst[1] );
 908     }
 909
 910     cvReleaseImage( &mx );
 911     cvReleaseImage( &my );
 912     return k >= 2;
 913 }
 914
 915
 916 CvCapture* cvCreateCameraCapture_DC1394_2(int index)
 917 {
 918     CvCaptureCAM_DC1394_v2_CPP* capture = new CvCaptureCAM_DC1394_v2_CPP;
 919
 920     if (capture->open(index))
 921         return capture;
 922
 923     delete capture;
 924     return 0;
 925 }
 926
 927 #endif