#include<opencv2/opencv.hpp>
#include<fstream>
void Calibration(const std::string &rootPath) {
std::ifstream fin(rootPath + "calibdata.txt");
std::ofstream fout(rootPath + "caliberation_result.txt");
std::cout << "start get feature from images………………\n";
int image_count = 0;
cv::Size image_size;
cv::Size board_size = cv::Size(9, 9);
std::vector<cv::Point2f> image_points_buf;
std::vector<std::vector<cv::Point2f>> image_points_seq;
std::string filename;
int count = -1;
while (std::getline(fin, filename)) {
cv::Mat imageInput = cv::imread(rootPath + filename);
if (imageInput.empty()) {
continue;
}
std::cout << "image_count = " << image_count++ << std::endl;
if (image_count == 1) {
image_size.width = imageInput.cols;
image_size.height = imageInput.rows;
std::cout << "image_size.width = " << image_size.width << std::endl;
std::cout << "image_size.height = " << image_size.height << std::endl;
}
if (0 == cv::findChessboardCorners(imageInput, board_size, image_points_buf)) {
std::cout << "can not find chessboard corners!\n";
continue;
}
else {
cv::Mat view_gray;
cv::cvtColor(imageInput, view_gray, cv::COLOR_RGB2GRAY);
cv::find4QuadCornerSubpix(view_gray, image_points_buf, cv::Size(5, 5));
cv::cornerSubPix(view_gray,
image_points_buf,
cv::Size(5, 5),
cv::Size(-1, -1),
cv::TermCriteria(cv::TermCriteria::Type::EPS + cv::TermCriteria::Type::MAX_ITER, 30, 0.1));
image_points_seq.push_back(image_points_buf);
std::cout << "-->count = " << ++count << std::endl;
}
}
int total = image_points_seq.size();
std::cout << "total = " << total << std::endl;
int CornerNum = board_size.width * board_size.height;
for (int ii = 0; ii < total; ii++) {
std::cout << " -->" << image_points_seq[ii][0].x;
std::cout << " -->" << image_points_seq[ii][0].y << std::endl;
}
std::cout << "end of feature extract!\n";
std::cout << "start calibration………………\n";
cv::Size square_size = cv::Size(10, 10);
std::vector<std::vector<cv::Point3f>> object_points;
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));
std::vector<int> point_counts;
cv::Mat distCoeffs = cv::Mat(1, 5, CV_32FC1, cv::Scalar::all(0));
std::vector<cv::Mat> tvecsMat;
std::vector<cv::Mat> rvecsMat;
int i, j, t;
for (t = 0; t < total; t++) {
std::vector<cv::Point3f> tempPointSet;
for (i = 0; i < board_size.height; i++) {
for (j = 0; j < board_size.width; j++) {
cv::Point3f realPoint;
realPoint.x = i * square_size.width;
realPoint.y = j * square_size.height;
realPoint.z = 0;
tempPointSet.push_back(realPoint);
}
}
object_points.push_back(tempPointSet);
}
for (i = 0; i < total; i++) {
point_counts.push_back(board_size.width * board_size.height);
}
cv::calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
std::cout << "end of calibration!\n";
std::cout << "start eval ………………\n";
double total_err = 0.0;
double err = 0.0;
std::vector<cv::Point2f> image_points2;
std::cout << "\t每幅图像的标定误差:\n";
fout << "每幅图像的标定误差:\n";
for (i = 0; i < total; i++) {
std::vector<cv::Point3f> tempPointSet = object_points[i];
/* 通过得到的摄像机内外参数,对空间的三维点进行重新投影计算,得到新的投影点 */
cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
/* 计算新的投影点和旧的投影点之间的误差*/
std::vector<cv::Point2f> tempImagePoint = image_points_seq[i];
err = 0.0;
for (int j = 0; j < tempImagePoint.size(); j++) {
double dx = image_points2[j].x - tempImagePoint[j].x;
double dy = image_points2[j].y - tempImagePoint[j].y;
err += std::sqrt(dx * dx + dy * dy);
}
total_err += err /= point_counts[i];
std::cout << "第" << i + 1 << "幅图像的平均误差:" << err << "像素" << std::endl;
fout << "第" << i + 1 << "幅图像的平均误差:" << err << "像素" << std::endl;
}
std::cout << "总体平均误差:" << total_err / total << "像素" << std::endl;
fout << "总体平均误差:" << total_err / total << "像素" << std::endl << std::endl;
std::cout << "评价完成!" << std::endl;
//保存定标结果
std::cout << "开始保存定标结果………………" << std::endl;
cv::Mat rotation_matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
fout << "相机内参数矩阵:" << std::endl;
fout << cameraMatrix << std::endl << std::endl;
fout << "畸变系数:\n";
fout << distCoeffs << std::endl << std::endl << std::endl;
for (int i = 0; i < total; i++) {
fout << "第" << i + 1 << "幅图像的旋转向量:" << std::endl;
fout << rvecsMat[i] << std::endl;
/* 将旋转向量转换为相对应的旋转矩阵 */
cv::Rodrigues(rvecsMat[i], rotation_matrix);
fout << "第" << i + 1 << "幅图像的旋转矩阵:" << std::endl;
fout << rotation_matrix << std::endl;
fout << "第" << i + 1 << "幅图像的平移向量:" << std::endl;
fout << tvecsMat[i] << std::endl << std::endl;
}
std::cout << "完成保存" << std::endl;
fout << std::endl;
}