opencv/src/highgui/cvcap_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 "_highgui.h"
  43
  44 #ifdef HAVE_DC1394_2
  45
  46 #include <unistd.h>
  47 #include <stdint.h>
  48 #include <sys/select.h>
  49 #include <dc1394/dc1394.h>
  50 #include <cv.h>
  51 #include <stdlib.h>
  52 #include <string.h>
  53
  54 static dc1394error_t adaptBufferStereoLocal(dc1394video_frame_t *in, dc1394video_frame_t *out)
  55 {
  56     uint32_t bpp;
  57
  58     // buffer position is not changed. Size is boubled in Y
  59     out->size[0] = in->size[0];
  60     out->size[1] = in->size[1] * 2;
  61     out->position[0] = in->position[0];
  62     out->position[1] = in->position[1];
  63
  64     // color coding is set to mono8 or raw8.
  65     switch (in->color_coding)
  66     {
  67     case DC1394_COLOR_CODING_RAW16:
  68         out->color_coding = DC1394_COLOR_CODING_RAW8;
  69         break;
  70     case DC1394_COLOR_CODING_MONO16:
  71     case DC1394_COLOR_CODING_YUV422:
  72         out->color_coding = DC1394_COLOR_CODING_MONO8;
  73         break;
  74     default:
  75         return DC1394_INVALID_COLOR_CODING;
  76     }
  77
  78     // keep the color filter value in all cases. if the format is not raw it will not be further used anyway
  79     out->color_filter = in->color_filter;
  80
  81     // the output YUV byte order must be already set if the buffer is YUV422 at the output
  82     // if the output is not YUV we don't care about this field.
  83     // Hence nothing to do.
  84     // we always convert to 8bits (at this point) we can safely set this value to 8.
  85     out->data_depth = 8;
  86
  87     // don't know what to do with stride... >>>> TODO: STRIDE SHOULD BE TAKEN INTO ACCOUNT... <<<<
  88     // out->stride=??
  89     // the video mode should not change. Color coding and other stuff can be accessed in specific fields of this struct
  90     out->video_mode = in->video_mode;
  91
  92     // padding is kept:
  93     out->padding_bytes = in->padding_bytes;
  94
  95     // image bytes changes:    >>>> TODO: STRIDE SHOULD BE TAKEN INTO ACCOUNT... <<<<
  96     dc1394_get_color_coding_bit_size(out->color_coding, &bpp);
  97     out->image_bytes = (out->size[0] * out->size[1] * bpp) / 8;
  98
  99     // total is image_bytes + padding_bytes
 100     out->total_bytes = out->image_bytes + out->padding_bytes;
 101
 102     // bytes-per-packet and packets_per_frame are internal data that can be kept as is.
 103     out->packet_size  = in->packet_size;
 104     out->packets_per_frame = in->packets_per_frame;
 105
 106     // timestamp, frame_behind, id and camera are copied too:
 107     out->timestamp = in->timestamp;
 108     out->frames_behind = in->frames_behind;
 109     out->camera = in->camera;
 110     out->id = in->id;
 111
 112     // verify memory allocation:
 113     if (out->total_bytes > out->allocated_image_bytes)
 114     {
 115         free(out->image);
 116         out->image = (uint8_t*)malloc(out->total_bytes * sizeof(uint8_t));
 117         out->allocated_image_bytes = out->total_bytes;
 118     }
 119
 120     // Copy padding bytes:
 121     memcpy(&(out->image[out->image_bytes]), &(in->image[in->image_bytes]), out->padding_bytes);
 122     out->little_endian = DC1394_FALSE; // not used before 1.32 is out.
 123     out->data_in_padding = DC1394_FALSE; // not used before 1.32 is out.
 124     return DC1394_SUCCESS;
 125 }
 126
 127 static dc1394error_t dc1394_deinterlace_stereo_frames_fixed(dc1394video_frame_t *in,
 128     dc1394video_frame_t *out, dc1394stereo_method_t method)
 129 {
 130     dc1394error_t err;
 131
 132     if((in->color_coding == DC1394_COLOR_CODING_RAW16) ||
 133        (in->color_coding == DC1394_COLOR_CODING_MONO16) ||
 134        (in->color_coding == DC1394_COLOR_CODING_YUV422))
 135     {
 136         switch (method)
 137         {
 138
 139         case DC1394_STEREO_METHOD_INTERLACED:
 140             err = adaptBufferStereoLocal(in, out);
 141 //FIXED by AB:
 142 //          dc1394_deinterlace_stereo(in->image, out->image, in->size[0], in->size[1]);
 143             dc1394_deinterlace_stereo(in->image, out->image, out->size[0], out->size[1]);
 144             break;
 145
 146         case DC1394_STEREO_METHOD_FIELD:
 147             err = adaptBufferStereoLocal(in, out);
 148             memcpy(out->image, in->image, out->image_bytes);
 149             break;
 150         }
 151
 152         return DC1394_INVALID_STEREO_METHOD;
 153     }
 154     else
 155         return DC1394_FUNCTION_NOT_SUPPORTED;
 156 }
 157
 158 static uint32_t getControlRegister(dc1394camera_t *camera, uint64_t offset)
 159 {
 160     uint32_t value = 0;
 161     dc1394error_t err = dc1394_get_control_register(camera, offset, &value);
 162
 163     assert(err == DC1394_SUCCESS);
 164     return err == DC1394_SUCCESS ? value : 0xffffffff;
 165 }
 166
 167 struct CvDC1394
 168 {
 169     CvDC1394();
 170     ~CvDC1394();
 171
 172     dc1394_t* dc;
 173     fd_set camFds;
 174 };
 175
 176 CvDC1394::CvDC1394()
 177 {
 178     dc = dc1394_new();
 179     FD_ZERO(&camFds);
 180 }
 181
 182 CvDC1394::~CvDC1394()
 183 {
 184     if (dc)
 185         dc1394_free(dc);
 186     dc = 0;
 187 }
 188
 189 static CvDC1394 dc1394;
 190
 191 class CvCaptureCAM_DC1394_v2_CPP : public CvCapture
 192 {
 193 public:
 194     CvCaptureCAM_DC1394_v2_CPP();
 195     virtual ~CvCaptureCAM_DC1394_v2_CPP()
 196     {
 197         close();
 198     }
 199
 200     virtual bool open(int index);
 201     virtual void close();
 202
 203     virtual double getProperty(int);
 204     virtual bool setProperty(int, double);
 205     virtual bool grabFrame();
 206     virtual IplImage* retrieveFrame(int);
 207         virtual int getCaptureDomain() { return CV_CAP_DC1394; } // Return the type of the capture object: CV_CAP_VFW, etc...
 208
 209
 210 protected:
 211     virtual bool startCapture();
 212     virtual bool getVidereCalibrationInfo( char* buf, int bufSize );
 213     virtual bool initVidereRectifyMaps( const char* info, IplImage* ml[2], IplImage* mr[2] );
 214
 215     uint64_t guid;
 216     dc1394camera_t* dcCam;
 217     int isoSpeed;
 218     int videoMode;
 219     int frameWidth, frameHeight;
 220     double fps;
 221     int nDMABufs;
 222     bool started;
 223
 224     enum { VIDERE = 0x5505 };
 225
 226     int cameraId;
 227     bool colorStereo;
 228     dc1394bayer_method_t bayer;
 229     dc1394color_filter_t bayerFilter;
 230
 231     enum { NIMG = 2 };
 232     IplImage *img[NIMG];
 233     dc1394video_frame_t* frameC;
 234     int nimages;
 235
 236     bool rectify;
 237     bool init_rectify;
 238     IplImage *maps[NIMG][2];
 239 };
 240
 241 CvCaptureCAM_DC1394_v2_CPP::CvCaptureCAM_DC1394_v2_CPP()
 242 {
 243     guid = 0;
 244     dcCam = 0;
 245     isoSpeed = 400;
 246     fps = 15;
 247     nDMABufs = 8;
 248     started = false;
 249     cameraId = 0;
 250     colorStereo = false;
 251     bayer = DC1394_BAYER_METHOD_BILINEAR;
 252     bayerFilter = DC1394_COLOR_FILTER_GRBG;
 253     frameWidth = 640;
 254     frameHeight = 480;
 255
 256     for (int i = 0; i < NIMG; i++)
 257         img[i] = maps[i][0] = maps[i][1] = 0;
 258     frameC = 0;
 259     nimages = 1;
 260     rectify = false;
 261 }
 262
 263
 264 bool CvCaptureCAM_DC1394_v2_CPP::startCapture()
 265 {
 266     int i;
 267     int code = 0;
 268     if (!dcCam)
 269         return false;
 270     if (isoSpeed > 0)
 271     {
 272         code = dc1394_video_set_iso_speed(dcCam,
 273                                           isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
 274                                           isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
 275                                           isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
 276                                           isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
 277                                           isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
 278                                           DC1394_ISO_SPEED_3200);
 279     }
 280
 281     if (frameWidth > 0 || frameHeight > 0)
 282     {
 283         dc1394video_mode_t bestMode = (dc1394video_mode_t) - 1;
 284         dc1394video_modes_t videoModes;
 285         dc1394_video_get_supported_modes(dcCam, &videoModes);
 286         for (i = 0; i < (int)videoModes.num; i++)
 287         {
 288             dc1394video_mode_t mode = videoModes.modes[i];
 289                         if (mode >= DC1394_VIDEO_MODE_FORMAT7_MIN && mode <= DC1394_VIDEO_MODE_FORMAT7_MAX)
 290                                 continue;
 291             int pref = -1;
 292             dc1394color_coding_t colorCoding;
 293             dc1394_get_color_coding_from_video_mode(dcCam, mode, &colorCoding);
 294
 295             uint32_t width, height;
 296             dc1394_get_image_size_from_video_mode(dcCam, mode, &width, &height);
 297             if ((int)width == frameWidth || (int)height == frameHeight)
 298             {
 299                 if (colorCoding == DC1394_COLOR_CODING_RGB8 ||
 300                         colorCoding == DC1394_COLOR_CODING_RGB16 ||
 301                         colorCoding == DC1394_COLOR_CODING_RAW8 ||
 302                         colorCoding == DC1394_COLOR_CODING_RAW16)
 303                 {
 304                     bestMode = mode;
 305                     break;
 306                 }
 307
 308                 if (colorCoding == DC1394_COLOR_CODING_YUV411 ||
 309                         colorCoding == DC1394_COLOR_CODING_YUV422 ||
 310                         (colorCoding == DC1394_COLOR_CODING_YUV444 &&
 311                         pref < 1))
 312                 {
 313                     bestMode = mode;
 314                     pref = 1;
 315                 }
 316
 317                 if (colorCoding == DC1394_COLOR_CODING_MONO8 ||
 318                         (colorCoding == DC1394_COLOR_CODING_MONO16 &&
 319                         pref < 0))
 320                 {
 321                     bestMode = mode;
 322                     pref = 0;
 323                 }
 324             }
 325         }
 326         if ((int)bestMode >= 0)
 327             code = dc1394_video_set_mode(dcCam, bestMode);
 328     }
 329
 330     if (fps > 0)
 331     {
 332         dc1394video_mode_t mode;
 333         dc1394framerates_t framerates;
 334         double minDiff = DBL_MAX;
 335         dc1394framerate_t bestFps = (dc1394framerate_t) - 1;
 336
 337         dc1394_video_get_mode(dcCam, &mode);
 338         dc1394_video_get_supported_framerates(dcCam, mode, &framerates);
 339
 340         for (i = 0; i < (int)framerates.num; i++)
 341         {
 342             dc1394framerate_t ifps = framerates.framerates[i];
 343             double fps1 = (1 << (ifps - DC1394_FRAMERATE_1_875)) * 1.875;
 344             double diff = fabs(fps1 - fps);
 345             if (diff < minDiff)
 346             {
 347                 minDiff = diff;
 348                 bestFps = ifps;
 349             }
 350         }
 351         if ((int)bestFps >= 0)
 352             code = dc1394_video_set_framerate(dcCam, bestFps);
 353     }
 354
 355     if (cameraId == VIDERE)
 356     {
 357         bayerFilter = DC1394_COLOR_FILTER_GBRG;
 358         nimages = 2;
 359         uint32_t value = 0;
 360         dc1394_get_control_register(dcCam, 0x50c, &value);
 361         colorStereo = (value & 0x80000000) != 0;
 362     }
 363
 364     code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
 365     if (code >= 0)
 366     {
 367         FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
 368         dc1394_video_set_transmission(dcCam, DC1394_ON);
 369         if (cameraId == VIDERE)
 370         {
 371             enum { PROC_MODE_OFF, PROC_MODE_NONE, PROC_MODE_TEST, PROC_MODE_RECTIFIED, PROC_MODE_DISPARITY, PROC_MODE_DISPARITY_RAW };
 372             int procMode = PROC_MODE_RECTIFIED;
 373             usleep(100000);
 374             uint32_t qval1 = 0x08000000 | (0x90 << 16) | ((procMode & 0x7) << 16);
 375             uint32_t qval2 = 0x08000000 | (0x9C << 16);
 376             dc1394_set_control_register(dcCam, 0xFF000, qval1);
 377             dc1394_set_control_register(dcCam, 0xFF000, qval2);
 378         }
 379         started = true;
 380     }
 381
 382     return code >= 0;
 383 }
 384
 385 bool CvCaptureCAM_DC1394_v2_CPP::open(int index)
 386 {
 387     bool result = false;
 388     dc1394camera_list_t* cameraList = 0;
 389     dc1394error_t err;
 390
 391     close();
 392
 393     if (!dc1394.dc)
 394         goto _exit_;
 395
 396     err = dc1394_camera_enumerate(dc1394.dc, &cameraList);
 397     if (err < 0 || !cameraList || (unsigned)index >= (unsigned)cameraList->num)
 398         goto _exit_;
 399
 400     guid = cameraList->ids[index].guid;
 401     dcCam = dc1394_camera_new(dc1394.dc, guid);
 402     if (!dcCam)
 403         goto _exit_;
 404
 405     cameraId = dcCam->vendor_id;
 406     result = true;
 407
 408 _exit_:
 409     if (cameraList)
 410         dc1394_camera_free_list(cameraList);
 411
 412     return result;
 413 }
 414
 415 void CvCaptureCAM_DC1394_v2_CPP::close()
 416 {
 417     if (dcCam)
 418     {
 419         if (FD_ISSET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds))
 420             FD_CLR(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
 421         dc1394_video_set_transmission(dcCam, DC1394_OFF);
 422         dc1394_capture_stop(dcCam);
 423         dc1394_camera_free(dcCam);
 424         dcCam = 0;
 425         started = false;
 426     }
 427
 428     for (int i = 0; i < NIMG; i++)
 429     {
 430         cvReleaseImage(&img[i]);
 431         cvReleaseImage(&maps[i][0]);
 432         cvReleaseImage(&maps[i][1]);
 433     }
 434     if (frameC)
 435     {
 436         if (frameC->image)
 437             free(frameC->image);
 438         free(frameC);
 439         frameC = 0;
 440     }
 441 }
 442
 443
 444 bool CvCaptureCAM_DC1394_v2_CPP::grabFrame()
 445 {
 446     dc1394capture_policy_t policy = DC1394_CAPTURE_POLICY_WAIT;
 447     bool code = false, isColor;
 448     dc1394video_frame_t *dcFrame = 0, *fs = 0;
 449     int i, nch;
 450
 451     if (!dcCam || (!started && !startCapture()))
 452         return false;
 453
 454     dc1394_capture_dequeue(dcCam, policy, &dcFrame);
 455
 456     if (!dcFrame)
 457         return false;
 458
 459     if (/*dcFrame->frames_behind > 1 ||*/ dc1394_capture_is_frame_corrupt(dcCam, dcFrame) == DC1394_TRUE)
 460     {
 461         goto _exit_;
 462     }
 463
 464     isColor = dcFrame->color_coding != DC1394_COLOR_CODING_MONO8 &&
 465               dcFrame->color_coding != DC1394_COLOR_CODING_MONO16 &&
 466               dcFrame->color_coding != DC1394_COLOR_CODING_MONO16S;
 467
 468     if (nimages == 2)
 469     {
 470         fs = (dc1394video_frame_t*)calloc(1, sizeof(*fs));
 471
 472         //dc1394_deinterlace_stereo_frames(dcFrame, fs, DC1394_STEREO_METHOD_INTERLACED);
 473         dc1394_deinterlace_stereo_frames_fixed(dcFrame, fs, DC1394_STEREO_METHOD_INTERLACED);
 474
 475         dc1394_capture_enqueue(dcCam, dcFrame); // release the captured frame as soon as possible
 476         dcFrame = 0;
 477         if (!fs->image)
 478             goto _exit_;
 479         isColor = colorStereo;
 480     }
 481     nch = isColor ? 3 : 1;
 482
 483     for (i = 0; i < nimages; i++)
 484     {
 485         IplImage fhdr;
 486         dc1394video_frame_t f = fs ? *fs : *dcFrame, *fc = &f;
 487         f.size[1] /= nimages;
 488         f.image += f.size[0] * f.size[1] * i; // TODO: make it more universal
 489         if (isColor)
 490         {
 491             if (!frameC)
 492                 frameC = (dc1394video_frame_t*)calloc(1, sizeof(*frameC));
 493             frameC->color_coding = nch == 3 ? DC1394_COLOR_CODING_RGB8 : DC1394_COLOR_CODING_MONO8;
 494             if (nimages == 1)
 495             {
 496                 dc1394_convert_frames(&f, frameC);
 497                 dc1394_capture_enqueue(dcCam, dcFrame);
 498                 dcFrame = 0;
 499             }
 500             else
 501             {
 502                 f.color_filter = bayerFilter;
 503                 dc1394_debayer_frames(&f, frameC, bayer);
 504             }
 505             fc = frameC;
 506         }
 507         if (!img[i])
 508             img[i] = cvCreateImage(cvSize(fc->size[0], fc->size[1]), 8, nch);
 509         cvInitImageHeader(&fhdr, cvSize(fc->size[0], fc->size[1]), 8, nch);
 510         cvSetData(&fhdr, fc->image, fc->size[0]*nch);
 511
 512         // Swap R&B channels:
 513         if (nch==3)
 514                 cvConvertImage(&fhdr,&fhdr,CV_CVTIMG_SWAP_RB);
 515
 516         if( rectify && cameraId == VIDERE && nimages == 2 )
 517         {
 518             if( !maps[0][0] || maps[0][0]->width != img[i]->width || maps[0][0]->height != img[i]->height )
 519             {
 520                 CvSize size = cvGetSize(img[i]);
 521                 cvReleaseImage(&maps[0][0]);
 522                 cvReleaseImage(&maps[0][1]);
 523                 cvReleaseImage(&maps[1][0]);
 524                 cvReleaseImage(&maps[1][1]);
 525                 maps[0][0] = cvCreateImage(size, IPL_DEPTH_16S, 2);
 526                 maps[0][1] = cvCreateImage(size, IPL_DEPTH_16S, 1);
 527                 maps[1][0] = cvCreateImage(size, IPL_DEPTH_16S, 2);
 528                 maps[1][1] = cvCreateImage(size, IPL_DEPTH_16S, 1);
 529                 char buf[4*4096];
 530                 if( getVidereCalibrationInfo( buf, (int)sizeof(buf) ) &&
 531                     initVidereRectifyMaps( buf, maps[0], maps[1] ))
 532                     ;
 533                 else
 534                     rectify = false;
 535             }
 536             cvRemap(&fhdr, img[i], maps[i][0], maps[i][1]);
 537         }
 538         else
 539             cvCopy(&fhdr, img[i]);
 540     }
 541
 542     code = true;
 543
 544 _exit_:
 545     if (dcFrame)
 546         dc1394_capture_enqueue(dcCam, dcFrame);
 547     if (fs)
 548     {
 549         if (fs->image)
 550             free(fs->image);
 551         free(fs);
 552     }
 553
 554     return code;
 555 }
 556
 557 IplImage* CvCaptureCAM_DC1394_v2_CPP::retrieveFrame(int idx)
 558 {
 559     return 0 <= idx && idx < nimages ? img[idx] : 0;
 560 }
 561
 562 double CvCaptureCAM_DC1394_v2_CPP::getProperty(int propId)
 563 {
 564     switch (propId)
 565     {
 566     case CV_CAP_PROP_FRAME_WIDTH:
 567         return frameWidth ? frameWidth : frameHeight*4 / 3;
 568     case CV_CAP_PROP_FRAME_HEIGHT:
 569         return frameHeight ? frameHeight : frameWidth*3 / 4;
 570     case CV_CAP_PROP_FPS:
 571         return fps;
 572     case CV_CAP_PROP_RECTIFICATION:
 573         return rectify ? 1 : 0;
 574 //    case CV_CAP_PROP_BRIGHTNESS :
 575 //    case CV_CAP_PROP_CONTRAST :
 576 //    case CV_CAP_PROP_WHITE_BALANCE :
 577     default:
 578         ;
 579     }
 580     return 0;
 581 }
 582
 583 bool CvCaptureCAM_DC1394_v2_CPP::setProperty(int propId, double value)
 584 {
 585     switch (propId)
 586     {
 587     case CV_CAP_PROP_FRAME_WIDTH:
 588         if(started)
 589                         return false;
 590         frameWidth = cvRound(value);
 591         frameHeight = 0;
 592         break;
 593     case CV_CAP_PROP_FRAME_HEIGHT:
 594         if(started)
 595                         return false;
 596         frameWidth = 0;
 597         frameHeight = cvRound(value);
 598         break;
 599     case CV_CAP_PROP_FPS:
 600         if(started)
 601                         return false;
 602         fps = value;
 603         break;
 604     case CV_CAP_PROP_RECTIFICATION:
 605         if( cameraId != VIDERE )
 606             return false;
 607         rectify = fabs(value) > FLT_EPSILON;
 608         break;
 609     default:
 610         return false;
 611     }
 612     return true;
 613 }
 614
 615
 616 bool CvCaptureCAM_DC1394_v2_CPP::getVidereCalibrationInfo( char* buf, int bufSize )
 617 {
 618     int pos;
 619
 620     for( pos = 0; pos < bufSize - 4; pos += 4 )
 621     {
 622         uint32_t quad = getControlRegister(dcCam, 0xF0800 + pos);
 623         if( quad == 0 || quad == 0xffffffff )
 624             break;
 625         buf[pos] = (uchar)(quad >> 24);
 626         buf[pos+1] = (uchar)(quad >> 16);
 627         buf[pos+2] = (uchar)(quad >> 8);
 628         buf[pos+3] = (uchar)(quad);
 629     }
 630
 631     if( pos == 0 )
 632         return false;
 633
 634     buf[pos] = '\0';
 635     return true;
 636 }
 637
 638
 639 bool CvCaptureCAM_DC1394_v2_CPP::initVidereRectifyMaps( const char* info,
 640     IplImage* ml[2], IplImage* mr[2] )
 641 {
 642     float identity_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 643     CvMat l_rect = cvMat(3, 3, CV_32F, identity_data), r_rect = l_rect;
 644     float l_intrinsic_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 645     float r_intrinsic_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 646     CvMat l_intrinsic = cvMat(3, 3, CV_32F, l_intrinsic_data);
 647     CvMat r_intrinsic = cvMat(3, 3, CV_32F, r_intrinsic_data);
 648     float l_distortion_data[] = {0,0,0,0,0}, r_distortion_data[] = {0,0,0,0,0};
 649     CvMat l_distortion = cvMat(1, 5, CV_32F, l_distortion_data);
 650     CvMat r_distortion = cvMat(1, 5, CV_32F, r_distortion_data);
 651     IplImage* mx = cvCreateImage(cvGetSize(ml[0]), IPL_DEPTH_32F, 1);
 652     IplImage* my = cvCreateImage(cvGetSize(ml[0]), IPL_DEPTH_32F, 1);
 653     int k, j;
 654
 655     for( k = 0; k < 2; k++ )
 656     {
 657         const char* section_name = k == 0 ? "[left_camera]" : "[right_camera]";
 658         static const char* param_names[] = { "f ", "fy", "Cx", "Cy" "kappa1", "kappa2", "tau1", "tau2", "kappa3", 0 };
 659         const char* section_start = strstr( info, section_name );
 660         CvMat* intrinsic = k == 0 ? &l_intrinsic : &r_intrinsic;
 661         CvMat* distortion = k == 0 ? &l_distortion : &r_distortion;
 662         CvMat* rectification = k == 0 ? &l_rect : &r_rect;
 663         IplImage** dst = k == 0 ? ml : mr;
 664         if( !section_start )
 665             break;
 666         section_start += strlen(section_name);
 667         for( j = 0; param_names[j] != 0; j++ )
 668         {
 669             const char* param_value_start = strstr(section_start, param_names[j]);
 670             float val=0;
 671             if(!param_value_start)
 672                 break;
 673             sscanf(param_value_start + strlen(param_names[j]), "%f", &val);
 674             if( j < 4 )
 675                 intrinsic->data.fl[j == 0 ? 0 : j == 1 ? 4 : j == 2 ? 2 : 5] = val;
 676             else
 677                 distortion->data.fl[j - 4] = val;
 678         }
 679         if( param_names[j] != 0 )
 680             break;
 681
 682         // some sanity check for the principal point
 683         if( fabs(mx->width*0.5 - intrinsic->data.fl[2]) > mx->width*0.1 ||
 684             fabs(my->height*0.5 - intrinsic->data.fl[5]) > my->height*0.1 )
 685         {
 686             cvScale( &intrinsic, &intrinsic, 0.5 ); // try the corrected intrinsic matrix for 2x lower resolution
 687             if( fabs(mx->width*0.5 - intrinsic->data.fl[2]) > mx->width*0.05 ||
 688                 fabs(my->height*0.5 - intrinsic->data.fl[5]) > my->height*0.05 )
 689                 cvScale( &intrinsic, &intrinsic, 2 ); // revert it back if the new variant is not much better
 690             intrinsic->data.fl[8] = 1;
 691         }
 692
 693         cvInitUndistortRectifyMap( intrinsic, distortion,
 694                     rectification, intrinsic, mx, my );
 695         cvConvertMaps( mx, my, dst[0], dst[1] );
 696     }
 697
 698     cvReleaseImage( &mx );
 699     cvReleaseImage( &my );
 700     return k >= 2;
 701 }
 702
 703
 704 CvCapture* cvCreateCameraCapture_DC1394_2(int index)
 705 {
 706     CvCaptureCAM_DC1394_v2_CPP* capture = new CvCaptureCAM_DC1394_v2_CPP;
 707
 708     if (capture->open(index))
 709         return capture;
 710
 711     delete capture;
 712     return 0;
 713 }
 714
 715 #endif