--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// Intel License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of Intel Corporation may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_highgui.h"
+
+#ifdef HAVE_DC1394_2
+
+#include <unistd.h>
+#include <stdint.h>
+#include <sys/select.h>
+#include <dc1394/dc1394.h>
+#include <cv.h>
+#include <stdlib.h>
+#include <string.h>
+
+static dc1394error_t adaptBufferStereoLocal(dc1394video_frame_t *in, dc1394video_frame_t *out)
+{
+ uint32_t bpp;
+
+ // buffer position is not changed. Size is boubled in Y
+ out->size[0] = in->size[0];
+ out->size[1] = in->size[1] * 2;
+ out->position[0] = in->position[0];
+ out->position[1] = in->position[1];
+
+ // color coding is set to mono8 or raw8.
+ switch (in->color_coding)
+ {
+ case DC1394_COLOR_CODING_RAW16:
+ out->color_coding = DC1394_COLOR_CODING_RAW8;
+ break;
+ case DC1394_COLOR_CODING_MONO16:
+ case DC1394_COLOR_CODING_YUV422:
+ out->color_coding = DC1394_COLOR_CODING_MONO8;
+ break;
+ default:
+ return DC1394_INVALID_COLOR_CODING;
+ }
+
+ // keep the color filter value in all cases. if the format is not raw it will not be further used anyway
+ out->color_filter = in->color_filter;
+
+ // the output YUV byte order must be already set if the buffer is YUV422 at the output
+ // if the output is not YUV we don't care about this field.
+ // Hence nothing to do.
+ // we always convert to 8bits (at this point) we can safely set this value to 8.
+ out->data_depth = 8;
+
+ // don't know what to do with stride... >>>> TODO: STRIDE SHOULD BE TAKEN INTO ACCOUNT... <<<<
+ // out->stride=??
+ // the video mode should not change. Color coding and other stuff can be accessed in specific fields of this struct
+ out->video_mode = in->video_mode;
+
+ // padding is kept:
+ out->padding_bytes = in->padding_bytes;
+
+ // image bytes changes: >>>> TODO: STRIDE SHOULD BE TAKEN INTO ACCOUNT... <<<<
+ dc1394_get_color_coding_bit_size(out->color_coding, &bpp);
+ out->image_bytes = (out->size[0] * out->size[1] * bpp) / 8;
+
+ // total is image_bytes + padding_bytes
+ out->total_bytes = out->image_bytes + out->padding_bytes;
+
+ // bytes-per-packet and packets_per_frame are internal data that can be kept as is.
+ out->packet_size = in->packet_size;
+ out->packets_per_frame = in->packets_per_frame;
+
+ // timestamp, frame_behind, id and camera are copied too:
+ out->timestamp = in->timestamp;
+ out->frames_behind = in->frames_behind;
+ out->camera = in->camera;
+ out->id = in->id;
+
+ // verify memory allocation:
+ if (out->total_bytes > out->allocated_image_bytes)
+ {
+ free(out->image);
+ out->image = (uint8_t*)malloc(out->total_bytes * sizeof(uint8_t));
+ out->allocated_image_bytes = out->total_bytes;
+ }
+
+ // Copy padding bytes:
+ memcpy(&(out->image[out->image_bytes]), &(in->image[in->image_bytes]), out->padding_bytes);
+ out->little_endian = DC1394_FALSE; // not used before 1.32 is out.
+ out->data_in_padding = DC1394_FALSE; // not used before 1.32 is out.
+ return DC1394_SUCCESS;
+}
+
+static dc1394error_t dc1394_deinterlace_stereo_frames_fixed(dc1394video_frame_t *in,
+ dc1394video_frame_t *out, dc1394stereo_method_t method)
+{
+ dc1394error_t err;
+
+ if((in->color_coding == DC1394_COLOR_CODING_RAW16) ||
+ (in->color_coding == DC1394_COLOR_CODING_MONO16) ||
+ (in->color_coding == DC1394_COLOR_CODING_YUV422))
+ {
+ switch (method)
+ {
+
+ case DC1394_STEREO_METHOD_INTERLACED:
+ err = adaptBufferStereoLocal(in, out);
+//FIXED by AB:
+// dc1394_deinterlace_stereo(in->image, out->image, in->size[0], in->size[1]);
+ dc1394_deinterlace_stereo(in->image, out->image, out->size[0], out->size[1]);
+ break;
+
+ case DC1394_STEREO_METHOD_FIELD:
+ err = adaptBufferStereoLocal(in, out);
+ memcpy(out->image, in->image, out->image_bytes);
+ break;
+ }
+
+ return DC1394_INVALID_STEREO_METHOD;
+ }
+ else
+ return DC1394_FUNCTION_NOT_SUPPORTED;
+}
+
+static uint32_t getControlRegister(dc1394camera_t *camera, uint64_t offset)
+{
+ uint32_t value = 0;
+ dc1394error_t err = dc1394_get_control_register(camera, offset, &value);
+
+ assert(err == DC1394_SUCCESS);
+ return err == DC1394_SUCCESS ? value : 0xffffffff;
+}
+
+struct CvDC1394
+{
+ CvDC1394();
+ ~CvDC1394();
+
+ dc1394_t* dc;
+ fd_set camFds;
+};
+
+CvDC1394::CvDC1394()
+{
+ dc = dc1394_new();
+ FD_ZERO(&camFds);
+}
+
+CvDC1394::~CvDC1394()
+{
+ if (dc)
+ dc1394_free(dc);
+ dc = 0;
+}
+
+static CvDC1394 dc1394;
+
+class CvCaptureCAM_DC1394_v2_CPP : public CvCapture
+{
+public:
+ CvCaptureCAM_DC1394_v2_CPP();
+ virtual ~CvCaptureCAM_DC1394_v2_CPP()
+ {
+ close();
+ }
+
+ virtual bool open(int index);
+ virtual void close();
+
+ virtual double getProperty(int);
+ virtual bool setProperty(int, double);
+ virtual bool grabFrame();
+ virtual IplImage* retrieveFrame(int);
+ virtual int getCaptureDomain() { return CV_CAP_DC1394; } // Return the type of the capture object: CV_CAP_VFW, etc...
+
+
+protected:
+ virtual bool startCapture();
+ virtual bool getVidereCalibrationInfo( char* buf, int bufSize );
+ virtual bool initVidereRectifyMaps( const char* info, IplImage* ml[2], IplImage* mr[2] );
+
+ uint64_t guid;
+ dc1394camera_t* dcCam;
+ int isoSpeed;
+ int videoMode;
+ int frameWidth, frameHeight;
+ double fps;
+ int nDMABufs;
+ bool started;
+
+ enum { VIDERE = 0x5505 };
+
+ int cameraId;
+ bool colorStereo;
+ dc1394bayer_method_t bayer;
+ dc1394color_filter_t bayerFilter;
+
+ enum { NIMG = 2 };
+ IplImage *img[NIMG];
+ dc1394video_frame_t* frameC;
+ int nimages;
+
+ bool rectify;
+ bool init_rectify;
+ IplImage *maps[NIMG][2];
+};
+
+CvCaptureCAM_DC1394_v2_CPP::CvCaptureCAM_DC1394_v2_CPP()
+{
+ guid = 0;
+ dcCam = 0;
+ isoSpeed = 400;
+ fps = 15;
+ nDMABufs = 8;
+ started = false;
+ cameraId = 0;
+ colorStereo = false;
+ bayer = DC1394_BAYER_METHOD_BILINEAR;
+ bayerFilter = DC1394_COLOR_FILTER_GRBG;
+ frameWidth = 640;
+ frameHeight = 480;
+
+ for (int i = 0; i < NIMG; i++)
+ img[i] = maps[i][0] = maps[i][1] = 0;
+ frameC = 0;
+ nimages = 1;
+ rectify = false;
+}
+
+
+bool CvCaptureCAM_DC1394_v2_CPP::startCapture()
+{
+ int i;
+ int code = 0;
+ if (!dcCam)
+ return false;
+ if (isoSpeed > 0)
+ {
+ code = dc1394_video_set_iso_speed(dcCam,
+ isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
+ isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
+ isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
+ isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
+ isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
+ DC1394_ISO_SPEED_3200);
+ }
+
+ if (frameWidth > 0 || frameHeight > 0)
+ {
+ dc1394video_mode_t bestMode = (dc1394video_mode_t) - 1;
+ dc1394video_modes_t videoModes;
+ dc1394_video_get_supported_modes(dcCam, &videoModes);
+ for (i = 0; i < (int)videoModes.num; i++)
+ {
+ dc1394video_mode_t mode = videoModes.modes[i];
+ int pref = -1;
+ dc1394color_coding_t colorCoding;
+ dc1394_get_color_coding_from_video_mode(dcCam, mode, &colorCoding);
+
+ uint32_t width, height;
+ dc1394_get_image_size_from_video_mode(dcCam, mode, &width, &height);
+ if ((int)width == frameWidth || (int)height == frameHeight)
+ {
+ if (colorCoding == DC1394_COLOR_CODING_RGB8 ||
+ colorCoding == DC1394_COLOR_CODING_RGB16 ||
+ colorCoding == DC1394_COLOR_CODING_RAW8 ||
+ colorCoding == DC1394_COLOR_CODING_RAW16)
+ {
+ bestMode = mode;
+ break;
+ }
+
+ if (colorCoding == DC1394_COLOR_CODING_YUV411 ||
+ colorCoding == DC1394_COLOR_CODING_YUV422 ||
+ (colorCoding == DC1394_COLOR_CODING_YUV444 &&
+ pref < 1))
+ {
+ bestMode = mode;
+ pref = 1;
+ }
+
+ if (colorCoding == DC1394_COLOR_CODING_MONO8 ||
+ (colorCoding == DC1394_COLOR_CODING_MONO16 &&
+ pref < 0))
+ {
+ bestMode = mode;
+ pref = 0;
+ }
+ }
+ }
+ if ((int)bestMode >= 0)
+ code = dc1394_video_set_mode(dcCam, bestMode);
+ }
+
+ if (fps > 0)
+ {
+ dc1394video_mode_t mode;
+ dc1394framerates_t framerates;
+ double minDiff = DBL_MAX;
+ dc1394framerate_t bestFps = (dc1394framerate_t) - 1;
+
+ dc1394_video_get_mode(dcCam, &mode);
+ dc1394_video_get_supported_framerates(dcCam, mode, &framerates);
+
+ for (i = 0; i < (int)framerates.num; i++)
+ {
+ dc1394framerate_t ifps = framerates.framerates[i];
+ double fps1 = (1 << (ifps - DC1394_FRAMERATE_1_875)) * 1.875;
+ double diff = fabs(fps1 - fps);
+ if (diff < minDiff)
+ {
+ minDiff = diff;
+ bestFps = ifps;
+ }
+ }
+ if ((int)bestFps >= 0)
+ code = dc1394_video_set_framerate(dcCam, bestFps);
+ }
+
+ if (cameraId == VIDERE)
+ {
+ bayerFilter = DC1394_COLOR_FILTER_GBRG;
+ nimages = 2;
+ uint32_t value = 0;
+ dc1394_get_control_register(dcCam, 0x50c, &value);
+ colorStereo = (value & 0x80000000) != 0;
+ }
+
+ code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
+ if (code >= 0)
+ {
+ FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
+ dc1394_video_set_transmission(dcCam, DC1394_ON);
+ if (cameraId == VIDERE)
+ {
+ enum { PROC_MODE_OFF, PROC_MODE_NONE, PROC_MODE_TEST, PROC_MODE_RECTIFIED, PROC_MODE_DISPARITY, PROC_MODE_DISPARITY_RAW };
+ int procMode = PROC_MODE_RECTIFIED;
+ usleep(100000);
+ uint32_t qval1 = 0x08000000 | (0x90 << 16) | ((procMode & 0x7) << 16);
+ uint32_t qval2 = 0x08000000 | (0x9C << 16);
+ dc1394_set_control_register(dcCam, 0xFF000, qval1);
+ dc1394_set_control_register(dcCam, 0xFF000, qval2);
+ }
+ started = true;
+ }
+
+ return code >= 0;
+}
+
+bool CvCaptureCAM_DC1394_v2_CPP::open(int index)
+{
+ bool result = false;
+ dc1394camera_list_t* cameraList = 0;
+ dc1394error_t err;
+
+ close();
+
+ if (!dc1394.dc)
+ goto _exit_;
+
+ err = dc1394_camera_enumerate(dc1394.dc, &cameraList);
+ if (err < 0 || !cameraList || (unsigned)index >= (unsigned)cameraList->num)
+ goto _exit_;
+
+ guid = cameraList->ids[index].guid;
+ dcCam = dc1394_camera_new(dc1394.dc, guid);
+ if (!dcCam)
+ goto _exit_;
+
+ cameraId = dcCam->vendor_id;
+ result = true;
+
+_exit_:
+ if (cameraList)
+ dc1394_camera_free_list(cameraList);
+
+ return result;
+}
+
+void CvCaptureCAM_DC1394_v2_CPP::close()
+{
+ if (dcCam)
+ {
+ if (FD_ISSET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds))
+ FD_CLR(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
+ dc1394_video_set_transmission(dcCam, DC1394_OFF);
+ dc1394_capture_stop(dcCam);
+ dc1394_camera_free(dcCam);
+ dcCam = 0;
+ started = false;
+ }
+
+ for (int i = 0; i < NIMG; i++)
+ {
+ cvReleaseImage(&img[i]);
+ cvReleaseImage(&maps[i][0]);
+ cvReleaseImage(&maps[i][1]);
+ }
+ if (frameC)
+ {
+ if (frameC->image)
+ free(frameC->image);
+ free(frameC);
+ frameC = 0;
+ }
+}
+
+
+bool CvCaptureCAM_DC1394_v2_CPP::grabFrame()
+{
+ dc1394capture_policy_t policy = DC1394_CAPTURE_POLICY_WAIT;
+ bool code = false, isColor;
+ dc1394video_frame_t *dcFrame = 0, *fs = 0;
+ int i, nch;
+
+ if (!dcCam || (!started && !startCapture()))
+ return false;
+
+ dc1394_capture_dequeue(dcCam, policy, &dcFrame);
+
+ if (!dcFrame)
+ return false;
+
+ if (/*dcFrame->frames_behind > 1 ||*/ dc1394_capture_is_frame_corrupt(dcCam, dcFrame) == DC1394_TRUE)
+ {
+ goto _exit_;
+ }
+
+ isColor = dcFrame->color_coding != DC1394_COLOR_CODING_MONO8 &&
+ dcFrame->color_coding != DC1394_COLOR_CODING_MONO16 &&
+ dcFrame->color_coding != DC1394_COLOR_CODING_MONO16S;
+
+ if (nimages == 2)
+ {
+ fs = (dc1394video_frame_t*)calloc(1, sizeof(*fs));
+
+ //dc1394_deinterlace_stereo_frames(dcFrame, fs, DC1394_STEREO_METHOD_INTERLACED);
+ dc1394_deinterlace_stereo_frames_fixed(dcFrame, fs, DC1394_STEREO_METHOD_INTERLACED);
+
+ dc1394_capture_enqueue(dcCam, dcFrame); // release the captured frame as soon as possible
+ dcFrame = 0;
+ if (!fs->image)
+ goto _exit_;
+ isColor = colorStereo;
+ }
+ nch = isColor ? 3 : 1;
+
+ for (i = 0; i < nimages; i++)
+ {
+ IplImage fhdr;
+ dc1394video_frame_t f = fs ? *fs : *dcFrame, *fc = &f;
+ f.size[1] /= nimages;
+ f.image += f.size[0] * f.size[1] * i; // TODO: make it more universal
+ if (isColor)
+ {
+ if (!frameC)
+ frameC = (dc1394video_frame_t*)calloc(1, sizeof(*frameC));
+ frameC->color_coding = nch == 3 ? DC1394_COLOR_CODING_RGB8 : DC1394_COLOR_CODING_MONO8;
+ if (nimages == 1)
+ {
+ dc1394_convert_frames(&f, frameC);
+ dc1394_capture_enqueue(dcCam, dcFrame);
+ dcFrame = 0;
+ }
+ else
+ {
+ f.color_filter = bayerFilter;
+ dc1394_debayer_frames(&f, frameC, bayer);
+ }
+ fc = frameC;
+ }
+ if (!img[i])
+ img[i] = cvCreateImage(cvSize(fc->size[0], fc->size[1]), 8, nch);
+ cvInitImageHeader(&fhdr, cvSize(fc->size[0], fc->size[1]), 8, nch);
+ cvSetData(&fhdr, fc->image, fc->size[0]*nch);
+
+ // Swap R&B channels:
+ if (nch==3)
+ cvConvertImage(&fhdr,&fhdr,CV_CVTIMG_SWAP_RB);
+
+ if( rectify && cameraId == VIDERE && nimages == 2 )
+ {
+ if( !maps[0][0] || maps[0][0]->width != img[i]->width || maps[0][0]->height != img[i]->height )
+ {
+ CvSize size = cvGetSize(img[i]);
+ cvReleaseImage(&maps[0][0]);
+ cvReleaseImage(&maps[0][1]);
+ cvReleaseImage(&maps[1][0]);
+ cvReleaseImage(&maps[1][1]);
+ maps[0][0] = cvCreateImage(size, IPL_DEPTH_16S, 2);
+ maps[0][1] = cvCreateImage(size, IPL_DEPTH_16S, 1);
+ maps[1][0] = cvCreateImage(size, IPL_DEPTH_16S, 2);
+ maps[1][1] = cvCreateImage(size, IPL_DEPTH_16S, 1);
+ char buf[4*4096];
+ if( getVidereCalibrationInfo( buf, (int)sizeof(buf) ) &&
+ initVidereRectifyMaps( buf, maps[0], maps[1] ))
+ ;
+ else
+ rectify = false;
+ }
+ cvRemap(&fhdr, img[i], maps[i][0], maps[i][1]);
+ }
+ else
+ cvCopy(&fhdr, img[i]);
+ }
+
+ code = true;
+
+_exit_:
+ if (dcFrame)
+ dc1394_capture_enqueue(dcCam, dcFrame);
+ if (fs)
+ {
+ if (fs->image)
+ free(fs->image);
+ free(fs);
+ }
+
+ return code;
+}
+
+IplImage* CvCaptureCAM_DC1394_v2_CPP::retrieveFrame(int idx)
+{
+ return 0 <= idx && idx < nimages ? img[idx] : 0;
+}
+
+double CvCaptureCAM_DC1394_v2_CPP::getProperty(int propId)
+{
+ switch (propId)
+ {
+ case CV_CAP_PROP_FRAME_WIDTH:
+ return frameWidth ? frameWidth : frameHeight*4 / 3;
+ case CV_CAP_PROP_FRAME_HEIGHT:
+ return frameHeight ? frameHeight : frameWidth*3 / 4;
+ case CV_CAP_PROP_FPS:
+ return fps;
+ case CV_CAP_PROP_RECTIFICATION:
+ return rectify ? 1 : 0;
+// case CV_CAP_PROP_BRIGHTNESS :
+// case CV_CAP_PROP_CONTRAST :
+// case CV_CAP_PROP_WHITE_BALANCE :
+ default:
+ ;
+ }
+ return 0;
+}
+
+bool CvCaptureCAM_DC1394_v2_CPP::setProperty(int propId, double value)
+{
+ switch (propId)
+ {
+ case CV_CAP_PROP_FRAME_WIDTH:
+ if(started)
+ return false;
+ frameWidth = cvRound(value);
+ frameHeight = 0;
+ break;
+ case CV_CAP_PROP_FRAME_HEIGHT:
+ if(started)
+ return false;
+ frameWidth = 0;
+ frameHeight = cvRound(value);
+ break;
+ case CV_CAP_PROP_FPS:
+ if(started)
+ return false;
+ fps = value;
+ break;
+ case CV_CAP_PROP_RECTIFICATION:
+ if( cameraId != VIDERE )
+ return false;
+ rectify = fabs(value) > FLT_EPSILON;
+ break;
+ default:
+ return false;
+ }
+ return true;
+}
+
+
+bool CvCaptureCAM_DC1394_v2_CPP::getVidereCalibrationInfo( char* buf, int bufSize )
+{
+ int pos;
+
+ for( pos = 0; pos < bufSize - 4; pos += 4 )
+ {
+ uint32_t quad = getControlRegister(dcCam, 0xF0800 + pos);
+ if( quad == 0 || quad == 0xffffffff )
+ break;
+ buf[pos] = (uchar)(quad >> 24);
+ buf[pos+1] = (uchar)(quad >> 16);
+ buf[pos+2] = (uchar)(quad >> 8);
+ buf[pos+3] = (uchar)(quad);
+ }
+
+ if( pos == 0 )
+ return false;
+
+ buf[pos] = '\0';
+ return true;
+}
+
+
+bool CvCaptureCAM_DC1394_v2_CPP::initVidereRectifyMaps( const char* info,
+ IplImage* ml[2], IplImage* mr[2] )
+{
+ float identity_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+ CvMat l_rect = cvMat(3, 3, CV_32F, identity_data), r_rect = l_rect;
+ float l_intrinsic_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+ float r_intrinsic_data[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+ CvMat l_intrinsic = cvMat(3, 3, CV_32F, l_intrinsic_data);
+ CvMat r_intrinsic = cvMat(3, 3, CV_32F, r_intrinsic_data);
+ float l_distortion_data[] = {0,0,0,0,0}, r_distortion_data[] = {0,0,0,0,0};
+ CvMat l_distortion = cvMat(1, 5, CV_32F, l_distortion_data);
+ CvMat r_distortion = cvMat(1, 5, CV_32F, r_distortion_data);
+ IplImage* mx = cvCreateImage(cvGetSize(ml[0]), IPL_DEPTH_32F, 1);
+ IplImage* my = cvCreateImage(cvGetSize(ml[0]), IPL_DEPTH_32F, 1);
+ int k, j;
+
+ for( k = 0; k < 2; k++ )
+ {
+ const char* section_name = k == 0 ? "[left_camera]" : "[right_camera]";
+ static const char* param_names[] = { "f ", "fy", "Cx", "Cy" "kappa1", "kappa2", "tau1", "tau2", "kappa3", 0 };
+ const char* section_start = strstr( info, section_name );
+ CvMat* intrinsic = k == 0 ? &l_intrinsic : &r_intrinsic;
+ CvMat* distortion = k == 0 ? &l_distortion : &r_distortion;
+ CvMat* rectification = k == 0 ? &l_rect : &r_rect;
+ IplImage** dst = k == 0 ? ml : mr;
+ if( !section_start )
+ break;
+ section_start += strlen(section_name);
+ for( j = 0; param_names[j] != 0; j++ )
+ {
+ const char* param_value_start = strstr(section_start, param_names[j]);
+ float val=0;
+ if(!param_value_start)
+ break;
+ sscanf(param_value_start + strlen(param_names[j]), "%f", &val);
+ if( j < 4 )
+ intrinsic->data.fl[j == 0 ? 0 : j == 1 ? 4 : j == 2 ? 2 : 5] = val;
+ else
+ distortion->data.fl[j - 4] = val;
+ }
+ if( param_names[j] != 0 )
+ break;
+
+ // some sanity check for the principal point
+ if( fabs(mx->width*0.5 - intrinsic->data.fl[2]) > mx->width*0.1 ||
+ fabs(my->height*0.5 - intrinsic->data.fl[5]) > my->height*0.1 )
+ {
+ cvScale( &intrinsic, &intrinsic, 0.5 ); // try the corrected intrinsic matrix for 2x lower resolution
+ if( fabs(mx->width*0.5 - intrinsic->data.fl[2]) > mx->width*0.05 ||
+ fabs(my->height*0.5 - intrinsic->data.fl[5]) > my->height*0.05 )
+ cvScale( &intrinsic, &intrinsic, 2 ); // revert it back if the new variant is not much better
+ intrinsic->data.fl[8] = 1;
+ }
+
+ cvInitUndistortRectifyMap( intrinsic, distortion,
+ rectification, intrinsic, mx, my );
+ cvConvertMaps( mx, my, dst[0], dst[1] );
+ }
+
+ cvReleaseImage( &mx );
+ cvReleaseImage( &my );
+ return k >= 2;
+}
+
+
+CvCapture* cvCreateCameraCapture_DC1394_2(int index)
+{
+ CvCaptureCAM_DC1394_v2_CPP* capture = new CvCaptureCAM_DC1394_v2_CPP;
+
+ if (capture->open(index))
+ return capture;
+
+ delete capture;
+ return 0;
+}
+
+#endif