ORB_SLAM2 论文与代码总结

ORB_SLAM论文与代码阅读总结

论文链接：ORB_SLAM Paper

论文关键点笔记

1. Main thread is for tracking, the `System` make 3 more threads for local mapping, loop-closing and viewing.
2. In TRACKING, a local visible map is retrieved using the covisibility graph of keyframes that is maintained by the system，local visible map is used for camera pose refinement;  This local map contains the set of keyframes K1, that share map points with the current frame, and a set K2 with neighbors to the keyframes K1 in the covisibility graph. TRACKING decides inserting keyframes, conditions: >20 frames from relocalization or last keyframe; track features > 50, or <90% of points of reference keyframe.
3. New correspondences for unmatched ORB in the new keyframe are searched in connected keyframes in the covisibility graph to triangulate new points, local mapping controls to delete keypoints and keyframes.
4. 帧的全部特征点包括没有对应地图点的，都被保存下来，帧中特征点与地图点的vector 元素怎么匹配，一一对应，没有地图点的，相应位置为空指针。
5. LOOP-CLOSING，both sides of the loop are aligned and duplicated points are merged.
6. Covisibility Gaph is weighted graph. First build a spanning tree from 1st keyframe, added keyframe is linked to whose common observation is most; Essential graph contain same tree, with edges which has > 100 edges and loop closure edges. Spanning-tree 与Covisibility Graph比较，Spanning-tree是主干，一个关键帧将与自己共有的同名地图点最多的关键帧当做自己的父母节点；Essential Graph是从CG中提取的，没有实际的成员变量。
7. Use BoW to match features and loop closing. 
8. ORB pairs are triangulated, and to accept the new points, positive depth in both cameras, parallax, reprojection error and scale consistency are checked.
9. matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches)
10. 2 view initialize，如何从单应矩阵分解结果选取正解，参考方法VS重建方法
11. 地图初始化： 1. new 地图点，初始两帧中增加地图点，地图点记录对应帧指针和index，注意一个地图点的描述子，是从所有相关帧的描述子中统计的
12. Tracking采用匀速模型预测，获取3D-2D匹配数据，首先将3D点重投影到当前帧，利用距离阈值选候选2D特征点，再利用描述子进行匹配，取描述子距离最小，并利用描述子方向的全局一致性检验，优化位姿
13. 在前一步的基础上，扩大3D-2D匹配，筛选条件：a重投影点的图像范围约束，b当前帧的视线方向与地图点方位夹角余弦<cos \pie/3 c相机距离地图点范围，在最小值和最大值之间，最小值是按最大值一定比例确定的，第二次优化位姿.

运行命令

./ORB_SLAM2/Examples/Monocular/mono_tum /home/<your computer>/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/<your computer>/ORB_SLAM2/Examples/Monocular/TUM1.yaml /home/<your computer>/ORB_SLAM2/Examples/Monocular/rgbd_dataset_freiburg1_xyz

代码框架

ORB_SLAM：基于稀疏特征点的间接法V-SLAM
**ORB_SLAM由三个线程实现，

1. Tracking，负责估计每帧的相机位姿，判断关键帧；
2. Mapping，实现Bundle Adjustment，对新增加的关键点搜索关键点并重构，筛选并删除部分关键点。
3. Loop-closing，对每个关键帧实施闭环检测，若发现，进行环首尾的对准；进行pose的图优化。优化中利用了提出的Essential Graph概念。
   ##数据结构
4. 特征点与关键帧
5. Covisibility Graph and Essential Graph

程序主要变量

以kitti_mono为例

ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

System在主循环调用TrackMonocular(…)

class System {
  //Main function
  cv::Mat& TrackMonocular(const cv::Mat& image, const double time_stamp);  
}

在TrackMonocular(…)函数中调用Tracking类指针的GrabImageMonocular(…)

class Tracking {
  //main function
  cv::Mat& GrabImageMonocular(const cv::Mat& image, const double time_stamp);
}

代码实现细节

We perform 4 optimizations, after each optimization we classify observation as inlier/outlier At the next optimization, outliers are not included, but at the end they can be classified as inliers again.

• 运动模型预测当前帧Pose

// Update motion model
if (!mLastFrame.mTcw.empty()) {
cv::Mat LastTwc = cv::Mat::eye(4, 4, CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0, 3).colRange(0, 3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0, 3).col(3));
mVelocity = mCurrentFrame.mTcw * LastTwc;
}

// Delete temporal MapPoints
for (list<MapPoint *>::iterator lit = mlpTemporalPoints.begin(),
lend = mlpTemporalPoints.end();
lit != lend; lit++) {
  MapPoint *pMP = *lit;
  delete pMP;
}

• 数据抽象

//spanning tree for graph
class KeyFrame{
  std::vector<KeyFrame*> children;
  KeyFrame* parent;
}
class Map{
  std::set<MapPoint*>;
  std::set<KeyFrame*>;
  std::vector<MapPoint*>reference_map_points_;
}

• 指针作为初始化标志

if(!mpInitializer){
  Initial_frame = Frame();
  MpInitializer = new Initializer(); 
}

• 双目配置时，新增关键帧，地图点只选择景深较近的点
• 初始化匹配，将图像分为网格提取特征点，以第一帧图像特征点坐标为中心，在第二帧上搜索候选特征点，并利用描述子匹配取最高匹配，再统计候选匹配点的描述子方向夹角直方图，保留样本较多的前三个区间上的匹配。
• 利用射极约束+RANSAC实现匹配，代替描述子匹配或者，结合描述子做匹配；ORBSLAM采用描述子方向夹角过大的剔除。

const float a2 = f11 * u1 + f12 * v1 + f13;
const float b2 = f21 * u1 + f22 * v1 + f23;
const float c2 = f31 * u1 + f32 * v1 + f33;
const float num2 = a2 * u2 + b2 * v2 + c2;
const float squareDist1 = num2 * num2 / (a2 * a2 + b2 * b2);

无穷远点的重建数值结果深度可能为负，据此筛除。
创建初始地图，地图点与帧互联，要更新关键帧关系，该帧与另一帧建立联系的标准：两帧共有的地图点个数大于某阈值。

• 优化接口
  void Optimizer::GlobalBundleAdjustment