多视图几何

介绍

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

2D-2D模型

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

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

                                                                    (1)

                                                            (2)

图1 2D-2D模型

        这里需要引入一个概念叫尺度意义上相等，比如齐次像素坐标,于就相差了一个尺度,我们称和在尺度意义上相等，记作。令,。和代表空间点在相机归一化平面上的坐标。带入公式(1) 和式(2)得：

                                                                (3)

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

                                                                (4)

然后在左右两边同乘，得到：

                                                          (5)

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

                                                              (6)

                                                     (7)

这两个式子都称为对极约束。令, ，这两个式子可以简化成如下形式：

,                                                          

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

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

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

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

        那么怎么通过匹配点求解本质矩阵呢？本质矩阵，是一个3X3的矩阵，具有以下性质：

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

2、可以证明，E的奇异值必定是的形式，这称为本质矩阵的内在性质。

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

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

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

                                                                 (8)

整理得到：

                                                                   (9)

 

                                                                     

                                                                     

                                                                     

所以得到：

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

讨论：

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

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

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

三角测量

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

                                                       (10)

左右两边同时乘以:

                                           (11)

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

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

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的长度，利用像素坐标可以求解夹角, , 的大小以及边Oa,Ob,Oc的大小。根据这些信息，利用余弦定理可以求得边OA，OB，OC的长度。然后根据夹角就可以求出点A，B，C在当前相机坐标系下的坐标值。然后就将3D-2D问题转换成了3D-3D问题，利用ICP求解相机位姿。

图2 P3P

EPnP

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

图3 控制点关系

 

        可以从世界坐标系中选取n个已知地图点，为参考点，而从世界坐标系中选取4个已知地图点，为控制点。也就是说控制点只有4个，是选好就固定了。而参考点可以有很多个。而每个参考点都通过一套加权参数，与4个固定的控制点，基于加权和的方式联系。

最小化重投影误差求解PnP

        还可以将PnP看成是优化问题进行求解。如图3所示，已知点P在世界坐标系的坐标为，在图像上的投影坐标为。假设相机位姿为T， 则根据T可以将点P投影到图像上，因为相机位姿T有误差，导致将点P投影到了其他位置，观测点与预测点之间会产生误差。如果有很多个点对，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

                                          (12)

要求解T使得上式最小需要知道误差e关于位姿T的导数。利用李群与李代数求导可以很方便的得到导数。具体的求导过程这里暂不讨论。

图3 重投影误差示意图

3D-3D模型

         假设我们有一组配对好的3D点（例如我门对两幅RGBD图像进行了匹配）：

现在。想要一个欧式变换R，t，使得：

这个问题可以用迭代最近点（Iterative Closest Point, ICP）求解。构建目标函数：

利用非线性优化不断迭代就可以求得最小值。