深入理解张正友相机标定法：数学理论详细推导

最新推荐文章于 2024-05-29 16:04:09 发布

自动驾驶小学生

最新推荐文章于 2024-05-29 16:04:09 发布

阅读量2.9k

点赞数 7

分类专栏：论文笔记

本文链接：https://blog.csdn.net/cg129054036/article/details/106292009

版权

论文笔记专栏收录该内容

68 篇文章 161 订阅

订阅专栏

最近在项目中需要在激光雷达（Lidar）和相机（Camera）之间进行标定，即需要标定出相机内参和外参，使用的标定方法是张正友标定法，这里给出其数学理论推导过程。

论文原文：《A Flexible New Technique for Camera Calibration》

论文翻译：https://blog.csdn.net/humanking7/article/details/46689293

参考资料：https://blog.csdn.net/hongbin_xu/article/details/78965037

1.基本方程

2.标定平面到图像平面的单应性(Homography)

3.内参约束条件

4. 求相机标定参数

1.基本方程

这里参考原论文的符号规定，2维点表示为： $\mathbf{m}=[u, v]^{T}$ ，三维点表示为： $M=[X, Y, Z]^{T}$ 。

对应的齐次表示为： $\tilde{\mathbf{m}}=[u, v, 1]^{T}$ ， $\widetilde{\mathrm{M}}=[X, Y, Z, 1]^{T}$ 。则三维点和二维点之间的关系可以写为： $s\tilde{\mathbf{m}}=\mathbf{A}[\mathbf{R} \quad \mathbf{t}] \tilde{\mathbf{M}}$ 。

其中：

$\mathbf{A}=\left[\begin{array}{lll} \alpha & \gamma & u_{0} \\ 0 & \beta & v_{0} \\ 0 & 0 & 1 \end{array}\right]$

$s$ 为任意比例因子， $R,t$ 都是相机外参，用于将外界坐标通过旋转和平移操作转换为相机坐标。 $R$ 为旋转矩阵， $t$ 为平移向量。A是相机内参， $(u_0,v_0)$ 是坐标的主点， $\alpha$ 和 $\beta$ 是图像在 $u$ 轴和 $v$ 轴的比例因子， $\gamma$ 是描述两个坐标轴亲倾斜角的参数（如果两个坐标轴相互垂直，则 $\gamma$ =0，默认 $\gamma$ 为0）。

2.标定平面到图像平面的单应性(Homography)

设 $R$ 的第 $i$ 列旋转矩阵为 $r_i$ ，那么 $R$ 可以表示为： $R=\left[\begin{array}{lll} r_{1} & r_{2} & r_{3} \end{array}\right]$

代入原方程中：

$s\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]=\mathbf{A}\left[\begin{array}{llll} \mathbf{r}_{1} & \mathbf{r}_{2} & \mathbf{r}_{3} & \mathbf{t} \end{array}\right]\left[\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right]$

不失一般性，假设标定平面在世界坐标系中Z坐标为0，则方程简化为：

$\begin{aligned} s\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right] &=\mathbf{A}\left[\begin{array}{llll} \mathbf{r}_{1} & \mathbf{r}_{2} & \mathbf{r}_{3} & \mathbf{t} \end{array}\right]\left[\begin{array}{l} X \\ Y \\ 0 \\ 1 \end{array}\right] =\mathbf{A}\left[\mathbf{r}_{1} \quad \mathbf{r}_{2} \quad \mathbf{t}\right]\left[\begin{array}{l} X \\ Y \\ 1 \end{array}\right] \end{aligned}$

同理，齐次坐标此时为： $\widetilde{\mathrm{M}}=[X, Y, 1]^{T}$ 。 $\left[\begin{array}{lll} \mathbf{h}_{1} & \mathbf{h}_{2} & \mathbf{h}_{3} \end{array}\right]=\lambda \mathbf{A}\left[\begin{array}{lll} \mathbf{r}_{1} & \mathbf{r}_{2} & \mathbf{t} \end{array}\right]$

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

$s\tilde{\mathbf{m}}=\mathbf{H} \tilde{\mathbf{M}} \quad \text { with } \quad \mathbf{H}=\mathbf{A}\left[\mathbf{r}_{1} \quad \mathbf{r}_{2} \quad \mathbf{t}\right]$
很显然， $H$ 是一个维度为3×3的矩阵。其中 $A$ 为内参矩阵， $\left[\begin{array}{lll} \mathbf{r}_{1} & \mathbf{r}_{2} & \mathbf{t} \end{array}\right]$ 为外参矩阵。

3.内参约束条件

令 $\mathbf{H}=\left[\begin{array}{lll} \mathbf{h}_{1} & \mathbf{h}_{2} & \mathbf{h}_{3} \end{array}\right]$ ，则有：

$\left[\begin{array}{lll} \mathbf{h}_{1} & \mathbf{h}_{2} & \mathbf{h}_{3} \end{array}\right]=\lambda \mathbf{A}\left[\begin{array}{lll} \mathbf{r}_{1} & \mathbf{r}_{2} & \mathbf{t} \end{array}\right]$

其中中λ是任意的标量。

因为旋转矩阵的每一列两两正交，即 $r_1$ 与 $r_2$ 正交，可以得到两个约束条件：

$\begin{array}{l} r_{1}^{T} r_{2}=0 \\ r_{1}^{T} r_{1}=r_{2}^{T} r_{2}=1 \end{array}$

从前面的公式可的方程组：

${\begin{array}{l} h_{1}=\lambda A r_{1} \\ h_{2}=\lambda A r_{2} \\ h_{3}=\lambda A t \end{array}}$

可以推导出：

$\begin{array}{l} r_{1}=\lambda^{-1} A^{-1} h_{1} \\ r_{2}=\lambda^{-1} A^{-1} h_{2} \end{array}$

代入可得：

$r_{1}^{T} r_{2}=\lambda^{-T} h_{1}^{T} A^{-T} A^{-1} h_{2} \lambda^{-1}=0$

$\lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{1}=\lambda^{-2} h_{2}^{T} A^{-T} A^{-1} h_{2} =1$

也即：

$\begin{array}{l} \mathbf{h}_{1}^{T} \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}_{2}=0 \\ \mathbf{h}_{1}^{T} \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}_{1}=\mathbf{h}_{2}^{T} \mathbf{A}^{-T} \mathbf{A}^{-1} \mathbf{h}_{2} \end{array}$

对于矩阵 $H$ 来说，它是一个3×3的矩阵，有9个参数，那么就有8个自由度。对应的外参有6个（旋转矩阵 $R$ 有3个，平移向量 $t$ 有3个）。下面我们来求解相机标定参数。

4. 求相机标定参数

为方便讨论，定义 $B$ ：

$\mathbf{B}=\mathbf{A}^{-T} \mathbf{A}^{-1} \equiv\left[\begin{array}{lll} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{array}\right]$

且已知 $A$ ，则 $A^{-1}$ ：

$A^{-1}=\left[\begin{array}{ccc} \frac{1}{\alpha} & -\frac{\gamma}{\alpha \beta} & \frac{t v \gamma-u_{0} \beta}{\alpha \beta} \\ 0 & \frac{1}{\beta} & -\frac{v_{0}}{\beta} \\ 0 & 0 & 1 \end{array}\right]$

则 $B$ 为：

$B=\left[\begin{array}{ccc} \frac{1}{\alpha^{2}} & -\frac{\gamma}{\alpha^{2} \beta} & \frac{v_{0} \gamma-u_{0} \beta}{\alpha^{2} \beta} \\ -\frac{\gamma}{\alpha^{2} \beta} & \frac{\gamma^{2}}{\alpha^{2} \beta^{2}}+\frac{1}{\beta^{2}} & -\frac{\gamma\left(v_{0} \gamma-u_{0} \beta\right)}{\alpha^{2} \beta^{2}}-\frac{w_{0}}{\beta^{2}} \\ \frac{\operatorname{tn} \gamma-u_{0} \beta}{\alpha^{2} \beta} & -\frac{\gamma\left(v_{0} \gamma-u_{0} \beta\right)}{\alpha^{2} \beta^{2}}-\frac{v_{0}}{\beta^{2}} & \frac{\left(\cos \gamma-u_{0} \beta\right)^{2}}{\alpha^{2} \beta^{2}}+\frac{v_{0}^{2}}{\beta^{2}}+1 \end{array}\right]$

不难发现是 $B$ 对称的，我们可以使用6个变量来表示出 $B$ 。定义一个6维向量 $b$ ：

$\mathbf{b}=\left[B_{11}, B_{12}, B_{22}, B_{13}, B_{23}, B_{33}\right]^{T}$

令 $H$ 的第 $i$ 列 $\mathbf{h}_{i}=\left[h_{i 1}, h_{i 2}, h_{i 3}\right]^{T}$ ，对于 $h_{i}^{T} B h_{j}$ ，则有：

$\begin{aligned} h_{i}^{T} B h_{j} &=\left[\begin{array}{lll} h_{i 1} & h_{i 2} & h_{i 3} \end{array}\right] \cdot\left[\begin{array}{lll} B_{11} & B_{12} & B_{13} \\ B_{12} & B_{22} & B_{23} \\ B_{13} & B_{23} & B_{33} \end{array}\right] \cdot\left[\begin{array}{l} h_{j 1} \\ h_{j 2} \\ h_{j 3} \end{array}\right] \\ &=\left(h_{i 1} B_{11}+h_{i 2} B_{12}+h_{i 3} B_{13}\right) h_{j 1}+\left(h_{i 1} B_{12}+h_{i 2} B_{22}+h_{i 3} B_{23}\right) h_{j 2}+\left(h_{i 3} B_{13}+h_{i 2} B_{23}+h_{i 3} B_{33}\right) h_{j 3} \\ &=h_{i 1} h_{j 1} B_{11}+\left(h_{i 2} h_{j 1}+h_{i 1} h_{j 2}\right) B_{12}+h_{i 2} h_{j 2} B_{22}+\left(h_{i 3} h_{j 1}+h_{i 1} h_{j 3}\right) B_{13}+\left(h_{i 3} h_{j 2}+h_{i 2} h_{j 3}\right) B_{23}+h_{i 3} h_{j 3} B_{33} \end{aligned}$

代入 $b$ ，则上式简化为：

$\mathbf{h}_{i}^{T} \mathbf{B h}_{j}=\mathbf{v}_{i j}^{T} \mathbf{b}$

其中：

$v_{i j}=\left[\begin{array}{c} h_{i 1} h_{j 1} \\ \left(h_{i 2} h_{j 1}+h_{i 1} h_{j 2}\right) \\ h_{i 2} h_{j 2} \\ \left(h_{i 3} h_{j 1}+h_{i 1} h_{j 3}\right) \\ \left(h_{i 3} h_{j 2}+h_{i 2} h_{j 3}\right) \\ h_{i 3} h_{j 3} \end{array}\right]$

回到我们前面推导出的约束条件：

$r_{1}^{T} r_{2}=\lambda^{-T} h_{1}^{T} A^{-T} A^{-1} h_{2} \lambda^{-1}=0$

$\lambda^{-2} h_{1}^{T} A^{-T} A^{-1} h_{1}=\lambda^{-2} h_{2}^{T} A^{-T} A^{-1} h_{2} =1$

这两个约束条件可以改写为齐次形式：

$\begin{array}{l} v_{12}^{T} b=0 \\ \left(v_{11}-v_{22}\right)^{T} b=0 \end{array}$

用一个新的矩阵 $\mathbf{V}$ 来表示这两个式子：

$\left[\begin{array}{c} \mathbf{v}_{12}^{T} \\ \left(\mathbf{v}_{11}-\mathbf{v}_{22}\right)^{T} \end{array}\right] \mathbf{b}=$$ \mathbf{V} \mathbf{b} $$=\mathbf{0}$

如果观察了n张图片，那么可以得到n个方程V⋅b=0。我们想要解出b，b是一个6维向量，要求出唯一解，则至少需要6个方程。一个V⋅b=0有2个约束条件，那么要求出唯一解，至少需要3个V⋅b=0，即至少需要3张图片（n≥3）。

如果求出了唯一解 $b$ ，那么就可以得到 $B$ ，那就也可以求出相机内参 $A$ 。

下面直接给出结果：

$\begin{aligned} v_{0} &=\left(B_{12} B_{13}-B_{11} B_{23}\right) /\left(B_{11} B_{22}-B_{12}^{2}\right) \\ \lambda &=B_{33}-\left[B_{13}^{2}+v_{0}\left(B_{12} B_{13}-B_{11} B_{23}\right)\right] / B_{11} \\ \alpha &=\sqrt{\lambda / B_{11}} \\ \beta &=\sqrt{\lambda B_{11} /\left(B_{11} B_{22}-B_{12}^{2}\right)} \\ \gamma &=-B_{12} \alpha^{2} \beta / \lambda \\ u_{0} &=\gamma v_{0} / \beta-B_{13} \alpha^{2} / \lambda \end{aligned}$

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

从前面的方程已知：

${\begin{array}{l} h_{1}=\lambda A r_{1} \\ h_{2}=\lambda A r_{2} \\ h_{3}=\lambda A t \end{array}}$

可得外参：

$\begin{array}{l} 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{array}$

整理可得：

$\begin{array}{c} \left\|\lambda^{-1} A^{-1} h_{1}\right\|=\left\|\lambda^{-1} A^{-1} h_{2}\right\|=1 \\ \Rightarrow \lambda=\left\|A^{-1} h_{1}\right\|=\left\|A^{-1} h_{2}\right\| \end{array}$