张正友相机标定算法（Zhang’s camera calibration method）原理和过程推导（附OpenCV代码示例）

一、算法原理概述

张正友方法通过 平面靶标图像 （如棋盘格）的 多个视角图像，提取角点特征，求解从平面到图像的单应性矩阵，再从中估计相机内参、外参及畸变参数。

---

二、数学推导核心步骤

1️.相机模型

假设无畸变，针孔相机模型为：

s \[ u v 1 \] = K \[ R ∣ t \] \[ X Y Z 1 \] \] \[ s \\begin{bmatrix} u \\\\ v \\\\ 1 \\end{bmatrix} = \\mathbf{K} \[\\mathbf{R} \| \\mathbf{t}\] \\begin{bmatrix} X \\\\ Y \\\\ Z \\\\ 1 \\end{bmatrix} \] \[s uv1 =K\[R∣t\] XYZ1

• ( ( X , Y , Z ) ) ( (X, Y, Z) ) ((X,Y,Z))：世界坐标

• ( ( u , v ) ) ( (u, v) ) ((u,v))：像素坐标

• ( K ) ( \mathbf{K} ) (K)：内参矩阵（待求）

  K = \[ f x γ c x 0 f y c y 0 0 1 \] \] \[ \\mathbf{K} = \\begin{bmatrix} f_x \& \\gamma \& c_x \\\\ 0 \& f_y \& c_y \\\\ 0 \& 0 \& 1 \\end{bmatrix} \] \[K= fx00γfy0cxcy1

注：γ 为主点偏移，一般为 0。

---

2️. 平面标定板简化（Z=0）

令 ( Z = 0 ) ( Z = 0 ) (Z=0)，即标定板在 Z=0 平面上：

s m = K \[ r 1 , r 2 , t \] M \] \[ s \\mathbf{m} = \\mathbf{K} \[\\mathbf{r}_1, \\mathbf{r}_2, \\mathbf{t}\] \\mathbf{M} \] \[sm=K\[r1,r2,t\]M

这个映射是一个 单应性变换 H ，可表示为：

s m = H M , H = K \[ r 1 , r 2 , t \] \] \[ s \\mathbf{m} = \\mathbf{H} \\mathbf{M}, \\quad \\mathbf{H} = \\mathbf{K} \[\\mathbf{r}_1, \\mathbf{r}_2, \\mathbf{t}\] \] \[sm=HM,H=K\[r1,r2,t\]

---

3️ .从单应性估计内参

对每个视图估计单应矩阵 H，根据正交约束：

v 12 ⊤ b = 0 v 11 ⊤ b = v 22 ⊤ b \] \[ \\mathbf{v}_{12}\^\\top \\mathbf{b} = 0 \\\\ \\mathbf{v}_{11}\^\\top \\mathbf{b} = \\mathbf{v}_{22}\^\\top \\mathbf{b} \] \[v12⊤b=0v11⊤b=v22⊤b

其中 ( b ) ( \mathbf{b} ) (b) 为包含内参的 6 个未知量向量，线性方程组可以通过 SVD 求解 得到内参。从单应矩阵 ( H ) ( \mathbf{H} ) (H) 计算相机内参矩阵 ( K ) ( \mathbf{K} ) (K) 推导过程的如下：

前提设定

我们已知：

• 相机满足 针孔模型。
• 世界坐标系中的点 ( M ) ( \mathbf{M} ) (M) 在平面 ( Z = 0 ) ( Z=0 ) (Z=0) 上，即 ( M = [ X , Y , 0 , 1 ] ⊤ ) ( \mathbf{M} = [X, Y, 0, 1]^\top ) (M=[X,Y,0,1]⊤)。
• 单应性关系为：

  s ⋅ m = H ⋅ M 其中 H = K \[ r 1 r 2 t \] \] \[ s \\cdot \\mathbf{m} = \\mathbf{H} \\cdot \\mathbf{M} \\quad\\text{其中}\\quad \\mathbf{H} = \\mathbf{K} \[\\mathbf{r}_1 \~ \\mathbf{r}_2 \~ \\mathbf{t}\] \] \[s⋅m=H⋅M其中H=K\[r1 r2 t\]

---

步骤 1：引入 H 与 K 的关系

由：

H = K \[ r 1 r 2 t \] ⇒ K − 1 H = \[ r 1 r 2 t \] \] \[ \\mathbf{H} = \\mathbf{K} \[\\mathbf{r}_1 \~ \\mathbf{r}_2 \~ \\mathbf{t}\] \\Rightarrow \\mathbf{K}\^{-1} \\mathbf{H} = \[\\mathbf{r}_1 \~ \\mathbf{r}_2 \~ \\mathbf{t}\] \] \[H=K\[r1 r2 t\]⇒K−1H=\[r1 r2 t\]

设 ( h 1 , h 2 , h 3 ) ( \mathbf{h}_1, \mathbf{h}_2, \mathbf{h}_3 ) (h1,h2,h3) 是单应矩阵 H 的列向量，则：

r 1 = λ ⋅ K − 1 h 1 , r 2 = λ ⋅ K − 1 h 2 \] \[ \\mathbf{r}_1 = \\lambda \\cdot \\mathbf{K}\^{-1} \\mathbf{h}_1, \\quad \\mathbf{r}_2 = \\lambda \\cdot \\mathbf{K}\^{-1} \\mathbf{h}_2 \] \[r1=λ⋅K−1h1,r2=λ⋅K−1h2

因为 ( r 1 , r 2 ) ( \mathbf{r}_1, \mathbf{r}_2 ) (r1,r2) 是旋转矩阵的一部分，所以满足以下约束：

r 1 ⊤ r 2 = 0 ( 正交性 ) ∥ r 1 ∥ = ∥ r 2 ∥ = 1 ( 单位向量 ) \] \[ \\mathbf{r}_1\^\\top \\mathbf{r}_2 = 0 \\quad (\\text{正交性}) \\\\ \\\|\\mathbf{r}_1\\\| = \\\|\\mathbf{r}_2\\\| = 1 \\quad (\\text{单位向量}) \] \[r1⊤r2=0(正交性)∥r1∥=∥r2∥=1(单位向量)

---

步骤 2：构造线性约束方程组

令：

v i j = \[ h i 1 h j 1 h i 1 h j 2 + h i 2 h j 1 h i 2 h j 2 h i 3 h j 1 + h i 1 h j 3 h i 3 h j 2 + h i 2 h j 3 h i 3 h j 3 \] \] \[ \\mathbf{v}_{ij} = \\begin{bmatrix} h_{i1} h_{j1} \\\\ h_{i1} h_{j2} + h_{i2} h_{j1} \\\\ h_{i2} h_{j2} \\\\ h_{i3} h_{j1} + h_{i1} h_{j3} \\\\ h_{i3} h_{j2} + h_{i2} h_{j3} \\\\ h_{i3} h_{j3} \\end{bmatrix} \] \[vij= hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3

对于每张图像（每个单应矩阵 ( H ) ( H ) (H)），可写出两个约束方程：

• ( v 12 ⊤ ⋅ b = 0 ) ( \mathbf{v}_{12}^\top \cdot \mathbf{b} = 0 ) (v12⊤⋅b=0)
• ( ( v 11 − v 22 ) ⊤ ⋅ b = 0 ) ( (\mathbf{v}{11} - \mathbf{v}{22})^\top \cdot \mathbf{b} = 0 ) ((v11−v22)⊤⋅b=0)

其中 ( b ) ( \mathbf{b} ) (b) 为含有内参的 6 维向量：

b = \[ B 11 , B 12 , B 22 , B 13 , B 23 , B 33 \] ⊤ \] \[ \\mathbf{b} = \[B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}\]\^\\top \] \[b=\[B11,B12,B22,B13,B23,B33\]⊤

而这些系数组成了内参矩阵 B 的对称形式：

B = K − ⊤ K − 1 = \[ B 11 B 12 B 13 B 12 B 22 B 23 B 13 B 23 B 33 \] \] \[ \\mathbf{B} = \\mathbf{K}\^{-\\top} \\mathbf{K}\^{-1} = \\begin{bmatrix} B_{11} \& B_{12} \& B_{13} \\\\ B_{12} \& B_{22} \& B_{23} \\\\ B_{13} \& B_{23} \& B_{33} \\end{bmatrix} \] \[B=K−⊤K−1= B11B12B13B12B22B23B13B23B33

将多个图像产生的方程组合在一起，形成矩阵 ( V ⋅ b = 0 ) ( V \cdot \mathbf{b} = 0 ) (V⋅b=0)，可通过 SVD 解 b 的最小二乘解。

---

步骤 3：从 ( B ) ( \mathbf{B} ) (B) 解析求出内参矩阵 ( K ) ( \mathbf{K} ) (K)

根据 ( B = K − ⊤ K − 1 ) ( \mathbf{B} = \mathbf{K}^{-\top} \mathbf{K}^{-1} ) (B=K−⊤K−1)，可以反推 K（这是已知对称正定矩阵求分解的经典问题）：

v 0 = B 12 B 13 − B 11 B 23 B 11 B 22 − B 12 2 λ = B 33 − B 13 2 + v 0 ( B 12 B 13 − B 11 B 23 ) B 11 α = λ / B 11 β = λ ⋅ B 11 / ( B 11 B 22 − B 12 2 ) γ = − B 12 ⋅ α 2 ⋅ β / λ u 0 = γ ⋅ v 0 / β − B 13 ⋅ α 2 / λ \] \[ v_0 = \\frac{B_{12} B_{13} - B_{11} B_{23}}{B_{11} B_{22} - B_{12}\^2} \\\\ \\lambda = B_{33} - \\frac{B_{13}\^2 + v_0 (B_{12} B_{13} - B_{11} B_{23})}{B_{11}} \\\\ \\alpha = \\sqrt{\\lambda / B_{11}} \\\\ \\beta = \\sqrt{\\lambda \\cdot B_{11} / (B_{11} B_{22} - B_{12}\^2)} \\\\ \\gamma = -B_{12} \\cdot \\alpha\^2 \\cdot \\beta / \\lambda \\\\ u_0 = \\gamma \\cdot v_0 / \\beta - B_{13} \\cdot \\alpha\^2 / \\lambda \] \[v0=B11B22−B122B12B13−B11B23λ=B33−B11B132+v0(B12B13−B11B23)α=λ/B11 β=λ⋅B11/(B11B22−B122) γ=−B12⋅α2⋅β/λu0=γ⋅v0/β−B13⋅α2/λ

于是得到相机内参矩阵：

K = \[ α γ u 0 0 β v 0 0 0 1 \] \] \[ \\mathbf{K} = \\begin{bmatrix} \\alpha \& \\gamma \& u_0 \\\\ 0 \& \\beta \& v_0 \\\\ 0 \& 0 \& 1 \\end{bmatrix} \] \[K= α00γβ0u0v01

---

数值求解示意（伪代码）

// Step 1: For each image, compute H using cv::findHomography or known correspondences.
// Step 2: For each H, compute v_12 and v_11 - v_22
// Step 3: Stack into V matrix, solve V * b = 0 using SVD
// Step 4: Recover B matrix, compute K as above.

---

小结

符号	含义
( H ) ( \mathbf{H} ) (H)	单应矩阵，从世界平面到图像平面
( K ) ( \mathbf{K} ) (K)	相机内参矩阵
( B ) ( \mathbf{B} ) (B)	( K − ⊤ K − 1 ) ( \mathbf{K}^{-\top} \mathbf{K}^{-1} ) (K−⊤K−1)，用于线性估计
( v i j ) ( \mathbf{v}_{ij} ) (vij)	用于构造线性方程的向量形式
( b ) ( \mathbf{b} ) (b)	包含内参的 6 维向量

---

---

4️.求解外参

已知内参和 H，可以从以下关系恢复 R 和 t：

r 1 = λ K − 1 h 1 , r 2 = λ K − 1 h 2 , t = λ K − 1 h 3 \] \[ \\mathbf{r}_1 = \\lambda \\mathbf{K}\^{-1} \\mathbf{h}_1,\\quad \\mathbf{r}_2 = \\lambda \\mathbf{K}\^{-1} \\mathbf{h}_2,\\quad \\mathbf{t} = \\lambda \\mathbf{K}\^{-1} \\mathbf{h}_3 \] \[r1=λK−1h1,r2=λK−1h2,t=λK−1h3

---

5️. 畸变参数估计

考虑径向畸变后，采用非线性最小二乘（Levenberg-Marquardt）优化所有参数（内参 + 外参 + 畸变），最小化重投影误差：

min ⁡ ∑ i ∥ m i − m \^ i ( θ ) ∥ 2 \] \[ \\min \\sum_{i} \\\| \\mathbf{m}_i - \\hat{\\mathbf{m}}_i(\\theta) \\\|\^2 \] \[mini∑∥mi−m\^i(θ)∥2

---

三、C++ 示例代码（OpenCV）

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

int main() {
    int board_w = 9, board_h = 6;
    float square_size = 0.025; // 单位：米
    cv::Size board_size(board_w, board_h);

    std::vector<std::vector<cv::Point2f>> image_points;
    std::vector<std::vector<cv::Point3f>> object_points;

    std::vector<cv::String> image_files;
    cv::glob("calib_images/*.jpg", image_files); // 自行准备棋盘格图像

    for (const auto& fname : image_files) {
        cv::Mat img = cv::imread(fname);
        std::vector<cv::Point2f> corners;

        if (cv::findChessboardCorners(img, board_size, corners)) {
            cv::Mat gray;
            cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
            cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
                cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.01));
            image_points.push_back(corners);

            std::vector<cv::Point3f> objp;
            for (int i = 0; i < board_h; ++i)
                for (int j = 0; j < board_w; ++j)
                    objp.emplace_back(j * square_size, i * square_size, 0);
            object_points.push_back(objp);
        }
    }

    // 相机标定
    cv::Mat cameraMatrix, distCoeffs;
    std::vector<cv::Mat> rvecs, tvecs;
    cv::calibrateCamera(object_points, image_points, board_size,
                        cameraMatrix, distCoeffs, rvecs, tvecs);

    std::cout << "Camera Matrix:\n" << cameraMatrix << std::endl;
    std::cout << "Distortion Coefficients:\n" << distCoeffs << std::endl;
    return 0;
}

---