三维重建学习(3)：张正友相机标定推导

最新推荐文章于 2024-07-01 17:30:54 发布

hongbin_xu

最新推荐文章于 2024-07-01 17:30:54 发布

阅读量4.4k

点赞数 11

分类专栏：三维重建文章标签：三维重建张正友相机标定

本文链接：https://blog.csdn.net/hongbin_xu/article/details/78965037

版权

三维重建专栏收录该内容

6 篇文章 10 订阅

订阅专栏

前言

前面的几篇博客中介绍了有关相机标定的基础知识（三维重建学习(1)：基础知识：旋转矩阵与旋转向量、三维重建学习(2)：相机标定基础）。这次介绍一个十分经典的单目相机标定方法——张正友标定，并给出数学理论推导。

基本方程

模型

我们首先约定如下表示：
二维点坐标： $m = \begin{bmatrix} u \\ v \end{bmatrix}$ ，三维点坐标： $M = \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}$
将上面的二维点和三维点的坐标表示成齐次形式：
二维点坐标： $\tilde{m} = \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}$ ，，三维点坐标： $\tilde{M} = \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}$

参考前面的博客：三维重建学习(2)：相机标定基础，我们可以建立一个常见的相机小孔成像模型：

s \cdot m ~ = A [R t] M ~

$s \cdot \tilde{m} = A \begin{bmatrix} R & t \end{bmatrix} \tilde{M}$
其中：

A = ⎡ ⎣ ⎢ α 00 c β 0 u 0 v 0 1 ⎤ ⎦ ⎥

$A = \begin{bmatrix} \alpha & c & u_0 \\ 0 & \beta & v_0 \\ 0 & 0 & 1 \end{bmatrix}$

s $s$ 为任意比例因子，

R $R$ 、

t $t$ 都是相机外参，

R $R$ 为旋转矩阵，

t $t$ 对应三个轴的平移。

A $A$ 是相机内参，

(u0,v0) $(u_0, v_0)$ 是坐标的主点，

α $\alpha$ 和

β $\beta$ 是图像在

u $u$ 轴和

v $v$ 轴的比例因子，

c $c$ 是描述两个坐标轴亲倾斜角的参数（注：如果两个坐标轴相互垂直，则

c=0 $c=0$ ，即默认情况下这个

c $c$ 都是为0的）。

模型平面与图像之间的单应性关系

设 $R$ 的第 $i$ 列旋转矩阵为 $r_i$ ，那么 $R$ 可以表示为：

R = [r 1 r 2 r 3]

$R = \begin{bmatrix} r_1 & r_2 & r_3 \end{bmatrix}$
代入原方程中：

s \cdot ⎡ ⎣ ⎢ u v 1 ⎤ ⎦ ⎥ = A [r 1 r 2 r 3 t] ⎡ ⎣ ⎢ ⎢ ⎢ X Y Z 1 ⎤ ⎦ ⎥ ⎥ ⎥

$s \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = A \begin{bmatrix} r_1 & r_2 & r_3 & t \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}$
假设模型平面在世界坐标系中

Z $Z$ 坐标为0，那么

Z $Z$ 坐标的值都为0：

s \cdot ⎡ ⎣ ⎢ u v 1 ⎤ ⎦ ⎥ = A [r 1 r 2 r 3 t] ⎡ ⎣ ⎢ ⎢ ⎢ X Y 01 ⎤ ⎦ ⎥ ⎥ ⎥

$s \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = A \begin{bmatrix} r_1 & r_2 & r_3 & t \end{bmatrix} \begin{bmatrix} X \\ Y \\ 0 \\ 1 \end{bmatrix}$
乘出来都还是0，省略掉这一部分：

s \cdot [u v] = A [r 1 r 2 t] ⎡ ⎣ ⎢ X Y 1 ⎤ ⎦ ⎥

$s \cdot \begin{bmatrix} u \\ v \end{bmatrix} = A \begin{bmatrix} r_1 & r_2 & t \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}$
此时坐标表示也要变换一下：
二维点坐标：

m~=[uv] $\tilde{m} = \begin{bmatrix} u \\ v \end{bmatrix}$ ，，三维点坐标：

M~=⎡⎣⎢XY1⎤⎦⎥ $\tilde{M} = \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}$

因此点 $M$ 和它在图像上的映射点 $m$ 之间的关系可以使用单应矩阵 $H$ 来表示：

s \cdot m ~ = H \cdot M ~

$s \cdot \tilde{m} = H \cdot \tilde{M}$
其中：

H = A [r 1 r 2 t]

$H = A \begin{bmatrix} r_1 & r_2 & t \end{bmatrix}$
很显然，

H $H$ 是一个

3×3 $3 \times 3$ 的矩阵。

A $A$ 对应着内参，

[r1r2t] $\begin{bmatrix} r_1 & r_2 & t \end{bmatrix}$ 对应着外参。

内参的约束条件

令 $H = \begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix}$ ， $h_1$ 、 $h_2$ 、 $h_3$ 都是 $3 \times 1$ 的矩阵，各自对应一列。
则有 $\begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix} = \lambda A \begin{bmatrix} r_1 & r_2 & t \end{bmatrix}$ ，式中 $\lambda$ 是任意的标量。

我们知道旋转矩阵的每一列两两正交，即 $r_1$ 与 $r_2$ 正交。这里不对基础知识赘述，如果有疑问，请查看：旋转矩阵(Rotate Matrix)的性质分析。（吐槽一下：旋转矩阵真的是一个完美的矩阵）
根据 $r_1$ 与 $r_2$ 正交，我们能得到条件：

r T 1 r 2 = 0, ∥ r 1 ∥ = ∥ r 2 ∥ = 1

$r_1^Tr_2 = 0,\|r_1\|=\|r_2\|=1$
上面这个条件先放在这里，后面再用。
由前面公式：

[h1h2h3]=λA[r1r2t] $\begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix} = \lambda A \begin{bmatrix} r_1 & r_2 & t \end{bmatrix}$ 一一对应得到方程组：

⎧ ⎩ ⎨ h 1 = λ A r 1 h 2 = λ A r 2 h 3 = λ A t

$\begin{cases} h_1 = \lambda A r_1 \\ h_2 = \lambda A r_2 \\ h_3 = \lambda A t \end{cases}$
推出：

{r 1 = λ - 1 A - 1 h 1 r 2 = λ - 1 A - 1 h 2

$\begin{cases} r_1 = \lambda^{-1} A^{-1} h_1 \\ r_2 = \lambda^{-1} A^{-1} h_2 \end{cases}$
代入前面的条件：

r T 1 r 2 = 0, ∥ r 1 ∥ = ∥ r 2 ∥ = 1

$r_1^Tr_2 = 0,\|r_1\|=\|r_2\|=1$
得到：（

λ $\lambda$ 是常数）

r T 1 r 2 = λ - T h T 1 A - T A - 1 h 2 λ - 1 = λ - 2 h T 1 A - T A - 1 h 2 = 0

$r_1^Tr_2 = \lambda^{-T} h_1^{T} A^{-T} A^{-1} h_2 \lambda^{-1} = \lambda^{-2} h_1^{T} A^{-T} A^{-1} h_2=0$

{∥ r 1 ∥ 2 = λ - 2 h T 1 A - T A - 1 h 1 = 1 ∥ r 2 ∥ 2 = λ - 2 h T 2 A - T A - 1 h 2 = 1 \Rightarrow λ - 2 h T 1 A - T A - 1 h 1 = λ - 2 h T 2 A - T A - 1 h 2

$\begin{cases} \|r_1\|^2 = \lambda^{-2} h_1^{T} A^{-T} A^{-1} h_1 = 1 \\\|r_2\|^2 = \lambda^{-2} h_2^{T} A^{-T} A^{-1} h_2 = 1 \end{cases} \\ \Rightarrow \lambda^{-2} h_1^{T} A^{-T} A^{-1} h_1 = \lambda^{-2} h_2^{T} A^{-T} A^{-1} h_2$

由上面的式子我们可以发现，对于一个给定的单应性矩阵 $H$ ，对于内参有2个基本的约束条件： $\lambda^{-2} h_1^{T} A^{-T} A^{-1} h_2=0$ 和 $\lambda^{-2} h_1^{T} A^{-T} A^{-1} h_1 = \lambda^{-2} h_2^{T} A^{-T} A^{-1} h_2$ 。
对于 $H$ 矩阵来说，它是一个 $3\times3$ 的矩阵，有9个参数，那么就有8个自由度。对应的外参有6个（注：旋转矩阵 $R$ 有3个，平移向量 $t$ 有3个）。到这一步，我们只能得到内参的约束条件，却没法解出来。

解决相机标定

利用约束条件求出内参矩阵 $A$

好的，还是看一下前面给出的约束条件，注意到中间都有这么一个东西： $A^{-T} A^{-1}$ 。
我们试着将它表示出来：令 $B=A^{-T} A^{-1}$ 。
我们在最前面给出了 $A$ 的定义：

A = ⎡ ⎣ ⎢ α 00 c β 0 u 0 v 0 1 ⎤ ⎦ ⎥

$A = \begin{bmatrix} \alpha & c & u_0 \\ 0 & \beta & v_0 \\ 0 & 0 & 1 \end{bmatrix}$
计算A的逆矩阵，步骤就省略了，很简单。得到：

A = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ 1 α 00 - c α β 1 β 0 v 0 c - u 0 β α β - v 0 β 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥

$A = \begin{bmatrix} \frac{1}{\alpha} &- \frac{c}{\alpha \beta} & \frac{v_0 c - u_0 \beta}{\alpha \beta} \\ 0 & \frac{1}{\beta} & - \frac{v_0}{\beta} \\ 0 & 0 & 1 \end{bmatrix}$
计算出

B $B$ 来：

B = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 α - c α β v 0 c - u 0 β α β 0 1 β - v 0 β 001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ \cdot ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ 1 α 00 - c α β 1 β 0 v 0 c - u 0 β α β - v 0 β 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 α 2 - c α 2 β v 0 c - u 0 β α 2 β - c α 2 β c 2 α 2 β + 1 β 2 - c ( v 0 c - u 0 β ) α 2 β 2 - v 0 β 2 v 0 c - u 0 β α 2 β - c ( v 0 c - u 0 β ) α 2 β 2 - v 0 β 2 ( v 0 c - u 0 β ) 2 α 2 β 2 + v 2 0 β 2 + 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$\begin{align} B &= \begin{bmatrix} \frac{1}{\alpha} & 0 & 0 \\ - \frac{c}{\alpha \beta} & \frac{1}{\beta} & 0 \\ \frac{v_0 c - u_0 \beta}{\alpha \beta} & - \frac{v_0}{\beta} & 1 \end{bmatrix} \cdot \begin{bmatrix} \frac{1}{\alpha} &- \frac{c}{\alpha \beta} & \frac{v_0 c - u_0 \beta}{\alpha \beta} \\ 0 & \frac{1}{\beta} & - \frac{v_0}{\beta} \\ 0 & 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} \frac{1}{\alpha^2} & -\frac{c}{\alpha^2 \beta} & \frac{v_0 c - u_0 \beta}{\alpha^2 \beta} \\ -\frac{c}{\alpha^2 \beta} & \frac{c^2}{\alpha^2 \beta} + \frac{1}{\beta^2} & -\frac{c(v_0 c - u_0 \beta)}{\alpha^2 \beta^2} - \frac{v_0}{\beta^2} \\ \frac{v_0 c - u_0 \beta}{\alpha^2 \beta} & -\frac{c(v_0 c - u_0 \beta)}{\alpha^2 \beta^2} - \frac{v_0}{\beta^2} & \frac{(v_0 c - u_0 \beta)^2}{\alpha^2 \beta^2} + \frac{v_0^2}{\beta^2} + 1 \end{bmatrix} \end{align}$
不难发现

B $B$ 是对称的，我们可以使用6个变量来表示出

B $B$ 。定义一个6维向量：

b = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ B 11 B 12 B 22 B 13 B 23 B 33 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$b = \begin{bmatrix} B_{11} \\ B_{12} \\ B_{22} \\ B_{13} \\ B_{23} \\ B_{33} \end{bmatrix}$

B $B$ 可以表示为：

B = ⎡ ⎣ ⎢ B 11 B 12 B 13 B 12 B 22 B 23 B 13 B 23 B 33 ⎤ ⎦ ⎥

$B = \begin{bmatrix} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{bmatrix}$
与前面表示旋转矩阵时类似，我们假设

H $H$ 的第

i $i$ 列为：

hi=⎡⎣⎢hi1hi2hi3⎤⎦⎥ $h_i = \begin{bmatrix} h_{i1} \\ h_{i2} \\ h_{i3} \end{bmatrix}$ ，那么

H=[h1h2h3] $H = \begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix}$ 。
对于

hTiBhj $h_i^T B h_j$ ，我们把前面的公式代入看看：

h T i B h j = [h i 1 h i 2 h i 3] \cdot ⎡ ⎣ ⎢ B 11 B 12 B 13 B 12 B 22 B 23 B 13 B 23 B 33 ⎤ ⎦ ⎥ \cdot ⎡ ⎣ ⎢ h j 1 h j 2 h j 3 ⎤ ⎦ ⎥ = (h i 1 B 11 + h i 2 B 12 + h i 3 B 13) h j 1 + (h i 1 B 12 + h i 2 B 22 + h i 3 B 23) h j 2 + (h i 3 B 13 + h i 2 B 23 + h i 3 B 33) h j 3 = h i 1 h j 1 B 11 + (h i 2 h j 1 + h i 1 h j 2) B 12 + h i 2 h j 2 B 22 + (h i 3 h j 1 + h i 1 h j 3) B 13 + (h i 3 h j 2 + h i 2 h j 3) B 23 + h i 3 h j 3 B 33

$\begin{align} h_i^T B h_j &= \begin{bmatrix} h_{i1} & h_{i_2} & h_{i3} \end{bmatrix} \cdot \begin{bmatrix} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{bmatrix} \cdot \begin{bmatrix} h_{j1} \\ h_{j2} \\ h_{j3} \end{bmatrix} \\ &= (h_{i1} B_{11} + h_{i2}B_{12} + h_{i3}B_{13}) h_{j1} + (h_{i1} B_{12} + h_{i2} B_{22} + h_{i3} B_{23})h_{j2} + (h_{i3} B_{13} + h_{i2} B_{23} + h_{i3} B_{33}) h_{j3} \\ &= h_{i1} h_{j1} B_{11} + (h_{i2} h_{j1} + h_{i1} h_{j2})B_{12} + h_{i2} h_{j2} B_{22} + (h_{i3} h_{j1} + h_{i1} h_{j3})B_{13} + (h_{i3} h_{j2} + h_{i2} h_{j3})B_{23} + h_{i3} h_{j3} B_{33} \end{align}$
我们前面已经给出了一个6维向量

b $b$ ：

b = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ B 11 B 12 B 22 B 13 B 23 B 33 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$b = \begin{bmatrix} B_{11} \\ B_{12} \\ B_{22} \\ B_{13} \\ B_{23} \\ B_{33} \end{bmatrix}$
那么上面那一堆东西可以表示为：

h T i B h j = v T i j b

$h_i^T B h_j = v_{ij}^T b$
其中：

v i j = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ h i 1 h j 1 (h i 2 h j 1 + h i 1 h j 2) 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 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$v_{ij} = \begin{bmatrix} h_{i1} h_{j1} \\ (h_{i2} h_{j1} + h_{i1} h_{j2}) \\ 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}$
回到我们前面推导出的约束条件：

{λ - 2 h T 1 A - T A - 1 h 2 = 0 λ - 2 h T 1 A - T A - 1 h 1 = λ - 2 h T 2 A - T A - 1 h 2

$\begin{cases} \lambda^{-2} h_1^{T} A^{-T} A^{-1} h_2=0 \\ \lambda^{-2} h_1^{T} A^{-T} A^{-1} h_1 = \lambda^{-2} h_2^{T} A^{-T} A^{-1} h_2 \end{cases}$
这两个约束条件可以改写为齐次形式：

{v T 12 b = 0 (v 11 - v 22) T b = 0

$\begin{cases} v_{12}^T b = 0 \\ (v_{11} - v_{22})^T b = 0 \end{cases}$
我们用一个新的矩阵

V $V$ 来表示这两个式子：

V = [v T 12 (v 11 - v 22) T]

$V = \begin{bmatrix} v_{12}^T \\ (v_{11} - v_{22})^T \end{bmatrix}$
这里的

V $V$ 是一个

2×6 $2 \times 6$ 的矩阵。约束条件变为：

V⋅b=0 $V \cdot b = 0$
如果观察了n张图片，那么可以得到

n $n$ 个方程

V⋅b=0 $V \cdot b = 0$ 。我们想要解出

b $b$ ，

b $b$ 是一个6维向量，要求出唯一解，则至少需要6个方程。一个

V⋅b=0 $V \cdot b = 0$ 有2个约束条件，那么要求出唯一解，至少需要3个

V⋅b=0 $V \cdot b = 0$ ，即至少需要3张图片（

n≥3 $n \geq 3$ ）。
如果我们求出了唯一解

b $b$ ，那么就可以得到

B $B$ ，那就也可以求出相机内参

A $A$ （注：使用cholesky分解）。
下面直接给出结果：（已知

B $B$ ，求解出

A $A$ 中的各个参数）

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ v 0 = (B 12 B 13 - B 11 B 23) / (B 11 B 22 - B 212) λ = B 33 - [B 213 + v 0 (B 12 B 13 - B 11 B 23)] / B 11 α = λ / B 11 - - - - - \sqrt β = λ B 11 / (B 11 B 22 - B 212) - - - - - - - - - - - - - - - - - \sqrt c = - B 12 α 2 β / λ u 0 = c v 0 / α - B 13 α 2 / λ

$\begin{cases} v_0 = (B_{12}B_{13} - B_{11} B_{23}) / (B_{11} B_{22} - B_{12}^2) \\ \lambda = B_{33} - [B_{13}^2 + v_0(B_{12} B_{13} - B_{11} B_{23})] / B_{11} \\ \alpha = \sqrt{ \lambda / B_{11} } \\ \beta = \sqrt{ \lambda B_{11} / (B_{11} B_{22} - B_{12}^2)} \\ c = -B_{12} \alpha^2 \beta / \lambda \\ u_0 = c v_0 / \alpha - B_{13} \alpha^2 / \lambda \end{cases}$

利用内参矩阵 $A$ 求解外参矩阵

通过前面的方法，假设我们已经求到了内参矩阵 $A$ 。
我们使用下面的式子表示点 $M$ 和它在图像上的映射点 $m$ 之间的关系：

s \cdot m ~ = H \cdot M ~

$s \cdot \tilde{m} = H \cdot \tilde{M}$
如果我们已知图片中的点

m $m$ 的坐标，以及点

M $M$ 在三维空间中的坐标，s又只是个常数，那么我们可以求出单应性矩阵

H $H$ 。
对于已知的单应性矩阵

H=[h1h2h3] $H = \begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix}$ ，我们有：

[h 1 h 2 h 3] = λ A [r 1 r 2 t]

$\begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix} = \lambda A \begin{bmatrix} r_1 & r_2 & t \end{bmatrix}$
解出外参：

⎧ ⎩ ⎨ ⎪ ⎪ r 1 = λ - 1 A - 1 h 1 r 2 = λ - 1 A - 1 h 2 t = λ - 1 A - 1 h 3

$\begin{cases} r_1 = \lambda^{-1} A^{-1} h_1 \\ r_2 = \lambda^{-1} A^{-1} h_2 \\ t = \lambda^{-1} A^{-1} h_3 \end{cases}$
由旋转矩阵的性质得到：

{r 3 = r 1 \times r 2 ∥ r 1 ∥ = ∥ r 2 ∥ = 1

$\begin{cases} r_3 = r_1 \times r_2 \\ \|r_1\| = \|r_2\| = 1 \end{cases}$
这些东西整合一下：

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ r 1 = λ - 1 A - 1 h 1 r 2 = λ - 1 A - 1 h 2 r 3 = r 1 \times r 2 t = λ - 1 A - 1 h 3 ∥ λ - 1 A - 1 h 1 ∥ = ∥ λ - 1 A - 1 h 2 ∥ = 1 \Rightarrow λ = ∥ A - 1 h 1 ∥ = ∥ A - 1 h 2 ∥

$\begin{cases} r_1 = \lambda^{-1} A^{-1} h_1 \\ r_2 = \lambda^{-1} A^{-1} h_2 \\ r_3 = r_1 \times r_2 \\ t = \lambda^{-1} A^{-1} h_3 \end{cases} \\ \| \lambda^{-1} A^{-1} h_1 \| = \| \lambda^{-1} A^{-1} h_2 \| = 1 \\ \Rightarrow \lambda = \| A^{-1} h_1 \| = \| A^{-1} h_2 \|$
这样就求出了外参的各项参数。

极大似然估计

普通形式

张正友在论文中提到，前面的这些数学原理和推导并没有太多的物理意义，仅仅是为后面的极大似然优化提供了一个初值。
假设我们得到了模型平面的n幅图片，模型平面上有m个点，假设图像上像素点的噪声服从独立的同一分布，下面给出极大似然优化问题：

\sum i = 1 n \sum j = 1 m ∥ m i j - m^(A, R i, t i, M j) ∥

$\sum_{i=1}^{n} \sum_{j=1}^{m} \| m_{ij} - \hat{m}(A,R_i,t_i,M_j) \|$
其中：

m^(A,Ri,ti,Mj) $\hat{m}(A,R_i,t_i,M_j)$ 表示的是点

Mj $M_j$ 在第

i $i$ 幅图像上的投影，旋转矩阵

R $R$ 使用有三个参数的向量

r $r$ 表示，

r $r$ 平行于旋转轴，模长为旋转角度。这里的

R $R$ 和

r $r$ 关系符合Rodrigue公式（参考前面的博客：三维重建学习(1)：基础知识：旋转矩阵与旋转向量）。
好了，这是一个非线性优化问题，他衡量的是点的实际坐标与估计值的误差，我们期望它尽可能地小。解决这个优化问题有很多种工具，但不是这里的重点，所以不做赘述。只是有一点需要注意，它需要一个

A $A$ 的初值，我们使用前面那一大堆公式导出一个猜测值赋给它。为什么这么大费周章？很简单，因为这比随机初始化初值收敛的快得多，有助于加快算法。

径向畸变的处理

如前面的博客：三维重建学习(2)：相机标定基础中所说，通常畸变有两种：径向畸变、切向畸变。通常切向畸变可以忽略不计，我们只考虑径向畸变。
令 $(u,v)$ 为点在理想的（无畸变的）像素坐标系中的坐标，令 $(\tilde{u}, \tilde{v})$ 为实际观察到的坐标（有畸变）。同样地，令 $(x,y)$ 表示理想的（无畸变）点在图像坐标系中的坐标， $\tilde{x}, \tilde{y}$ 为实际观察到的坐标（有畸变）。
下面给出张正友在论文中给出的公式：

x ~ = x [1 + k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2] y ~ = y [1 + k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2]

$\tilde{x} = x[1 + k_1(x^2 + y^2) + k_2(x^2 + y^2)^2] \\ \tilde{y} = y[1 + k_1(x^2 + y^2) + k_2(x^2 + y^2)^2]$

k1 $k_1$ 、

k2 $k_2$ 是径向畸变参数。
从公式：

u~=u0+αx~+cy~ $\tilde{u} = u_0 + \alpha \tilde{x} + c \tilde{y}$ 、

v~=v0+βy~ $\tilde{v} = v_0 + \beta \tilde{y}$ （注：由内参矩阵的参数导出），得到：

u ~ = u + (u - u 0) [k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2] v ~ = v + (v - v 0) [k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2]

$\tilde{u} = u + (u - u_0)[k_1(x^2 + y^2) + k_2(x^2 + y^2)^2] \\ \tilde{v} = v + (v - v_0)[k_1(x^2 + y^2) + k_2(x^2 + y^2)^2]$
从上面的式子我们可以得到两个方程：

u ~ - u = (u - u 0) [k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2] v ~ - v = (v - v 0) [k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2]

$\tilde{u} - u = (u - u_0)[k_1(x^2 + y^2) + k_2(x^2 + y^2)^2] \\ \tilde{v} - v = (v - v_0)[k_1(x^2 + y^2) + k_2(x^2 + y^2)^2]$
表示成矩阵形式：

[(u - u 0) (x 2 + y 2) (v - v 0) (x 2 + y 2) (u - u 0) (x 2 + y 2) 2 (v - v 0) (x 2 + y 2) 2] \cdot [k 1 k 2] = [u ~ - u v ~ - v]

$\begin{bmatrix} (u - u_0)(x^2 + y^2) & (u - u_0) (x^2 + y^2)^2 \\ (v - v_0)(x^2 + y^2) & (v - v_0) (x^2 + y^2)^2 \end{bmatrix} \cdot \begin{bmatrix} k_1 \\ k_2 \end{bmatrix} = \begin{bmatrix} \tilde{u} - u \\ \tilde{v} - v \end{bmatrix}$

n $n$ 幅图像中各有

m $m$ 个点，迭代所有的方程会得到一个有

2mn $2mn$ 个方程的方程组。
用矩阵表示来表示这个方程组：

D⋅k=d $D \cdot k = d$ ，其中

k=[k1k2] $k = \begin{bmatrix} k_1 \\ k_2 \end{bmatrix}$ 。
使用最小二乘法求解出

k $k$ ，直接套公式：

k = (D T D) - 1 D T d

$k = (D^T D)^{-1} D^T d$
如此求出了畸变参数后，我们可以回到前面的优化问题中再求解其他参数。

完整形式

下面给出完整的极大似然估计优化问题：

\sum i = 1 n \sum j = 1 m ∥ m i j - m^(A, k 1, k 2, R i, t i, M j) ∥

$\sum_{i=1}^{n} \sum_{j=1}^{m} \| m_{ij} - \hat{m}(A,k_1,k_2,R_i,t_i,M_j) \|$
其中：

m^(A,ki,kj,Ri,ti,Mj) $\hat{m}(A,k_i,k_j,R_i,t_i,M_j)$ 表示的是点

Mj $M_j$ 在第

i $i$ 幅图像上的投影。
与前面类似，这里依然回一个非线性优化问题。其中内参矩阵

A $A$ 和

[Rt] $\begin{bmatrix} R & t \end{bmatrix}$ 的初始值选取采用前面的方法求出，

k1 $k_1$ 和

k2 $k_2$ 的选取可以采用上一段的方法。
到这里，整个算法的数学原理已经推导完毕了。
在论文中，张正友还给出了相机标定的程序流程：
这里写图片描述

个人感觉用中文翻译意思上总是会有点偏差，姑且还是给出翻译后的流程：

建议采用如下校准步骤：
1. 打印一个图案，并把它贴到一个平面上；
2. 通过移动平面或者相机，从不同的方向拍摄一些图片；
3. 检测图片中的特征点；
4. 采用3.1节所述的方法，估计5个内参和全部的外参；
5. 使用最小二乘法(13)估计径向畸变系数；
6. 最小化优化问题(14)，改进所有的参数

这里用到的数学公式全部都在前面介绍了，理应很清楚了，不做赘述。下一篇博客再进行程序实现。

参考资料：