1基本原理

1.1 单应性矩阵（Homography）的建立

相机模型：世界坐标系下棋盘格平面（Z=0）到图像平面的投影关系为：
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$$

其中：

• $(X,Y)$：棋盘格角点的世界坐标（Z=0）。

• $(u,v)$：图像平面上的像素坐标。

• $K$：内参矩阵，形式为：
  $$K=\left[\begin{array}{ccc}{f}_x& \gamma & u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]$$

• $r_1,r_2,t$：外参（旋转矩阵的前两列和平移向量）。

• $s$：尺度因子。

单应性矩阵：将投影关系简化为：
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=H\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$$

其中 $H=K[r_1{r}_{2}t]$ 是一个3×3矩阵，称为单应性矩阵。

---

1.2 单应性矩阵的求解

对每个棋盘格图像，利用至少4组对应点（世界坐标和图像坐标），通过最小二乘法或直接线性变换（DLT）求解 $H$。对于每组点：
$$\left[\begin{array}{ccccccccc}X& Y& 1& 0& 0& 0& -uX& -uY& -u\\ 0& 0& 0& X& Y& 1& -vX& -vY& -v\end{array}\right]\left[\begin{array}{c}{h}_{11}\\ h_{12}\\ h_{13}\\ h_{21}\\ h_{22}\\ h_{23}\\ h_{31}\\ h_{32}\\ h_{33}\end{array}\right]=0$$

通过SVD分解求解 $H$ 的9个参数（归一化后）。

---

1.3 内参矩阵的约束条件

张正友标定法中，通过旋转矩阵的正交性推导两个约束方程的过程是核心步骤。以下是结合正交矩阵性质与单应性矩阵的详细推导：

1.3.1 旋转矩阵的正交性

在张正友标定法中，旋转矩阵 $R$ 是正交矩阵，满足以下性质：

1. 列向量正交性：$R$ 的列向量 $r_1,r_2,r_3$ 两两正交。
2. 单位模长约束：每个列向量的模长为1，即 $\Vert r_1\Vert =\Vert r_2\Vert =\Vert r_3\Vert =1$。

由于标定棋盘格平面位于世界坐标系的 $Z=0$ 平面，投影模型中仅涉及 $R$ 的前两列 $r_1$ 和 $r_2$，因此正交性约束简化为：
$$\{\begin{array}{l}{r}_1^T{r}_2=0\text{（正交性）}\\ r_1^T{r}_1=r_2^T{r}_2=1\text{（单位模长）}\end{array}$$

1.3.2 单应性矩阵 $ H $ 与旋转矩阵的关联

单应性矩阵 $H$ 将棋盘格平面（$Z=0$）映射到图像平面，其表达式为：
$$H=\lambda K[r_1{r}_{2}t]$$

其中：

• $\lambda$：尺度因子，
• $K$：内参矩阵，
• $r_1,r_2$：旋转矩阵的前两列，
• $t$：平移向量。

将 $H$ 的列向量表示为 $h_1,h_2,h_3$，则有：
$$h_1=\lambda K{r}_1,h_2=\lambda K{r}_2,h_3=\lambda Kt$$

1.3.3 正交性约束的代数转化

通过 $h_1$ 和 $h_2$ 表达正交性条件：

1. 正交性条件 $r_1^T{r}_2=0$:
   $$(\frac{1}{\lambda }{K}^{-1}{h}_1{)}^T(\frac{1}{\lambda }{K}^{-1}{h}_2)=0$$

   化简后得到：
   $$h_1^T{K}^{-T}{K}^{-1}{h}_2=0$$

2. 单位模长条件 $ r_1^T r_1 = r_2^T r_2 = 1 $:
   $$(\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)$$

   化简后得到：
   $$h_1^T{K}^{-T}{K}^{-1}{h}_1=h_2^T{K}^{-T}{K}^{-1}{h}_2$$

1.3.4 引入对称矩阵 $ B $ 简化计算

定义对称矩阵 $B=K^{-T}{K}^{-1}$，其元素仅与内参矩阵 $K$ 相关。将上述两个条件转化为：
$$\{\begin{array}{l}{h}_1^{T}B{h}_2=0\\ h_1^{T}B{h}_1=h_2^{T}B{h}_2\end{array}$$

矩阵 $ B $ 的表达式为：
$$B=\left[\begin{array}{ccc}\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{array}\right]$$

其中 $f_x,f_y,u_0,v_0,\gamma$ 为内参参数。

1.3.5 构建线性方程组求解 $B$

将单应性矩阵 $H$ 的元素代入约束方程：

1. 正交性方程：
   $$h_{11}{h}_{21}{B}_{11}+(h_{11}{h}_{22}+h_{12}{h}_{21})B_{12}+\cdots +h_{31}{h}_{32}{B}_{33}=0$$

2. 单位模长方程：
   $$h_{11}^2{B}_{11}+2h_{11}{h}_{12}{B}_{12}+\cdots +h_{31}^2{B}_{33}=h_{21}^2{B}_{11}+\cdots +h_{32}^2{B}_{33}$$

每幅标定图像提供一个 $ H $，对应两个方程。B为对称矩阵所以有6个自由度，内参矩阵有5个自由度，因此最少需要3张照片提供6个方程求解B及内参。若使用 $n$ 幅图像，可构建 $2n$ 个方程的线性方程组：
$$Vb=0$$

其中：

• $V$ 是系数矩阵，
• $b=[B_{11},B_{12},B_{13},B_{22},B_{23},B_{33}{]}^T$ 是 $B$ 的向量化形式。

通过 奇异值分解（SVD） 求解 $b$，再通过 Cholesky分解 从 $B$ 中恢复内参矩阵 $K$ 。

这一过程将几何约束（旋转矩阵的正交性）与代数计算（线性方程求解）结合，是张正友标定法能够仅用平面棋盘格实现高精度标定的核心。

1.4 外参求解

已知 $ K $ 后，通过 $ H $ 分解外参：
$$r_1=\lambda K^{-1}{h}_1,r_2=\lambda K^{-1}{h}_2,t=\lambda K^{-1}{h}_3$$

其中 $\lambda =1/\Vert K^{-1}{h}_1\Vert$。
旋转矩阵 $R=[r_1{r}_2{r}_1\times r_2]$，需正交化处理（如QR分解）。

---

1.5 非线性优化与畸变校正

优化目标函数：最小化重投影误差：
$$\sum _{i=1}^n\sum _{j=1}^m\Vert p_{ij}-\hat{p}(K,R_i,t_i,k_1,k_2,X_j){\Vert }^2$$

其中 $ k_1, k_2 $ 为径向畸变系数，畸变模型为：
$$u_{\text{畸变}}=u(1+k_1{r}^2+k_2{r}^4)$$

采用Levenberg-Marquardt算法优化所有参数。

---

总结

张正友标定法通过单应性矩阵将棋盘格平面与图像平面关联，利用旋转矩阵的正交性建立内参约束，最终通过线性与非线性优化联合求解参数。公式推导的关键在于：

1. 单应性矩阵的线性求解；
2. 内参约束条件的正交性展开；
3. 非线性优化的重投影误差最小化。

该方法仅需平面棋盘格，无需精密设备，且精度较高，成为计算机视觉中广泛应用的标定方法。

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$ 满足 $s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=H\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$，通过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=0
    V.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参数模型
};

畸变校正公式：
$$\{\begin{array}{l}{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{array}$$

其中 $ r^2 = x^2 + y^2 $。优化过程中会根据flags决定是否固定某些系数（如CALIB_FIX_TANGENT_DIST）。

---

写在后面的话

旋转矩阵性质

一、旋转矩阵作为正交矩阵的数学定义

旋转矩阵是正交矩阵的一种特殊形式。根据正交矩阵的定义：

1. 正交性：列向量（或行向量）两两正交，即内积为零。
2. 单位模长：每个列向量的模长为1。
3. 行列式为1：若行列式为+1，则为纯旋转矩阵；若为-1，则为反射矩阵（含镜像变换）。

数学推导：

• 正交矩阵满足 $ R^T R = I $，展开后得到：
  $$\{\begin{array}{l}{r}_i^T{r}_j=0(i\ne j)\\ \Vert r_i\Vert =1(i=j)\end{array}$$

  因此，旋转矩阵的列向量 $ r_1, r_2, r_3 $ 必然满足正交性和单位模长。对于n阶正交矩阵，其列向量组是n维向量空间的一组标准正交基。

---