ORB写的好的博客:
https://www.cnblogs.com/wall-e2/p/8057448.html
https://www.cnblogs.com/mafuqiang/p/6932679.html
orb_slam2有注释的代码:https://gitee.com/paopaoslam/ORB-SLAM2
地图初始化:适用于平面场景的单应矩阵H和适用于非平面场景的基础矩阵F,然后根据一些具体的方法来选择一个模型。
从前一帧估计位姿:先利用运动学模型与上一帧匹配,如果数量少,则与关键帧匹配,利用词袋模型加速。
局部地图跟踪:是在上步运行完,当前帧与局部地图能够寻找更多匹配点,匹配所有地图点,然后被优化。
FUCTION2.2.3.1.1:TrackLocalMap
local map意思是当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系,1. 更新局部地图,包括局部关键帧和关键点、2. 对局部MapPoints进行投影匹配、3. 根据匹配对估计当前帧的姿态、4. 根据姿态剔除误匹配。
motion-only BA:有时候意思是计算单帧的重投影误差。
重定位:计算当前帧的词袋向量与之前关键帧的词典数据比较,利用PNP+RANSAC 求出位姿。
正式跟踪的步骤:
1.先判断mVelocity(之前求得的“t-2”,"t-1"两帧之间的位姿变换)速度模型是否计算成功,如果成功就用速度模型,这时获得当前帧的一个初始位姿了,然后当前帧与上一帧进行匹配意思是将上一帧特征点对应3Dmappoints投影到当前帧,其中用到了project加速匹配,用的是3D点的描述子。
2.如果速度模型没有成功,或者成功了但是匹配失败了,则使用TrackReferenceKeyframe(),位姿初值就用上一帧的,通过当前帧与参考关键帧进行匹配(使用了BOW加速),优化每个特征点对应的3D重投影误差得到当前帧的位姿。参考关键帧就选与当前帧共视程度最高的关键帧。
3.重定位:计算与当前帧具有公共单词数的关键帧,计算当前帧与这些关键帧的相似度得到一个候选帧向量,然后对每一帧进行BOW加速匹配,如果匹配点对超过15,则调用SolvePnpRansac,一直迭代直到候选帧数为0。
4.TrackLocalMap(),当前帧能看到的所有地图点为MapPoints,遍历MapPoints,找到同样能观测到它的关键帧和相邻关键帧(相邻的意思是与有共识关系帧的那些最佳共视的关键帧)上述这些关键帧及其Mappoints作为LocalMap。遍历局部地图中的所有地图点(但要去除掉MapPoints,因为已经有了),判断1.其投影过去是否在当前帧的图像范围内,2.计算MapPoint到相机中心的距离是否在尺度范围内,3.计算MapPoint视角与平局场景视角,如果角度小则可以。对满足上述三条的点,通过投影加速(重投影加速中包括,金字塔层级的剔除外点,和方向直方图的剔除外带逆),与当前帧的特征点进行匹配,做只改变当前帧位姿的优化。
5.收尾工作:NeedNewKeyFrame():如下面关键帧判别标准。
地图点的删除:为了保存地图点,必须在创建该点云的前三帧测试通过约束,才能真正被保存,这样才能保证可跟踪且不容易在三角化时出现较大误差。一个点要被加入Map,需要满足下面条件:
(1)这个点要在可预测到能够观察到该点的关键帧中,有超过25%的关键帧能够跟踪到这个点;
(2)如果一个地图点被构建,它必须被超过三个关键帧观察到(在代码中,可以发现如果是单摄像头,这个阈值被设置为2)。
新的地图点的创建:
通过将检测到的ORB特征点,找到Covisibility Graph中与之相连的关键帧Kc,进行特征匹配,然后将匹配到的特征点进行三角化。对于没有匹配上的点,本文又与其他关键帧中未被匹配的特征点进行匹配。匹配方法使用的是之前的方法,并且将不满足对极几何约束的匹配点舍弃。ORB特征点对三角化后,检查正向景深、视差、反投影误差和尺度一致性,这时才得到地图点。一个地图点是通过两个关键帧观察到的,而它也可以投影到与之相连的其他关键帧中,这个时候可以使用Tracking部分的跟踪局部地图来在附近的关键帧中找到匹配。
关键帧剔除标准KeyFrameCulling:
1)被超过3个其他关键帧观测到
遍历该局部关键帧的MapPoints,判断是否90%以上的MapPoints能被其它关键帧(至少3个)观测到,该局部关键帧90%以上的MapPoints能被其它关键帧(至少3个)观测到,则认为是冗余关键帧。
关键帧删除
共视图(covisibility graph):将每个关键帧作为一个节点,若两个关键帧存在一定数量(至少15个)共视地图点则这两个这两个关键帧之间存在一条边,边的权重为共视地图点的数量。
本质图(essential graph):使用位姿图来校正闭环处的累积误差,由于共视图中边的数量较多,我们创建共视图的子图-本质图。本质图包含生成树、共视地图点数量超过100的边和回环的边
生成树(spanning tree):ORB-SLAM系统在第一个关键帧开始就维护一个生成树,每个关键帧只与共视点最多的相邻帧产生边。本质图包含生成树、共视地图点数量超过100的边和回环的边。
局部集束调整
首先计算 KiKi 和 convisibility graph 相连的帧最小相似度 sminsmin,回环检测时在 recognition database 中去掉相似度小于 sminsmin 的帧,并且去掉 convisibility graph 中和当前帧相连的帧。在判断某一帧为回环时,还需要判断连续的 3 帧都为回环(To accept a loop candidate we must detect consecutively three loop candidates that are consistent (keyframes connected in the covisibility graph).)
单目的 SLAM 中,重建的地图在旋转,平移和尺度上都可以有漂移,所以在 close a loop 时,需要计算当前帧和回环帧之间的相似变换。首先计算当前帧和回环帧之间的匹配点,匹配点用 vocabulary tree 搜索,找到的匹配点是 3D-3D 的点,然后用 RANSAC 算法对这这些点计算一个相似变换,对于得到的相似变换如果能有足够的 inliers,则接受回环。
回环的地方,地图会被重建多次,所以在 loop fusion 时,需要把回环两端的地图融合在一起,并且回环两端的 convisibility graph 也需要添加新的边。
首先将计算的相似变换作用到当前帧,并且传播到和当前帧连接的关键帧,通过这样做回环两端的地图可以对齐。
然后回环帧,和回环帧相连的关键帧的所有地图点,向当前帧,和当前帧相连接的关键帧投影寻找匹配点,所有的这些匹配点和计算相似变换时的匹配点进行融合,融合会使得回环两端的关键帧建立新的共视点,从而需要 convisibility graph 中更新关键帧之间的边连接。
Essential Graph /pose graph Optimization:To effectively close the loop, we perform a pose graph optimization over the Essential Graph,对于单目 SLAM 优化是针对相似变换做的,优化后地图点根据能观测到它的一帧关键帧进行位置的更新。
优化的参数:每个关键帧的 pose,优化参数 pose 是个相似变换,优化完后再作用到关键帧位姿,作用后,关键帧位姿仍然是个欧式变换,优化后也作用到地图点。
优化约束
这里的约束都是位姿约束,位姿约束指帧和帧之间的位姿变换。
约束有:
当前回环两端新建立的边(回环两端地图点融合产生连接回环两端新边)形成的位姿约束。
spanning tree 位姿约束。
每个关键帧的回环约束。
convisibility graph 中权值大于一定阈值的边约束。
Global BA:优化地图中所有关键帧的 pose 和所有地图点的坐标。优化的变量包括所有关键帧 pose,所有特征点坐标,优化的约束是所有特征点所有观测的投影误差。
如果在./build_ros.sh 中,export了后仍然不好使,则参考:http://86rev0.smartapps.cn/pages/blog/article-detail?userName=Robot_Starscream&articleId=90245477 就ok了
关掉局部BA需要注释掉LocalMapping.cc的第80行和81行,即:
// Local BA
if(mpMap->KeyFramesInMap()>2)
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
关掉回环需要注释掉LoopClosing.cc中的第71-75行:
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
关掉全局BA需要注释掉LoopClosing.cc中的第579行:
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
关掉生成的点云,Tracking.cc 注释掉
// insert Key Frame into point cloud viewer
//mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );
system.cc 里 mpPointCloudMapping 关键字的句子注释掉,搜pointcloud注释掉