ORB-SLAM个人总结

ORB-SLAM

此文为个人总结,仅供参考,欢迎批评指正。
本文部分内容来自:泡泡机器人。

Covisilibilty Graph 的顶点是相机的Pose，而边是Pose-Pose的变换关系——所以也算是Pose Graph 一种吧。当两个相机看到相似的空间点时，它们对应的Pose就会产生联系（我们就可以根据这些空间点在照片上的投影计算两个相机间的运动）。根据观测到的空间点的数量，给这个边加上一个权值，度量这个边的可信程度。

Essential Graph 比Covisibility Graph更为简单，ORB-SLAM主要用它来进行全局优化。为了限制优化的规模，ORB-SLAM试图尽量减少优化边的数量。而尽量减少边，又保持连通性的方法，就是做一个最小生成树。

baseline：两个关键帧之间的位移

金字`：为提高效率，判断特征点位置在哪层上面（他就是把相机的视锥分成了八层，比例为1.2，对每个特征点计算它的距离判断在哪层）

---

代码结构：

// 程序中变量名的第一个字母如果为"m"则表示为类中的成员变量，member
// 第一个、第二个字母:
// "p"表示指针数据类型
// "n"表示int类型
// "b"表示bool类型
// "s"表示set类型
// "v"表示vector数据类型
// 'l'表示list数据类型
// "KF"表示KeyPoint数据类型
// "Cov"表示协方差
// "Th"表示阈值
// "Prior"表示先验
// "sigma"表示随机变量的标准差（因此在算协方差时用sigma2）

此处有一个连通性检测对每一个候选帧计算他的相邻帧集合，和当前帧的相邻帧比较，如果三帧以上一致则认为是闭环：

MapPoint的操作：

初始化阶段（Tracking内）：

vector<Match> mvMatches12; ///< Match的数据结构是pair,mvMatches12只记录Reference到Current匹配上的特征点对（它由mvIniMaches筛选得到）在初始化时使用，生成3D点并和特征点匹配CheckRT实现最后存入Tracking的mvIniP3D中。
mvIniP3D的点转化成MapPoint类型存入KeyFream中的mvpMapPoint算是特征点。
mspMapPoint是存在Map中的对应的特征点。
Local Mapping线程可能会将关键帧中某些MapPoints进行替换，由于tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints（在mpReplaced中存储）
TrackReferenceKeyFrame中mCurrentFrame.mvpMapPoints被赋值成当前真和关键真相匹配的特征点
HISTO_LENGTH:用于记录统计直方图
SearchByProjection:将上一真的特征点对应的3D点对应到当前真
mvpLocalMapPoints:存储mvpLocalKeyFrames对应的所有MapPoints
SearchByProjection：将mvpLocalMapPoints投影到当前真（所有3D特征点投影到真中一定是与真中的ORB特征点相对应）

LocalMaping内

ProcessNewKeyFrame：将双目或RGBD跟踪过程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待检查
MapPointCulling:对mlpRecentAddedMapPints进行剔除
CreateNewMapPoints:通过SearchForTriangulation找到对应的2D对应点（没有3D点的），然后各种检测通过收生成新的MapPoint。唯一向map中添加点的方法
SearchInNeighbors:检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoint

LoopClosing内

mvpLoopMapPoints存储vpLoopConnectedKFs（由闭环帧和他的相邻帧组成）中所有的MapPoint通过sim3和当前帧中的进行匹配
CorrectLoop:通过sim3计算矫正后点的位置
mvpCurrentMatchedPoints：LoopClosing中的变量用于存储sim3检测出来的MapPoints，用于SwarchFuse中检测当前帧MapPoints与匹配的MapPoints是否冲突
向Map中添加KeyFrame只有在初始化和LocalMapping的ProcessNewKeyFrame可以

KeyFrame的操作：

初始化阶段（Tracking内）：

CreateInitialMapMonocular: 将初始化的帧添加到map中。在此!!!将MapPoints的中值深度归一化到1，并归一化两帧之间变换。将前两帧存入mpLocalMapper中。并且存入自己的mvpLocalKeyFrames中。
LocalKeyFrames: 策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧；策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧；步骤3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
***CreateNewKeyFrame:***添加关键帧。并插入mpLocalMapper中。

LocalMaping内

***ProcessNewKeyFrame:***将关键帧放入map
***KeyFrameCulling:***剔除关键帧。Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。

LoopClosing

优化

lobalBundleAdjustemnt ：对所有map中的点和帧进行优化。在Track()调用。

 // 3D-2D 最小化重投影 e = (u,v) - project(Tcw*Pw)
 // 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
        g2o::VertexSBAPointXYZ()，MapPoint的mWorldPos
 // 2. Edge:
 //    - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 //        + Vertex：待优化当前帧的Tcw
 //        + Vertex：待优化MapPoint的mWorldPos
 //        + measurement：MapPoint在当前帧中的二维位置(u,v)
 //        + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 //        
 //@param   vpKFs    关键帧 
 //         vpMP     MapPoints
 //         nIterations 迭代次数（20次）
//    pbStopFlag  是否强制暂停
//       nLoopKF  关键帧的个数
//          bRobust  是否使用核函数

PoseOptimization: 对每一个帧都执行一次

  // 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
  // 2. Edge:
 //- g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge
 //    + Vertex：待优化当前帧的Tcw
 //    + measurement：MapPoint在当前帧中的二维位置(u,v)
 //    + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 //- g2o::EdgeStereoSE3ProjectXYZOnlyPose()，BaseUnaryEdge
 //    + Vertex：待优化当前帧的Tcw
//     + measurement：MapPoint在当前帧中的二维位置(ul,v,ur)
//     + InfoMatrix: invSigma2(与特征点所在的尺度有关)

LocalBundleAdjustment: 只在LocalMapping中调用：已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping

//1. Vertex:
 //    - g2o::VertexSE3Expmap()，LocalKeyFrames，即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
 //    - g2o::VertexSE3Expmap()，FixedCameras，即能观测到LocalMapPoints的关键帧（并且不属于LocalKeyFrames）的位姿，在优化中这些关键帧的位姿不变
 //     - g2o::VertexSBAPointXYZ()，LocalMapPoints，即LocalKeyFrames能观测到的所有MapPoints的位置
 //2. Edge:
//     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 //        + Vertex：关键帧的Tcw，MapPoint的Pw
 //         + measurement：MapPoint在关键帧中的二维位置(u,v)
 //        + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 //    - g2o::EdgeStereoSE3ProjectXYZ()，BaseBinaryEdge
 //        + Vertex：关键帧的Tcw，MapPoint的Pw
 //        + measurement：MapPoint在关键帧中的二维位置(ul,v,ur)
 //        + InfoMatrix: invSigma2(与特征点所在的尺度有关)

IMU部分

IMUData

Frame:mvIMUDataSinceLastFrame

在读入图片时根据IMU时间戳分配到每一帧。

KeyFrame:mvIMUData

创建关键帧时Tracking:mvIMUSinceLastKF拿过来就用。

Tracking:mvIMUSinceLastKF

每次来新帧就把他的mvIMUDataSinceLastFrame加进来。

IMUPreInt

KeyFrame:mIMUPreInt

KeyFrame:mvIMUData积分得来。

Frame

没有（Frame只在Tracking阶段使用，之后全是KeyFrame的事）

Tracking:mIMUPreIntInTrack

Tracking:mvIMUSinceLastKF积分得到

NavState

Frame:mNavState

TrackWithIMU时通过上一帧位姿以及IMU预计分信息初始，再通过PoseOptimization进行优化。

KeyFrame:mNavState

KeyFrame初始化时frame传过来，之后全局优化时不断更新。