如何基于四个点校准相机焦距、平移与旋转参数？

阿华AIGC实验室

2026-5-29

求解相机焦距、位置与姿态的方法（OpenCV + 手动实现）

首先，咱们先理清你的问题核心：你有单张图像的平面标定板点对（世界坐标是Z=0的矩形，图像坐标已归一化且去畸变），需要求解相机的内参（fx、fy）和外参（旋转矩阵R、平移向量t）。你之前尝试的几个OpenCV函数不适用的原因我先解释清楚，再给你可行的解决方案。

为什么你之前的OpenCV尝试失败？

getPerspectiveTransform：这个函数计算的是平面到平面的单应性矩阵H，但它不会直接输出相机参数——你需要手动从H分解出K（内参）、R、t，这也是我们接下来要做的。
initCameraMatrix2D：这个函数需要多张不同姿态的标定板图像来估计内参，你只有单张图像，所以返回无效值是正常的。
calibrateCamera：同样需要多视图的点对数据（至少2张不同姿态的标定板图像），单张图像触发断言错误是因为输入不满足函数的前提条件。

用OpenCV实现的正确方法：单应性矩阵分解

因为你的世界点在平面Z=0上，所以可以用单应性矩阵H关联世界平面和图像平面，再从H分解出所需的相机参数。具体步骤如下：

1. 计算单应性矩阵H

首先把3D世界点转成2D（因为Z=0），然后用findHomography计算H：

#include <opencv2/opencv.hpp>
#include <vector>

int main() {
    std::vector<cv::Point3f> worldPoints = {
        cv::Point3f(0, 0, 0),
        cv::Point3f(2000, 0, 0),
        cv::Point3f(0, 3000, 0),
        cv::Point3f(2000, 3000, 0)
    };
    std::vector<cv::Point2f> imagePoints = {
        cv::Point2f(-0.958707, -0.219624),
        cv::Point2f(-1.22234, 0.577061),
        cv::Point2f(0.0837469, -0.1783),
        cv::Point2f(0.205473, 0.428184)
    };

    // 转换为2D世界点（Z=0）
    std::vector<cv::Point2f> worldPoints2D;
    for (const auto& p : worldPoints) {
        worldPoints2D.emplace_back(p.x, p.y);
    }

    // 计算单应性矩阵H
    cv::Mat H = cv::findHomography(worldPoints2D, imagePoints, cv::RANSAC);

2. 从H求解fx和fy

单应性矩阵H满足H = λ*K*[R | t]，其中K是内参矩阵（你的情况是K = [[fx,0,0],[0,fy,0],[0,0,1]]，因为图像坐标已归一化，cx=cy=0），λ是缩放因子，R是旋转矩阵，t是平移向量。

利用旋转矩阵的列向量正交且模长为1的性质，我们可以列出两个方程求解fx和fy：

// 提取H的前两列
    cv::Mat h1 = H.col(0);
    cv::Mat h2 = H.col(1);

    // 设a=1/fx²，b=1/fy²，构建线性方程组
    double eq1_a = h1.at<double>(0) * h2.at<double>(0);
    double eq1_b = h1.at<double>(1) * h2.at<double>(1);
    double eq1_c = -h1.at<double>(2) * h2.at<double>(2);

    double eq2_a = (h1.at<double>(0)*h1.at<double>(0)) - (h2.at<double>(0)*h2.at<double>(0));
    double eq2_b = (h1.at<double>(1)*h1.at<double>(1)) - (h2.at<double>(1)*h2.at<double>(1));
    double eq2_c = (h2.at<double>(2)*h2.at<double>(2)) - (h1.at<double>(2)*h1.at<double>(2));

    // 解线性方程组
    cv::Mat A = (cv::Mat_<double>(2,2) << eq1_a, eq1_b, eq2_a, eq2_b);
    cv::Mat B = (cv::Mat_<double>(2,1) << eq1_c, eq2_c);
    cv::Mat X;
    cv::solve(A, B, X);

    double a = X.at<double>(0);
    double b = X.at<double>(1);
    double fx = 1 / sqrt(a);
    double fy = 1 / sqrt(b);

    // 构建内参矩阵K
    cv::Mat K = (cv::Mat_<double>(3,3) << fx, 0, 0, 0, fy, 0, 0, 0, 1);

3. 分解H得到旋转矩阵R和平移向量t

用OpenCV的decomposeHomographyMat分解H，会得到多个可能的解，我们需要筛选出符合物理意义的解（旋转矩阵行列式为1，相机在世界平面上方即tz>0）：

std::vector<cv::Mat> Rs, ts, normals;
    int numSolutions = cv::decomposeHomographyMat(H, K, Rs, ts, normals);

    // 筛选正确的解
    cv::Mat R, t;
    for (int i = 0; i < numSolutions; ++i) {
        double detR = cv::determinant(Rs[i]);
        // 旋转矩阵行列式应为1，且相机在世界平面上方（tz>0）
        if (std::abs(detR - 1) < 1e-6 && ts[i].at<double>(2) > 0) {
            R = Rs[i].clone();
            t = ts[i].clone();
            break;
        }
    }

    // 输出结果
    std::cout << "fx: " << fx << std::endl;
    std::cout << "fy: " << fy << std::endl;
    std::cout << "Rotation matrix R:\n" << R << std::endl;
    std::cout << "Translation vector t:\n" << t << std::endl;

    return 0;
}

手动求解的方法：非线性最小二乘法

如果不想依赖OpenCV，你可以手动构建非线性方程组，用Levenberg-Marquardt等迭代方法求解：

1. 参数化旋转矩阵

用欧拉角（roll, pitch, yaw）来表示旋转矩阵R，这样旋转的3个自由度转化为3个角度变量，总共有8个未知量：fx, fy, roll, pitch, yaw, tx, ty, tz。

2. 构建投影方程

对于每个点，消去缩放因子s_i后得到两个方程：
$$
x_i(R_{31}X_i + R_{32}Y_i + t_z) = f_x(R_{11}X_i + R_{12}Y_i + t_x)
$$
$$
y_i(R_{31}X_i + R_{32}Y_i + t_z) = f_y(R_{21}X_i + R_{22}Y_i + t_y)
$$
其中$R_{ij}$是旋转矩阵R的元素，由欧拉角计算得到。