Update LocalMapping::KeyFrameCulling

blog/ORB_SLAM3实战(1).md

@@ -0,0 +1,7 @@
+# ORB_SLAM3实战——前言
+## Intro
+## 技能与知识准备
+## 系统与环境
+## 数据
+### 公开数据集
+### 自己的相机

blog/ORB_SLAM3实战(2).md

@@ -0,0 +1,106 @@
+# ORB_SLAM3实战——代码理解
+以ORB_SLAM3的双目IMU ros接口为例，介绍其代码的运行逻辑。本文以顺序逻辑来介绍各个线程的任务和部分结合现有的公开资料共同完成。
+## 关键数据结构：
+- Map point
+  - observation
+    - map<KeyFrame*, tuple<int,int>>
+    - 代表在KeyFrame关键帧中的左右双目的index序号中看到了该地图点 
+- Keyframe
+  - 共视图(covisible graph)代表关键帧之间可视范围的大小，是一个比较重要的数据结构，可以参考[这篇博文](https://blog.csdn.net/qq_53377744/article/details/120829801)
+## ROS节点
+启动ros节点之后，首先会根据yaml文件设置SLAM的参数，以及图像和IMU数据的收集函数。如果采用了整张图片矫正的参数，则会记录相关信息。初始化完成后，将相关的subscriber接到对应的topic上，对应的回调函数中，GrabImu会用一个队列保存收到的每一帧IMU数据，但是图像的队列只会保留最近的一张图。之后开始主线程。初始化的进程会被挂起，并且在SLAM主线程结束之后进行数据的记录和最终结果的保存。
+
+主线程的SyncWithImu函数负责通过while循环不断把数据发送给SLAM部分。对于数据，时间戳以图像为基准，首先要保证发送的左右眼图片的时间戳差异不会太大。如果获取到了一堆timestamp差异在阈值之内的图片，就将队列中的IMU数据全部取出来，与图片数据一起发送给tracking线程（如果是全局去畸变则直接在这里把rectify的图算出来传进去）。数据会先经过`System::TrackStereo`来对于系统的状态进行简单判断，以确定图片是否会被发送给tracking线程，如果没问题的话图片会被送入`Tracking::GrabImageStereo`该函数会负责创建一个`Frame`数据结构。
+
+## Frame生成
+
+
+
+
+
+### 提取ORB特征点
+左右眼图片会被分别分配一个extractor对象，利用两个线程来并行的对特征点进行提取。核心函数是被extractor对象被重载的操作运算符()，相关代码在` ORBextractor::operator()`。
+
+对于图片的处理，和传统的ORB的处理没有太多的区别。首先算法会设置一个边的阈值，这里设置的是19，防止过于边缘的点被引入计算。
+
+之后计算图像金字塔，以方便后面获取不同level的图像特征。每层都会根据scale factor来计算尺寸放缩后的大小，总体上是越来越小的。
+
+获取金字塔之后会根据金字塔来计算特征点。特征点vector的维度为orb层数×最大特征点数量，注意Fast特征点是建立在半径为3的圆上的，所以相关的边界会用到3。首先会提取一边Fast特征点，如果没有提取出特征点的话，会尝试降低阈值，再提一遍。另外有一个W的概念（w=35），提取特征点的时候是间隔W来提取的
+
+提取特征点之后会有很多特征点，但是需要做一个分配，让特征点的分布尽可能均匀。选取的相关内容可以参考[知乎回答](https://zhuanlan.zhihu.com/p/61738607)
+
+在选取特征点后再给特征点们计算方向，特征点就从一个普通的FAST角点变成ORB特征点了。左眼特征点Nleft个，右眼Nright个，总共N个
+
+### FishEye特征点匹配
+
+ORB_SLAM3中新加了lapping功能，对于双目摄像头来说，可以设置图像的共视区域。在双目的特征点匹配中只计算共视区域的特征点，两张图以左图为准，并且计算深度和三维点。匹配的计算方法是朴素的knn-match。
+
+### 收尾
+之后会把每个特征点放入grid中，方便加速匹配。另一方面对于鱼眼镜头，会对特征点进行去畸变，获取其在归一化平面上的位置。一个Frame便创建完成了。这个帧会被送入tracking进程中来尝试获取他的位姿。
+
+## Tracking
+处理好当前帧之后，我们将其送到tracking线程中进行跟踪。在进行了一系列检查之后首先对IMU进行预积分，来分别获取从上一个关键帧，以及从上一帧开始的IMU数据的预积分值。
+### 初始化
+首先来判断是否初始化，如果没有就进入初始化流程。
+
+初始化要求：
+    特征点数目大于500（左右加起来）
+    需要至少有两帧的IMU预积分
+    有足够的加速度
+
+对于在左右都有匹配的图片，添加地图点即可，将第一帧作为初始化，并将其位姿设置为0
+### 追踪
+如果前一帧是重定位成功的
+
+如果刚刚重定位 或者 没有速度并且当且地图IMU未初始化 -> 利用参考关键帧追踪
+
+- Tracking::TrackReferenceKeyFrame()
+  - 计算当前帧特征点的BoW，与参考关键帧进行匹配（利用直方图的方法及逆行加速）
+  - 特征点匹配后进行优化，得到内外点和位姿[优化详见](https://zhuanlan.zhihu.com/p/350456670)
+  - 将外点从地图点中删除掉
+  - 如果带IMU就直接返回True，否则需要匹配的数量大于10个才行？？？（这里是为什么）
+否则先用motion model追踪，不行再用参考关键帧追踪。
+- Tracking::TrackWithMotionModel()
+  - 运动模型用到的orbmatcher的阈值相对要高一些，由0.7提升到了0.9
+  - 首先更新上一帧
+  - 如果地图IMU已经初始化并且不需要reset（Line2863看上去应该是小于？）就利用IMU来预测位姿。并且直接返回OK
+  - 否则直接利用速度算出来当前帧的初值，然后与前一帧进行投影（SearchByProjection，line1677）就是利用位姿从地图点的投影点的周围选取关键点，并且进行匹配，如果匹配的数目没有达到20个，就放宽阈值，再来一次。如果还不够，IMU返回true，没有imu返回false
+  - 拥有匹配点之后再效仿刚刚的参考关键帧，剔除外点并且返回
+考虑到我们应用的是双目IMU的形式，因此bOK一直是true的（这个是为什么？）
+
+如果前一帧是追踪失败的就纯用imu进行追踪，如果5s后还是失败的就设为lost（如果没有imu则会调用重定位，如果重定位还是失败就设置为lost
+
+
+除了相邻帧的追踪之外还需要追踪局部地图，寻找更多匹配来优化当前帧的位姿。
+- Tracking::TrackLocalMap()
+  - 首先对局部地图进行更新，分为更新局部关键帧和更新局部地图点两部分。
+    - 更新局部关键帧：
+      - 首先看当前地图点都在哪些关键帧中被观测到，并且计数。有观测到地图点的关键帧（如果是好的话）都会被放入局部关键帧中，同时记录下观测最多的关键帧。
+      - 对于已经加入其中的关键帧，历遍每一个，获取有共视关系的相邻帧，并且设置该帧保证不重复，然后将其加入局部关键帧中，再将其的一个子关键帧和父关键帧加入，凑够80
+      - 对于IMU模式，再将最近20个关键帧没有加入的也放进去。
+      - 更新参考关键帧为观测到地图点最多的关键帧
+    - 更新局部地图点：将局部关键帧里面的点都丢进局部地图点中
+  - 之后根据当前地图状况决定是否使用带IMU的g2o优化，修改位姿。最终根据内点的数量来确定返回true还是false
+
+  - 最后是后处理以及判断是否需要插入关键帧，tracking线程的工作到这里结束了。
+
+- Tracking::NeedNewKeyFrame()
+  - 由于这里的逻辑比较复杂，多种情况可以参考[这篇笔记](https://zhuanlan.zhihu.com/p/349770246)详细了解。这里只看双目imu的情况
+  - 1. 如果imu地图没有初始化：0.25秒就插入一个关键帧
+  - 2. 如果地图坏了或者要求停止或者离上一帧太近就不插入
+  - 另外对于关键点有些要求，但是核心是IMU状态下每0.5秒（初始化之后）就要插入一张
+
+## Local Mapping
+
+1. 处理关键帧，计算观测点的BoW，计算观测关系和共视图权重
+2. 对于新来的mappoint做去重
+3. 建立新的mappoint，如果近期没有关键帧的话还要从共视关键帧中去掉一些
+4. 利用新的地图点去做局部的BA
+5. 如过IMU没有初始化的话就初始化IMU
+6. 剔除多余关键帧
+7. 准备做VIBA，先做视觉的再做带IMU的，VIBA2做完之后才算是正式的初始化完成
+
+
+
+
+

blog/ORB_SLAM3实战(3).md

@@ -0,0 +1,7 @@
+# ORB_SLAM3实战——（前言）
+## Intro
+## 技能与知识准备
+## 系统与环境
+## 数据
+### 公开数据集
+### 自己的相机

blog/ORB_SLAM3实战(4).md

@@ -0,0 +1,7 @@
+# ORB_SLAM3实战——（前言）
+## Intro
+## 技能与知识准备
+## 系统与环境
+## 数据
+### 公开数据集
+### 自己的相机

src/LocalMapping.cc

@@ -958,7 +958,7 @@ void LocalMapping::KeyFrameCulling()
                 {
                     if(!mbMonocular)
                     {
-                        if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
+                        if((pKF->NLeft == -1||i < pKF->NLeft)&&(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0))
                             continue;
                     }
 
@@ -967,7 +967,7 @@ void LocalMapping::KeyFrameCulling()
                     {
                         const int &scaleLevel = (pKF -> NLeft == -1) ? pKF->mvKeysUn[i].octave
                                                                      : (i < pKF -> NLeft) ? pKF -> mvKeys[i].octave
-                                                                                          : pKF -> mvKeysRight[i].octave;
+                                                                                          : pKF -> mvKeysRight[i - pKF -> NLeft].octave;
                         const map<KeyFrame*, tuple<int,int>> observations = pMP->GetObservations();
                         int nObs=0;
                         for(map<KeyFrame*, tuple<int,int>>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)