一、算法原理概述
张正友方法通过 平面靶标图像（如棋盘格）的 多个视角图像，提取角点特征，求解从平面到图像的单应性矩阵，再从中估计相机内参、外参及畸变参数。
二、数学推导核心步骤
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;
}