ORB-SLAM2代码/流程详解

Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989
​
Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314
​
Camera.width: 640
Camera.height: 480
​
Camera.fps: 30.0 # Camera frames per second
Camera.bf: 40.0 # IR projector baseline times fx (aprox.)
Camera.RGB: 1 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
ThDepth: 40.0 # Close/Far threshold. Baseline times.
DepthMapFactor: 5000.0 # Deptmap values factor
​

ORBextractor.nFeatures: 1000 # ORB Extractor: Number of features per image
ORBextractor.scaleFactor: 1.2 # ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.nLevels: 8 # ORB Extractor: Number of levels in the scale pyramid
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
1.2. 阅读代码之前你应该知道的事情
1.2.1 变量命名规则
ORB-SLAM2中的变量遵循一套命名规则:

变量名的第一个字母为m表示该变量为某类的成员变量.
变量名的第一、二个字母表示数据类型:
p表示指针类型
n表示int类型
b表示bool类型
s表示std::set类型
v表示std::vector类型
l表示std::list类型
KF表示KeyFrame类型
这种将变量类型写进变量名的命名方法叫做匈牙利命名法.

1.3 理解多线程
1.3.1 为什么要使用多线程?
加快运算速度:

bool Initializer::Initialize(const Frame &CurrentFrame) {
// …
thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));
thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));
// …
}
1
2
3
4
5
6
开两个线程同时计算两个矩阵,在多核处理器上会加快运算速度.

因为系统的随机性,各步骤的运行顺序是不确定的.

Tracking线程不产生关键帧时,LocalMapping和LoopClosing线程基本上处于空转的状态.

而Tracking线程产生关键帧的频率和时机不是固定的,因此需要3个线程同时运行,LocalMapping和LoopClosing线程不断循环查询Tracking线程是否产生关键帧,产生了的话就处理.

// Tracking线程主函数
void Tracking::Track() {
// 进行跟踪
// …

// 若跟踪成功,根据条件判定是否产生关键帧
if (NeedNewKeyFrame())
    // 产生关键帧并将关键帧传给LocalMapping线程
    KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
    mpLocalMapper->InsertKeyFrame(pKF); 

}
​
// LocalMapping线程主函数
void LocalMapping::Run() {
// 死循环
while (1) {
// 判断是否接收到关键帧
if (CheckNewKeyFrames()) {
// 处理关键帧
// …

        // 将关键帧传给LoopClosing线程
        mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
    }
    
    // 线程暂停3毫秒,3毫秒结束后再从while(1)循环首部运行
    std::this_thread::sleep_for(std::chrono::milliseconds(3));
}

}
​
// LoopClosing线程主函数
void LoopClosing::Run() {
// 死循环
while (1) {
// 判断是否接收到关键帧
if (CheckNewKeyFrames()) {
// 处理关键帧
// …
}
​
// 查看是否有外部线程请求复位当前线程
ResetIfRequested();
​
// 线程暂停5毫秒,5毫秒结束后再从while(1)循环首部运行
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
1.3.2 多线程中的锁
为防止多个线程同时操作同一变量造成混乱,引入锁机制:

将成员函数本身设为私有变量(private或protected),并在操作它们的公有函数内加锁.

class KeyFrame {
protected:
KeyFrame* mpParent;

public:
void KeyFrame::ChangeParent(KeyFrame *pKF) {
unique_lock lockCon(mMutexConnections); // 加锁
mpParent = pKF;
pKF->AddChild(this);
}
​
KeyFrame *KeyFrame::GetParent() {
unique_lock lockCon(mMutexConnections); // 加锁
return mpParent;
}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
一把锁在某个时刻只有一个线程能够拿到,如果程序执行到某个需要锁的位置,但是锁被别的线程拿着不释放的话,当前线程就会暂停下来;直到其它线程释放了这个锁,当前线程才能拿走锁并继续向下执行.

什么时候加锁和释放锁?

unique_lock lockCon(mMutexConnections);这句话就是加锁,锁的有效性仅限于大括号{}之内,也就是说,程序运行出大括号之后就释放锁了.因此可以看到有一些代码中加上了看似莫名其妙的大括号.

void KeyFrame::EraseConnection(KeyFrame *pKF) {
// 第一部分加锁
{
unique_lock lock(mMutexConnections);
if (mConnectedKeyFrameWeights.count(pKF)) {
mConnectedKeyFrameWeights.erase(pKF);
bUpdate = true;
}
}// 程序运行到这里就释放锁,后面的操作不需要抢到锁就能执行

UpdateBestCovisibles();

}
1
2
3
4
5
6
7
8
9
10
11
12
1.4 SLAM主类System
1.4.1 System`类是ORB-SLAM2系统的主类,先分析其主要的成员函数和成员变量:
成员变量/函数 访问控制 意义
eSensor mSensor private 传感器类型MONOCULAR,STEREO,RGBD
ORBVocabulary* mpVocabulary private ORB字典,保存ORB描述子聚类结果
KeyFrameDatabase* mpKeyFrameDatabase private 关键帧数据库,保存ORB描述子倒排索引
Map* mpMap private 地图
Tracking* mpTracker private 追踪器
LocalMapping* mpLocalMapper std::thread* mptLocalMapping private private 局部建图器 局部建图线程
LoopClosing* mpLoopCloser std::thread* mptLoopClosing private private 回环检测器 回环检测线程
Viewer* mpViewer FrameDrawer* mpFrameDrawer MapDrawer* mpMapDrawer std::thread* mptViewer private private private private 查看器 帧绘制器 地图绘制器 查看器线程
System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true) public 构造函数
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp) int mTrackingState std::mutex mMutexState public public public private private 跟踪双目相机,返回相机位姿 跟踪RGBD相机,返回相机位姿 跟踪单目相机,返回相机位姿 追踪状态 追踪状态锁
bool mbActivateLocalizationMode bool mbDeactivateLocalizationMode std::mutex mMutexMode void ActivateLocalizationMode() void DeactivateLocalizationMode() private private private public public 开启/关闭纯定位模式
bool mbReset std::mutex mMutexReset void Reset() private private public 系统复位
void Shutdown() public 系统关闭
void SaveTrajectoryTUM(const string &filename) void SaveKeyFrameTrajectoryTUM(const string &filename) void SaveTrajectoryKITTI(const string &filename) public public public 以TUM/KITTI格式保存相机运动轨迹和关键帧位姿
1.4.2 构造函数
System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true): 构造函数

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer) :
mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false), mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) {

// step1. 初始化各成员变量
// step1.1. 读取配置文件信息
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
// step1.2. 创建ORB词袋
mpVocabulary = new ORBVocabulary();
// step1.3. 创建关键帧数据库,主要保存ORB描述子倒排索引(即根据描述子查找拥有该描述子的关键帧)
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
// step1.4. 创建地图
mpMap = new Map();

​
// step2. 创建3大线程: Tracking、LocalMapping和LoopClosing
// step2.1. 主线程就是Tracking线程,只需创建Tracking对象即可
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
// step2.2. 创建LocalMapping线程及mpLocalMapper
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
// step2.3. 创建LoopClosing线程及mpLoopCloser
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

// step3. 设置线程间通信
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
LocalMapping和LoopClosing线程在System类中有对应的std::thread线程成员变量,为什么Tracking线程没有对应的std::thread成员变量?

因为Tracking线程就是主线程,而LocalMapping和LoopClosing线程是其子线程,主线程通过持有两个子线程的指针(mptLocalMapping和mptLoopClosing)控制子线程.

(ps: 虽然在编程实现上三大主要线程构成父子关系,但逻辑上我们认为这三者是并发的,不存在谁控制谁的问题).

1.4.3 跟踪函数
System对象所在的主线程就是跟踪线程,针对不同的传感器类型有3个用于跟踪的函数,其内部实现就是调用成员变量mpTracker的GrabImageMonocular(GrabImageStereo或GrabImageRGBD)方法.

传感器类型 用于跟踪的成员函数
MONOCULAR cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
STEREO cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
RGBD cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp)
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) {
cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);
unique_lock lock(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}

1
2
3
4
5
6
7
8
9
2. ORB-SLAM2代码详解02_特征点提取器ORBextractor

2.1各成员函数/变量
2.1.1 构造函数: ORBextractor()
FAST特征点和ORB描述子本身不具有尺度信息,ORBextractor通过构建图像金字塔来得到特征点尺度信息.将输入图片逐级缩放得到图像金字塔,金字塔层级越高,图片分辨率越低,ORB特征点越大.

构造函数ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST, int minThFAST)的流程:

初始化图像金字塔相关变量:

下面成员变量从配置文件TUM1.yaml中读入:

成员变量 访问控制 意义 配置文件TUM1.yaml中变量名 值
int nfeatures protected 所有层级提取到的特征点数之和金字塔层数 ORBextractor.nFeatures 1000
double scaleFactor protected 图像金字塔相邻层级间的缩放系数 ORBextractor.scaleFactor 1.2
int nlevels protected 金字塔层级数 ORBextractor.nLevels 8
int iniThFAST protected 提取特征点的描述子门槛(高) ORBextractor.iniThFAST 20
int minThFAST protected 提取特征点的描述子门槛(低) ORBextractor.minThFAST 7
根据上述变量的值计算出下述成员变量:

成员变量 访问控制 意义 值
std::vector mnFeaturesPerLevel protected 金字塔每层级中提取的特征点数 正比于图层边长,总和为nfeatures {61, 73, 87, 105, 126, 151, 181, 216}
std::vector mvScaleFactor protected 各层级的缩放系数 {1, 1.2, 1.44, 1.728, 2.074, 2.488, 2.986, 3.583}
std::vector mvInvScaleFactor protected 各层级缩放系数的倒数 {1, 0.833, 0.694, 0.579, 0.482, 0.402, 0.335, 0.2791}
std::vector mvLevelSigma2 protected 各层级缩放系数的平方 {1, 1.44, 2.074, 2.986, 4.300, 6.190, 8.916, 12.838}
std::vector mvInvLevelSigma2 protected 各层级缩放系数的平方倒数 {1, 0.694, 0.482, 0.335, 0.233, 0.162, 0.112, 0.078}
初始化用于计算描述子的pattern变量,pattern是用于计算描述子的256对坐标,其值写死在源码文件ORBextractor.cc里,在构造函数里做类型转换将其转换为const cv::Point*变量.

static int bit_pattern_31_[2564] ={
8,-3, 9,5/mean (0), correlation (0)/,
4,2, 7,-12/mean (1.12461e-05), correlation (0.0437584)/,
-11,9, -8,2/mean (3.37382e-05), correlation (0.0617409)/,
7,-12, 12,-13/mean (5.62303e-05), correlation (0.0636977)/,
2,-13, 2,12/mean (0.000134953), correlation (0.085099)/,
// 共256行…
}
​
const Point pattern0 = (const Point*)bit_pattern_31_;
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));

1
2
3
4
5
6
7
8
9
10
11
12
计算一个半径为16的圆的近似坐标
后面计算的是特征点主方向上的描述子,计算过程中要将特征点周围像素旋转到主方向上,因此计算一个半径为16的圆的近似坐标，用于后面计算描述子时进行旋转操作.

成员变量std::vector umax里存储的实际上是逼近圆的第一象限内圆周上每个v坐标对应的u坐标.为保证严格对称性,先计算下45°圆周上点的坐标,再根据对称性补全上45°圆周上点的坐标.

int vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); // 45°射线与圆周交点的纵坐标
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); // 45°射线与圆周交点的纵坐标
​
// 先计算下半45度的umax
for (int v = 0; v <= vmax; ++v) {
umax[v] = cvRound(sqrt(15 * 15 - v * v));
}
​
// 根据对称性补出上半45度的umax
for (int v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v) {
while (umax[v0] == umax[v0 + 1])
++v0;
umax[v] = v0;
++v0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
2.2 构建图像金字塔: ComputePyramid()
根据上述变量的值计算出下述成员变量:

成员变量 访问控制 意义
std::vector mvImagePyramid public 图像金字塔每层的图像
const int EDGE_THRESHOLD 全局变量 为计算描述子和提取特征点补的padding厚度
函数void ORBextractor::ComputePyramid(cv::Mat image)逐层计算图像金字塔,对于每层图像进行以下两步:

先进行图片缩放,缩放到mvInvScaleFactor对应尺寸.
在图像外补一圈厚度为19的padding(提取FAST特征点需要特征点周围半径为3的圆域,计算ORB描述子需要特征点周围半径为16的圆域).
下图表示图像金字塔每层结构:

深灰色为缩放后的原始图像.
包含绿色边界在内的矩形用于提取FAST特征点.
包含浅灰色边界在内的整个矩形用于计算ORB描述子.

void ORBextractor::ComputePyramid(cv::Mat image) {
for (int level = 0; level < nlevels; ++level) {
// 计算缩放+补padding后该层图像的尺寸
float scale = mvInvScaleFactor[level];
Size sz(cvRound((float)image.colsscale), cvRound((float)image.rowsscale));
Size wholeSize(sz.width + EDGE_THRESHOLD * 2, sz.height + EDGE_THRESHOLD * 2);
Mat temp(wholeSize, image.type());

    // 缩放图像并复制到对应图层并补边
    mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
    if( level != 0 ) {
        resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, cv::INTER_LINEAR);
        copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, 
                       BORDER_REFLECT_101+BORDER_ISOLATED);            
    } else {
        copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, 
                       BORDER_REFLECT_101);            
    }
}

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
copyMakeBorder函数实现了复制和padding填充,其参数BORDER_REFLECT_101参数指定对padding进行镜像填充.

2.3 提取特征点并进行筛选: ComputeKeyPointsOctTree()

提取特征点最重要的就是力求特征点均匀地分布在图像的所有部分,为实现这一目标,编程实现上使用了两个技巧:

分CELL搜索特征点,若某CELL内特征点响应值普遍较小的话就降低分数线再搜索一遍.
对得到的所有特征点进行八叉树筛选,若某区域内特征点数目过于密集,则只取其中响应值最大的那个.

CELL搜索的示意图如下,每个CELL的大小约为30✖30,搜索到边上,剩余尺寸不够大的时候,最后一个CELL有多大就用多大的区域.

需要注意的是相邻的CELL之间会有6像素的重叠区域,因为提取FAST特征点需要计算特征点周围半径为3的圆周上的像素点信息,实际上产生特征点的区域比传入的搜索区域小3像素.

void ORBextractor::ComputeKeyPointsOctTree(vector& allKeypoints) {
for (int level = 0; level < nlevels; ++level)
// 计算图像边界
const int minBorderX = EDGE_THRESHOLD-3;
const int minBorderY = minBorderX;
const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
const float width = (maxBorderX-minBorderX);
const float height = (maxBorderY-minBorderY);
const int nCols = width/W; // 每一列有多少cell
const int nRows = height/W; // 每一行有多少cell
const int wCell = ceil(width/nCols); // 每个cell的宽度
const int hCell = ceil(height/nRows); // 每个cell的高度
​
// 存储需要进行平均分配的特征点
vectorcv::KeyPoint vToDistributeKeys;

    // step1. 遍历每行和每列,依次分别用高低阈值搜索FAST特征点
    for(int i=0; i vKeysCell;
            
            // 先用高阈值搜索FAST特征点
            FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell, iniThFAST, true);
            // 高阈值搜索不到的话,就用低阈值搜索FAST特征点
            if(vKeysCell.empty()) {
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell, minThFAST, true);
            }
            // 把 vKeysCell 中提取到的特征点全添加到 容器vToDistributeKeys 中
            for(KeyPoint point :vKeysCell) {
                point.pt.x+=j*wCell;
                point.pt.y+=i*hCell;
                vToDistributeKeys.push_back(point);
            }
        }
    }
    
    // step2. 对提取到的特征点进行八叉树筛选,见 DistributeOctTree() 函数
    keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY, mnFeaturesPerLevel[level], level);
}
// 计算每个特征点的方向
for (int level = 0; level < nlevels; ++level)
    computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);               
}

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-E2ZSyl6g-1632407590934)(…/AppData/Roaming/Typora/typora-user-images/1629211584953.png)]

2.4 八叉树筛选特征点: DistributeOctTree()：Uniform Distribution
ORB-SLAM中使用四叉树来快速筛选特征点，筛选的目的是非极大值抑制，取局部特征点邻域中FAST角点相应值最大的点，而如何搜索到这些扎堆的的点，则采用的是四叉树的分块思想，递归找到成群的点，并从中找到相应值最大的点。

函数DistributeOctTree()进行**八叉树（实际上是个四叉树）**筛选(非极大值抑制),不断将存在特征点的图像区域进行4等分,直到分出了足够多的分区,每个分区内只保留响应值最大的特征点.

其代码实现比较琐碎,程序里还定义了一个ExtractorNode类用于进行八叉树分配,知道原理就行,不看代码.

NMS (Non-Maximal Suppression)
非极大值抑制主要是为了避免图像上得到的“角点”过于密集，主要过程是，每个特征点会计算得到相应的响应得分，然后以当前像素点p为中心，取其邻域（如3x3 的邻域），判断当前像素p的响应值是否为该邻域内最大的，如果是，则保留，否则，则抑制。

Harris响应：
参考论文 http://www.bmva.org/bmvc/1988/avc-88-023.pdf
https://zhuanlan.zhihu.com/p/36382429

https://www.cnblogs.com/polly333/p/5416172.html

https://blog.csdn.net/lwzkiller/article/details/54633670

角点检测算法基本思想是什么？

算法基本思想是使用一个固定窗口在图像上进行任意方向上的滑动，比较滑动前与滑动后两种情况，窗口中的像素灰度变化程度，如果存在任意方向上的滑动，都有着较大灰度变化，那么我们可以认为该窗口中存在角点。

2.5 计算特征点方向computeOrientation()
函数computeOrientation()计算每个特征点的方向: 使用特征点周围半径19大小的圆的重心方向作为特征点方向.

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-au2xw1XJ-1632407590935)(…/AppData/Roaming/Typora/typora-user-images/1629249426785.png)]

static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax)
{
for (vector::iterator keypoint : keypoints) {
// 调用IC_Angle 函数计算这个特征点的方向
keypoint->angle = IC_Angle(image, keypoint->pt, umax);
}
}
​
static float IC_Angle(const Mat& image, Point2f pt, const vector & u_max)
{
int m_01 = 0, m_10 = 0; // 重心方向
const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x));
for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
m_10 += u * center[u];
int step = (int)image.step1();
for (int v = 1; v <= HALF_PATCH_SIZE; ++v) {
int v_sum = 0;
int d = u_max[v];
for (int u = -d; u <= d; ++u) {
int val_plus = center[u + vstep], val_minus = center[u - vstep];
v_sum += (val_plus - val_minus);
m_10 += u * (val_plus + val_minus);
}
m_01 += v * v_sum;
}
​
// 为了加快速度使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
return fastAtan2((float)m_01, (float)m_10);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
2.6 rBRIEF: Rotation-Aware Brief 计算特征点描述子computeOrbDescriptor()
补充
Brief of BRIEF (Binary robust independent elementary feature)
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-a4RC61iC-1632407590937)(https://cggos.github.io/images/image_features/brief.png)]

In brief, each keypoint is described by a feature vector which is 128–512 bits string.

vector dim = 256 bits (32 bytes)

each vector ⟷each keypoint

for each bit, select a pair of points in a patch I II which centered a corner pp and compare their intensity
S = ( p 1 , . . . p n q 1 , . . . q n ) ∈ R ( 2 × 2 ) × 256 S= \left(
p1,…pnq1,…qn
p1,…pnq1,…qn
\right) ∈\mathbb {R}^{(2×2)×256}
S=(
p
1
​
,…p
n
​

q
1
​
,…q
n
​

​
)∈R
(2×2)×256

KaTeX parse error: \cr valid only within a tabular/array environment

the descriptor (each bit ⟷⟷ each pair of points ( p i , q i ) (p_i,q_i)(p
i
​
,q
i
​
):
f ( n ) = n ∑ i = 12 i − 1 τ ( I ; p i , q i ) , ( n = 256 ) f(n)=n∑i=12i−1τ(I;pi,qi),(n=256)
f(n)=n∑i=12i−1τ(I;pi,qi),(n=256)

steered BRIEF
为了具有旋转不变性，引入该算法，但方差很小、相关性高
R θ = [ c o s θ − s i n θ s i n θ c o s θ ] S θ = R θ S = ( p ′ 1 , … , p ′ n q ′ 1 , … , q ′ n ) R_θ= \left[
cosθsinθ−sinθcosθ
cosθ−sinθsinθcosθ
\right] \S_θ=R_θS=\left(
p’1,…,p’nq’1,…,q’n
p′1,…,p′nq′1,…,q′n
\right)
R
θ
​
=[
cosθ
sinθ
​

−sinθ
cosθ
​
]
S
θ
​
=R
θ
​
S=(
p′1,…,p′n
q′1,…,q′n
​
)

rBRIEF
rBRIEF shows significant improvement in the variance and correlation over steered BRIEF
为了把steered BRIEF方差增大，相关性降低
基于统计规律，利用了贪心算法进行筛选
construct a lookup table of precomputed BRIEF patterns
计算BRIEF描述子的核心步骤是在特征点周围半径为16的圆域内选取256对点对,每个点对内比较得到1位,共得到256位的描述子,为保计算的一致性,工程上使用特定设计的点对pattern,在程序里被硬编码为成员变量了.

在computeOrientation()中我们求出了每个特征点的主方向,在计算描述子时,应该将特征点周围像素旋转到主方向上来计算;为了编程方便,实践上对pattern进行旋转.

static void computeOrbDescriptor(const KeyPoint& kpt, const Mat& img, const Point* pattern, uchar* desc) {

float angle = (float)kpt.angle*factorPI;
float a = (float)cos(angle), b = (float)sin(angle);

​//获得图像中心指针
const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
const int step = (int)img.step;//获得图像的每行的字节数
​
// 旋转公式
// x’= xcos(θ) - ysin(θ)
// y’= xsin(θ) + ycos(θ)
#define GET_VALUE(idx)
center[cvRound(pattern[idx].xb + pattern[idx].ya)step + cvRound(pattern[idx].xa - pattern[idx].y*b)]
for (int i = 0; i < 32; ++i, pattern += 16) {
int t0, t1, val;
t0 = GET_VALUE(0); t1 = GET_VALUE(1);
val = t0 < t1; // 描述子本字节的bit0
t0 = GET_VALUE(2); t1 = GET_VALUE(3);
val |= (t0 < t1) << 1; // 描述子本字节的bit1
t0 = GET_VALUE(4); t1 = GET_VALUE(5);
val |= (t0 < t1) << 2; // 描述子本字节的bit2
t0 = GET_VALUE(6); t1 = GET_VALUE(7);
val |= (t0 < t1) << 3; // 描述子本字节的bit3
t0 = GET_VALUE(8); t1 = GET_VALUE(9);
val |= (t0 < t1) << 4; // 描述子本字节的bit4
t0 = GET_VALUE(10); t1 = GET_VALUE(11);
val |= (t0 < t1) << 5; // 描述子本字节的bit5
t0 = GET_VALUE(12); t1 = GET_VALUE(13);
val |= (t0 < t1) << 6; // 描述子本字节的bit6
t0 = GET_VALUE(14); t1 = GET_VALUE(15);
val |= (t0 < t1) << 7; // 描述子本字节的bit7
​
//保存当前比较的出来的描述子的这个字节
desc[i] = (uchar)val;
}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
2.7 ORBextractor类的用途

ORBextractor被用于Tracking线程对输入图像预处理的第一步.

2.7.1 ORBextractor类提取特征点的主函数void operator()()
这个函数重载了()运算符,使得其他类可以将ORBextractor类型变量当作函数来使用.

该函数是ORBextractor的主函数,内部依次调用了上面提到的各过程.

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Fq9xR2jz-1632407590938)(…/AppData/Roaming/Typora/typora-user-images/1628640469328.png)]

提取特征点void operator()()计算特征点并进行八叉树筛选
ComputeKeyPointsOctTree()检查图像有效性计算特征点并进行八叉树筛选
ComputeKeyPointsOctTree()遍历每一层图像,计算描述子
computeOrbDescriptor()逐层遍历
按CELL提取FAST特征点调用DistributeOctTree()
筛选特征点,进行非极大值抑制调用computeOrientation()
计算每个特征点的主方向

void ORBextractor::operator()(InputArray _image, InputArray _mask, vector& _keypoints, OutputArray _descriptors) {
// step1. 检查图像有效性
if(_image.empty())
return;
Mat image = _image.getMat();
assert(image.type() == CV_8UC1 );
​
// step2. 构建图像金字塔
ComputePyramid(image);
​
// step3. 计算特征点并进行八叉树筛选
vector allKeypoints;
ComputeKeyPointsOctTree(allKeypoints);
​
// step4. 遍历每一层图像,计算描述子
int offset = 0;
for (int level = 0; level < nlevels; ++level) {
Mat workingMat = mvImagePyramid[level].clone();
// 计算描述子之前先进行一次高斯模糊
GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
computeDescriptors(workingMat, allKeypoints[level], descriptors.rowRange(offset, offset + allKeypoints[level].size());, pattern);
offset += allKeypoints[level].size();
}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
这个重载()运算符的用法被用在Frame类的ExtractORB()函数中了,这也是ORBextractor类在整个项目中唯一被调用的地方.

// 函数中mpORBextractorLeft和mpORBextractorRight都是ORBextractor对象
void Frame::ExtractORB(int flag, const cv::Mat &im) {
if(flag==0)
(*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
else
(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

1
2
3
4
5
6
7
8
9
2.8 ORBextractor类与其它类间的关系
Frame类中与ORBextractor有关的成员变量和成员函数

成员变量/函数 访问控制 意义
ORBextractor* mpORBextractorLeft public 左目特征点提取器
ORBextractor* mpORBextractorRight public 右目特征点提取器,单目/RGBD模式下为空指针
Frame() public Frame类的构造函数,其中调用ExtractORB()函数进行特征点提取
ExtractORB() public 提取ORB特征点,其中调用了mpORBextractorLeft和mpORBextractorRight的()方法
// Frame类的两个ORBextractor是在调用构造函数时传入的,构造函数中调用ExtractORB()提取特征点
Frame::Frame(ORBextractor *extractorLeft, ORBextractor *extractorRight)
: mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight) {
​
// …
​
// 提取ORB特征点
thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);
thread threadRight(&Frame::ExtractORB, this, 1, imRight);
threadLeft.join();
threadRight.join();
​
// …
}
​
// 提取特征点
void Frame::ExtractORB(int flag, const cv::Mat &im) {
if (flag == 0)
(*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
else
(*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
Frame类的两个ORBextractor指针指向的变量是Tracking类的构造函数中创建的

// Tracking构造函数
Tracking::Tracking() {
// …

// 创建两个ORB特征点提取器
mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
if (sensor == System::STEREO)
    mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);

​
// …
}
​
// Tracking线程每收到一帧输入图片,就创建一个Frame对象,创建Frame对象时将提取器mpORBextractorLeft和mpORBextractorRight给构造函数
cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp) {
// …

// 创建Frame对象
mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight);

// ...

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
由上述代码分析可知,每次完成ORB特征点提取之后,图像金字塔信息就作废了,下一帧图像到来时调用ComputePyramid()函数会覆盖掉本帧图像的图像金字塔信息;但从金字塔中提取的图像特征点的信息会被保存在Frame对象中.所以ORB-SLAM2是稀疏重建,对每帧图像只保留最多nfeatures个特征点(及其对应的地图点).
构造函数ORBextractor()初始化图像金字塔相关变量初始化用于计算描述子的pattern计算近似圆形的边界坐标umax

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-lITAKkdo-1632407590939)(…/AppData/Roaming/Typora/typora-user-images/1628640595104.png)]

遍历每个30*30的CELL,依次分别使用高低阈值提取FAST特征点找到特征点找到特征点没找到特征点没找到特征点没遍历完所有CELL遍历完所有CELL使用高响应阈值iniThFAST搜索特征点使用低响应阈值minThFAST搜索特征点记录特征点移动到下一块CELL取第一个CELL调用DistributeOctTree()对上一步找到的所有特征点进行八叉树筛选
对特征点密集区域进行非极大值抑制调用computeOrientation()计算每个特征点的主方向

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-LXjSk7hb-1632407590940)(…/AppData/Roaming/Typora/typora-user-images/1628640638080.png)]

3. ORB-SLAM2代码详解03_地图点MapPoint
   MapPoint的生命周期

3.1 各成员函数/变量
3.1.1 地图点的世界坐标: mWorldPos
成员函数/变量 访问控制 意义
cv::Mat mWorldPos protected 地图点的世界坐标
cv::Mat GetWorldPos() public mWorldPos的get方法
void SetWorldPos(const cv::Mat &Pos) public mWorldPos的set方法
std::mutex mMutexPos protected mWorldPos的锁
3.1.2 与关键帧的观测关系: mObservations
成员函数/变量 访问控制 意义
std::map mObservations protected 当前地图点在某KeyFrame中的索引
map GetObservations() public mObservations的get方法
void AddObservation(KeyFrame* pKF,size_t idx) public 添加当前地图点对某KeyFrame的观测
void EraseObservation(KeyFrame* pKF) public 删除当前地图点对某KeyFrame的观测
bool IsInKeyFrame(KeyFrame* pKF) public 查询当前地图点是否在某KeyFrame中
int GetIndexInKeyFrame(KeyFrame* pKF) public 查询当前地图点在某KeyFrame中的索引
int nObs public 记录当前地图点被（多少）相机观测到的次数（质量评估）。单目帧每次观测加1,双目帧每次观测加2
int Observations() public nObs的get方法
成员变量std::map mObservations保存了当前关键点对关键帧KeyFrame的观测关系,std::map是一个key-value结构,其key为某个关键帧,value为当前地图点在该关键帧中的索引(是在该关键帧成员变量std::vector mvpMapPoints中的索引).

成员int nObs记录了当前地图点被多少个关键帧相机观测到了(单目关键帧每次观测算1个相机,双目/RGBD帧每次观测算2个相机).

函数AddObservation()和EraseObservation()同时维护mObservations和nObs
// 向参考帧pKF中添加对本地图点的观测,本地图点在pKF中的编号为idx
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) {
unique_lock lock(mMutexFeatures);
// 如果已经添加过观测，返回
if(mObservations.count(pKF))
return;
// 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
mObservations[pKF]=idx;
​
// 根据观测形式是单目还是双目更新观测计数变量nObs
if(pKF->mvuRight[idx]>=0)
nObs += 2;
else
nObs++;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// 从参考帧pKF中移除本地图点
void MapPoint::EraseObservation(KeyFrame* pKF) {
bool bBad=false;
{
unique_lock lock(mMutexFeatures);
// 查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数
if(mObservations.count(pKF)) {
if(pKF->mvuRight[mObservations[pKF]]>=0)
nObs-=2;
else
nObs–;
​
mObservations.erase(pKF);
​
// 如果该keyFrame是参考帧，该Frame被删除后重新指定RefFrame
if(mpRefKF == pKF)
mpRefKF = mObservations.begin()->first; // ???参考帧指定得这么草率真的好么?
​
// 当观测到该点的相机数目少于2时，丢弃该点(至少需要两个观测才能三角化)
if(nObs<=2)
bBad=true;
}
}
​
if(bBad)
// 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
SetBadFlag();
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
函数GetIndexInKeyFrame()和IsInKeyFrame()( 检查该地图点是否在关键帧中（有对应的二维特征点）)就是对mObservations(能够观测到当前地图点的所有关键帧及该地图点在KF中的索引)的简单查询

int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) {
unique_lock lock(mMutexFeatures);
if(mObservations.count(pKF))
return mObservations[pKF];
else
return -1;
}
​
bool MapPoint::IsInKeyFrame(KeyFrame *pKF) {
unique_lock lock(mMutexFeatures);
return (mObservations.count(pKF));
}

1
2
3
4
5
6
7
8
9
10
11
12
13
3.2 观测尺度
成员函数/变量 访问控制 意义
cv::Mat mNormalVector protected 平均观测方向
float mfMinDistance protected 平均观测距离的下限
float mfMaxDistance protected 平均观测距离的上限
cv::Mat GetNormal() public mNormalVector的get方法
float GetMinDistanceInvariance() public mfMinDistance的get方法
float GetMaxDistanceInvariance() public mNormalVector的get方法
void UpdateNormalAndDepth() public 更新平均观测距离和方向
int PredictScale(const float ¤tDist, KeyFrame* pKF) int PredictScale(const float ¤tDist, Frame* pF) public public 估计当前地图点在某Frame中对应特征点的金字塔层级
KeyFrame* mpRefKF protected 当前地图点的参考关键帧
KeyFrame* GetReferenceKeyFrame() public mpRefKF的get方法
3.2.1 平均观测距离: mfMinDistance和mfMaxDistance
特征点的观测距离与其在图像金字塔中的图层呈线性关系.直观上理解,如果一个图像区域被放大后才能识别出来,说明该区域的观测深度较深.

特征点的平均观测距离的上下限由成员变量mfMaxDistance和mfMinDistance表示:

mfMaxDistance表示若地图点匹配在某特征提取器图像金字塔第7层上的某特征点,观测距离值
mfMinDistance表示若地图点匹配在某特征提取器图像金字塔第0层上的某特征点,观测距离值
这两个变量是基于地图点在其参考关键帧上的观测得到的.

// pFrame是当前MapPoint的参考帧
const int level = pFrame->mvKeysUn[idxF].octave;
const float levelScaleFactor = pFrame->mvScaleFactors[level];
const int nLevels = pFrame->mnScaleLevels;
mfMaxDistance = dist*levelScaleFactor;
mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];

1
2
3
4
5
6
7
函数int PredictScale(const float ¤tDist, KeyFrame* pKF)和int PredictScale(const float ¤tDist, Frame* pF)根据某地图点到某帧的观测深度估计其在该帧图片上的层级,是上述过程的逆运算.

c u r r e n t D i s t m f M a x D i s t a n c e = 1. 2 l e v e l l e v e l = [ l o g 1.2 c u r r e n t D i s t m f M a x D i s t a n c e ] \displaystyle\frac{currentDist}{mfMaxDistance}=1.2^{level} \ level =[log_{1.2}\frac{currentDist}{mfMaxDistance}]
mfMaxDistance
currentDist
​
=1.2
level

level=[log
1.2
​

mfMaxDistance
currentDist
​
]

PredictScale @brief 根据地图点到光心的距离来预测一个类似特征金字塔的尺度

int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF) {
float ratio;
{
unique_lock lock(mMutexPos);
ratio = mfMaxDistance/currentDist;
}
​
int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
if(nScale<0)
nScale = 0;
else if(nScale>=pKF->mnScaleLevels)
nScale = pKF->mnScaleLevels-1;
​
return nScale;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
3.3 更新平均观测方向和距离: UpdateNormalAndDepth()
函数UpdateNormalAndDepth()更新当前地图点的平均观测方向和距离,其中平均观测方向是根据mObservations中所有观测到本地图点的关键帧取平均得到的;平均观测距离是根据参考关键帧得到的.

void MapPoint::UpdateNormalAndDepth() {
// step1. 获取地图点相关信息
map observations; // 获得观测到该地图点的所有关键帧
KeyFrame *pRefKF; // 观测到该点的参考关键帧（第一次创建时的关键帧）
cv::Mat Pos; // 地图点在世界坐标系中的位置
{
unique_lock lock1(mMutexFeatures);
unique_lock lock2(mMutexPos);
​
observations = mObservations;//包含可以观测到该地图点的关键帧数组
pRefKF = mpRefKF;
Pos = mWorldPos.clone();
}
​
// step2. 根据观测到地图点的所有关键帧 取平均,计算平均观测方向
// 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
// 初始值为0向量，累加为归一化向量，最后除以总数n
cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
int n = 0;
for (KeyFrame *pKF : observations.begin()) {
normal = normal + normali / cv::norm(mWorldPos - pKF->GetCameraCenter());
n++;
}
​
// step3. 根据参考帧计算平均观测距离
cv::Mat PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向地图点的向量，世界坐标系下的表示）
const float dist = cv::norm(PC); // 该点到参考关键帧相机的距离
// 观测到该地图点的当前帧的特征点在金字塔的第几层
const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;

// 当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数
const float levelScaleFactor = pRefKF->mvScaleFactors[level];    
const int nLevels = pRefKF->mnScaleLevels;  // 金字塔总层数，默认为8                    

​
{
unique_lock lock3(mMutexPos);
mfMaxDistance = dist * levelScaleFactor;// 观测到该点的距离上限
// 观测到该点的距离下限
mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
// 观测到该点的距离下限
mNormalVector = normal / n; // 获得地图点平均的观测方向
}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
地图点的平均观测距离是根据其参考关键帧计算的,那么参考关键帧KeyFrame* mpRefKF是如何指定的呢?

构造函数中,创建该地图点的参考帧被设为参考关键帧.

若当前地图点对参考关键帧的观测被删除(EraseObservation(KeyFrame* pKF)),则取第一个观测到当前地图点的关键帧做参考关键帧.

函数MapPoint::UpdateNormalAndDepth()的调用时机:

例如：

• @brief 双目和rgbd的地图初始化，比单目简单很多*
• 由于具有深度信息，直接生成MapPoints *
  void Tracking::StereoInitialization()

1
2
3
4
创建地图点时调用UpdateNormalAndDepth()初始化其观测信息.

if(z>0)
{
// 通过反投影得到该特征点的世界坐标系下3D坐标
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
// 将3D点构造为MapPoint
MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);

            // 为该MapPoint添加属性：
            // a.观测到该MapPoint的关键帧
            // b.该MapPoint的描述子
            // c.该MapPoint的平均观测方向和深度范围

            // a.表示该MapPoint可以被哪些KeyFrame的哪些特征点观测到
            pNewMP->AddObservation(pKFini,i);
            // b.从众多观测到该MapPoint的特征点中挑选区分度最高的描述子             
            pNewMP->ComputeDistinctiveDescriptors();
            // c.更新该MapPoint平均观测方向以及观测距离的范围
            pNewMP->UpdateNormalAndDepth();

            // 在地图中添加该MapPoint
            mpMap->AddMapPoint(pNewMP);
            // 表示该KeyFrame的哪个特征点可以观测到哪个3D点
            pKFini->AddMapPoint(pNewMP,i);

            // 将该MapPoint添加到当前帧的mvpMapPoints中
            // 为当前Frame的特征点与MapPoint之间建立索引
            mCurrentFrame.mvpMapPoints[i]=pNewMP;

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
地图点对关键帧的观测mObservations更新时,调用UpdateNormalAndDepth()初始化其观测信息.

具体体现：

跟踪局部地图添加或删除对关键帧的观测时、

LocalMapping线程删除冗余关键帧时，创建新的地图点的时候，或者检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点

或LoopClosing线程闭环矫正时

pMP->AddObservation(mpCurrentKeyFrame, i);
pMP->UpdateNormalAndDepth();

1
2
3
地图点世界坐标mWorldPos发生变化时(BA优化之后),调用UpdateNormalAndDepth()初始化其观测信息.

pMP->SetWorldPos(cvCorrectedP3Dw);
pMP->UpdateNormalAndDepth();

1
2
3
总结成一句话: 只要地图点本身或关键帧对该地图点的观测发生变化,就应该调用函数MapPoint::UpdateNormalAndDepth()更新其观测尺度和方向信息.

3.4 特征描述子
成员函数/变量 访问控制 意义
cv::Mat mDescriptor protected 当前关键点的特征描述子(所有描述子的中位数)
cv::Mat GetDescriptor() public mDescriptor的get方法
void ComputeDistinctiveDescriptors() public 计算mDescriptor
一个地图点在不同关键帧中对应不同的特征点和描述子,其特征描述子mDescriptor是其在所有观测关键帧中描述子的中位数(准确地说,该描述子与其他所有描述子的中值距离最小).

特征描述子的更新时机:

一旦某地图点对关键帧的观测mObservations发生改变,就调用函数MapPoint::ComputeDistinctiveDescriptors()更新该地图点的特征描述子.

特征描述子的用途:

在函数ORBmatcher::SearchByProjection()和ORBmatcher::Fuse()中,通过比较地图点的特征描述子与图片特征点描述子,实现将地图点与图像特征点的匹配(3D-2D匹配).

3.5 地图点的删除与替换
成员函数/变量 访问控制 意义
bool mbBad protected 坏点标记
bool isBad() public 查询当前地图点是否被删除(本质上就是查询mbBad)
void SetBadFlag() public 删除当前地图点
MapPoint* mpReplaced protected 用来替换当前地图点的新地图点
void Replace(MapPoint *pMP) public 使用地图点pMP替换当前地图点
3.6 地图点的删除: SetBadFlag()
变量mbBad用来表征当前地图点是否被删除.

删除地图点的各成员变量是一个较耗时的过程,因此函数SetBadFlag()删除关键点时采取先标记再清除的方式,具体的删除过程分为以下两步:

先将坏点标记mbBad置为true,逻辑上删除该地图点.(地图点的社会性死亡)
再依次清空当前地图点的各成员变量,物理上删除该地图点.(地图点的肉体死亡)
这样只有在设置坏点标记mbBad时需要加锁,之后的操作就不需要加锁了.

void MapPoint::SetBadFlag() {
map obs;
{
unique_lock lock1(mMutexFeatures);
unique_lock lock2(mMutexPos);
mbBad = true; // 标记mbBad,逻辑上删除当前地图点
obs = mObservations;
mObservations.clear();
}

// 删除关键帧对当前地图点的观测
for (KeyFrame *pKF : obs.begin()) {
    pKF->EraseMapPointMatch(mit->second);
}

​
// 在地图类上注册删除当前地图点,这里会发生内存泄漏
mpMap->EraseMapPoint(this);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
成员变量mbBad表示当前地图点逻辑上是否被删除,在后面用到地图点的地方,都要通过isBad()函数确认当前地图点没有被删除,再接着进行其它操作.

int KeyFrame::TrackedMapPoints(const int &minObs) {
// …

for (int i = 0; i < N; i++) {
    MapPoint *pMP = mvpMapPoints[i];
    if (pMP && !pMP->isBad()) {         // 依次检查该地图点物理上和逻辑上是否删除,若删除了就不对其操作
        // ...
    }
}

// ...

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
3.7 地图点的替换: Replace()
函数Replace(MapPoint* pMP)将当前地图点的成员变量叠加到新地图点pMP上.

void MapPoint::Replace(MapPoint *pMP) {
// 如果是同一地图点则跳过
if (pMP->mnId == this->mnId)
return;
​ //要替换当前地图点,有两个工作:
// 1. 将当前地图点的观测数据等其他数据都"叠加"到新的地图点上
// 2. 将观测到当前地图点的关键帧的信息进行更新

// step1. 逻辑上删除当前地图点
int nvisible, nfound;
map obs;
{
    unique_lock lock1(mMutexFeatures);
    unique_lock lock2(mMutexPos);
    obs = mObservations;//清除当前地图点的原有观测 
    mObservations.clear();//当前的地图点被删除了
    mbBad = true;//暂存当前地图点的可视次数和被找到的