bool CameraCalibration::runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
vector<float>& reprojErrs, double& totalAvgErr )
{
cameraMatrix = Mat::eye( 3, 3, CV_64F );
if ( s.flag & CALIB_FIX_ASPECT_RATIO ) {
cameraMatrix.at<double>( 0, 0 ) = 1.0;
}
distCoeffs = Mat::zeros( 8, 1, CV_64F );
vector<vector<Point3f> > objectPoints( 1 );
calcBoardCornerPositions( s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern );
objectPoints.resize( imagePoints.size(), objectPoints[0] );
// Find intrinsic and extrinsic camera parameters
double rms = calibrateCamera( objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, s.flag | CALIB_FIX_K4 | CALIB_FIX_K5 );
cout << "Re-projection error reported by calibrateCamera: " << rms << endl;
bool ok = checkRange( cameraMatrix ) && checkRange( distCoeffs );
totalAvgErr = computeReprojectionErrors( objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs );
return( ok );
}
void QCvCameraCalibration::calibrate()
{
this->setEnable(false);
QString msg;
if (imagePoints.size() >= s.minNumSamples)
{
// compute board geometry
vector<vector<cv::Point3f> > objectPoints(1);
calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0]);
objectPoints.resize(imagePoints.size(),objectPoints[0]);
rmsError = calibrateCamera(objectPoints, imagePoints, imgSize, cameraMatrix,
distCoeffs, rvecs, tvecs, CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
msg = "updated object points successfully, RMS = "+QString::number(rmsError);
s.bCALIBRATED=true;
saveCameraParams();
}
else
{
msg = "Insufficient number of samples taken!";
}
emit statusUpdate(msg);
this->setEnable(true);
}
void ImgLoader::calibCamera()
{
const cv::Size board_size(6,9);
const float square_size = 0.025;
const cv::TermCriteria term_criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON);
std::vector<std::vector<cv::Point2f> > rgbImgPoints;
std::vector<std::vector<cv::Point2f> > irImgPoints;
std::vector<cv::Mat> rvec,tvec;
if(rgbImgPath_.back()!='/')
rgbImgPath_+=std::string("/");
if(irImgPath_.back()!='/')
irImgPath_+=std::string("/");
unsigned int img_num = rgbImgFiles_.size();
for(unsigned int i=0;i<img_num;i++)
std::cout<<"Files are "<<rgbImgFiles_.at(i)<<std::endl;
for(unsigned int i=0; i<img_num; i++)
{
std::string rgb_name = rgbImgPath_ + rgbImgFiles_.at(i);
std::string ir_name = irImgPath_ + irImgFiles_.at(i);
rgbImg = cv::imread(rgb_name,0);
irImg = cv::imread(ir_name,0);
std::vector<cv::Point2f > camera1ImagePoints;
bool found1 = cv::findChessboardCorners(rgbImg, board_size, camera1ImagePoints, cv::CALIB_CB_FAST_CHECK);
std::vector<cv::Point2f> camera2ImagePoints;
bool found2 = cv::findChessboardCorners(irImg, board_size, camera2ImagePoints, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
if(found1){
cv::cornerSubPix(rgbImg, camera1ImagePoints, cv::Size(11, 11), cv::Size(-1, -1), term_criteria);
rgbImgPoints.push_back(camera1ImagePoints);
}
if(found2){
cv::cornerSubPix(irImg, camera2ImagePoints, cv::Size(11, 11), cv::Size(-1, -1), term_criteria);
irImgPoints.push_back(camera2ImagePoints);
}
}
calibsize_depth_ = irImg.size();
calibsize_rgb_ = rgbImg.size();
std::vector<std::vector<cv::Point3f> > pointsBoard(1);
calcBoardCornerPositions(board_size, square_size, pointsBoard[0]);
pointsBoard.resize(img_num,pointsBoard[0]);
double error_1 = cv::calibrateCamera(pointsBoard, rgbImgPoints, calibsize_rgb_,
rgb_camera_matrix_, rgb_distortion_, rvec, tvec);
double error_2 = cv::calibrateCamera(pointsBoard, irImgPoints, calibsize_depth_,
depth_camera_matrix_, depth_distortion_, rvec, tvec);
double rms = cv::stereoCalibrate(pointsBoard, rgbImgPoints, irImgPoints,
rgb_camera_matrix_, rgb_distortion_,
depth_camera_matrix_, depth_distortion_,
calibsize_rgb_, rotation_, translation_, essential_, fundamental_,
term_criteria,
cv::CALIB_FIX_INTRINSIC
);
std::cout<<"Camera calibration done"<<std::endl;
}
void stereo_vision::calibrating_get_corners(cv::Mat left,cv::Mat right)
{
cv::cvtColor(left,left_gray,CV_RGB2GRAY);
cv::cvtColor(right,right_gray,CV_RGB2GRAY);
left_found= cv::findChessboardCorners(left_gray, settings.boardSize, pointBuf_left, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
right_found= cv::findChessboardCorners(right_gray, settings.boardSize, pointBuf_right, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
cv::drawChessboardCorners(left,settings.boardSize, pointBuf_left, left_found);
cv::drawChessboardCorners(right,settings.boardSize, pointBuf_right, right_found);
if((left_found)&&(right_found))
{
qDebug("Both found");
cv::cornerSubPix(left_gray, pointBuf_left, cv::Size(11,11), cv::Size(-1,-1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
cv::cornerSubPix(right_gray, pointBuf_right, cv::Size(11,11), cv::Size(-1,-1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
// std::vector<std::vector<cv::Point3f> > imagePoints1(frames);
imagePoints1.push_back(pointBuf_left);
imagePoints2.push_back(pointBuf_right);
frame_n++;
emit calibration_progress((frame_n*100/frames));
if(frame_n >= frames)
{
qDebug("Finished getting images, now starting calculations");
emit(calibration_running(0));
objectPoints.resize(frames);
calcBoardCornerPositions(settings.boardSize, settings.squareSize, objectPoints);
imageSize = left.size();
qDebug("%d %d", imageSize.height, imageSize.width);
cameraMatrix_left = cv::Mat::eye(3, 3, CV_64F);
cameraMatrix_right = cv::Mat::eye(3, 3, CV_64F);
imagePoints1.resize(frames);
imagePoints2.resize(frames);
double rms = stereoCalibrate(objectPoints,imagePoints1,imagePoints2, cameraMatrix_left, distCoeffs_left,
cameraMatrix_right, distCoeffs_right, imageSize, R, T, E, F, cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),CV_CALIB_FIX_ASPECT_RATIO +
CV_CALIB_ZERO_TANGENT_DIST +
CV_CALIB_SAME_FOCAL_LENGTH +
CV_CALIB_RATIONAL_MODEL +
CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
qDebug("Kalibravimo rezultatas %f",rms);
cv::stereoRectify(cameraMatrix_left, distCoeffs_left, cameraMatrix_right, distCoeffs_right, imageSize, R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY, 0, imageSize, &roi_left, &roi_right );
bm.state->roi1 = roi_left;
bm.state->roi2 = roi_right;
cv::initUndistortRectifyMap(cameraMatrix_left, distCoeffs_left, R1, P1, imageSize, CV_16SC2, rmap_left[0], rmap_left[1]);
cv::initUndistortRectifyMap(cameraMatrix_right, distCoeffs_right, R2, P2, imageSize, CV_16SC2, rmap_right[0], rmap_right[1]);
/*
double err = 0;
int npoints = 0;
std::vector<cv::Vec3f> lines1;
std::vector<cv::Vec3f> lines2;
for( int i = 0; i < frames; i++ )
{
int npt = (int)imagePoints1[i].size();
cv::Mat imgpt1,imgpt2;
imgpt1 = cv::Mat(imagePoints1[i]);
cv::undistortPoints(imgpt1, imgpt1, cameraMatrix_left, distCoeffs_left, cv::Mat(), cameraMatrix_left);
cv::computeCorrespondEpilines(imgpt1, 1, F, lines1);
imgpt2 = cv::Mat(imagePoint[i]);
cv::undistortPoints(imgpt2, imgpt2, cameraMatrix_right, distCoeffs_right, cv::Mat(), cameraMatrix_right);
cv::computeCorrespondEpilines(imgpt2, 2, F, lines2);
for( int j = 0; j < npt; j++ )
{
double errij = fabs(imagePoints1[i][j].x*lines2[j][0] +
imagePoints1[i][j].y*lines2[j][1] + lines2[j][2]) +
fabs(imagePoints2[i][j].x*lines1[j][0] +
imagePoints2[i][j].y*lines1[j][1] + lines1[j][2]);
err += errij;
}
npoints += npt;
}
qDebug("Reprojection error: %d", err/npoints); << "average reprojection err = " << err/npoints << endl;
}
*/
mode = CALIBRATED;
}
}
}