单目标定分析+外参数求解+重投影误差验证

可以验证 RANSAC + PNP + BA 对单目标定中重投影误差的计算和  opencv 中 calibrateCamera输出的误差一模一样！！！，当然你也可以用Projection直接计算重投影误差。

看代码，此代码改编自opencv安装目录下 “calibration.cpp”，这里我以三种方式打印每张棋盘的重投影误差：

1、calibrateCamera 输出的每张图的重投影误差(cv::Mat perViewErrors 中每一行代表一张棋盘的重投影误差)

2、借助 calibrateCamera 输出的棋盘外参数 rvecs, tvecs 和 projectPoints()计算重投影点，接着计算每张棋盘重投影误差

3、借助 solvePnPRansac + solvePnPRefineLM 求得 rvecs, tvecs，接着..同2

      你看看输出，我测了三组棋盘(SB VS2019让第二组图挂了，我这里暂时上传两组图，请放大看，或者下载看)

 

 

 

      其实在实验过程中，  solvePnPRansac的作用在于梯度下降过程中取一个好的起点。如果我把solvePnPRansac 参数误差阈值 float reprojectionError 设置得非常小，我们从 inliers 可知内点比例不是100%，甚至不到50%。这时候，起点选的不够好，容易陷入极小值。ransanc的作用在于选择最优起点，所以我们应该把迭代次数设置得越高越好，把门槛误差设置得越松(大)越好。而在梯度下降过程中，迭代次数也要要高，至于门槛误差，，那当然是要无限小最好。

#define _CRT_SECURE_NO_WARNINGS
#include <iostream>     // cout
#include<fstream>
#include <algorithm>    // std::sort
using namespace std;
using std::string;
using std::cout;
using std::cerr;
using std::endl;

#include <opencv2/opencv.hpp>
using namespace cv;

void readFileNames(const string& fileList, vector<string>& fileNames)
{
    fstream txt;
    txt.open(fileList, ios::in);
    while (!txt.eof())
    {
        string s;
        getline(txt, s);
        if (!s.empty())
        {
            fileNames.push_back(s);
        }
    }
    txt.close();
    return;
}

// camera -> pixel
// (X Y Z)-> (u v) 
void CameraToPixel(const Point3d& pt3d, const Mat& M, Point2d& pt2d)
{
    double fx = M.at<double>(0, 0);
    double fy = M.at<double>(1, 1);
    double cx = M.at<double>(0, 2);
    double cy = M.at<double>(1, 2);
    pt2d.x = fx * pt3d.x / pt3d.z + cx;
    pt2d.y = fy * pt3d.y / pt3d.z + cy;
}

int main(int argc, const char* argv[])
{
    // 第一组棋盘
    //string folder = "calibration1\\";//
    //string fileList = "filelist_1.txt";

    //float image_sf = 1.f;    // 后面要resize
    //int delay = 250;          // 延时，方便观察
    //int board_w = 12;
    //int board_h = 12;

    // 第二组棋盘
    string folder = "calibration2\\";//
    string fileList = "filelist_2.txt";

    float image_sf = 1.f;    // 后面要resize
    int delay = 250;          // 延时，方便观察
    int board_w = 7;
    int board_h = 5;

    // 第三组棋盘
    //string folder = "calibration3\\";//
    //string fileList = "filelist_3.txt";

    //float image_sf = 1.0;    // 后面要resize
    //int delay = 10;          // 延时，方便观察
    //int board_w = 9;
    //int board_h = 6;

    vector<string> fileNames;
    readFileNames(fileList, fileNames);
    int num_files = (int)fileNames.size();
    cout << "   ... Done. Number of files = " << num_files << "\n" << endl;
    int n_boards = num_files;

    int board_n = board_w * board_h;  // 一张图棋盘点数量
    cv::Size board_sz = cv::Size(board_w, board_h);  //棋盘的是12X12

    // 提取样本点
    vector<vector<cv::Point2f> > image_points;
    vector<vector<cv::Point3f> > object_points;

    // <1>、检测2D角点、指定3D棋盘点
    cv::Size image_size;
    int board_count = 0;
    for (size_t i = 0; (i < fileNames.size()) && (board_count < n_boards); ++i) {
        cv::Mat image, image0 = cv::imread(folder + fileNames[i]);
        board_count += 1;
        if (!image0.data)
        {
            cerr << folder + fileNames[i] << ", file #" << i << ", is not an image" << endl;
            continue;
        }
        image_size = image0.size();
        cv::resize(image0, image, cv::Size(), image_sf, image_sf, cv::INTER_LINEAR);//这里是在干鸡毛？？？

        // 在图上找到棋盘

        vector<cv::Point2f> corners;
        bool found = cv::findChessboardCorners(image, board_sz, corners);

        // 画出来
        drawChessboardCorners(image, board_sz, corners, found);  // 当然是找到了才能画出来

        //  如果找到了，就可以提取2D焦点了
        if (found) {
            image ^= cv::Scalar::all(255); // 异或运算
            cv::Mat mcorners(corners);

            // do not copy the data
            mcorners *= (1.0 / image_sf); // 角点坐标也要缩放

            // 缩放角点坐标
            image_points.push_back(corners);
            object_points.push_back(vector<cv::Point3f>());// malloc?
            vector<cv::Point3f>& opts = object_points.back();

            opts.resize(board_n);
            for (int j = 0; j < board_n; j++) {
                opts[j] = cv::Point3f(static_cast<float>(j / board_w),static_cast<float>(j % board_w), 0.0f);
            }
            cout << "Collected " << static_cast<int>(image_points.size())
                << "total boards. This one from chessboard image #"
                << i << ", " << folder + fileNames[i] << endl;
        }
        cv::namedWindow("Calibration", cv::WINDOW_NORMAL);
        cv::imshow("Calibration", image);

        // show in color if we did collect the image
        if ((cv::waitKey(delay) & 255) == 27) {
            return -1;
        }
    }

    cv::destroyWindow("Calibration");
    cout << "\n\n*** CALIBRATING THE CAMERA...\n" << endl;

    // <2>、调用算法开始标定
    cv::Mat intrinsic_matrix, distortion_coeffs;
    std::vector<cv::Mat> rvecs;
    std::vector<cv::Mat> tvecs;
    cv::Mat perViewErrors;// 每一张棋盘的重投影误差
    double err = cv::calibrateCamera(   object_points, image_points, image_size, intrinsic_matrix, distortion_coeffs,
                                        rvecs, tvecs, cv::noArray(), cv::noArray(), perViewErrors/*,  // 我劝你不会用这些参数，就不要设置了
                                        cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_PRINCIPAL_POINT*/);
    
    double fx = intrinsic_matrix.at<double>(0, 0);
    double fy = intrinsic_matrix.at<double>(1, 1);
    double cx = intrinsic_matrix.at<double>(0, 2);
    double cy = intrinsic_matrix.at<double>(1, 2);

    // <2.1> 验证 perViewErrors
    cout << "err = " << endl;
    cout << err << endl;
    cout << "sum(perViewErrors) / rows =" << endl;
    double s1 = 0.;
    for (size_t i = 0; i < perViewErrors.rows; i++)
    {
        s1 += std::pow(perViewErrors.at<double>((int)i, 0), 2);
    }
    cout << std::sqrt(s1 / perViewErrors.rows) << endl;
    cout << "---------------" << endl;

    // <2.2> 验证重投影误差
    cout << "每张棋盘重投影误差【calibrateCamera输出】：" << endl;
    cout << perViewErrors.t() << endl;

    // 遍历所有棋盘(利用张正友棋盘法估算的外参)
    cout << "每张棋盘重投影误差【利用外参计算】：" << endl;
    for (size_t k = 0; k < object_points.size(); k++)
    {
        vector<Point2f> pts2d1; // [u' v'] 重投影后的点
        double perViewError1 = 0.;
        projectPoints(Mat(object_points[k]), rvecs[k], tvecs[k], intrinsic_matrix, distortion_coeffs, pts2d1);
        perViewError1 = norm(Mat(image_points[k]), Mat(pts2d1), NORM_L2);
        cout << std::sqrt(perViewError1 * perViewError1/ (int)pts2d1.size()) << " ";
       // // 外参
       // Mat rvec = rvecs[k];
       // Mat t = tvecs[k];
       // Mat R;
       // Rodrigues(rvec, R);
       // // T = (R|t)
       // cv::Affine3d T(
       //     cv::Affine3d::Mat3( R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
       //                         R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
       //                         R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2)),
       //     cv::Affine3d::Vec3(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0)));

       ///* cout << T.rotation() << endl;
       // cout << T.translation() << endl;*/

       // // 遍历一张棋盘中所有点
       // for (size_t i = 0; i < object_points[k].size(); i++)
       // {
       //     Point3d ptR3d = (Point3d)object_points[k][i]; 
       //     Point2d ptL2d = (Point2d) image_points[k][i];

       //     // <1> 仿射
       //     Point3d ptL3d = T * ptR3d;

       //     // <2> 投影 + 畸变矫正
       //     Point2d ptL2d_;
       //     CameraToPixel(ptL3d, intrinsic_matrix, ptL2d_);
       //     undistortPoints();

       //     
       // }
       //cv::projectPoints
    }

    // <2.3> 外参数验证
    double sum_errs_t = 0.;
    Mat rvec1, t1, inliers;
    // 遍历所有棋盘
    cout << endl;
    cout << "每张棋盘重投影误差【非线性优化】：" << endl;
    for (size_t k = 0; k < object_points.size(); k++)
    {
        // 标定的外参数t2
        Mat t2 = tvecs[k];
        //cout << "t2 = " << t2.t() << " ";

        // pnp 
        // 注意调参数，如果误差阈值卡得太严格,尽量大，反正迭代会让误差收敛，可能需要在GUI做一个手动设置
        // 当你误差设得很大的时候，此时，你需要将迭代次数射得大一些，这样才能确保得到一个好的姿态
        // 置信度？
        solvePnPRansac(object_points[k], image_points[k], intrinsic_matrix, distortion_coeffs, rvec1, t1, false, 300, 30.0, 0.99, inliers); // 为什么内点为空? 因为阈值太低
       /* if (inliers.rows < ((int)object_points[k].size() / 3))
        {
            cout << " 内点比例:"<< (float)inliers.rows / (int)object_points[k].size() << endl;
            return -1;
        }*/
        // TermCriteria::EPS = FLT_EPSILON，这个精度不可能达到，所以还是迭代300次
        // TermCriteria::COUNT = 300
        // + 表示或的关系不是与
        solvePnPRefineLM(object_points[k], image_points[k], intrinsic_matrix, distortion_coeffs, rvec1, t1, TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 300, FLT_EPSILON));
        //cout << "t1 = " << t1.t() << " ";

        sum_errs_t += norm(t1, t2, NORM_L2);
        //cout << norm(t1, t2, NORM_L2) << endl;// 梯度下降后的外参数t1 与 张正友外参数差距
        
        // 2D点重投影误差(利用solvePnPRefineLM 求解出的外参数)
        vector<Point2f> pts2d2; // [u' v'] 重投影后的点
        double perViewError2 = 0.;
        projectPoints(Mat(object_points[k]), rvec1, t1, intrinsic_matrix, distortion_coeffs, pts2d2);
        perViewError2 = norm(Mat(image_points[k]), Mat(pts2d2), NORM_L2);
        cout << std::sqrt(perViewError2 * perViewError2 / (int)pts2d2.size()) << " ";
    }
    cout << endl;
    cout << "内点数量 =" << inliers.rows << endl;
    cout << sum_errs_t / (int)object_points.size() << endl;


    // undistortPoints
    /*cvProjectPoints2*/
    return 0;
}