void init( const SP_Image& i ) {
CvSize imageSize = cvSize( i->width, i->height );
IplImage *MapX = cvCreateImage( imageSize, IPL_DEPTH_32F, 1);
IplImage *MapY = cvCreateImage( imageSize, IPL_DEPTH_32F, 1);
float kk[9]={0};
for(int x=0; x<9; x++) kk[x]=0;
kk[0] = i->intrinsicLinearCalibration[0];
kk[2] = i->intrinsicLinearCalibration[2];
kk[4] = i->intrinsicLinearCalibration[1];
kk[5] = i->intrinsicLinearCalibration[3];
kk[8] = 1.;
CvMat cvK = cvMat( 3, 3, CV_32FC1, kk);
float kk_c[5];
for(int x=0; x<4; x++) kk_c[x]= i->intrinsicNonlinearCalibration[x];
kk_c[4] = 0;
CvMat dist = cvMat( 5, 1, CV_32FC1, kk_c );
#ifdef HAVE_CV_UNDISTORT_RECTIFY_MAP
float feye[9] = {1,0,0,0,1,0,0,0,1};
CvMat eye = cvMat(3,3,CV_32F, feye);
cvInitUndistortRectifyMap(&cvK, &dist, NULL, &cvK, MapX, MapY);
#else
cvInitUndistortMap(&cvK, &dist, MapX, MapY);
#endif
distortionMaps[ i ] = make_pair( MapX, MapY );
}
void CV_InitUndistortRectifyMapBadArgTest::run_func()
{
if (useCPlus)
{
cv::initUndistortRectifyMap(camera_mat,distortion_coeffs,R,new_camera_mat,img_size,mat_type,mapx,mapy);
}
else
{
cvInitUndistortRectifyMap(_camera_mat,_distortion_coeffs,matR,_new_camera_mat,_mapx,_mapy);
}
}
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;
}
int StereoVision::calibrationEnd() {
calibrationStarted = false;
// ARRAY AND VECTOR STORAGE:
double M1[3][3], M2[3][3], D1[5], D2[5];
double R[3][3], T[3], E[3][3], F[3][3];
CvMat _M1,_M2,_D1,_D2,_R,_T,_E,_F;
_M1 = cvMat(3, 3, CV_64F, M1 );
_M2 = cvMat(3, 3, CV_64F, M2 );
_D1 = cvMat(1, 5, CV_64F, D1 );
_D2 = cvMat(1, 5, CV_64F, D2 );
_R = cvMat(3, 3, CV_64F, R );
_T = cvMat(3, 1, CV_64F, T );
_E = cvMat(3, 3, CV_64F, E );
_F = cvMat(3, 3, CV_64F, F );
// HARVEST CHESSBOARD 3D OBJECT POINT LIST:
objectPoints.resize(sampleCount*cornersN);
for(int k=0; k<sampleCount; k++)
for(int i = 0; i < cornersY; i++ )
for(int j = 0; j < cornersX; j++ )
objectPoints[k*cornersY*cornersX + i*cornersX + j] = cvPoint3D32f(i, j, 0);
npoints.resize(sampleCount,cornersN);
int N = sampleCount * cornersN;
CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
cvSetIdentity(&_M1);
cvSetIdentity(&_M2);
cvZero(&_D1);
cvZero(&_D2);
//CALIBRATE THE STEREO CAMERAS
cvStereoCalibrate( &_objectPoints, &_imagePoints1,
&_imagePoints2, &_npoints,
&_M1, &_D1, &_M2, &_D2,
imageSize, &_R, &_T, &_E, &_F,
cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST + CV_CALIB_SAME_FOCAL_LENGTH
);
//Always work in undistorted space
cvUndistortPoints( &_imagePoints1, &_imagePoints1,&_M1, &_D1, 0, &_M1 );
cvUndistortPoints( &_imagePoints2, &_imagePoints2,&_M2, &_D2, 0, &_M2 );
//COMPUTE AND DISPLAY RECTIFICATION
double R1[3][3], R2[3][3];
CvMat _R1 = cvMat(3, 3, CV_64F, R1);
CvMat _R2 = cvMat(3, 3, CV_64F, R2);
//HARTLEY'S RECTIFICATION METHOD
double H1[3][3], H2[3][3], iM[3][3];
CvMat _H1 = cvMat(3, 3, CV_64F, H1);
CvMat _H2 = cvMat(3, 3, CV_64F, H2);
CvMat _iM = cvMat(3, 3, CV_64F, iM);
cvStereoRectifyUncalibrated(
&_imagePoints1,&_imagePoints2, &_F,
imageSize,
&_H1, &_H2, 3
);
cvInvert(&_M1, &_iM);
cvMatMul(&_H1, &_M1, &_R1);
cvMatMul(&_iM, &_R1, &_R1);
cvInvert(&_M2, &_iM);
cvMatMul(&_H2, &_M2, &_R2);
cvMatMul(&_iM, &_R2, &_R2);
//Precompute map for cvRemap()
cvReleaseMat(&mx1);
cvReleaseMat(&my1);
cvReleaseMat(&mx2);
cvReleaseMat(&my2);
mx1 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
my1 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
mx2 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
my2 = cvCreateMat( imageSize.height,imageSize.width, CV_32F );
cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_M2,mx2,my2);
calibrationDone = true;
return RESULT_OK;
}
