极简版VO

https://github.com/avisingh599/mono-vo

主要过程：

1. 通过FAST提取特征

FAST(img_1, keypoints_1, fast_threshold, nonmaxSuppression);

2. 利用稀疏光流，追踪到相邻帧的对应特征点

calcOpticalFlowPyrLK(img_1, img_2, points1, points2, status, err, winSize, 3, termcrit, 0, 0.001);

3. 利用特征点对恢复本质矩阵

E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask);

4. 利用本质矩阵恢复R、t

recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask);

5. 结合scale，迭代计算相对起始帧的R_f、t_f，输出最终轨迹t_f

t_f = t_f + scale*(R_f*t);

 R_f = R_f*R;