SLAM前端：对极几何、三角测量

对极几何在ORBSLAM初始化过程中求解两帧的R、t，输入两帧图像的多对匹配点，对极几何计算得到R、t

三角测量在单目ORBslam初始化过程中计算初始化点云的3D座标，输入两帧的R、t+匹配特征点的像素座标，输出点的3D座标

1 对极几何

对极几何在两帧的图像中，其中包含n个匹配的特征点，用于估计相机的位姿：旋转矩阵R与平移向量t。一般只在单目SLAM初始化过程中使用。

 

情景如图，两幅图分别为相机在两位置的图像，共同观测到P点。可以理解为，相机从位置变换到位置，旋转平移矩阵分别为R、t。现在需要求解R、t。

空间中的一对匹配点如图所示，左图投影点像素座标，右图投影点。以左相机相机座标系为参考座标系有：

                                                                                                                          （1-1）

K为相机内参矩阵，可逆，可得：

                                                                                                                         （1-2）

分别为P点在两相机座标系下的深度，即z座标。

取，则：

                                                                                                                                        （1-3）

两边同时左乘，并且得到：

                                                                                                                                       （1-4）

两边同时左乘得到：

                                                                                                                              （1-5）

与垂直，因此上式左边为0，得到：

                                                                                                                                        （1-6）

将代入可得：

                                                                                                                       （1-7）

上式即为对极约束公式，简洁的表述了两幅图像之间一组匹配点之间的关系。求解出上式即可求解出R、t。

2 三角测量

三角测量利用两帧图像匹配的像素点、以及两帧图像的位姿关系R、t，计算匹配点的3D座标。

已知两帧图像的匹配点P在两帧图像下的像素座标、两帧图像的旋转矩阵R、平移矩阵t。求解匹配点P的3D座标。

由（1-2）（1-3）可以得到：

                                                                                                                                 （1-8）

同时左乘得到：

                                                                                                                              （1-9）

上式可以求解得到，代入式（1-8）可以求解得到。

但是由于观测噪声，求解会带来误差，一般用优化进行求解。如下图：