#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/legacy/legacy.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
#include <string>
#include <vector>
#include <time.h>
using namespace std;
// This function initializes the object points of chessboard corners.
// The coordinates are stored in a vector<vector<cv::Point3f>>.
// The object points can be used for calibrateCamera().
void chessboardObjectPoints(const cv::Size patternSize, // # of inner corners per a chessboard row and column
int nViews, // # of views (images) of chessboard
double squareSizeX, // width of square (x distance between corners)
double squareSizeY, // height of square (y distance between corners)
vector<vector<cv::Point3f>> & object_points) // object points of chessboard corners
{
int iView, iY, iX, nY, nX, ic;
nY = patternSize.height;
nX = patternSize.width;
// resize the object_points object
object_points.clear();
object_points.resize(nViews);
for (iView = 0; iView < nViews; iView++) {
object_points[iView].resize(nY*nX);
}
// fill the object points
for (iView = 0; iView < nViews; iView++) {
ic = 0;
for (iY = nY - 1; iY >= 0; iY--) {
for (iX = 0; iX < nX; iX++) {
object_points[iView][ic].x = (float)(iX * squareSizeX);
object_points[iView][ic].y = (float)(iY * squareSizeY);
object_points[iView][ic].z = (float) 0.0;
ic++;
}
}
}
}
int main() {
double t0;
// read the image file and put the image into my variable.
vector<string> calFilenames;
calFilenames.push_back("sample/20130120_222211.JPG");
calFilenames.push_back("sample/20130120_222233.JPG");
calFilenames.push_back("sample/20130120_222247.JPG");
calFilenames.push_back("sample/20130120_222254.JPG");
calFilenames.push_back("sample/20130120_222313.JPG");
calFilenames.push_back("sample/20130120_222320.JPG");
// open an OpenCV window
cv::namedWindow("Chessboard");
cv::Size patternSize( 10, 6 ); // patternSize – # of inner corners per a chessboard row and column
cv::Size myCalibImgSize;
vector<cv::Mat> cornersVec;
cornersVec.resize(calFilenames.size());
for (int i=0; i< (int) calFilenames.size(); i++) {
cout << "Reading cal image " << i << "...";
t0 = clock();
cv::Mat myCalibImg= cv::imread(calFilenames[i], CV_LOAD_IMAGE_GRAYSCALE );
cout << (clock()-t0)/CLOCKS_PER_SEC << " sec.\n";
myCalibImgSize.width = myCalibImg.cols;
myCalibImgSize.height = myCalibImg.rows;
// cout << "Size: " << myCalibImg.cols << "x" << myCalibImg.rows << endl;
// find chessboard corner
bool found;
cv::Mat corners(10, 6, CV_32FC2);
cout << "Finding cal image " << i << "...";
t0 = clock();
found = cv::findChessboardCorners( myCalibImg, patternSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH );
cout << (clock()-t0)/CLOCKS_PER_SEC << " sec.\n";
if ( found == true ) {
cout << "Finding subpixel of image " << i << "...";
t0 = clock();
cv::cornerSubPix( myCalibImg, corners, cv::Size(5,5), cv::Size(-1,-1),
cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01));
cout << (clock()-t0)/CLOCKS_PER_SEC << " sec.\n";
} else {
cout << "Can not find corners of image " << i << "\n";
}
// draw chessboard
drawChessboardCorners(myCalibImg, patternSize, corners, found);
cv::resize(myCalibImg, myCalibImg, cv::Size(800,600), 0, 0, cv::INTER_LINEAR );
// show image
cv::imshow("Chessboard", myCalibImg );
cv::waitKey( 100 );
// data copy
cornersVec[i] = corners;
}
// generate object points
vector<vector<cv::Point3f>> object_points;
chessboardObjectPoints( patternSize, calFilenames.size(), 23.8, 23.8,
object_points );
// calibration
cv::Mat cameraMatrix(3, 3, CV_64FC1);
cv::Mat distCoeffs (1, 8, CV_64FC1);
vector<vector<double>> rvecs, tvecs;
int flag = CV_CALIB_RATIONAL_MODEL ;
double reprojectionError =
cv::calibrateCamera(object_points, cornersVec, myCalibImgSize,
cameraMatrix,
distCoeffs,
rvecs, tvecs,
flag,
cv::TermCriteria( cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, DBL_EPSILON) );
cout << "Camera matrix:\n" << cameraMatrix << endl;
cout << "Image size: " << myCalibImgSize.width << " " <<
myCalibImgSize.height << endl;
cout << "Camera matrix:\n" << cameraMatrix << endl;
cout << "Dist. coeffic:\n" << distCoeffs << endl;
cout << "Fx: " << cameraMatrix.at<double>(0,0) << endl;
cout << "Fy: " << cameraMatrix.at<double>(1,1) << endl;
cout << "Cx: " << cameraMatrix.at<double>(0,2) << endl;
cout << "Cy: " << cameraMatrix.at<double>(1,2) << endl;
cout << "k1: " << distCoeffs .at<double>(0) << endl;
cout << "k2: " << distCoeffs .at<double>(1) << endl;
cout << "p1: " << distCoeffs .at<double>(2) << endl;
cout << "p2: " << distCoeffs .at<double>(3) << endl;
cout << "k3: " << distCoeffs .at<double>(4) << endl;
cout << "k4: " << distCoeffs .at<double>(5) << endl;
cout << "k5: " << distCoeffs .at<double>(6) << endl;
cout << "k6: " << distCoeffs .at<double>(7) << endl;
cout << "Reproj. error: " << reprojectionError << endl;
cout << "Finished.\n";
cv::waitKey( 0 );
return 0; // Or return 1;
}