opencv-calibration

#include
#include

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 image_points_buf;
    std::vector> 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> object_points;

    cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));
    std::vector point_counts;
    cv::Mat distCoeffs = cv::Mat(1, 5, CV_32FC1, cv::Scalar::all(0));
    std::vector tvecsMat;
    std::vector rvecsMat;
    int i, j, t;
    for (t = 0; t < total; t++) {
        std::vector 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 image_points2;
    std::cout << "\t每幅图像的标定误差：\n";
    fout << "每幅图像的标定误差：\n";
    for (i = 0; i < total; i++) {
        std::vector tempPointSet = object_points[i];
        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
        /* 计算新的投影点和旧的投影点之间的误差*/
        std::vector 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;
}