SLAM位姿估计部分

---

一、坐标系

  世界坐标系经过外参矩阵变换到相机坐标系，相机坐标系投影到归一化平面，经过内参矩阵变换到像素坐标系。在SLAM中，第一帧的相机坐标系就是世界坐标系（VIO要和重力对齐）。

二、对极几何

  对于单目相机来讲，只有2D信息，对于2D-2D之间的位姿关系，通过对极几何求解

  设P在第一帧相机坐标系下的坐标为[X;Y;Z],现在已知P在两帧图像中的像素坐标，假设两帧图像之间的位姿变化为R，t。则满足以下约束关系：



估计位姿步骤：

  通过本质矩阵或者基础矩阵分解出的R,t.其中t是不具备尺度信息的，因此这个估计并不能代表两帧之间的位姿关系。知道R，t后，通过三角测量得到特征点在相机坐标系下的三维坐标。这样就可以获得一对3D-2D点，但是此时的位姿t还是不具备尺度信息，因此还需要PnP求解位姿。

三、PnP

  上面经过三角测量获得深度信息后，有了3D-2D的点对，其中3D是特征点在上一帧相机坐标系下的三维坐标，2D是本帧像素坐标系下的坐标，由于不知道两帧之间的位姿，因此不知道特征点在本帧坐标系下的三维坐标。通过PnP可以求解两帧之间的位姿。PnP的求解方式有很多种；直接线性变换，P3P，非线性优化等。其中P3P是通过相似三角形的方法，求出特征点在本帧相机坐标系下的三维坐标，这样特征点在相邻两帧相机坐标系下的坐标就都知道了，可以通过ICP求解3D-3D点对之间的位姿。而直接线性变换和非线性优化等都直接可以通过3D-2D的对应关系求解两帧之间的位姿，然后根据位姿计算特征点在本帧相机坐标系下的三维坐标，以此类推。