VINS-MONO之三角化算法

VINS-MONO之三角化算法

今天讲一讲VINs-MONO的三角形法。

VINS的三角化沿袭ORB-SLAM三角化的风格，采用直接线性变化（DLT）。利用同一个特征点在不同两帧下的投影坐标，求出该特征点在世界坐标系下的3D坐标。注意，在VINS的定义里，世界坐标系指的就是窗口中第一帧图像的坐标系。同样的，所有存储的特征点的3D坐标都是相对这一帧来表示的，所以每当需要marginalize掉最后一帧时，都要调用FeatureManager类中的removeBackShift()来把所有的特征点坐标调整一遍，让它们参考倒数第二帧来表示。

在继续阅读之前，请先把《三角形法恢复空间点深度》这篇文章看完。看完后再看下面的代码和解释会更好的帮助你理解。

我们假设以第i帧为参考系的相机位置为 pi p i ，以世界坐标系为参考系的3D点坐标为 pw p w ，从第i帧参考系到世界参考系的旋转矩阵为 Rwi R i w ，在世界坐标系中表示的，第i帧相机的相对位移(translation)为 twi t i w 。在VINS的系统里，我们已知：
- fi f i ： 特征点表示在i帧中的3D坐标 (x,y,1) ( x , y , 1 )
- Rs[i] R s [ i ] ：从第i帧的imu参考系旋转到世界参考系的旋转矩阵 Rwi R i w
- Ps[i] P s [ i ] ：第i帧在世界坐标系下的位移 twi t i w
- tic[0] t i c [ 0 ] ：imu参考系下第1个相机的位移 timui t i i m u

有了这些，我们先要转换成DLT需要的信息：

• 两帧之间的相对旋转和位移（从第i帧到第i+1帧），也就是代码中的 R R 和t t 。由于R和t是第i+1到第i帧的旋转和位移，因此需要进行一次处理，再填入矩阵$P$中。这一操作这对应代码的前六行。

在这之后，我们参考上文中的公式，即可求得我们想要的，相对于世界坐标下的特征点3D姿态。下面附上代码
“` cpp
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
Eigen::Vector3d t = R0.transpose() * (t1 - t0);
Eigen::Matrix3d R = R0.transpose() * R1;
Eigen::Matrix