OpenCV笔记（10） 相机模型与标定

void Rodrigues( InputArray src, //输入旋转向量or矩阵
　　　　　　　　　　OutputArray dst, //输出旋转矩阵or向量
　　　　　　　　　　OutputArray jacobian = noArray() );

#include <opencv.hpp>
using namespace cv;
using namespace std;
int main() {
    Mat r = (cv::Mat_<float>(3, 1) <<
        -2.100418, -2.167796, 0.273330);
    Mat R;
    Rodrigues(r, R, noArray());
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            printf("%f ", R.at<float>(i,j));
        }
        printf("\n");
    }
    return 0;
}

-0.036254 0.978364 -0.203692
0.998304 0.026168 -0.051995
-0.045539 -0.205232 -0.977653

bool findChessboardCorners( InputArray image,  //必须是8bit图像
　　　　　　　　　　　　　　　　　　Size patternSize, //棋盘图，8UC1 OR 8UC3
　　　　　　　　　　　　　　　　　　OutputArray corners, //棋盘每行每列有多少角点 Size(cols,rows)
                              int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );
　　　　　　　　　　　　　　　　　　//用于实现一个或多个附加滤波步骤，以帮助找到棋盘上的角点

void drawChessboardCorners( InputOutputArray image, //由于角点是用有颜色的圆圈表示的，因此必须为8位的彩色图像。
　　　　　　　　　　　　　　　　　　Size patternSize,
                              InputArray corners, 
　　　　　　　　　　　　　　　　　　bool patternWasFound );//是否整个棋盘图案上的角点都被成功找到

double calibrateCamera( InputArrayOfArrays objectPoints,//在x和y维中是整数，在z维中为零
　　　　　　　　　　　　    InputArrayOfArrays imagePoints, //图像中每个点的位置
　　　　           　　   Size imageSize,//图像的大小（以像素为单位）
                        InputOutputArray cameraMatrix, //包含线性内在参数， 应为3x3矩阵
　　　　　　　　　　　　    InputOutputArray distCoeffs,//畸变参数，可以是4,5或8个元素
                        OutputArrayOfArrays rvecs, //旋转矩阵（以Rodrigues形式表示)
　　　　　　　     　　    OutputArrayOfArrays tvecs,//平移矩阵
                        int flags = 0, 
　　　　　　　            TermCriteria criteria = TermCriteria(//终止标准
                        TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );

bool solvePnP( InputArray objectPoints, InputArray imagePoints,
               InputArray cameraMatrix, InputArray distCoeffs,
               OutputArray rvec, OutputArray tvec,
               bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );

bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
                     InputArray cameraMatrix, InputArray distCoeffs,
                     OutputArray rvec, OutputArray tvec,
                     bool useExtrinsicGuess = false, int iterationsCount = 100,
                     float reprojectionError = 8.0, double confidence = 0.99,
                     OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );

void initUndistortRectifyMap(InputArray cameraMatrix, InputArray distCoeffs,
                             InputArray R, //补偿相机相对千相机所处的全局坐标系的旋转
　　　　　　　　　　　　　　　　  InputArray newCameraMatrix,// 对于单目图像，设为noArray()
                             Size size, //输出映射的尺寸
　　　　　　　　　　　　　　　　  int m1type, //最终的映射类型，可能值为CV_32FC1或CV_16SC2
　　　　　　　　　　　　　　　　  OutputArray map1, OutputArray map2);

void remap( InputArray src, OutputArray dst,
            InputArray map1, InputArray map2,//initUndistortRectifyMap()计算得到的映射参数
            int interpolation, int borderMode = BORDER_CONSTANT,
            const Scalar& borderValue = Scalar());

void undistort( InputArray src, OutputArray dst,
                InputArray cameraMatrix,
                InputArray distCoeffs,
                InputArray newCameraMatrix = noArray() );

#include<iostream>
#include<opencv2\opencv.hpp>

using namespace std;
using namespace cv;

int main(int argc, char* argv[]) {

    int n_boards = 10;
    float image_sf = 0.5f;
    float delay = 1.f;
    int board_w = 3;
    int board_h = 3;
    int board_n = board_w * board_h;
    Size board_sz = Size(board_w, board_h);

    //开启摄像头
    VideoCapture capture(0);
    if (!capture.isOpened()) {
        cout << "\nCouldn't open the camera\n";
        return -1;
    }

    //分配存储空间
    vector<vector<Point2f>> image_points;
    vector<vector<Point3f>> object_points;

    double last_captured_timestamp = 0;
    Size image_size;

    //不断取图，直到取够n_boards张
    while (image_points.size() < (size_t)n_boards) {
        Mat image0, image;
        capture >> image0;
        image_size = image0.size();
        resize(image0, image, Size(), image_sf, INTER_LINEAR);

        //Find the board
        vector<Point2f> corners;
        bool found = findChessboardCorners(image, board_sz, corners);

        //Draw it 
        drawChessboardCorners(image, board_sz, corners, found);

        double timestamp = (double)clock() / CLOCKS_PER_SEC;

        if (found && timestamp - last_captured_timestamp > 1) {
            last_captured_timestamp = timestamp;
            image ^= Scalar::all(255);
            Mat mcorners(corners);
            mcorners *= (1. / image_sf);
            image_points.push_back(corners);
            object_points.push_back(vector<Point3f>());
            vector<Point3f>& opts = object_points.back();
            opts.resize(board_n);
            for (int j = 0; j < board_n; j++) {
                opts[j] = Point3f((float)(j / board_w), (float)(j % board_w), 0.f);
            }
            cout << "Collected our " << (int)image_points.size() << "of" << n_boards << "needed chessboard images\n" << endl;
        }
        imshow("calibration", image);
        if ((waitKey(30) & 255) == 27)
            return -1;
    }
    destroyWindow("calibration");
    cout << "\n\n*** CALIBRATIING THE CAMERA... \n" << endl;

    //Calibrate the camera
    Mat intrinsic_matrix, distortion_coeffs;
    double err = calibrateCamera(
        object_points,
        image_points,
        image_size,
        intrinsic_matrix,
        distortion_coeffs,
        noArray(), noArray(),
        CALIB_ZERO_TANGENT_DIST | CALIB_FIX_PRINCIPAL_POINT);

    //保存相机内参和畸变
    cout << "***DONE!\n\n Reprojection error is" << err << "\nStoring Intrinsics.xml and Distortions.xml files\n\n";
    FileStorage fs("intrinsics.xml", FileStorage::WRITE);
    fs << "image_width" << image_size.width << "image_height" << image_size.height
        << "camera_matrix" << intrinsic_matrix << "distortion_coeffs" << distortion_coeffs;
    fs.release();

    //加载这些参数
    fs.open("intrinsics.xml", FileStorage::READ);
    cout << "\nimage width:" << (int)fs["image_width"];
    cout << "\nimage height:" << (int)fs["image_height"];

    Mat intrinsic_matrix_loaded, distortion_coeffs_loaded;
    fs["camera_matrix"] >> intrinsic_matrix_loaded;
    fs["distortion_coeffs"] >> distortion_coeffs_loaded;
    cout << "\nintrinsic matrix:" << intrinsic_matrix_loaded;
    cout << "\ndistortion coefficients:" << distortion_coeffs_loaded << endl;

    //矫正映射
    Mat map1, map2;
    initUndistortRectifyMap(
        intrinsic_matrix_loaded,
        distortion_coeffs_loaded,
        Mat(),
        intrinsic_matrix_loaded,
        image_size,
        CV_16SC2,
        map1,
        map2
    );

    //传入图像,显示的是没有畸变的图像
    for (;;) {
        Mat image, image0;
        capture >> image0;
        if (image0.empty()) break;
        remap(
            image0,
            image,
            map1,
            map2,
            INTER_LINEAR,
            BORDER_CONSTANT, 
            Scalar()
        );
        imshow("Undistorted", image);
        if ((waitKey(30) & 255) == 27) break;
    }
    return 0;
}

<?xml version="1.0"?>

-<opencv_storage>

<image_width>640</image_width>

<image_height>480</image_height>


-<camera_matrix type_id="opencv-matrix">

<rows>3</rows>

<cols>3</cols>

<dt>d</dt>

<data>5.7907667726308171e+02 0. 3.1950000000000000e+02 0.1.1801417596095703e+03 2.3950000000000000e+02 0. 0. 1.</data>

</camera_matrix>


-<distortion_coeffs type_id="opencv-matrix">

<rows>1</rows>

<cols>5</cols>

<dt>d</dt>

<data>1.3156136239488735e-02 -2.0824275792988209e-01 0. 0. -3.1422421138402745e-01</data>

</distortion_coeffs>

</opencv_storage>

[0, 0, 0;
 0, 1, 0;
 0, 2, 0;
 0, 3, 0;
 1, 0, 0;
 1, 1, 0;
 1, 2, 0;
 1, 3, 0;
 2, 0, 0;
 2, 1, 0;
 2, 2, 0;
 2, 3, 0;
 3, 0, 0;
 3, 1, 0;
 3, 2, 0;
 3, 3, 0]