SLAM前端：对极几何、三角测量
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）可以求解得到
。
但是由于观测噪声，求解会带来误差，一般用优化进行求解。如下图：