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;
}
