相机标定(张正友标定法)

相机模型

针孔相机模型（Pinhole Camera Model）

• 透视投影，物体离相机越远在图像中看起来越小，能够表现场景的深度信息，平行线在图像中会汇聚到消失点。

远心相机模型（Telecentric Camera Model）

• 平行投影，物体在图像中的大小与其实际大小成正比，无法表现场景的深度信息，平行线在图像中仍然保持平行。

坐标系统

1. 世界坐标系（World Coordinate System）

• 定义：描述物体在三维空间中的位置，通常由用户定义，单位为米或毫米。
• 表示：点 $ P_w = (X_w, Y_w, Z_w) $。

2. 相机坐标系（Camera Coordinate System）

• 定义：以相机光心为原点，Z轴沿光轴方向，X轴和Y轴与图像平面平行，单位为米或毫米。
• 表示：点 $ P_c = (X_c, Y_c, Z_c) $。

3. 图像坐标系（Image Coordinate System）

• 定义：图像平面上的二维坐标系，原点在图像中心，单位为毫米。
• 表示：点 $ P_i = (x_i, y_i) $。

4. 像素坐标系（Pixel Coordinate System）

• 定义：图像在计算机中的表示，原点在左上角，单位为像素。
• 表示：点 $ P_p = (u, v) $。

转换公式

从世界坐标系到像素坐标系的变换关系为： \(Z_c\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\begin{bmatrix} s_x&0&u_0\\ 0&s_y&v_0\\ 0&0&1 \end{bmatrix}\begin{bmatrix} f&0&0\\ 0&f&0\\ 0&0&1 \end{bmatrix}\begin{bmatrix} \mathbf{R}&\mathbf{t} \end{bmatrix}\begin{bmatrix} \mathbf{X}_w\\ 1 \end{bmatrix}=\mathbf{K}\begin{bmatrix} \mathbf{R}&\mathbf{t} \end{bmatrix}\begin{bmatrix} \mathbf{X}_w\\ 1 \end{bmatrix}\) 其中相机内参矩阵 \(\mathbf{K}=\begin{bmatrix} \alpha& \gamma&u_0\\ 0&\beta&v_0\\ 0&0&1 \end{bmatrix}\) $\alpha = s_x f$，$\beta = s_y f$，$\gamma$ 为倾斜因子，通常 $\gamma = 0$。

简化变换（棋盘格平面 $Z_w = 0$）

在张正友标定法中，使用棋盘格作为标定物，将棋盘格平面置于世界坐标系的 $Z_w = 0$ 平面上。此时 \(\begin{bmatrix} \mathbf{X}_w\\ 1 \end{bmatrix}=\begin{bmatrix} X_w\\ Y_w\\ 0\\ 1 \end{bmatrix}\) 则有： \(\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\frac{1}{Z_c}\mathbf{K}\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 0\\ 1 \end{bmatrix}=\lambda\mathbf{K}\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}=\lambda\mathbf{H}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\) 其中 $\lambda=\frac{1}{Z_c}$ 是一个非零尺度因子，$\mathbf{H}=\mathbf{K}\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}$ 是一个 $3\times3$ 的单应性矩阵，$\mathbf{r}_1$ 和 $\mathbf{r}_2$ 是旋转矩阵 $\mathbf{R}$ 的前两列。

求解单应性矩阵 $\mathbf{H}$

对于每一幅棋盘格图像，已知棋盘格角点的世界坐标 $\mathbf{X_w} = [X_w, Y_w, 1]^T$ 和对应的像素坐标 $\mathbf{x}=[u, v, 1]^T$，可以通过最小二乘法求解单应性矩阵 $\mathbf{H}=(h_{ij})$。

因为 \(\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\lambda\begin{bmatrix} h_{11}&h_{12}&h_{13}\\ h_{21}&h_{22}&h_{23}\\ h_{31}&h_{32}&h_{33} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\)，即 \(\begin{cases} u=\lambda(h_{11}X_w + h_{12}Y_w+h_{13})\\ v=\lambda(h_{21}X_w + h_{22}Y_w+h_{23})\\ 1=\lambda(h_{31}X_w + h_{32}Y_w+h_{33}) \end{cases}\) 消去 $\lambda$ 可得：

\(\begin{cases} u(h_{31}X_w + h_{32}Y_w+h_{33})=h_{11}X_w + h_{12}Y_w+h_{13}\\ v(h_{31}X_w + h_{32}Y_w+h_{33})=h_{21}X_w + h_{22}Y_w+h_{23} \end{cases}\) 整理成线性方程组的形式： \(\begin{bmatrix} X_w&Y_w&1&0&0&0&-uX_w&-uY_w&-u\\ 0&0&0&X_w&Y_w&1&-vX_w&-vY_w&-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}=\begin{bmatrix} 0\\ 0 \end{bmatrix}\) 对于 $n$ 个角点，可得到 $2n$ 个方程，写成矩阵形式 $\mathbf{A}\mathbf{h}=\mathbf{0}$，其中 $\mathbf{h}=[h_{11},h_{12},h_{13},h_{21},h_{22},h_{23},h_{31},h_{32},h_{33}]^T$。通过奇异值分解（SVD）求解该线性方程组，取最小奇异值对应的右奇异向量作为 $\mathbf{h}$，从而得到单应性矩阵 $\mathbf{H}$。

求解内参矩阵$\mathbf{K}$

由于 $\mathbf{H}=\lambda\mathbf{K}\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}$，设 $\mathbf{H}=[\mathbf{h}_1,\mathbf{h}_2,\mathbf{h}_3]$，其中 $\mathbf{h}_i$ 是 $\mathbf{H}$ 的第 $i$ 列向量。则有 $\mathbf{h}_1=\lambda\mathbf{K}\mathbf{r}_1$，$\mathbf{h}_2=\lambda\mathbf{K}\mathbf{r}_2$。

因为旋转矩阵 $\mathbf{R}$ 的列向量是正交的，即 $\mathbf{r_1}^T\mathbf{r_2} = 0$ 且 $|\mathbf{r_1}|=|\mathbf{r_2}| = 1$，所以有： \(\mathbf{h_1}^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h_2} = 0\) \(\mathbf{h_1}^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h_1}=\mathbf{h_2}^T\mathbf{K}^{-T}\mathbf{K}^{-1}\mathbf{h_2}\) 设 \(\mathbf{B}=\mathbf{K}^{-T}\mathbf{K}^{-1}=\begin{bmatrix} B_{11}&B_{12}&B_{13}\\ B_{21}&B_{22}&B_{23}\\ B_{31}&B_{32}&B_{33} \end{bmatrix}\)，由于 $\mathbf{B}$ 是对称矩阵，实际上只有 6 个独立元素。

定义向量 $\mathbf{b}=[B_{11},B_{12},B_{22},B_{13},B_{23},B_{33}]^T$，对于单应性矩阵的列向量 $\mathbf{h_i}=(h_{i1},h_{i2},h_{i3})^T$，可以得到： \(\mathbf{h_i}^T\mathbf{B}\mathbf{h_j}=\begin{bmatrix} h_{i1}h_{j1}&h_{i1}h_{j2}+h_{i2}h_{j1}&h_{i2}h_{j2}&h_{i1}h_{j3}+h_{i3}h_{j1}&h_{i2}h_{j3}+h_{i3}h_{j2}&h_{i3}h_{j3} \end{bmatrix}\mathbf{b}\) 对于每一个单应性矩阵 $\mathbf{H}$，可以得到两个关于 $\mathbf{b}$ 的线性方程： \(\begin{cases} \mathbf{h_1}^T\mathbf{B}\mathbf{h_2} = 0\\ \mathbf{h_1}^T\mathbf{B}\mathbf{h_1}-\mathbf{h_2}^T\mathbf{B}\mathbf{h_2} = 0 \end{cases}\) 通过拍摄多幅棋盘格图像，得到多个单应性矩阵，从而得到多个关于 $\mathbf{b}$ 的线性方程，写成矩阵形式 $\mathbf{V}\mathbf{b}=\mathbf{0}$。使用最小二乘法求解该线性方程组，得到 $\mathbf{b}$。

已知 \(\mathbf{B}=\mathbf{K}^{-T}\mathbf{K}^{-1}=\begin{bmatrix} B_{11}&B_{12}&B_{13}\\ B_{21}&B_{22}&B_{23}\\ B_{31}&B_{32}&B_{33} \end{bmatrix}\)，相机内参矩阵 \(\mathbf{K}=\begin{bmatrix} \alpha& \gamma&u_0\\ 0&\beta&v_0\\ 0&0&1 \end{bmatrix}\)，则有： \(\mathbf{B}=\begin{bmatrix} \frac{1}{\alpha^2}&-\frac{\gamma}{\alpha^2\beta}&\frac{\gamma v_0 - \alpha^2u_0}{\alpha^2\beta}\\ -\frac{\gamma}{\alpha^2\beta}&\frac{\gamma^2}{\alpha^2\beta^2}+\frac{1}{\beta^2}&-\frac{\gamma(\gamma v_0 - \alpha^2u_0)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2}\\ \frac{\gamma v_0 - \alpha^2u_0}{\alpha^2\beta}&-\frac{\gamma(\gamma v_0 - \alpha^2u_0)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2}&\frac{(\gamma v_0 - \alpha^2u_0)^2}{\alpha^2\beta^2}+\frac{v_0^2}{\beta^2}+1 \end{bmatrix}\) 根据 $\mathbf{b}$ 中的元素，可以推导出相机内参： \(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{\frac{\lambda}{B_{11}}}\) \(\beta=\sqrt{\frac{\lambda B_{11}}{B_{11}B_{22}-B_{12}^2}}\) \(\gamma=-\frac{B_{12}\alpha^2\beta}{\lambda}\) \(u_0=\frac{\gamma v_0}{\beta}-\frac{B_{13}\alpha^2}{\lambda}\)

求解外参矩阵($\mathbf{R}$、$\mathbf{t}$)

1. 求解旋转矩阵 $ R $ 的前两列

根据 $ H = K [r_1 \quad r_2 \quad t] $，可以得到： \(r_1 = K^{-1} h_1, \quad r_2 = K^{-1} h_2\)

由于旋转矩阵 $ R $ 是正交矩阵，其列向量满足单位长度和正交性： \(\|r_1\| = 1, \quad \|r_2\| = 1, \quad r_1^T r_2 = 0\)

因此，需要对 $ r_1 $ 和 $ r_2 $ 进行归一化： \(r_1 = \frac{K^{-1} h_1}{\|K^{-1} h_1\|}, \quad r_2 = \frac{K^{-1} h_2}{\|K^{-1} h_2\|}\)

2. 求解旋转矩阵 $ R $ 的第三列

旋转矩阵 $ R $ 的第三列 $ r_3 $ 可以通过前两列的叉积得到： \(r_3 = r_1 \times r_2\)

因此，完整的旋转矩阵为： \(R = [r_1 \quad r_2 \quad r_3]\)

3. 求解平移向量 $ t $

根据 $ H = K [r_1 \quad r_2 \quad t] $，可以得到： \(t = K^{-1} h_3\)

其中 $ h_3 $ 是单应性矩阵 $ H $ 的第三列。

4. 正交化旋转矩阵 $ R $

由于数值计算误差，得到的 $ R $ 可能不严格满足正交性。因此，需要对 $ R $ 进行正交化处理。常用的方法是通过 SVD 分解：

1. 对 $ R $ 进行 SVD 分解： \(R = U \Sigma V^T\)
2. 将奇异值矩阵 $ \Sigma $ 替换为单位矩阵 $ I $，得到正交化的旋转矩阵： \(R_{\text{orth}} = U I V^T = U V^T\)

求解畸变系数

1. 径向畸变（Radial Distortion）

径向畸变是由于镜头形状的不完美导致的，通常表现为图像中心附近的点向外或向内偏移。径向畸变可以用多项式模型来描述，通常使用二阶或三阶多项式：

\[x_{\text{distorted}} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)\] \[y_{\text{distorted}} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)\]

其中：

• $(x, y)$ 是理想的无畸变图像坐标。
• $(x_{\text{distorted}}, y_{\text{distorted}})$ 是畸变后的图像坐标。
• $r$ 是图像点到图像中心的距离，即 $r = \sqrt{x^2 + y^2}$。
• $k_1, k_2, k_3$ 是径向畸变系数。

2. 切向畸变（Tangential Distortion）

切向畸变是由于镜头制造和安装过程中的不完美导致的，通常表现为图像点沿着切向方向的偏移。切向畸变可以用以下模型来描述：

\[x_{\text{distorted}} = x + [2p_1xy + p_2(r^2 + 2x^2)]\] \[y_{\text{distorted}} = y + [p_1(r^2 + 2y^2) + 2p_2xy]\]

其中：

• $p_1, p_2$ 是切向畸变系数。

3. 综合畸变模型

在实际应用中，径向畸变和切向畸变通常会同时存在，因此可以将两者结合起来，形成一个综合的畸变模型：

\[x_{\text{distorted}} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2p_1xy + p_2(r^2 + 2x^2)\] \[y_{\text{distorted}} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2p_2xy\]

4. 畸变校正

在相机标定过程中，通过估计畸变系数 $k_1, k_2, k_3, p_1, p_2$，可以对图像进行畸变校正，从而得到更准确的图像坐标。张正友标定法忽略了切向畸变，只考虑径向畸变的前两项$k_1$和$k_2$。

• 公式回顾：世界坐标系到像素坐标系转换公式

\[\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\frac{1}{Z_c}\mathbf{K}\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 0\\ 1 \end{bmatrix}=\lambda\mathbf{K}\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}=\lambda\mathbf{H}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\]

• 坐标归一化
  • 将标定板角点的世界坐标通过外参转换到相机坐标系，投影到归一化平面，得到理想坐标 $(x,y,1)$
  • 通过内参矩阵将检测到的图像坐标$(u,v)$转换为归一化畸变坐标$(x,y,1)$

\[\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=K\begin{bmatrix} x\\ y\\ 1 \end{bmatrix}=K\lambda\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\] \[K^{-1} \begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\begin{bmatrix} x\\ y\\ 1 \end{bmatrix}=\lambda\begin{bmatrix} \mathbf{r}_1&\mathbf{r}_2&\mathbf{t} \end{bmatrix}\begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\] \[K^{-1} = \begin{pmatrix} \frac{1}{\alpha} & 0 & -\frac{u_0}{\alpha} \\ 0 & \frac{1}{\beta} & -\frac{v_0}{\beta} \\ 0 & 0 & 1 \end{pmatrix}\]

• 带入模型求解

\[\begin{array}{l} \breve{x}=x+x\left[k_{1}\left(x^{2}+y^{2}\right)+k_{2}\left(x^{2}+y^{2}\right)^{2}\right] \\ \breve{y}=y+y\left[k_{1}\left(x^{2}+y^{2}\right)+k_{2}\left(x^{2}+y^{2}\right)^{2}\right] \end{array}\] \[\begin{array}{l} \breve{u}=u+\left(u-u_{0}\right)\left[k_{1}\left(x^{2}+y^{2}\right)+k_{2}\left(x^{2}+y^{2}\right)^{2}\right] \\ \breve{v}=v+\left(v-v_{0}\right)\left[k_{1}\left(x^{2}+y^{2}\right)+k_{2}\left(x^{2}+y^{2}\right)^{2}\right] \end{array}\] \[\left[\begin{array}{cc} \left(u-u_{0}\right)\left(x^{2}+y^{2}\right) & \left(u-u_{0}\right)\left(x^{2}+y^{2}\right)^{2} \\ \left(v-v_{0}\right)\left(x^{2}+y^{2}\right) & \left(v-v_{0}\right)\left(x^{2}+y^{2}\right)^{2} \end{array}\right]\left[\begin{array}{l} k_{1} \\ k_{2} \end{array}\right]=\left[\begin{array}{l} \breve{u}-u \\ \breve{v}-v \end{array}\right]\] \[\mathbf{D}k=\mathbf{d}\] \[\mathbf{k}=[k_1,k_2]^T=\left(\mathbf{D}^{T} \mathbf{D} \right)^{-1} \mathbf{D}^{T} \mathbf{d}\]

非线性优化

通过最大似然估计（Maximum Likelihood Estimation, MLE）最小化重投影误差，优化相机参数（内参、外参、畸变系数）。目标函数定义为： \(\min_{K, R_i, t_i, k_1, k_2} \sum_{i=1}^{N} \sum_{j=1}^{M} \left\| p_{ij} - \hat{p}(K, R_i, t_i, k_1, k_2, P_j) \right\|^2\) 其中：

• $ N $: 图像数量（不同视角的标定板图像）。
• $ M $: 每幅图像中提取的角点数量。
• $ p_{ij} $: 第 $ i $ 幅图像中第 $ j $ 个角点的观测像素坐标。
• $ \hat{p}(·) $: 根据当前参数投影计算出的理论像素坐标。
• $ K $: 内参矩阵。
• $ R_i, t_i $: 第 $ i $ 幅图像对应的外参（旋转矩阵和平移向量）。
• $ k_1, k_2 $: 径向畸变系数。
• $ P_j $: 标定板上第 $ j $ 个角点的3D世界坐标（已知，通常设为 $ (X_w, Y_w, 0) $）。

优化步骤

1. 初始化参数
   使用线性解法（单应性矩阵分解）得到的 $ K $, $ R_i $, $ t_i $ 作为初始值，畸变系数初始化为 $ k_1 = 0 $, $ k_2 = 0 $。

2. 构造残差方程
   对每个角点 $ (i,j) $，计算理论投影坐标 $ \hat{p} $： \(\hat{p} = \text{Project}(K, R_i, t_i, k_1, k_2, P_j)\) 投影过程包含以下步骤：
   • 坐标变换：世界坐标系 → 相机坐标系
     \(\begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} = R_i \begin{bmatrix} X_w \\ Y_w \\ 0 \end{bmatrix} + t_i\)
   • 归一化坐标：
     \(x_n = \frac{X_c}{Z_c}, \quad y_n = \frac{Y_c}{Z_c}\)
   • 畸变校正（径向畸变模型）：
     \(\begin{cases} x_d = x_n (1 + k_1 r^2 + k_2 r^4) \\ y_d = y_n (1 + k_1 r^2 + k_2 r^4) \end{cases}, \quad r^2 = x_n^2 + y_n^2\)
   • 像素坐标投影：
     \(\hat{p} = \begin{bmatrix} f_x x_d + \gamma y_d + c_x \\ f_y y_d + c_y \end{bmatrix}\)
3. 最小化残差
   使用 Levenberg-Marquardt算法（一种阻尼最小二乘法）迭代优化参数，直至收敛。
   • 雅可比矩阵：计算残差对参数的偏导数（需显式推导或数值差分）。
   • 参数更新：
     \(\Delta \theta = -(J^T J + \lambda I)^{-1} J^T r\) 其中 $ J $ 为雅可比矩阵，$ r $ 为残差向量，$ \lambda $ 为阻尼因子。

总结

1. 所有求解过程都可以消掉$ \lambda $ 。
2. 求解过程多次使用SVD分解计算方程($ Ax=b $ 、$Ax = 0 $ )的最小二乘解。

3. 畸变校正可以在图像坐标系也可以在像素坐标系下完成

   • $ K^{-1}(u,v,1)^T\Rightarrow (x,y,1)^T\Leftarrow\frac{1}{Z_c}[r_1,r_2,t] (X_w,Y_w,1)^T $
   • $ (u,v,1)^T\Leftarrow K(x,y,1)^T\Leftarrow K\frac{1}{Z_c}[r_1,r_2,t] (X_w,Y_w,1)^T $
4. 非线性优化使用Levenberg-Marquardt算法。