【三维重建】摄像机标定（张正友相机标定法）_张正友标定如何求出得到最优的r和t

摄像机标定的目的是为了求解摄像机的内、外参数

求解投影矩阵M

通过建立特殊的场景，我们能过得到多对世界坐标和对应图像坐标

根据摄像机几何可知 ： ，M是一个3*4的矩阵，令

        

通过一对点可以得到两个方程组，M中一共有11个位置量，因此至少需要6对点，通过最小二乘法求解 可以得到。需要注意的是在求解 这个齐次方程组中，是方程组的解，对于任意 ，也是方程组的解，所以我们加了一个约束，使得，因此，我们求解出来的值和实际值的常数倍。

求解内参矩阵K

R是旋转矩阵，因此

同样因为R旋转矩阵，

内参矩阵全部求出。

求解外参矩阵R、T

求解畸变参数

所求得的是理想情况下的坐标，下面的建模相当于对理想点进行缩放，d表示坐标点离图像中心的距离，d越大畸变得越厉害。若   ，理想点会被往里扯，那对应的就是桶形畸变，若   ，理想点会被往外扯，那对应的就是枕形畸变。

由于Q中包含，因此这不是一个线性模型，只能用迭代法求取最优解

张正友标定法

接下来我们将介绍张正友标定法，以及相应代码。

GitHub - ldx-star/Zhangzhenyou-Calibration

在张正友标定法中，我们并不需要一个立体的模型，只需要一个平面的标定板。

Step1  获取世界坐标和像素坐标的对应点对

张正友标定法假设Z=0，将世界坐标系建立在靶面上，因此我们在提取世界坐标的时候只需要得到（X,Y）。

我们的棋盘格中每个方格的宽度是15mm，将第一个角点位置定为（0，0），第二个角点就是（0，0.15）... ，而像素坐标就是对应角点在图像上像素的坐标。

bool CamCalibration::getKeyPoints() {
    auto chessBoards = chessBoards_;
    const float square_size = square_size_;
    auto &points_3d_vec = points_3d_vec_;
    auto &points_2d_vec = points_2d_vec_;
 
    const int row = row_;
    const int col = col_;
    //采集世界坐标
    for (int i = 0; i < chessBoards.size(); i++) {
        std::vector<cv::Point2f> points_3d; // 一张图的世界坐标
        for (int r = 0; r < row; r++) {
            for (int c = 0; c < col; c++) {
                cv::Point2f point;
                point.x = r * square_size;
                point.y = c * square_size;
                points_3d.push_back(point);
            }
        }
        points_3d_vec.push_back(points_3d);
    }
    //采集像素坐标,使用opencv库提取角点
    for (auto img: chessBoards) {
        std::vector<cv::Point2f> points_2d;
 
        bool found_flag = cv::findChessboardCorners(img, cv::Size(col, row), points_2d, cv::CALIB_CB_ADAPTIVE_THRESH +
                                                                                        cv::CALIB_CB_NORMALIZE_IMAGE); //cv::Size(col,row)
        if (!found_flag) {
            std::cerr << "found chess board corner failed";
            return false;
        }
        //指定亚像素计算迭代标注
        cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 40, 0.001);
        cv::cornerSubPix(img, points_2d, cv::Size(5, 5), cv::Size(-1, -1), criteria);
 
        //display
//        cv::cvtColor(img,img,cv::COLOR_GRAY2BGR);
//        cv::drawChessboardCorners(img, cv::Size(col, row), points_2d, found_flag);
//        cv::namedWindow("corner img", cv::WINDOW_NORMAL);
//        cv::resizeWindow("corner img", img.cols / 2, img.rows / 2);
//        cv::imshow("corner img", img);
//        cv::waitKey(300);
 
        points_2d_vec.push_back(points_2d);
    }
    std::cout << "getKeyPoints succeed" << std::endl;
    return true;
}

Step2  计算单应性矩阵H

之前我们讲过世界坐标到像素坐标的映射关系 :

为了和论文对应，我们将字母都换成论文中的字母。A是之前讲的K，表示内参矩阵；s是之间讲的表示系数。

由于Z=0，

单应性矩阵

同样的

        

通过最小二乘法可以将H求出。

为了使计算稳定，需要先对数据进行Z-Score标准化

转换成矩阵的形式

我们需要将归一化矩阵保存下来，在求出H后将数据还原

/**
 * Z-Score 标准化（均值为0，方差为1）
 * @param points 原始数据点
 * @param normal_points 输出型参数，标准化后的数据点
 * @param normT 输出型参数，归一化矩阵的转置
 */
void CamCalibration::Normalize(const std::vector<cv::Point2f> &points, std::vector<cv::Point2f> &normal_points,
                               cv::Mat &normT) {
    //求均值
    float mean_x = 0;
    float mean_y = 0;
    for (const auto &point: points) {
        mean_x += point.x;
        mean_y += point.y;
    }
    mean_x /= points.size();
    mean_y /= points.size();
    //求方差
    for (const auto &point: points) {
        mean_x += point.x;
        mean_y += point.y;
    }
    float variance_x = 0;
    float variance_y = 0;
    for (const auto &point: points) {
        float tmp_x = pow(point.x - mean_x, 2);
        float tmp_y = pow(point.y - mean_y, 2);
        variance_x += tmp_x;
        variance_y += tmp_y;
    }
    variance_x = sqrt(variance_x);
    variance_y = sqrt(variance_y);
 
    for (const auto &point: points) {
        cv::Point2f p;
        p.x = (point.x - mean_x) / variance_x;
        p.y = (point.y - mean_y) / variance_y;
        normal_points.push_back(p);
    }
    normT.at<float>(0, 0) = 1 / variance_x;
    normT.at<float>(0, 2) = -mean_x / variance_x;
    normT.at<float>(1, 1) = 1 / variance_y;
    normT.at<float>(1, 2) = -mean_y / variance_y;
}
 

**
 * 计算单应性矩阵
 */
void CamCalibration::CalcH() {
    const auto &points_3d_vec = points_3d_vec_;
    const auto &points_2d_vec = points_2d_vec_;
    for (int i = 0; i < points_2d_vec.size(); i++) {
        //每一张图的世界坐标和像素坐标
        const auto &points_3d = points_3d_vec[i];
        const auto &points_2d = points_2d_vec[i];
        std::vector<cv::Point2f> normal_points_3d, normal_points_2d;
        cv::Mat normT_3d, normT_2d;
        Normalize(points_3d, normal_points_3d, normT_3d);
        Normalize(points_2d, normal_points_2d, normT_2d);
 
        cv::Mat H = cv::Mat::eye(3, 3, CV_32F);
 
        int corner_size = normal_points_2d.size();
        if (corner_size < 4) {
            std::cerr << "corner size < 4";
            exit(-1);
        }
 
        cv::Mat A(corner_size * 2, 9, CV_32F, cv::Scalar(0));
        for (int i = 0; i < corner_size; i++) {
            cv::Point2f point_3d = points_3d[i];
            cv::Point2f point_2d = points_2d[i];
            A.at<float>(i * 2, 0) = point_3d.x;
            A.at<float>(i * 2, 1) = point_3d.y;
            A.at<float>(i * 2, 2) = 1;
            A.at<float>(i * 2, 3) = 0;
            A.at<float>(i * 2, 4) = 0;
            A.at<float>(i * 2, 5) = 0;
            A.at<float>(i * 2, 6) = -point_2d.x * point_3d.x;
            A.at<float>(i * 2, 7) = -point_2d.x * point_3d.y;
            A.at<float>(i * 2, 8) = -point_2d.x;
 
            A.at<float>(i * 2 + 1, 0) = 0;
            A.at<float>(i * 2 + 1, 1) = 0;
            A.at<float>(i * 2 + 1, 2) = 0;
            A.at<float>(i * 2 + 1, 3) = point_3d.x;
            A.at<float>(i * 2 + 1, 4) = point_3d.y;
            A.at<float>(i * 2 + 1, 5) = 1;
            A.at<float>(i * 2 + 1, 6) = -point_2d.y * point_3d.x;
            A.at<float>(i * 2 + 1, 7) = -point_2d.y * point_3d.y;
            A.at<float>(i * 2 + 1, 8) = -point_2d.y;
        }
        cv::Mat U, W, VT;                                                    // A =UWV^T
        cv::SVD::compute(A, W, U, VT,
                         cv::SVD::MODIFY_A | cv::SVD::FULL_UV); // Eigen 返回的是V,列向量就是特征向量, opencv 返回的是VT，所以行向量是特征向量
        H = VT.row(8).reshape(0, 3);
        // H = inv_N2 * H * N3
        cv::Mat normT_2d_inv;
        cv::invert(normT_2d, normT_2d_inv);
        H = normT_2d_inv * H * normT_3d;
        H_vec_.push_back(H);
    }
}

Step3  计算内参矩阵A

已知

B是对称矩阵，所以只需存储上半矩阵

求出B就可以得到内参矩阵中的参数，需要注意的是，我们之前求的B是不带尺度因子的，真实的B与求出的B差一个系数

step4 计算外参矩阵R、T

由于存在噪声，我们求解的R不一定满足选装矩阵的特性

因此需要求解最优的旋转矩阵:

整个优化就可以看作最小化

令 ,当时

void CamCalibration::CalcRT() {
    const auto &K = K_;
    const auto &H_vec = H_vec_;
    auto &R_vec = R_vec_;
    auto &t_vec = t_vec_;
 
    cv::Mat K_inverse;
    cv::invert(K, K_inverse);
 
    for (const auto &H: H_vec) {
        cv::Mat M = K_inverse * H;
 
        cv::Vec3d r1(M.at<double>(0, 0), M.at<double>(1, 0), M.at<double>(2, 0));
        cv::Vec3d r2(M.at<double>(0, 1), M.at<double>(1, 1), M.at<double>(2, 1));
        cv::Vec3d r3 = r1.cross(r2);
        cv::Mat Q = cv::Mat::eye(3, 3, CV_64F);
 
        Q.at<double>(0, 0) = r1(0);
        Q.at<double>(1, 0) = r1(1);
        Q.at<double>(2, 0) = r1(2);
        Q.at<double>(0, 1) = r2(0);
        Q.at<double>(1, 1) = r2(1);
        Q.at<double>(2, 1) = r2(2);
        Q.at<double>(0, 2) = r3(0);
        Q.at<double>(1, 2) = r3(1);
        Q.at<double>(2, 2) = r3(2);
        cv::Mat normQ;
        cv::normalize(Q, normQ);
 
        cv::Mat U, W, VT;
        cv::SVD::compute(normQ, W, U, VT, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
        cv::Mat R = U * VT;
 
        R_vec.push_back(R);
        cv::Mat t = cv::Mat::eye(3, 1, CV_64F);
        M.col(2).copyTo(t.col(0));
        t_vec.push_back(t);
    }
}

step4 计算畸变参数

张正友标定法只考虑了切向畸变，畸变公式如下：

和 分别是理想情况的归一化图像坐标和畸变后的归一化图像坐标，r为图像像素点到图像中心点的距离，距离越大畸变越大。

像平面一般接近90度，所以为0，因此

通过最小二乘法可求得k.

void CamCalibration::CalDistCoeff() {
    std::vector<double> r2_vec;
    std::vector<cv::Point2f> ideal_point_vec;
 
    //用一副图进行计算
    const cv::Mat &K = K_;
    const cv::Mat &R = R_vec_[0];
    const cv::Mat &t = t_vec_[0];
    auto points_3d = points_3d_vec_[0];
    auto &dist_coeff = dist_coeff_;
    for (const auto &point_3d: points_3d) {
        cv::Mat p_3d = (cv::Mat_<double>(3, 1) << point_3d.x, point_3d.y, 0);
        //世界坐标转相机坐标
        cv::Mat p_cam = R * p_3d + t;
        //转换成欧式坐标
        p_cam.at<double>(0, 0) = p_cam.at<double>(0, 0) / p_cam.at<double>(2, 0);
        p_cam.at<double>(1, 0) = p_cam.at<double>(1, 0) / p_cam.at<double>(2, 0);
        p_cam.at<double>(2, 0) = 1;
        double x = p_cam.at<double>(0, 0);
        double y = p_cam.at<double>(1, 0);
        double r2 = x * x + y * y;
        r2_vec.push_back(r2);
 
        //相机坐标转像素坐标
        cv::Mat p_pix = K * p_cam;
        ideal_point_vec.emplace_back(p_pix.at<double>(0, 0), p_pix.at<double>(1, 0));
    }
    const std::vector<cv::Point2f> &dist_point_vec = points_2d_vec_[0];
    double u0 = K.at<double>(0, 2);
    double v0 = K.at<double>(1, 2);
 
    cv::Mat D = cv::Mat::eye(ideal_point_vec.size() * 2, 2, CV_64F);
    cv::Mat d = cv::Mat::eye(ideal_point_vec.size() * 2, 1, CV_64F);
    for (int i = 0; i < ideal_point_vec.size(); i++) {
        double r2 = r2_vec[i];
        const auto &ideal_p = ideal_point_vec[i];
        const auto &dist_p = dist_point_vec[i];
        D.at<double>(i * 2, 0) = (ideal_p.x - u0) * r2;
        D.at<double>(i * 2, 1) = (ideal_p.x - u0) * r2 * r2;
        D.at<double>(i * 2 + 1, 0) = (ideal_p.y - v0) * r2;
        D.at<double>(i * 2 + 1, 1) = (ideal_p.y - v0) * r2 * r2;
        d.at<double>(2 * i, 0) = dist_p.x - ideal_p.x;
        d.at<double>(2 * i + 1, 0) = dist_p.y - ideal_p.y;
 
    }
    cv::Mat DT;
    cv::transpose(D, DT);
//    std::cout << "D:" << D << std::endl;
 
    cv::Mat DTD_inverse;
    cv::invert(DT * D, DTD_inverse);
//    std::cout << "DTD_inv:" << DTD_inverse << std::endl;
 
    dist_coeff = DTD_inverse * DT * d;
    std::cout << "distort coeff: " << dist_coeff.at<double>(0, 0) << ", " << dist_coeff.at<double>(1, 0) << std::endl;
}

自此张正友相机标定完成，当然，我们所求出来的参数都是独立的，是理想情况下的值，因此需要使用最大似然估计进行优化，这部分大家自行了解。

参考内容：

计算机视觉之三维重建（深入浅出SfM与SLAM核心算法）——2.摄像机标定_哔哩哔哩_bilibili

张正友标定论文的解读和C++代码编写-CSDN博客

A flexible new technique for camera calibration | IEEE Journals & Magazine | IEEE Xplore