VINS-MONO之三角化算法
今天讲一讲VINs-MONO的三角形法。
VINS的三角化沿袭ORB-SLAM三角化的风格,采用直接线性变化(DLT)。利用同一个特征点在不同两帧下的投影坐标,求出该特征点在世界坐标系下的3D坐标。注意,在VINS的定义里,世界坐标系指的就是窗口中第一帧图像的坐标系。同样的,所有存储的特征点的3D坐标都是相对这一帧来表示的,所以每当需要marginalize掉最后一帧时,都要调用FeatureManager类中的removeBackShift()来把所有的特征点坐标调整一遍,让它们参考倒数第二帧来表示。
在继续阅读之前,请先把《三角形法恢复空间点深度》这篇文章看完。看完后再看下面的代码和解释会更好的帮助你理解。
我们假设以第i帧为参考系的相机位置为 pi p i ,以世界坐标系为参考系的3D点坐标为 pw p w ,从第i帧参考系到世界参考系的旋转矩阵为 Rwi<