一、基本设定

1. 相机系坐标

点 $P_i$ 在相机坐标系下的坐标 $\rho$ 为：
$\boldsymbol{\rho}_{i}=\left[\begin{array}{l}x_{i} \\ y_{i} \\ z_{i}\end{array}\right]$

2. 像素系坐标

相机系坐标转换成像素系齐次坐标：
$\mathbf{q}_{i}=\mathbf{K}_{i} \frac{1}{z_{i}} \boldsymbol{\rho}_{i}$
其中： $q_i$ 为像素系齐次坐标， $K_i$ 为相机内参矩阵。

3. 平面参数

$P$ 所在平面在相机系下的参数：
$\left\{\mathbf{n}_{i}, d_{i}\right\}$
其中： $n_i$ 为平面法向量， $d_i$ 相机系原点到平面距离。

点 $\rho$ 在平面 $\{n,d\}$ 上由以下公式表达：
$\mathbf{n}_{i}^{T} \boldsymbol{\rho}_{i}+d_{i}=0 \tag{1.1}$

二、平面参数：由像素系计算相机系

1. 由像素系坐标计算相机系坐标：

$\boldsymbol{\rho}_i=z_i\mathbf{K}_i^{-1}\mathbf{q}_i \tag{2.2}$

存在未知数深度 $z_i$ ，因此无法由像素系计算出相机系坐标。

2. 加入平面参数

结合(1.1)(2.2)得：

$z_{i} \mathbf{n}_{i}^{T} \mathbf{K}_{i}^{-1} \mathbf{q}_{i}+d_{i}=0 \tag{2.3}$

整理：
$z_{i}=-\frac{d_{i}}{\mathbf{n}_{i}^{T} \mathbf{K}_{i}^{-1} \mathbf{q}_{i}} \tag{2.4}$

结合(2.2)(2.4)得：
$\boldsymbol{\rho}_{i}=-\frac{d_{i}}{\mathbf{n}_{i}^{T} \mathbf{K}_{i}^{-1} \mathbf{q}_{i}} \mathbf{K}^{-1} \mathbf{q}_{i} \tag{2.5}$

可知，加入平面信息 ${n,d}$ 后，可完全由像素坐标还原出相机坐标。

三、单应矩阵：由像素系a计算像素系b

1. 由a系像素计算b系像素

有两个相机系，a 和 b，其下坐标有关系：

$\left[\begin{array}{c}\boldsymbol{\rho}_{b} \\ 1\end{array}\right]=\underbrace{\left[\begin{array}{cc}\mathbf{C}_{b a} & \mathbf{r}_{b}^{a b} \\ \mathbf{0}^{T} & 1\end{array}\right]}_{\mathbf{T}_{b a}}\left[\begin{array}{c}\boldsymbol{\rho}_{a} \\ 1\end{array}\right]$

$\boldsymbol{\rho}_{b}=\mathbf{C}_{b a} \boldsymbol{\rho}_{a}+\mathbf{r}_{b}^{a b}$

其中：
$C_{ba}$ 表示从坐标系b原点运动到坐标系a原点的旋转矩阵；
$r_b^{ab}$ 表示b坐标系下，从b到a的向量。

转到对应像素系，有关系：
$z_{b} \mathbf{K}_{b}^{-1} \mathbf{q}_{b}=z_{a} \mathbf{C}_{b a} \mathbf{K}_{a}^{-1} \mathbf{q}_{a}+\mathbf{r}_{b}^{a b}$

可得到由a系的像素表达出的b系像素：
$\mathbf{q}_{b}=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \mathbf{C}_{b a} \mathbf{K}_{a}^{-1} \mathbf{q}_{a}+\frac{1}{z_{b}} \mathbf{K}_{b} \mathbf{r}_{b}^{b a} \tag{3.1}$

但是存在未知数 $z_a,z_b$ ，所以无法直接通过a系像素得到b系像素。

2. 加入平面参数

结合(2.4)(3.1)

$\begin{align*} \mathbf{q}_{b} &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \mathbf{C}_{b a} \mathbf{K}_{a}^{-1} \mathbf{q}_{a}+\frac{1}{z_{b}} \mathbf{K}_{b} \mathbf{r}_{b}^{b a} \\ &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \left( \mathbf{C}_{b a} \mathbf{K}_{a}^{-1} \mathbf{q}_{a} +\frac{1}{z_{a}} \mathbf{r}_{b}^{b a} \right) \\ &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \left( \mathbf{C}_{b a} \mathbf{K}_{a}^{-1} \mathbf{q}_{a} -\frac{\mathbf{n}_{a}^{T} \mathbf{K}_{a}^{-1} \mathbf{q}_{a}} {d_{a}} \mathbf{r}_{b}^{b a} \right) \\ &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \left( \mathbf{C}_{b a} -\frac{\mathbf{n}_{a}^{T}} {d_{a}} \mathbf{r}_{b}^{b a} \right) \mathbf{K}_{a}^{-1} \mathbf{q}_{a}\\ &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \left( \mathbf{C}_{b a} -\frac{\mathbf{n}_{a}^{T}} {d_{a}} \mathbf{C}_{b a}\mathbf{r}_{a}^{b a} \right) \mathbf{K}_{a}^{-1} \mathbf{q}_{a}\\ &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \mathbf{C}_{b a} \left( 1-\frac{\mathbf{n}_{a}^{T}} {d_{a}}\mathbf{r}_{a}^{b a} \right) \mathbf{K}_{a}^{-1} \mathbf{q}_{a}\\ &=\frac{z_{a}}{z_{b}} \mathbf{K}_{b} \mathbf{C}_{b a} \left( 1+\frac{\mathbf{n}_{a}^{T}} {d_{a}}\mathbf{r}_{a}^{a b} \right) \mathbf{K}_{a}^{-1} \mathbf{q}_{a}\\ &=\mathbf{K}_{b} \mathbf{C}_{b a} \left( 1+\frac{\mathbf{n}_{a}^{T}} {d_{a}}\mathbf{r}_{a}^{a b} \right) \mathbf{K}_{a}^{-1} \mathbf{q}_{a}\\ \end{align*}$

上面推导用到的理论：

$\mathbf{r}_{b}^{a b}=-\mathbf{C}_{b a} \mathbf{r}_{a}^{b a}$
齐次坐标与系数无关，因此可省去 $\frac{z_a}{z_b}$

3. 定义单应矩阵

符号简化：
$\mathbf{q}_{b}=\mathbf{H}_{b a} \mathbf{q}_{a}$

其中 $H_{ba}$ 为 单应矩阵：
$\mathbf{H}_{b a}= \mathbf{K}_{b} \mathbf{C}_{b a}\left(\mathbf{1}+\frac{1}{d_{a}} \mathbf{r}_{a}^{b a} \mathbf{n}_{a}^{T} \right) \mathbf{K}_{a}^{-1} \tag{3.2}$

单应矩阵包含了相机内参矩阵、旋转、平移和平面参数信息。
引入单应矩阵后，可以直接通过a系像素得到b系像素。

四、匹配点计算单应矩阵

(3.2)给出的单应矩阵的定义是通过旋转平移信息计算的，现实中有时不知道旋转平移信息，而知道两张图像中的匹配点，可以由匹配点计算出单应矩阵。

对于图片上的一对匹配点有如下关系：
$\begin{align*} q_2&=Hq_1 \\ \left(\begin{array}{c}u_{2} \\ v_{2} \\ 1\end{array}\right)&=\left(\begin{array}{lll}H_{11} & H_{12} & H_{13} \\ H_{21} & H_{22} & H_{23} \\ H_{31} & H_{32} & H_{33}\end{array}\right)\left(\begin{array}{c}u_{1} \\ v_{1} \\ 1\end{array}\right) \end{align*}$

1. 单应矩阵有8个未知数

因为转换的是齐次坐标，所以单应矩阵H与尺度无关，也即 $aH$ 与 $H$ 的作用是相同的，因此自由度为 8，使用 $H_{33}=1$ 来进行归一化。

$\left( \begin{array}{c}u_{2} \\ v_{2} \\ 1\end{array}\right)=\left(\begin{array}{lll}H_{11} & H_{12} & H_{13} \\ H_{21} & H_{22} & H_{23} \\ H_{31} & H_{32} & 1\end{array}\right)\left(\begin{array}{c}u_{1} \\ v_{1} \\ 1 \end{array}\right)$

故 $H$ 共8个未知数，需要8个方程来解。

2. 一对匹配点提供2个方程

由于是齐次坐标，所以展开是这种形式：
$\left\{\begin{aligned} u_{2} &= \frac{H_{11} u_{1}+H_{12} v_{1}+H_{13}}{H_{31} u_{1}+H_{32} v_{1}+1} \\ v_{2} &= \frac{H_{21} u_{1}+H_{22} v_{1}+H_{23}}{H_{31} u_{1}+H_{32} v_{1}+1} \end{aligned}\right.$

3. 四对匹配点提供8个方程

$\left(\begin{array}{cccccccc}u_{1}^{1} & v_{1}^{1} & 1 & 0 & 0 & 0 & -u_{1}^{1} u_{2}^{1} & -v_{1}^{1} u_{2}^{1} \\ 0 & 0 & 0 & u_{1}^{1} & v_{1}^{1} & 1 & -u_{1}^{1} v_{2}^{1} & -v_{1}^{1} v_{2}^{1} \\ u_{1}^{2} & v_{1}^{2} & 1 & 0 & 0 & 0 & -u_{1}^{2} u_{2}^{2} & -v_{1}^{2} u_{2}^{2} \\ 0 & 0 & 0 & u_{1}^{2} & v_{1}^{2} & 1 & -u_{1}^{2} v_{2}^{2} & -v_{1}^{2} v_{2}^{2} \\ u_{1}^{3} & v_{1}^{3} & 1 & 0 & 0 & 0 & -u_{1}^{3} u_{2}^{3} & -v_{1}^{3} u_{2}^{3} \\ 0 & 0 & 0 & u_{1}^{3} & v_{1}^{3} & 1 & -u_{1}^{3} v_{2}^{3} & -v_{1}^{3} v_{2}^{3} \\ u_{1}^{4} & v_{1}^{4} & 1 & 0 & 0 & 0 & -u_{1}^{4} u_{2}^{4} & -v_{1}^{4} u_{2}^{4} \\ 0 & 0 & 0 & u_{1}^{4} & v_{1}^{4} & 1 & -u_{1}^{4} v_{2}^{4} & -v_{1}^{4} v_{2}^{4}\end{array}\right) \left(\begin{array}{l}H_{11} \\ H_{12} \\ H_{13} \\ H_{21} \\ H_{22} \\ H_{23} \\ H_{31} \\ H_{32}\end{array}\right) = \left(\begin{array}{c}u_{2}^{1} \\ v_{2}^{1} \\ u_{2}^{2} \\ v_{2}^{2} \\ u_{2}^{3} \\ v_{2}^{3} \\ u_{2}^{4} \\ v_{2}^{4}\end{array}\right)$

解此方程组，可得单应矩阵 $H$ 。

参考:

STATE ESTIMATION FOR ROBOTICS
视觉slam十四讲
https://www.cnblogs.com/ml-cv/p/5871052.html
https://www.zhihu.com/question/23310855

单应矩阵推导

单应矩阵推导

一、基本设定

1. 相机系坐标

2. 像素系坐标

3. 平面参数

二、平面参数：由像素系计算相机系

1. 由像素系坐标计算相机系坐标：

2. 加入平面参数

三、单应矩阵：由像素系a计算像素系b

1. 由a系像素计算b系像素

2. 加入平面参数

3. 定义单应矩阵

四、匹配点计算单应矩阵

1. 单应矩阵有8个未知数

2. 一对匹配点提供2个方程

3. 四对匹配点提供8个方程