多视图几何

ros漫步

已于 2023-01-04 09:50:42 修改

阅读量1k

点赞数

文章标签：人工智能计算机视觉算法

于 2022-12-27 17:10:46 首次发布

本文链接：https://blog.csdn.net/qq_34493401/article/details/128445338

版权

介绍

在视觉slam中，相机位姿估计是重要的环节，其中使用的就是多视图几何模型，本文将简要介绍各种多视图几何模型。

2D-2D模型

已知在不同视角下拍摄的2张图像，则可利用这2张图像的匹配点对求解地图点以及相机位姿这些位置量。这个模型中，给定的已知条件是从一张2D图像到另一张2D图像的匹配信息，所以称为2D-2D模型。比如，在单目slam初始化新地图点时就需要用到这种模型。

如图1所示，假设空间中有一个点P，首先相机在 $O_1$ 处观测到点P，其对应的像素点为 $p_1$ ，然后相机在 $O_2$ 处也观测到点P，其对应的像素点为 $p_2$ 。相机从 $O_1$ 变换到 $O_2$ 的旋转部分用R表示，平移部分用t表示。那么根据针孔相机模型，可以得到下面公式：

$s_1 p_1 = K P$ (1)

$s_2 p _2 =K(RP+t)$ (2)

这里需要引入一个概念叫尺度意义上相等，比如齐次像素坐标 $p_1=[u_1, v_1, 1]^T$ , $s_1 p_1$ 于 $p_1$ 就相差了一个尺度 $s_1$ ,我们称 $s_1 p_1$ 和 $p_1$ 在尺度意义上相等，记作 $s_1 p_1 \simeq p_1$ 。令 $x_1 = K^{-1} p_1$ , $x_2 = K^{-1} p_2$ 。 $x_1$ 和 $x_2$ 代表空间点在相机归一化平面上的坐标。带入公式(1) 和式(2)得：

$x_2 \simeq Rx_1+t$ (3)

在公式(3）左右两边同时左乘 $t \ \hat{}$ ，因为 $\hat{}$ 可以看成叉乘符号，自己和自己叉乘等于0，所以式（3）可以变成：

$t\ \hat{} x_2 \simeq t\ \hat{} R x_1$ (4)

然后在左右两边同乘 $x_2^T$ ，得到：

$x_2^T t \hat{} x_2 \simeq x_2^T t \hat{} R x_1$ (5)

因为 $t \hat{} x_2$ 是一个垂直于 $x_2$ 的向量，该向量再于 $x_2$ 做内积等于零，所以式（5）左边恒等于0，因此可知式（5）右边也恒等于0.

$x_2^T t \hat{}R x_1 = 0$ (6)

$p_2^T K^{-T} t \hat{} R K^{-1} p_1 = 0$ (7)

这两个式子都称为对极约束。令 $E = t\hat{} R$ , $F = K^{-T} t\hat{ } R K^{-1}$ ，这两个式子可以简化成如下形式：

$x_2^{T} E x_1 = 0$ , $p_2^{T} F p_1 = 0$

其中E称为本质矩阵，F称为基础矩阵。

在2D-2D模型中，估计相机位姿的过程如下：

1、通过匹配点对计算本质矩阵或者基础矩阵。

2、对本质矩阵或基础矩阵分解得到位姿。

那么怎么通过匹配点求解本质矩阵呢？本质矩阵 $E = t\hat{} R$ ，是一个3X3的矩阵，具有以下性质：

1、E乘以任意非零常数后，对极约束依然满足，这称为E在不同尺度下是等价的。

2、可以证明，E的奇异值必定是 $[\sigma, \sigma, 0] ^ T$ 的形式，这称为本质矩阵的内在性质。

3、平移和旋转各有3个自由度，故E有6个自由度，但由于尺度等价性，E实际上只有5个自由度。

因为E只有5个自由度，所以理论上最少只需要5对匹配点就可以求解E，但是，E的内在性质是一种飞线性性质，在估计时会带来麻烦，一般采用八点法求解E。求解出本质矩阵E后，通过奇异值分解，可以得到旋转矩阵R和平移向量t。因为基础矩阵F和本质矩阵E之间只相差了内参矩阵K，因此求解基础矩阵或者本质矩阵是一样的。

当特征点位于一个平面时，我们可以得到另一个很重要的矩阵，单应矩阵H。假设点P位于平面，该平面方程满足：

$n^T P + d = 0$ (8)

整理得到：

$- \frac{n^TP}{d} =1$ (9)

$p_2 \simeq K(RP+t)$

$\simeq K\left( RP + t \left( -\frac{n^TP}{d} \right ) \right )$

$\simeq K \left( R-\frac{n^Tt}{d} \right )P$

$\simeq K\left( R-\frac{n^Tt}{d} \right ) K^{-1}p_1$

所以得到： $p_2 \simeq H p_1$

最少需要4对点就可以求解单应矩阵。然后对单应矩阵分解得到旋转矩阵R和平移向量t。

讨论：

1、因为E具有尺度等价性，因此分解得到的t和R也具有尺度等价性，导致t的尺度不确定，也就是说得到的t的数值可以是米，厘米，毫米等长度单位。

2、纯旋转时，t等于0，E恒等于零，此时无法通过本质矩阵分解得到t和R。

3、对于多于八对点的情况，一般求解最小二乘解。

三角测量

估计了相机位姿后，可以通过三角测量得到特征点的空间位置。 $x_1$ 和 $x_2$ 是图1中点P在相机坐标系1 和相机坐标系2上的归一化平面上的坐标。所以有下面的关系：

$s_2 x_2 = s_1 R x_1 +t$ (10)

左右两边同时乘以 $x_2 \hat{}$ :

$s_2 x_2 \hat{} x_s = 0 = s_1 x_2 \hat{} R x_1 + x_2 \hat{}t$ (11)

该式左侧等于零，右侧是 $s_1$ 的方程，可以直接求得 $s_1$ ，有了 $s_1$ ， $s_2$ 很容易求出。工程实际中是求解最小二乘解。

三角测量的矛盾：当相机平移过小时，测得的深度值不确定性很大，当相机平移过大时，有可能图像变化过大，无法得到匹配的特征点，导致三角化失败。

3D-2D模型

3D-2D模型已知n个3D点的世界坐标，以及在图像中的投影点坐标，估计相机位姿。采用的方法称为PnP（perspective-n-Point）。PnP最少采用3对点即可求解相机位姿。PnP不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是一种最重要的姿态估计方法。

P3P

P3P只使用3对不共线的点，对数据要求较少。如图2所示，已知三个空间点A，B，C在世界坐标系下的坐标，以及该三点在相机成像平面的像素坐标a,b,c。由A,B,C的世界坐标可以计算得到边AB， BC， AC的长度，利用像素坐标可以求解夹角 $\angle aOb$ , $\angle bOc$ , $\angle aOc$ 的大小以及边Oa,Ob,Oc的大小。根据这些信息，利用余弦定理可以求得边OA，OB，OC的长度。然后根据夹角就可以求出点A，B，C在当前相机坐标系下的坐标值。然后就将3D-2D问题转换成了3D-3D问题，利用ICP求解相机位姿。

EPnP

相比于P3P，EPnP算法在求解PnP问题上表现更稳定、更高效。EPnP算法的关键在于从地图点中选出4个控制点 $c_1$ ， $c_2$ ， $c_3$ ， $c_4$ 以及1个对应的参考点 $p_i$ ，4个控制点与1个参考点通过加权和的方式关联，如图3所示。

可以从世界坐标系中选取n个已知地图点 $p_i$ ， $i=1,2,\cdots ,n$ 为参考点，而从世界坐标系中选取4个已知地图点 $c_j$ ， $j=1,2,3,4$ 为控制点。也就是说控制点只有4个，是选好就固定了。而参考点可以有很多个。而每个参考点 $p_i$ 都通过一套加权参数 $\alpha_{ij}$ ， $j=1,2,3,4$ 与4个固定的控制点 $c_j$ ， $j=1,2,3,4$ 基于加权和的方式联系。先来看在世界坐标系下 $p_i^w$ 与 $c_j^w$ 之间的关系。

$p_i^w=\sum_{j=1}^{4}\alpha_{ij}c_j^w, \quad \sum_{j=1}^{4}\alpha_{ij}=1$ (12)

然后看在相机坐标系下 $p_i^c$ 与 $c_j^c$ 之间的关系，因为只是坐标值改变，点在空间的相对位置保持不变，因此加权关系保持不变。

$p_i^c=\sum_{j=1}^{4}\alpha_{ij}c_j^c,\quad \sum_{j=1}^{4}\alpha_{ij}=1$ (13)

参考点 $p_i$ 可以投影到像素平面。

$w_i\begin{bmatrix} u_i\\ 1 \end{bmatrix}=Kp_i^c=K\sum_{j=1}^{4}\alpha_{ij}c_j^c$

=> $w_i\begin{bmatrix} u_i\\ v_i\\ 1 \end{bmatrix}=\begin{bmatrix} f_u & 0 &u_c \\ 0 & f_v &v_c \\ 0& 0 & 1 \end{bmatrix}\sum_{j=1}^{4}\alpha_{ij}\begin{bmatrix} x_j^c\\ y_j^c\\ z_j^c \end{bmatrix}$

=> $\left\{\begin{matrix} \sum_{j=1}^{4}\left [ \alpha_{ij} f_u x_j^c + \alpha_{ij} \left ( u_c - u_i \right ) z_j^c \right ] = 0\\ \sum_{j=1}^{4} \left [ \alpha_{ij} f_v y_j^c + \alpha_{ij} \left ( v_c - v_i \right ) z_j^c \right ] = 0 \end{matrix}\right.$

写成矩阵形式：

$A_{2\times 12}\cdot H_{12 \times 1} = 0$

其中A是由参考点的加权系数、像素坐标和相机内参组成，H是由4个控制点在相机坐标系下的坐标值组成。一个参考点可以构建一个线性方程组 $A_{2\times12}\cdot H_{12\times1}=0$ ，n个参考点可以构建一个线性方程组 $A_{2n\times12}\cdot H_{12\times1}=0$ 。接下来通过最小二乘和SVD等方法求解方程组，得到控制点在相机坐标系下的坐标，然后根据控制点在世界坐标系下的坐标值，用ICP算法找到这4个控制点在两坐标系间的变换关系这个关系就是 $\left ( R,t \right )$ 。

最小化重投影误差求解PnP

还可以将PnP看成是优化问题进行求解。如图3所示，已知点P在世界坐标系的坐标为 $[X, Y Z]^T$ ，在图像上的投影坐标为 $p_2 = [u, v]^T$ 。假设相机位姿为T，则根据T可以将点P投影到图像上，因为相机位姿T有误差，导致将点P投影到了其他位置 $\hat{p_2}$ ，观测点 $p_2$ 与预测点 $\hat{p_2}$ 之间会产生误差 $e = p_2 - \frac {1}{s_1} K (TP)$ 。如果有很多个点对，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：