多视图几何--相机标定--从0-1理解张正友标定法
1基本原理
1.1 单应性矩阵（Homography）的建立
相机模型：世界坐标系下棋盘格平面（Z=0）到图像平面的投影关系为：
s [ u v 1 ] = K [ r 1 r 2 t ] [ X Y 1 ] s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} r_1 & r_2 & t \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix} s uv1 =K[r1r2t] XY1
其中：
-
( X , Y ) (X,Y) (X,Y)：棋盘格角点的世界坐标（Z=0）。
-
( u , v ) (u,v) (u,v)：图像平面上的像素坐标。
-
K K K：内参矩阵，形式为：
K = [ f x γ u 0 0 f y v 0 0 0 1 ] K = \begin{bmatrix} f_x & \gamma & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1 \end{bmatrix} K= fx00γfy0u0v01 -
r 1 , r 2 , t r_1, r_2, t r1,r2,t：外参（旋转矩阵的前两列和平移向量）。
-
s s s：尺度因子。
单应性矩阵：将投影关系简化为：
s [ u v 1 ] = H [ X Y 1 ] s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = H \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix} s uv1 =H XY1
其中 H = K [ r 1 r 2 t ] H = K [r_1 \quad r_2 \quad t] H=K[r1r2t] 是一个3×3矩阵，称为单应性矩阵。
1.2 单应性矩阵的求解
对每个棋盘格图像，利用至少4组对应点（世界坐标和图像坐标），通过最小二乘法或直接线性变换（DLT）求解 H H H。对于每组点：
[ X Y 1 0 0 0 − u X − u Y − u 0 0 0 X Y 1 − v X − v Y − v ] [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] = 0 \begin{bmatrix} X & Y & 1 & 0 & 0 & 0 & -uX & -uY & -u \\ 0 & 0 & 0 & X & Y & 1 & -vX & -vY & -v \end{bmatrix} \begin{bmatrix} h_{11} \\ h_{12} \\ h_{13} \\ h_{21} \\ h_{22} \\ h_{23} \\ h_{31} \\ h_{32} \\ h_{33} \end{bmatrix} = 0 [X0Y0100X0Y01−uX−vX−uY−vY−u−v] h11h12h13h21h22h23h31h32h33 =0
通过SVD分解求解 H H H 的9个参数（归一化后）。
1.3 内参矩阵的约束条件
张正友标定法中，通过旋转矩阵的正交性推导两个约束方程的过程是核心步骤。以下是结合正交矩阵性质与单应性矩阵的详细推导：
1.3.1 旋转矩阵的正交性
在张正友标定法中，旋转矩阵 R R R 是正交矩阵，满足以下性质：
- 列向量正交性： R R R 的列向量 r 1 , r 2 , r 3 r_1, r_2, r_3 r1,r2,r3 两两正交。
- 单位模长约束：每个列向量的模长为1，即 ∥ r 1 ∥ = ∥ r 2 ∥ = ∥ r 3 ∥ = 1 \|r_1\| = \|r_2\| = \|r_3\| = 1 ∥r1∥=∥r2∥=∥r3∥=1。
由于标定棋盘格平面位于世界坐标系的 Z = 0 Z=0 Z=0 平面，投影模型中仅涉及 R R R 的前两列 r 1 r_1 r1 和 r 2 r_2 r2，因此正交性约束简化为：
{ r 1 T r 2 = 0 （正交性） r 1 T r 1 = r 2 T r 2 = 1 （单位模长） \begin{cases} r_1^T r_2 = 0 \quad \text{（正交性）} \\ r_1^T r_1 = r_2^T r_2 = 1 \quad \text{（单位模长）} \end{cases} {r1Tr2=0（正交性）r1Tr1=r2Tr2=1（单位模长）
1.3.2 单应性矩阵 $ H $ 与旋转矩阵的关联
单应性矩阵 H H H 将棋盘格平面（ Z = 0 Z=0 Z=0）映射到图像平面，其表达式为：
H = λ K [ r 1 r 2 t ] H = \lambda K [r_1 \quad r_2 \quad t] H=λK[r1r2t]
其中：
- λ \lambda λ：尺度因子，
- K K K：内参矩阵，
- r 1 , r 2 r_1, r_2 r1,r2：旋转矩阵的前两列，
- t t t：平移向量。
将 H H H 的列向量表示为 h 1 , h 2 , h 3 h_1, h_2, h_3 h1,h2,h3，则有：
h 1 = λ K r 1 , h 2 = λ K r 2 , h 3 = λ K t h_1 = \lambda K r_1, \quad h_2 = \lambda K r_2, \quad h_3 = \lambda K t h1=λKr1,h2=λKr2,h3=λKt
1.3.3 正交性约束的代数转化
通过 h 1 h_1 h1 和 h 2 h_2 h2 表达正交性条件：
-
正交性条件 r 1 T r 2 = 0 r_1^T r_2 = 0 r1Tr2=0:
( 1 λ K − 1 h 1 ) T ( 1 λ K − 1 h 2 ) = 0 (\frac{1}{\lambda} K^{-1} h_1)^T (\frac{1}{\lambda} K^{-1} h_2) = 0 (λ1K−1h1)T(λ1K−1h2)=0化简后得到：
h 1 T K − T K − 1 h 2 = 0 h_1^T K^{-T} K^{-1} h_2 = 0 h1TK−TK−1h2=0 -
单位模长条件 $ r_1^T r_1 = r_2^T r_2 = 1 $:
( 1 λ K − 1 h 1 ) T ( 1 λ K − 1 h 1 ) = ( 1 λ K − 1 h 2 ) T ( 1 λ K − 1 h 2 ) (\frac{1}{\lambda} K^{-1} h_1)^T (\frac{1}{\lambda} K^{-1} h_1) = (\frac{1}{\lambda} K^{-1} h_2)^T (\frac{1}{\lambda} K^{-1} h_2) (λ1K−1h1)T(λ1K−1h1)=(λ1K−1h2)T(λ1K−1h2)化简后得到：
h 1 T K − T K − 1 h 1 = h 2 T K − T K − 1 h 2 h_1^T K^{-T} K^{-1} h_1 = h_2^T K^{-T} K^{-1} h_2 h1TK−TK−1h1=h2TK−TK−1h2
1.3.4 引入对称矩阵 $ B $ 简化计算
定义对称矩阵 B = K − T K − 1 B = K^{-T} K^{-1} B=K−TK−1，其元素仅与内参矩阵 K K K 相关。将上述两个条件转化为：
{ h 1 T B h 2 = 0 h 1 T B h 1 = h 2 T B h 2 \begin{cases} h_1^T B h_2 = 0 \\ h_1^T B h_1 = h_2^T B h_2 \end{cases} {h1TBh2=0h1TBh1=h2TBh2
矩阵 $ B $ 的表达式为：
B = [ 1 f x 2 − γ f x 2 f y γ v 0 − f y u 0 f x 2 f y − γ f x 2 f y γ 2 f x 2 f y 2 + 1 f y 2 − γ ( γ v 0 − f y u 0 ) f x 2 f y 2 − v 0 f y 2 γ v 0 − f y u 0 f x 2 f y − γ ( γ v 0 − f y u 0 ) f x 2 f y 2 − v 0 f y 2 ( γ v 0 − f y u 0 ) 2 f x 2 f y 2 + v 0 2 f y 2 + 1 ] B = \begin{bmatrix} \frac{1}{f_x^2} & -\frac{\gamma}{f_x^2 f_y} & \frac{\gamma v_0 - f_y u_0}{f_x^2 f_y} \\ -\frac{\gamma}{f_x^2 f_y} & \frac{\gamma^2}{f_x^2 f_y^2} + \frac{1}{f_y^2} & -\frac{\gamma (\gamma v_0 - f_y u_0)}{f_x^2 f_y^2} - \frac{v_0}{f_y^2} \\ \frac{\gamma v_0 - f_y u_0}{f_x^2 f_y} & -\frac{\gamma (\gamma v_0 - f_y u_0)}{f_x^2 f_y^2} - \frac{v_0}{f_y^2} & \frac{(\gamma v_0 - f_y u_0)^2}{f_x^2 f_y^2} + \frac{v_0^2}{f_y^2} + 1 \end{bmatrix} B= fx21−fx2fyγfx2fyγv0−fyu0−fx2fyγfx2fy2γ2+fy21−fx2fy2γ(γv0−fyu0)−fy2v0fx2fyγv0−fyu0−fx2fy2γ(γv0−fyu0)−fy2v0fx2fy2(γv0−fyu0)2+fy2v02+1
其中 f x , f y , u 0 , v 0 , γ f_x, f_y, u_0, v_0, \gamma fx,fy,u0,v0,γ 为内参参数。
1.3.5 构建线性方程组求解 B B B
将单应性矩阵 H H H 的元素代入约束方程：
-
正交性方程：
h 11 h 21 B 11 + ( h 11 h 22 + h 12 h 21 ) B 12 + ⋯ + h 31 h 32 B 33 = 0 h_{11} h_{21} B_{11} + (h_{11} h_{22} + h_{12} h_{21}) B_{12} + \cdots + h_{31} h_{32} B_{33} = 0 h11h21B11+(h11h22+h12h21)B12+⋯+h31h32B33=0 -
单位模长方程：
h 11 2 B 11 + 2 h 11 h 12 B 12 + ⋯ + h 31 2 B 33 = h 21 2 B 11 + ⋯ + h 32 2 B 33 h_{11}^2 B_{11} + 2 h_{11} h_{12} B_{12} + \cdots + h_{31}^2 B_{33} = h_{21}^2 B_{11} + \cdots + h_{32}^2 B_{33} h112B11+2h11h12B12+⋯+h312B33=h212B11+⋯+h322B33
每幅标定图像提供一个 $ H $，对应两个方程。B为对称矩阵所以有6个自由度，内参矩阵有5个自由度，因此最少需要3张照片提供6个方程求解B及内参。若使用 n n n 幅图像，可构建 2 n 2n 2n 个方程的线性方程组：
V b = 0 V b = 0 Vb=0
其中：
- V V V 是系数矩阵，
- b = [ B 11 , B 12 , B 13 , B 22 , B 23 , B 33 ] T b = [B_{11}, B_{12}, B_{13}, B_{22}, B_{23}, B_{33}]^T b=[B11,B12,B13,B22,B23,B33]T 是 B B B 的向量化形式。
通过 奇异值分解（SVD） 求解 b b b，再通过 Cholesky分解 从 B B B 中恢复内参矩阵 K K K 。
这一过程将几何约束（旋转矩阵的正交性）与代数计算（线性方程求解）结合，是张正友标定法能够仅用平面棋盘格实现高精度标定的核心。
1.4 外参求解
已知 $ K $ 后，通过 $ H $ 分解外参：
r 1 = λ K − 1 h 1 , r 2 = λ K − 1 h 2 , t = λ K − 1 h 3 r_1 = \lambda K^{-1} h_1, \quad r_2 = \lambda K^{-1} h_2, \quad t = \lambda K^{-1} h_3 r1=λK−1h1,r2=λK−1h2,t=λK−1h3
其中 λ = 1 / ∥ K − 1 h 1 ∥ \lambda = 1 / \|K^{-1} h_1\| λ=1/∥K−1h1∥。
旋转矩阵 R = [ r 1 r 2 r 1 × r 2 ] R = [r_1 \quad r_2 \quad r_1 \times r_2] R=[r1r2r1×r2]，需正交化处理（如QR分解）。
1.5 非线性优化与畸变校正
优化目标函数：最小化重投影误差：
∑ i = 1 n ∑ j = 1 m ∥ p i j − p ^ ( K , R i , t i , k 1 , k 2 , X j ) ∥ 2 \sum_{i=1}^n \sum_{j=1}^m \| p_{ij} - \hat{p}(K, R_i, t_i, k_1, k_2, X_j) \|^2 i=1∑nj=1∑m∥pij−p^(K,Ri,ti,k1,k2,Xj)∥2
其中 $ k_1, k_2 $ 为径向畸变系数，畸变模型为：
u 畸变 = u ( 1 + k 1 r 2 + k 2 r 4 ) u_{\text{畸变}} = u (1 + k_1 r^2 + k_2 r^4) u畸变=u(1+k1r2+k2r4)
采用Levenberg-Marquardt算法优化所有参数。
总结
张正友标定法通过单应性矩阵将棋盘格平面与图像平面关联，利用旋转矩阵的正交性建立内参约束，最终通过线性与非线性优化联合求解参数。公式推导的关键在于：
- 单应性矩阵的线性求解；
- 内参约束条件的正交性展开；
- 非线性优化的重投影误差最小化。
该方法仅需平面棋盘格，无需精密设备，且精度较高，成为计算机视觉中广泛应用的标定方法。
2opencv源码解析
OpenCV的cv::calibrateCamera函数是相机标定算法的核心实现，其源码逻辑融合了张正友标定法的数学原理与非线性优化技术。以下从源码层面对其核心流程和关键模块进行深度剖析，并结合OpenCV 4.8版本代码结构展开说明。
2.1 函数入口与参数解析
函数原型（简化自modules/calib3d/src/calibration.cpp）：
double calibrateCamera(InputArrayOfArrays objectPoints, // 世界坐标点集（Z=0平面）InputArrayOfArrays imagePoints, // 图像坐标点集Size imageSize, // 图像尺寸InputOutputArray cameraMatrix, // 输入/输出内参矩阵InputOutputArray distCoeffs, // 输入/输出畸变系数OutputArrayOfArrays rvecs, // 输出旋转向量OutputArrayOfArrays tvecs, // 输出平移向量int flags, // 标定标志位TermCriteria criteria) // 优化终止条件
关键参数说明：
- flags：控制标定行为的标志位，例如：
CALIB_USE_INTRINSIC_GUESS：使用用户提供的初始内参矩阵。CALIB_FIX_ASPECT_RATIO：固定焦距比（fx/fy）。CALIB_ZERO_TANGENT_DIST：忽略切向畸变（p1=p2=0）。
- criteria：优化终止条件（默认迭代30次或误差<1e-6）。
2.2 源码核心流程
阶段1：数据校验与初始化
// 检查输入数据合法性
CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
CV_Assert(imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2);// 初始化内参矩阵和畸变系数
if (!(flags & CALIB_USE_INTRINSIC_GUESS)) {cameraMatrix = Mat::eye(3, 3, CV_64F); // 默认初始化为单位矩阵distCoeffs = Mat::zeros(5, 1, CV_64F); // 默认仅考虑k1,k2,p1,p2,k3
}
阶段2：计算单应性矩阵（Homography）
代码路径：modules/calib3d/src/homography.cpp
// 对每幅图像计算H矩阵
for (int i = 0; i < nimages; i++) {Mat H = findHomography(objectPoints[i], imagePoints[i], RANSAC);homographies.push_back(H);
}
数学原理：单应性矩阵 H H H 满足 s [ u v 1 ] = H [ X Y 1 ] s \begin{bmatrix}u \\ v \\ 1\end{bmatrix} = H \begin{bmatrix}X \\ Y \\ 1\end{bmatrix} s uv1 =H XY1 ，通过SVD分解最小化重投影误差求解。
阶段3：构建约束方程求解内参矩阵
核心代码（简化自modules/calib3d/src/calibration.cpp）：
// 步骤1：定义对称矩阵B = K^{-T}K^{-1}
Mat B(3, 3, CV_64F);
B.at<double>(0,0) = 1.0 / (fx*fx);
B.at<double>(0,1) = -gamma / (fx*fx*fy);
// ... 其他元素根据内参展开// 步骤2：构建线性方程组V*b=0
Mat V(2*nimages, 6, CV_64F); // 每幅图像贡献2个方程
for (int i = 0; i < nimages; i++) {Mat h1 = homographies[i].col(0);Mat h2 = homographies[i].col(1);// 填充正交性约束和单位模长约束V.row(2*i) = ...; // h1^T*B*h2=0V.row(2*i+1) = ...; // h1^T*B*h1 = h2^T*B*h2
}// 步骤3：SVD求解最小特征值对应的b向量
SVD::solveZ(V, b);// 步骤4：Cholesky分解恢复内参矩阵K
Mat KInv = chol(B);
K = KInv.inv();
数学推导：通过旋转矩阵的正交性 $ r_1^T r_2 = 0 $ 和单位模长约束 $ |r_1| = |r_2| = 1 $，将单应性矩阵 $ H $ 分解为内参矩阵 $ K $ 和外参的线性组合。
阶段4：外参（R,t）估计
for (int i = 0; i < nimages; i++) {Mat h1 = homographies[i].col(0);Mat h2 = homographies[i].col(1);Mat h3 = homographies[i].col(2);// 计算尺度因子λdouble lambda = 1.0 / norm(K.inv() * h1);// 分解外参Mat r1 = lambda * K.inv() * h1;Mat r2 = lambda * K.inv() * h2;Mat r3 = r1.cross(r2); // 通过叉乘保证正交性Mat t = lambda * K.inv() * h3;// 构建旋转矩阵并正交化Mat R;Rodrigues(rvec, R); // 旋转向量转矩阵SVDecomp(R, S, U, V, SVD::FULL_UV); // 正交化处理R = U * V.t();
}
阶段5：非线性优化（Levenberg-Marquardt算法）
代码路径：modules/calib3d/src/lm.cpp
// 定义目标函数：最小化重投影误差
class CalibFunc : public LMSolver::Function {
public:int getDims() const { return totalPoints * 2; }void compute(const Mat& params, Mat& err) const {// 解析参数：内参、畸变、外参Mat K = params.rowRange(0, 9).reshape(3,3);Mat dist = params.rowRange(9, 14);Mat rvecs = params.rowRange(14, 14 + 3*nimages);Mat tvecs = params.rowRange(14 + 3*nimages, end);// 计算重投影误差for (int i = 0; i < nimages; i++) {projectPoints(objectPoints[i], rvecs[i], tvecs[i], K, dist, reproj);err += norm(imagePoints[i] - reproj);}}
};// 调用优化器
LMSolver lm(solverFunc, criteria);
lm.run(params);
优化变量：将所有参数（内参、畸变、每幅图像的外参）拼接为一个长向量，通过迭代更新使重投影误差最小化。
2.3 畸变模型与参数处理
畸变系数定义（modules/calib3d/src/distortion_model.hpp）：
enum DistCoeffs {K1 = 0, K2 = 1, P1 = 2, P2 = 3, K3 = 4 // 默认支持5参数模型
};
畸变校正公式：
{ x corrected = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y corrected = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{cases} x_{\text{corrected}} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2p_1 xy + p_2(r^2 + 2x^2) \\ y_{\text{corrected}} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2p_2 xy \end{cases} {xcorrected=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ycorrected=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
其中 $ r^2 = x^2 + y^2 $。优化过程中会根据flags决定是否固定某些系数（如CALIB_FIX_TANGENT_DIST）。
写在后面的话
旋转矩阵性质
一、旋转矩阵作为正交矩阵的数学定义
旋转矩阵是正交矩阵的一种特殊形式。根据正交矩阵的定义：
- 正交性：列向量（或行向量）两两正交，即内积为零。
- 单位模长：每个列向量的模长为1。
- 行列式为1：若行列式为+1，则为纯旋转矩阵；若为-1，则为反射矩阵（含镜像变换）。
数学推导：
-
正交矩阵满足 $ R^T R = I $，展开后得到：
{ r i T r j = 0 ( i ≠ j ) ∥ r i ∥ = 1 ( i = j ) \begin{cases} r_i^T r_j = 0 \quad (i \neq j) \\ \|r_i\| = 1 \quad (i = j) \end{cases} {riTrj=0(i=j)∥ri∥=1(i=j)因此，旋转矩阵的列向量 $ r_1, r_2, r_3 $ 必然满足正交性和单位模长。对于n阶正交矩阵，其列向量组是n维向量空间的一组标准正交基。