bool Calibration::calibrate(Cv::Calibration::ChessboardFinder* finder1, Cv::Calibration::ChessboardFinder* finder2, Cv::Calibration::CamIntrinsics* cam1, Cv::Calibration::CamIntrinsics* cam2, int flags) { finder1->generateMatrices(); finder2->generateMatrices(); cvStereoCalibrate( // In finder1->objectPoints, // XXX Not &objectPoints ! finder1->imagePoints, finder2->imagePoints, finder1->pointCounts, // In/Inout/Out (Depends upon flag!) cam1->intrinsic, cam1->distortion, cam2->intrinsic, cam2->distortion, cam1->imageSize, // Out rotation, translation, essential, // TODO: Optional fundamental, // TODO: Optional cvTermCriteria( CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5 ), flags ); /* Flags: CV_CALIB_FIX_INTRINSIC CV_CALIB_USE_INTRINSIC_GUESS */ return true; // TODO: TEMP }
void ImageOverlayer::initializeCamParams() { int nframes=1; int n = 9; //Number of points used to process int N=nframes*n; vector<CvPoint2D32f> temp(n); vector<int> npoints; vector<CvPoint3D32f> objectPoints; vector<CvPoint2D32f> points[2]; points[0].resize(n); points[1].resize(n); double R[3][3], T[3], E[3][3], F[3][3]; double Q[4][4]; /*************************************************************/ _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 ); CvMat _R = cvMat(3, 3, CV_64F, R ); CvMat _T = cvMat(3, 1, CV_64F, T ); CvMat _E = cvMat(3, 3, CV_64F, E ); CvMat _F = cvMat(3, 3, CV_64F, F ); CvMat _Q = cvMat(4,4, CV_64F, Q); vector<CvPoint2D32f>& pts = points[0]; //Pontos XY pixels left points[0][0].x=34.0; points[0][0].y=336.0; points[0][1].x=502.0; points[0][1].y=156.0; points[0][2].x=1280.0; points[0][2].y=279.0; points[0][3].x=664.0; points[0][3].y=174.0; points[0][4].x=914.0; points[0][4].y=209.0; points[0][5].x=248.0; points[0][5].y=300.0; points[0][6].x=663.0; points[0][6].y=482.0; points[0][7].x=185.0; points[0][7].y=364.0; points[0][8].x=400.0; points[0][8].y=507.0; //Pontos XY pixels right points[1][0].x=866.0; points[1][0].y=942.0; points[1][1].x=98.0; points[1][1].y=376.0; points[1][2].x=856.0; points[1][2].y=72.0; points[1][3].x=445.0; points[1][3].y=222.0; points[1][4].x=690.0; points[1][4].y=128.0; points[1][5].x=779.0; points[1][5].y=442.0; points[1][6].x=1162.0; points[1][6].y=161.0; points[1][7].x=1061.0; points[1][7].y=413.0; points[1][8].x=1244.0; points[1][8].y=215.0; npoints.resize(nframes,n); objectPoints.resize(nframes*n); /* 3D points (x,y,z) related to the 2D points from left and right image - minimum: 8 points*/ objectPoints[0] = cvPoint3D32f(6.0,-4.5,0.0); objectPoints[1] = cvPoint3D32f(0.0,-4.5,0.0); objectPoints[2] = cvPoint3D32f(0.0,+4.5,0.0); objectPoints[3] = cvPoint3D32f(0.0,-1.5,0.0); objectPoints[4] = cvPoint3D32f(0.0,+1.5,0.0); objectPoints[5] = cvPoint3D32f(4.3,-2.7,0.0); objectPoints[6] = cvPoint3D32f(4.3,+2.7,0.0); objectPoints[7] = cvPoint3D32f(5.25,-1.75,0.0); objectPoints[8] = cvPoint3D32f(5.25,+1.75,0.0); for( int i = 1; i < nframes; i++ ) copy( objectPoints.begin(), objectPoints.begin() + n, objectPoints.begin() + i*n ); 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] ); /**************************************************************************/ double R1[3][3], R2[3][3], P1[3][4], P2[3][4]; CvMat _R1 = cvMat(3, 3, CV_64F, R1); CvMat _R2 = cvMat(3, 3, CV_64F, R2); CvMat _P1 = cvMat(3, 4, CV_64F, P1); CvMat _P2 = cvMat(3, 4, CV_64F, P2); /************************************** StereoCalibration - returns R T R1 R2 T1 T2 **************************************/ CvSize imageSize = cvSize(1294,964); cvStereoCalibrate( &_objectPoints, &_imagePoints1, &_imagePoints2, &_npoints, &_M1, &_D1, &_M2, &_D2, imageSize, &_R, &_T, &_E, &_F, cvTermCriteria(CV_TERMCRIT_ITER+ CV_TERMCRIT_EPS, 30, 1e-5), CV_CALIB_USE_INTRINSIC_GUESS+ CV_CALIB_FIX_ASPECT_RATIO); /*************************************************************************************************************************/ /***************************************** Extrinsic Parameters **********************************************************/ CvMat* Rvec_left = cvCreateMat( 3, 1, CV_64F ); Tvec_left = cvCreateMat( 3, 1, CV_64F ); CvMat* Rvec_right = cvCreateMat( 3, 1, CV_64F ); Tvec_right = cvCreateMat( 3, 1, CV_64F ); cvFindExtrinsicCameraParams2(&_objectPoints, &_imagePoints1,&_M1,&_D1,Rvec_left,Tvec_left); Rvec_left_n = cvCreateMat( 3, 3, CV_64F ); cvRodrigues2(Rvec_left,Rvec_left_n,0); cvFindExtrinsicCameraParams2(&_objectPoints, &_imagePoints2,&_M2,&_D2,Rvec_right,Tvec_right); Rvec_right_n = cvCreateMat( 3, 3, CV_64F ); cvRodrigues2(Rvec_right,Rvec_right_n,0); /*************************************************************************************************************************/ }
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; }