ORB-SLAM2代码详解

1. ORB-SLAM2代码详解01_ORB-SLAM2代码运行流程

1 运行官方Demo

以TUM数据集为例,运行Demo的命令:

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml PATH_TO_SEQUENCE_FOLDER ASSOCIATIONS_FILE

rgbd_tum.cc的源码:

int main(int argc, char **argv) {// 判断输入参数个数if (argc != 5) {cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;return 1;}
​// step1. 读取图片及左右目关联信息vector<string> vstrImageFilenamesRGB;vector<string> vstrImageFilenamesD;vector<double> vTimestamps;string strAssociationFilename = string(argv[4]);LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);    // step2. 检查图片文件及输入文件的一致性int nImages = vstrImageFilenamesRGB.size();if (vstrImageFilenamesRGB.empty()) {cerr << endl << "No images found in provided path." << endl;return 1;} else if (vstrImageFilenamesD.size() != vstrImageFilenamesRGB.size()) {cerr << endl << "Different number of images for rgb and depth." << endl;return 1;}
​// step3. 创建SLAM对象,它是一个 ORB_SLAM2::System 类型变量ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);vector<float> vTimesTrack;vTimesTrack.resize(nImages);cv::Mat imRGB, imD;// step4. 遍历图片,进行SLAMfor (int ni = 0; ni < nImages; ni++) {// step4.1. 读取图片imRGB = cv::imread(string(argv[3]) + "/" + vstrImageFilenamesRGB[ni], CV_LOAD_IMAGE_UNCHANGED);imD = cv::imread(string(argv[3]) + "/" + vstrImageFilenamesD[ni], CV_LOAD_IMAGE_UNCHANGED);double tframe = vTimestamps[ni];// step4.2. 进行SLAMSLAM.TrackRGBD(imRGB, imD, tframe);// step4.3. 加载下一张图片double T = 0;if (ni < nImages - 1)T = vTimestamps[ni + 1] - tframe;else if (ni > 0)T = tframe - vTimestamps[ni - 1];
​if (ttrack < T)usleep((T - ttrack) * 1e6);}
​// step5. 停止SLAMSLAM.Shutdown();
}

运行程序rgbd_tum时传入了一个重要的配置文件TUM1.yaml,其中保存了相机参数和ORB特征提取参数:

%YAML:1.0
​
## 相机参数
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 
​
## ORB特征提取参数
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. 阅读代码之前你应该知道的事情

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 为什么要使用多线程?

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));// ...
   }
   

   开两个线程同时计算两个矩阵,在多核处理器上会加快运算速度.

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

   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.3.2 多线程中的锁

为防止多个线程同时操作同一变量造成混乱,引入锁机制:

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

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

一把锁在某个时刻只有一个线程能够拿到,如果程序执行到某个需要锁的位置,但是锁被别的线程拿着不释放的话,当前线程就会暂停下来;直到其它线程释放了这个锁,当前线程才能拿走锁并继续向下执行.

• 什么时候加锁和释放锁?

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

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

1.4 SLAM主类System

1.4.1 System`类是ORB-SLAM2系统的主类,先分析其主要的成员函数和成员变量:

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<Viewer *>(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线程及mpLocalMappermpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);// step2.3. 创建LoopClosing线程及mpLoopClosermpLoopCloser = 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);
}

LocalMapping和LoopClosing线程在System类中有对应的std::thread线程成员变量,为什么Tracking线程没有对应的std::thread成员变量?

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

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

1.4.3 跟踪函数

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

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);unique_lock<mutex> lock(mMutexState);mTrackingState = mpTracker->mState;mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;return Tcw;
}

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)的流程:

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

   下面成员变量从配置文件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}

2. 初始化用于计算描述子的pattern变量,pattern是用于计算描述子的256对坐标,其值写死在源码文件ORBextractor.cc里,在构造函数里做类型转换将其转换为const cv::Point*变量.

static int bit_pattern_31_[256*4] ={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));

3. 计算一个半径为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;
}

2.2 构建图像金字塔: ComputePyramid()

根据上述变量的值计算出下述成员变量:

函数void ORBextractor::ComputePyramid(cv::Mat image)逐层计算图像金字塔,对于每层图像进行以下两步:

1. 先进行图片缩放,缩放到mvInvScaleFactor对应尺寸.
2. 在图像外补一圈厚度为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.cols*scale), cvRound((float)image.rows*scale));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);            }}
}

copyMakeBorder函数实现了复制和padding填充,其参数BORDER_REFLECT_101参数指定对padding进行镜像填充.

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

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

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

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

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

void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& 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;              // 每一列有多少cellconst int nRows = height/W;             // 每一行有多少cellconst int wCell = ceil(width/nCols);    // 每个cell的宽度const int hCell = ceil(height/nRows);   // 每个cell的高度
​// 存储需要进行平均分配的特征点vector<cv::KeyPoint> vToDistributeKeys;// step1. 遍历每行和每列,依次分别用高低阈值搜索FAST特征点for(int i=0; i<nRows; i++) {const float iniY = minBorderY + i * hCell;const float maxY = iniY + hCell + 6;for(int j=0; j<nCols; j++) {const float iniX =minBorderX + j * wCell;const float maxX = iniX + wCell + 6;vector<cv::KeyPoint> 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);               }
}

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(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<KeyPoint>& keypoints, const vector<int>& umax)
{for (vector<KeyPoint>::iterator keypoint : keypoints) {// 调用IC_Angle 函数计算这个特征点的方向keypoint->angle = IC_Angle(image, keypoint->pt, umax);  }
}
​
static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{int m_01 = 0, m_10 = 0;         // 重心方向const uchar* center = &image.at<uchar> (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 + v*step], val_minus = center[u - v*step];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);
}

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$ which centered a corner pp and compare their intensity
  $$S=\left(\begin{array}{c}{p}_1,...p_n\\ q_1,...q_n\end{array}\right)\in {\mathbb{R}}^{(2\times 2)\times 256}$$

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

the descriptor (each bit ⟷⟷ each pair of points $(p_i,q_i)$:
$$f(n)=n\sum i=12i-1\tau (I;pi,qi),(n=256)$$

steered BRIEF

• 为了具有旋转不变性，引入该算法，但方差很小、相关性高
  $$\begin{gathered} R_{\theta }=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right] \\ {S}_{\theta }=R_{\theta }S=\left(\begin{array}{c}p\prime 1,\dots ,p\prime n\\ q\prime 1,\dots ,q\prime n\end{array}\right) \end{gathered}$$

  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<uchar>(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].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - 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;                              // 描述子本字节的bit0t0 = GET_VALUE(2); t1 = GET_VALUE(3);val |= (t0 < t1) << 1;                      // 描述子本字节的bit1t0 = GET_VALUE(4); t1 = GET_VALUE(5);val |= (t0 < t1) << 2;                      // 描述子本字节的bit2t0 = GET_VALUE(6); t1 = GET_VALUE(7);val |= (t0 < t1) << 3;                      // 描述子本字节的bit3t0 = GET_VALUE(8); t1 = GET_VALUE(9);val |= (t0 < t1) << 4;                      // 描述子本字节的bit4t0 = GET_VALUE(10); t1 = GET_VALUE(11);val |= (t0 < t1) << 5;                      // 描述子本字节的bit5t0 = GET_VALUE(12); t1 = GET_VALUE(13);val |= (t0 < t1) << 6;                      // 描述子本字节的bit6t0 = GET_VALUE(14); t1 = GET_VALUE(15);val |= (t0 < t1) << 7;                      // 描述子本字节的bit7
​//保存当前比较的出来的描述子的这个字节desc[i] = (uchar)val;}
}

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<KeyPoint>& _keypoints, OutputArray _descriptors) { // step1. 检查图像有效性if(_image.empty())return;Mat image = _image.getMat();assert(image.type() == CV_8UC1 );
​// step2. 构建图像金字塔ComputePyramid(image);
​// step3. 计算特征点并进行八叉树筛选vector<vector<KeyPoint> > 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();}
}

这个重载()运算符的用法被用在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);
}

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);
}

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 &timestamp) {// ...// 创建Frame对象mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight);// ...
}

• 由上述代码分析可知,每次完成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

3.1.2 与关键帧的观测关系: mObservations

成员变量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<mutex> lock(mMutexFeatures);// 如果已经添加过观测，返回if(mObservations.count(pKF)) return;// 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引mObservations[pKF]=idx;
​// 根据观测形式是单目还是双目更新观测计数变量nObsif(pKF->mvuRight[idx]>=0)nObs += 2; elsenObs++; 
}

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

函数GetIndexInKeyFrame()和IsInKeyFrame()( 检查该地图点是否在关键帧中（有对应的二维特征点）)就是对mObservations(能够观测到当前地图点的所有关键帧及该地图点在KF中的索引)的简单查询

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

3.2 观测尺度

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];    

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

$$\begin{gathered} \frac{currentDist}{mfMaxDistance}=1.2^{level} \\ level=[lo{g}_{1.2}\frac{currentDist}{mfMaxDistance}] \end{gathered}$$
PredictScale @brief 根据地图点到光心的距离来预测一个类似特征金字塔的尺度

int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF) {float ratio;{unique_lock<mutex> 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;
}

3.3 更新平均观测方向和距离: UpdateNormalAndDepth()

函数UpdateNormalAndDepth()更新当前地图点的平均观测方向和距离,其中平均观测方向是根据mObservations中所有观测到本地图点的关键帧取平均得到的;平均观测距离是根据参考关键帧得到的.

void MapPoint::UpdateNormalAndDepth() {// step1. 获取地图点相关信息map<KeyFrame *, size_t> observations; // 获得观测到该地图点的所有关键帧KeyFrame *pRefKF; // 观测到该点的参考关键帧（第一次创建时的关键帧）cv::Mat Pos; // 地图点在世界坐标系中的位置{unique_lock<mutex> lock1(mMutexFeatures);unique_lock<mutex> lock2(mMutexPos);
​observations = mObservations;//包含可以观测到该地图点的关键帧数组pRefKF = mpRefKF;            Pos = mWorldPos.clone();    }
​// step2. 根据观测到地图点的所有关键帧 取平均,计算平均观测方向// 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向// 初始值为0向量，累加为归一化向量，最后除以总数ncv::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<mutex> lock3(mMutexPos);mfMaxDistance = dist * levelScaleFactor;// 观测到该点的距离上限// 观测到该点的距离下限mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1]; // 观测到该点的距离下限mNormalVector = normal / n;    // 获得地图点平均的观测方向}
}

地图点的平均观测距离是根据其参考关键帧计算的,那么参考关键帧KeyFrame* mpRefKF是如何指定的呢?

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

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

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

例如：

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

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

   if(z>0)
   {// 通过反投影得到该特征点的世界坐标系下3D坐标cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);// 将3D点构造为MapPointMapPoint* 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();// 在地图中添加该MapPointmpMap->AddMapPoint(pNewMP);// 表示该KeyFrame的哪个特征点可以观测到哪个3D点pKFini->AddMapPoint(pNewMP,i);// 将该MapPoint添加到当前帧的mvpMapPoints中// 为当前Frame的特征点与MapPoint之间建立索引mCurrentFrame.mvpMapPoints[i]=pNewMP;
   }           

2. 地图点对关键帧的观测mObservations更新时,调用UpdateNormalAndDepth()初始化其观测信息.

   具体体现：

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

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

   或**LoopClosing线程闭环矫正**时

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

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

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

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

3.4 特征描述子

一个地图点在不同关键帧中对应不同的特征点和描述子,其特征描述子mDescriptor是其在所有观测关键帧中描述子的中位数(准确地说,该描述子与其他所有描述子的中值距离最小).

• 特征描述子的更新时机:

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

• 特征描述子的用途:

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

3.5 地图点的删除与替换

3.6 地图点的删除: SetBadFlag()

变量mbBad用来表征当前地图点是否被删除.

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

• 先将坏点标记mbBad置为true,逻辑上删除该地图点.(地图点的社会性死亡)
• 再依次清空当前地图点的各成员变量,物理上删除该地图点.(地图点的肉体死亡)

这样只有在设置坏点标记mbBad时需要加锁,之后的操作就不需要加锁了.

void MapPoint::SetBadFlag() {map<KeyFrame *, size_t> obs;{unique_lock<mutex> lock1(mMutexFeatures);unique_lock<mutex> lock2(mMutexPos);mbBad = true;           // 标记mbBad,逻辑上删除当前地图点obs = mObservations;mObservations.clear();}// 删除关键帧对当前地图点的观测for (KeyFrame *pKF : obs.begin()) {pKF->EraseMapPointMatch(mit->second);}
​// 在地图类上注册删除当前地图点,这里会发生内存泄漏mpMap->EraseMapPoint(this);
}

成员变量mbBad表示当前地图点逻辑上是否被删除,在后面用到地图点的地方,都要通过isBad()函数确认当前地图点没有被删除,再接着进行其它操作.

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

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<KeyFrame *, size_t> obs;{unique_lock<mutex> lock1(mMutexFeatures);unique_lock<mutex> lock2(mMutexPos);obs = mObservations;//清除当前地图点的原有观测 mObservations.clear();//当前的地图点被删除了mbBad = true;//暂存当前地图点的可视次数和被找到的次数nvisible = mnVisible;nfound = mnFound;mpReplaced = pMP; //指明当前地图点已经被指定的地图点替换了}
​
// 所有能观测到原地图点的关键帧都要复制到替换的地图点上
//- 将观测到当前地图的的关键帧的信息进行更新// step2. 将当地图点的数据叠加到新地图点上for (map<KeyFrame *, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++) {// Replace measurement in keyframeKeyFrame *pKF = mit->first;if (!pMP->IsInKeyFrame(pKF)) {pKF->ReplaceMapPointMatch(mit->second, pMP);pMP->AddObservation(pKF, mit->second);} else {pKF->EraseMapPointMatch(mit->second);}}
​pMP->IncreaseFound(nfound);pMP->IncreaseVisible(nvisible);pMP->ComputeDistinctiveDescriptors();
​// step3. 删除当前地图点mpMap->EraseMapPoint(this);
}

3.8 MapPoint类的用途

MapPoint的生命周期

针对MapPoint的生命周期,我们关心以下3个问题:

• 创建MapPoint的时机: 几乎与UpdateNormalAndDepth调用时机同步

  1. Tracking线程中初始化过程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())
  2. Tracking线程中创建新的关键帧(Tracking::CreateNewKeyFrame())
  3. Tracking线程中恒速运动模型跟踪(Tracking::TrackWithMotionModel())也会产生临时地图点，但这些临时地图点在跟踪成功后会被马上删除(那跟踪失败怎么办?跟踪失败的话不会产生关键帧,这些地图点也不会被注册进地图).
  4. LocalMapping线程中创建新地图点的步骤(LocalMapping::CreateNewMapPoints())会将当前关键帧与前一关键帧进行匹配,生成新地图点.用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳

• 更新**创建MapPoint的时机 ** 几乎与UpdateNormalAndDepth调用时机同步

  1. LocalMapping`线程中LocalMapping::ProcessNewKeyFrame()处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等中更新地图点，在这个过程中需要对当前处理的这个关键帧中的所有的地图点展开遍历，如果地图点不是来自当前帧的观测（比如来自局部地图点），为当前地图点添加观测，获得该点的平均观测方向和观测距离范围，更新地图点的最佳描述子；如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验
  2. LocalMapping线程中LocalMapping::SearchInNeighbors()， 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点

• 替换MapPoint的时机:

  1. LoopClosing线程中闭环矫正(LoopClosing::CorrectLoop())时当前关键帧和闭环关键帧上的地图点发生冲突时,会使用闭环关键帧的地图点替换当前关键帧的地图点.

     @brief 闭环矫正步骤

     1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置（相连关键帧—当前帧）

     2. 将闭环帧以及闭环帧相连的关键帧的地图点和与当前帧相连的关键帧的点进行匹配（当前帧+相连关键帧—闭环帧+相连关键帧）

     3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph

     4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整

     5. 创建线程进行全局Bundle Adjustment

  2. LoopClosing线程中闭环矫正函数LoopClosing::CorrectLoop()会调用LoopClosing::SearchAndFuse()将闭环关键帧的共视关键帧组中所有地图点投影到当前关键帧的共视关键帧组中,发生冲突时就会替换.

• 删除MapPoint的时机:

1. LocalMapping线程中删除恶劣地图点的步骤(LocalMapping::MapPointCulling()).
2. 删除关键帧的函数KeyFrame::SetBadFlag()会调用函数MapPoint::EraseObservation()删除地图点对关键帧的观测,若地图点对关键帧的观测少于2,则地图点无法被三角化,就删除该地图点.

4. ORB-SLAM2代码详解04_帧Frame

4.1 各成员函数/变量

4.1.1 相机相关信息

Frame类与相机相关的参数大部分设为static类型,整个系统内的所有Frame对象共享同一份相机参数.

这些参数首先由Tracking对象从配置文件TUM1.yaml内读入,再传给Frame类的构造函数,第一次调用Frame的构造函数时为这些成员变量赋值。

Tracking::Tracking(const string &strSettingPath, ...) {
​// 从配置文件中读取相机参数并构造内参矩阵cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);float fx = fSettings["Camera.fx"];float fy = fSettings["Camera.fy"];float cx = fSettings["Camera.cx"];float cy = fSettings["Camera.cy"];
​cv::Mat K = cv::Mat::eye(3, 3, CV_32F);K.at<float>(0, 0) = fx;K.at<float>(1, 1) = fy;K.at<float>(0, 2) = cx;K.at<float>(1, 2) = cy;K.copyTo(mK);
​// ...
}
​
// 每传来一帧图像,就调用一次该函数
cv::Mat Tracking::GrabImageStereo(..., const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp) {mCurrentFrame = Frame(mImGray, mK, mDistCoef, mbf, mThDepth);
​Track();
​// ...
}
​
// Frame构造函数
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth): mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {// ...// 第一次调用Frame()构造函数时为所有static变量赋值if (mbInitialComputations) {fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;// ...mbInitialComputations = false;      // 赋值完毕后将mbInitialComputations复位}
​mb = mbf / fx;
}

4.2 特征点提取

在Frame类构造函数中调用成员变量mpORBextractorLeft和mpORBextractorRight的()运算符进行特征点提取.

mvKeys、 mvKeysUn、 mvuRight、 mvDepth的坐标索引是对应的,也就是说对于第i个图像特征点:

• 其畸变矫正前的左目特征点是mvKeys[i].有畸变
• 其畸变矫正后的左目特征点是mvKeysUn[i].无畸变
• 其在右目图片中对应特征点的横坐标为mvuRight[i],纵坐标与mvKeys[i]的纵坐标相同.
• 特征点的深度是mvDepth[i].

对于单目特征点(单目相机输入的特征点或没有找到右目匹配的左目图像特征点),其mvuRight和mvDepth均为-1.

4.2.1 特征点提取: ExtractORB()

void Frame::ExtractORB(int flag, const cv::Mat &im) {if (flag == 0)      // flag==0, 表示对左图提取ORB特征点(*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);else                // flag==1, 表示对右图提取ORB特征点(*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}

4.3 ORB-SLAM2对双目/RGBD特征点的预处理

双目/RGBD相机中可以得到特征点的立体信息,包括右目特征点信息(mvuRight)、特征点深度信息(mvDepth)

• 对于双目相机,通过双目特征点匹配关系计算特征点的深度值.
• 对于RGBD相机,根据特征点深度构造虚拟的右目图像特征点.

通过这种预处理,在后面SLAM系统的其他部分中不再区分双目特征点和RGBD特征点,它们以双目特征点的形式被处理.(仅通过判断mvuRight[idx]判断某特征点是否有深度).

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint *> &vpMapPoints, const float th) {// ...for (size_t idx : vIndices) {if (F.mvuRight[idx] > 0) {      // 通过判断 mvuRight[idx] 判断该特征点是否有深度// 针对有深度的特征点特殊处理} else {// 针对单目特征点的特殊处理}}// ...
}

4.4 双目视差公式

观测距离,基线,焦距,视差,根据三角形相似性:
$$\frac{z}{b}=\frac{z-f}{b-d}$$
得到:
$$d=\frac{z.f}{z}$$

4.5 双目特征点的处理:双目图像特征点匹配: ComputeStereoMatches()

双目相机分别提取到左右目特征点后对特征点进行双目匹配,并通过双目视差估计特征点深度.双目特征点匹配步骤:

1. 粗匹配: 根据特征点描述子距离和金字塔层级判断匹配.粗匹配关系是按行寻找的,对于左目图像中每个特征点,在右目图像对应行上寻找匹配特征点.
2. 精匹配: 根据特征点周围窗口内容相似度判断匹配.
3. 亚像素插值: 将特征点相似度与匹配坐标之间拟合成二次曲线,寻找最佳匹配位置(得到的是一个小数).
4. 记录右目匹配mvuRight和深度mvDepth信息.
5. 离群点筛选: 以平均相似度的2.1倍为标准,筛选离群点.

void Frame::ComputeStereoMatches() {
​mvuRight = vector<float>(N, -1.0f);mvDepth = vector<float>(N, -1.0f);// step0. 右目图像特征点逐行统计: 将右目图像中每个特征点注册到附近几行上vector<vector<size_t> > vRowIndices(nRows, vector<size_t>());   // 图像每行的1右目特征点索引for (int iR = 0; iR < mvKeysRight.size(); iR++) {const cv::KeyPoint &kp = mvKeysRight[iR];const float &kpY = kp.pt.y;const int maxr = ceil(kpY + 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);const int minr = floor(kpY - 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);for (int yi = minr; yi <= maxr; yi++)vRowIndices[yi].push_back(iR);}
​// step1. + 2. 粗匹配+精匹配const float minZ = mb, minD = 0, maxD = mbf/minZ; //根据视差公式计算两个特征点匹配搜索的范围const int thOrbDist = (ORBmatcher::TH_HIGH + ORBmatcher::TH_LOW) / 2;
​vector<pair<int, int> > vDistIdx;       // 保存特征点匹配
​for (int iL = 0; iL < N; iL++) {
​const cv::KeyPoint &kpL = mvKeys[iL];const int &levelL = kpL.octave;const float &vL = kpL.pt.y, &uL = kpL.pt.x;
​const vector<size_t> &vCandidates = vRowIndices[vL];if (vCandidates.empty()) continue;// step1. 粗匹配,根据特征点描述子和金字塔层级进行粗匹配int bestDist = ORBmatcher::TH_HIGH;size_t bestIdxR = 0;const cv::Mat &dL = mDescriptors.row(iL);for (size_t iC = 0; iC < vCandidates.size(); iC++) {
​const size_t iR = vCandidates[iC];const cv::KeyPoint &kpR = mvKeysRight[iR];if (kpR.octave < levelL - 1 || kpR.octave > levelL + 1)continue;const float &uR = kpR.pt.x;
​if (uR >= minU && uR <= maxU) {const cv::Mat &dR = mDescriptorsRight.row(iR);const int dist = ORBmatcher::DescriptorDistance(dL, dR);if (dist < bestDist) {bestDist = dist;bestIdxR = iR;}}}
​// step2. 精匹配: 滑动窗口匹配,根据匹配点周围5✖5窗口寻找精确匹配if (bestDist < thOrbDist) {const float uR0 = mvKeysRight[bestIdxR].pt.x;const float scaleFactor = mvInvScaleFactors[kpL.octave];const float scaleduL = round(kpL.pt.x * scaleFactor);const float scaledvL = round(kpL.pt.y * scaleFactor);const float scaleduR0 = round(uR0 * scaleFactor);const int w = 5;cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduL - w, scaleduL + w + 1);IL.convertTo(IL, CV_32F);IL = IL - IL.at<float>(w, w) * cv::Mat::ones(IL.rows, IL.cols, CV_32F);int bestDist = INT_MAX;int bestincR = 0;const int L = 5;vector<float> vDists;vDists.resize(2 * L + 1);const float iniu = scaleduR0 + L - w;const float endu = scaleduR0 + L + w + 1;for (int incR = -L; incR <= +L; incR++) {cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduR0 + incR - w, scaleduR0 + incR + w + 1);IR.convertTo(IR, CV_32F);IR = IR - IR.at<float>(w, w) * cv::Mat::ones(IR.rows, IR.cols, CV_32F);float dist = cv::norm(IL, IR, cv::NORM_L1);if (dist < bestDist) {bestDist = dist;bestincR = incR;}vDists[L + incR] = dist;}// step3. 亚像素插值: 将特征点匹配距离拟合成二次曲线,寻找二次曲线最低点(是一个小数)作为最优匹配点坐标const float dist1 = vDists[L + bestincR - 1];const float dist2 = vDists[L + bestincR];const float dist3 = vDists[L + bestincR + 1];const float deltaR = (dist1 - dist3) / (2.0f * (dist1 + dist3 - 2.0f * dist2));
​// step4. 记录特征点的右目和深度信息float bestuR = mvScaleFactors[kpL.octave] * ((float) scaleduR0 + (float) bestincR + deltaR);float disparity = (uL - bestuR);if (disparity >= minD && disparity < maxD) {mvDepth[iL] = mbf / disparity;mvuRight[iL] = bestuR;vDistIdx.push_back(pair<int, int>(bestDist, iL));}}}// step5. 删除离群点: 匹配距离大于平均匹配距离2.1倍的视为误匹配sort(vDistIdx.begin(), vDistIdx.end());const float median = vDistIdx[vDistIdx.size() / 2].first;const float thDist = 1.5f * 1.4f * median;for (int i = vDistIdx.size() - 1; i >= 0; i--) {if (vDistIdx[i].first < thDist)break;else {mvuRight[vDistIdx[i].second] = -1;mvDepth[vDistIdx[i].second] = -1;}}
}

4.6 RBGD特征点的处理: 根据深度信息构造虚拟右目图像: ComputeStereoFromRGBD()

对于RGB特征点,根据深度信息构造虚拟右目图像

void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth) {// 初始化 右目 和 深度 信息mvuRight = vector<float>(N, -1);mvDepth = vector<float>(N, -1);
​for (int i = 0; i < N; i++) {const cv::KeyPoint &kp = mvKeys[i];const cv::KeyPoint &kpU = mvKeysUn[i];// 从未畸变矫正的深度图中获取深度信息,从校正过后的左图中获取特征点位置信息,构造虚拟右目const float d = imDepth.at<float>(kp.pt.y, kp.pt.x);if (d > 0) {mvDepth[i] = d;mvuRight[i] = kpU.pt.x - mbf / d;}}
}

4.7 畸变矫正: UndistortKeyPoints()

实际上,畸变矫正只对单目和RGBD相机输入图像有效,双目相机的畸变矫正参数均为0,因为双目相机数据集在发布之前预先做了双目矫正.

• RGBD相机输入配置文件TUM1.yaml

  Camera.k1: 0.262383
  Camera.k2: -0.953104
  Camera.p1: -0.005358
  Camera.p2: 0.002628
  Camera.k3: 1.163314
  ​
  #....

• 双目相机输入配置文件EuRoC.yaml

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
​
# ...

双目矫正效果如下,双目矫正将两个相机的成像平面矫正到同一平面上.双目矫正之后两个相机的极线相互平行,极点在无穷远处,这也是我们在函数ComputeStereoMatches()中做极线搜索的理论基础.

双目匹配流程：

/*双目匹配函数

* 为左图的每一个特征点在右图中找到匹配点 \n

* 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n ‘

* 这里所说的SAD是一种双目立体视觉匹配算法，可参考[https://blog.csdn.net/u012507022/article/details/51446891]

* 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对，然后利用抛物线拟合得到亚像素精度的匹配 \n

* 这里所谓的亚像素精度，就是使用这个拟合得到一个小于一个单位像素的修正量，这样可以取得更好的估计结果，计算出来的点的深度也就越准确

* 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)*/

流程细节：

两帧图像稀疏立体匹配（即：ORB特征点匹配，非逐像素的密集匹配，但依然满足行对齐）

输入：两帧立体矫正后的图像img_left 和 img_right 对应的orb特征点集

过程：

1. 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断.

2. 粗匹配. 根据步骤1的结果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi

3. 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果

4. 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差

5. 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。

6. 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配

7. 输出：稀疏特征点视差图/深度图（亚像素精度）mvDepth 匹配结果 mvuRight

UndistortKeyPoints()函数和ComputeImageBounds()内调用了cv::undistortPoints()函数对特征点进行畸变矫正

void Frame::UndistortKeyPoints() {// step1. 若输入图像是双目图像,则已做好了双目矫正,其畸变参数为0if (mDistCoef.at<float>(0) == 0.0) {mvKeysUn = mvKeys;return;}
​// 将特征点坐标转为undistortPoints()函数要求的格式cv::Mat mat(N, 2, CV_32F);for (int i = 0; i < N; i++) {mat.at<float>(i, 0) = mvKeys[i].pt.x;mat.at<float>(i, 1) = mvKeys[i].pt.y;}mat = mat.reshape(2);// 进行畸变矫正cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);
​// 记录校正后的特征点mat = mat.reshape(1);mvKeysUn.resize(N);for (int i = 0; i < N; i++) {cv::KeyPoint kp = mvKeys[i];mvKeysUn[i].pt.x = mat.at<float>(i, 0);mvKeysUn[i].pt.y = mat.at<float>(i, 1);}
}
​
// 通过计算图片顶点畸变矫正后的坐标来计算畸变矫正后的图片有效范围
void Frame::ComputeImageBounds(const cv::Mat &imLeft) {if (mDistCoef.at<float>(0) != 0.0) {// 4个顶点坐标cv::Mat mat(4, 2, CV_32F);mat.at<float>(0, 0) = 0.0;         //左上mat.at<float>(0, 1) = 0.0;mat.at<float>(1, 0) = imLeft.cols; //右上mat.at<float>(1, 1) = 0.0;mat.at<float>(2, 0) = 0.0;         //左下mat.at<float>(2, 1) = imLeft.rows;mat.at<float>(3, 0) = imLeft.cols; //右下mat.at<float>(3, 1) = imLeft.rows;// 畸变矫正mat = mat.reshape(2);cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);mat = mat.reshape(1);// 记录图片有效范围mnMinX = min(mat.at<float>(0, 0), mat.at<float>(2, 0));     //左上和左下横坐标最小的mnMaxX = max(mat.at<float>(1, 0), mat.at<float>(3, 0));     //右上和右下横坐标最大的mnMinY = min(mat.at<float>(0, 1), mat.at<float>(1, 1));     //左上和右上纵坐标最小的mnMaxY = max(mat.at<float>(2, 1), mat.at<float>(3, 1));     //左下和右下纵坐标最小的} else {mnMinX = 0.0f;mnMaxX = imLeft.cols;mnMinY = 0.0f;mnMaxY = imLeft.rows;}
}

4.8 特征点分配: AssignFeaturesToGrid()

在对特征点进行预处理后,将特征点分配到48行64列的网格中以加速匹配

成员变量std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]是一个二维数组,数组中每个元素是对应网格的所有特征点索引列表.

static成员变量mfGridElementWidthInv、mfGridElementHeightInv表示网格宽度/高度,它们在第一次调用Frame构造函数时被计算赋值.

// Frame构造函数
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth): mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {// ...ComputeImageBounds(imGray);     // 计算去畸变后图像的边界// 计算特征点网格宽度/高度mfGridElementWidthInv = static_cast<float>(FRAME_GRID_COLS) / static_cast<float>(mnMaxX - mnMinX);mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) / static_cast<float>(mnMaxY - mnMinY);// 第一次调用Frame()构造函数时为所有static变量赋值if (mbInitialComputations) {fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;mbInitialComputations = false;      // 赋值完毕后将mbInitialComputations复位}
​mb = mbf / fx;
}

函数AssignFeaturesToGrid()将特征点分配到网格中

void Frame::AssignFeaturesToGrid() {for (int i = 0; i < N; i++) {// 遍历特征点,将每个特征点索引加入到对应网格中const cv::KeyPoint &kp = mvKeysUn[i];int nGridPosX, nGridPosY;if (PosInGrid(kp, nGridPosX, nGridPosY))mGrid[nGridPosX][nGridPosY].push_back(i);}
}

函数vector GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)获取点(y,x)周围半径为r的圆域内所有特征点编号.

4.9 构造函数: Frame()

Frame()构造函数依次进行上面介绍的步骤:

// 双目相机Frame构造函数
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor *extractorLeft, ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth): mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_cast<KeyFrame *>(NULL)) {// step0. 帧ID自增mnId = nNextId++;
​// step1. 计算金字塔参数mnScaleLevels = mpORBextractorLeft->GetLevels();mfScaleFactor = mpORBextractorLeft->GetScaleFactor();mfLogScaleFactor = log(mfScaleFactor);mvScaleFactors = mpORBextractorLeft->GetScaleFactors();mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
​// step2. 提取双目图像特征点thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);thread threadRight(&Frame::ExtractORB, this, 1, imRight);threadLeft.join();threadRight.join();
​N = mvKeys.size();if (mvKeys.empty())return;
​// step3. 畸变矫正,实际上UndistortKeyPoints()不对双目图像进行矫正UndistortKeyPoints();
​// step4. 双目图像特征点匹配ComputeStereoMatches();// step5. 第一次调用构造函数时计算static变量if (mbInitialComputations) {ComputeImageBounds(imLeft);mfGridElementWidthInv = static_cast<float>(FRAME_GRID_COLS) / static_cast<float>(mnMaxX - mnMinX);mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) / static_cast<float>(mnMaxY - mnMinY);fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;// 计算完成,标志复位mbInitialComputations = false;}mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));    // 初始化本帧的地图点mvbOutlier = vector<bool>(N, false);    // 标记当前帧的地图点不是外点mb = mbf / fx;      // 计算双目基线长度
​// step6. 将特征点分配到网格中AssignFeaturesToGrid();
}

// RGBD相机Frame构造函数
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth): mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast<ORBextractor *>(NULL)), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {// step0. 帧ID自增mnId = nNextId++;
​// step1. 计算金字塔参数mnScaleLevels = mpORBextractorLeft->GetLevels();mfScaleFactor = mpORBextractorLeft->GetScaleFactor();mfLogScaleFactor = log(mfScaleFactor);mvScaleFactors = mpORBextractorLeft->GetScaleFactors();mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
​// step2. 提取左目图像特征点ExtractORB(0, imGray);
​N = mvKeys.size();if (mvKeys.empty())return;
​// step3. 畸变矫正UndistortKeyPoints();// step4. 根据深度信息构造虚拟右目图像ComputeStereoFromRGBD(imDepth);
​mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));mvbOutlier = vector<bool>(N, false);
​// step5. 第一次调用构造函数时计算static变量if (mbInitialComputations) {ComputeImageBounds(imLeft);mfGridElementWidthInv = static_cast<float>(FRAME_GRID_COLS) / static_cast<float>(mnMaxX - mnMinX);mfGridElementHeightInv = static_cast<float>(FRAME_GRID_ROWS) / static_cast<float>(mnMaxY - mnMinY);fx = K.at<float>(0, 0);fy = K.at<float>(1, 1);cx = K.at<float>(0, 2);cy = K.at<float>(1, 2);invfx = 1.0f / fx;invfy = 1.0f / fy;// 计算完成,标志复位mbInitialComputations = false;}mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));    // 初始化本帧的地图点mvbOutlier = vector<bool>(N, false);    // 标记当前帧的地图点不是外点mb = mbf / fx;      // 计算双目基线长度
​
​// step6. 将特征点分配到网格中AssignFeaturesToGrid();
}

4.10 Frame类的用途

Tracking类有两个Frame类型的成员变量

Tracking线程每收到一帧图像,就调用函数Tracking::GrabImageMonocular()、Tracking::GrabImageStereo()或Tracking::GrabImageRGBD()创建一个Frame对象,赋值给mCurrentFrame.

// 每传来一帧图像,就调用一次这个函数
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp) {mImGray = im;// 图像通道转换if (mImGray.channels() == 3) {if (mbRGB)cvtColor(mImGray, mImGray, CV_RGB2GRAY);elsecvtColor(mImGray, mImGray, CV_BGR2GRAY);} else if (mImGray.channels() == 4) {if (mbRGB)cvtColor(mImGray, mImGray, CV_RGBA2GRAY);elsecvtColor(mImGray, mImGray, CV_BGRA2GRAY);}
​// 构造Frameif (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YETmCurrentFrame = Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);elsemCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
​// 跟踪Track();
​// 返回当前帧的位姿return mCurrentFrame.mTcw.clone();
}

Track()函数跟踪结束后,会将mCurrentFrame赋值给mLastFrame

void Tracking::Track() {// 进行跟踪// ...// 将当前帧记录为上一帧mLastFrame = Frame(mCurrentFrame);// ...
}

除了少数被选为KeyFrame的帧以外,大部分Frame对象的作用仅在于Tracking线程内追踪当前帧位姿,不会对LocalMapping线程和LoopClosing线程产生任何影响,在mLastFrame和mCurrentFrame更新之后就被系统销毁了.

5. ORB-SLAM2代码详解05_关键帧KeyFrame

5.1 各成员函数/变量

5.1.1 共视图: mConnectedKeyFrameWeights

能看到同一地图点的两关键帧之间存在共视关系,共视地图点的数量被称为权重.

共视图结构由3个成员变量维护:

• mConnectedKeyFrameWeights是一个std::map,无序地保存当前关键帧的共视关键帧及权重.
• mvpOrderedConnectedKeyFrames和mvOrderedWeights按权重降序分别保存当前关键帧的共视关键帧列表和权重列表.

5.1.2 基于对地图点的观测重新构造共视图: UpdateConnections()

这3个变量由函数KeyFrame::UpdateConnections()进行初始化和维护,基于当前关键帧看到的地图点信息重新生成共视关键帧.

共视图结构由3个成员变量维护:

• mConnectedKeyFrameWeights是一个std::map,无序地保存当前关键帧的共视关键帧及权重.
• mvpOrderedConnectedKeyFrames和mvOrderedWeights按权重降序分别保存当前关键帧的共视关键帧列表和 权重列表.

5.1.3 基于对地图点的观测重新构造共视图: UpdateConnections()

这3个变量由函数KeyFrame::UpdateConnections()进行初始化和维护,基于当前关键帧看到的地图点信息重新生成共视关键帧.

void KeyFrame::UpdateConnections() {// 1. 通过遍历当前帧地图点获取其与其它关键帧的共视程度,存入变量KFcounter中vector<MapPoint *> vpMP;{unique_lock<mutex> lockMPs(mMutexFeatures);vpMP = mvpMapPoints;}map<KeyFrame *, int> KFcounter; for (MapPoint *pMP : vpMP) {map<KeyFrame *, size_t> observations = pMP->GetObservations();for (map<KeyFrame *, size_t>::iterator mit = observations.begin(); mit != observations.end(); mit++) {if (mit->first->mnId == mnId)       // 与当前关键帧本身不算共视continue;KFcounter[mit->first]++;}}// step2. 找到与当前关键帧共视程度超过15的关键帧,存入变量vPairs中vector<pair<int, KeyFrame *> > vPairs;int th = 15;int nmax = 0;KeyFrame *pKFmax = NULL;   for (map<KeyFrame *, int>::iterator mit = KFcounter.begin(), mend = KFcounter.end(); mit != mend; mit++) {if (mit->second > nmax) {nmax = mit->second;pKFmax = mit->first;}if (mit->second >= th) {vPairs.push_back(make_pair(mit->second, mit->first));(mit->first)->AddConnection(this, mit->second); // 对超过阈值的共视边建立连接}}
​
//  step3. 对关键帧按照共视权重降序排序,存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中sort(vPairs.begin(), vPairs.end());list<KeyFrame *> lKFs;list<int> lWs;for (size_t i = 0; i < vPairs.size(); i++) {lKFs.push_front(vPairs[i].second);lWs.push_front(vPairs[i].first);}{unique_lock<mutex> lockCon(mMutexConnections);mConnectedKeyFrameWeights = KFcounter;mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
​// step4. 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧if (mbFirstConnection && mnId != 0) {mpParent = mvpOrderedConnectedKeyFrames.front();mpParent->AddChild(this);mbFirstConnection = false;}}
}

只要关键帧与地图点间的连接关系发生变化(包括关键帧创建和地图点重新匹配关键帧特征点),函数KeyFrame::UpdateConnections()就会被调用.具体来说,函数KeyFrame::UpdateConnections()的调用时机包括:

• Tracking线程中初始化函数Tracking::StereoInitialization()或Tracking::MonocularInitialization()函数创建关键帧后会调用KeyFrame::UpdateConnections()初始化共视图信息.
• LocalMapping线程接受到新关键帧时会调用函数LocalMapping::ProcessNewKeyFrame()处理跟踪过程中加入的地图点,之后会调用KeyFrame::UpdateConnections()初始化共视图信息.(实际上这里处理的是Tracking线程中函数Tracking::CreateNewKeyFrame()创建的关键帧)
• LocalMapping线程处理完毕缓冲队列内所有关键帧后会调用LocalMapping::SearchInNeighbors()融合当前关键帧和共视关键帧间的重复地图点,之后会调用KeyFrame::UpdateConnections()更新共视图信息.
• LoopClosing线程闭环矫正函数LoopClosing::CorrectLoop()会多次调用KeyFrame::UpdateConnections()更新共视图信息.

函数AddConnection(KeyFrame* pKF, const int &weight)和EraseConnection(KeyFrame* pKF)先对变量mConnectedKeyFrameWeights进行修改,再调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeights.

这3个函数都只在函数KeyFrame::UpdateConnections()内部被调用了,应该设为私有成员函数.

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) {// step1. 修改变量mConnectedKeyFrameWeights{unique_lock<mutex> lock(mMutexConnections);if (!mConnectedKeyFrameWeights.count(pKF) || mConnectedKeyFrameWeights[pKF] != weight)mConnectedKeyFrameWeights[pKF] = weight;elsereturn;}// step2. 调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeightsUpdateBestCovisibles();
}
​
​
void KeyFrame::EraseConnection(KeyFrame *pKF) {// step1. 修改变量mConnectedKeyFrameWeightsbool bUpdate = false;{unique_lock<mutex> lock(mMutexConnections);if (mConnectedKeyFrameWeights.count(pKF)) {mConnectedKeyFrameWeights.erase(pKF);bUpdate = true;}}
​// step2. 调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeightsif (bUpdate)UpdateBestCovisibles();
}
​
void KeyFrame::UpdateBestCovisibles() {    unique_lock<mutex> lock(mMutexConnections);// 取出所有关键帧进行排序,排序结果存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中vector<pair<int, KeyFrame *> > vPairs;vPairs.reserve(mConnectedKeyFrameWeights.size());for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)vPairs.push_back(make_pair(mit->second, mit->first));
​sort(vPairs.begin(), vPairs.end());list<KeyFrame *> lKFs; list<int> lWs; for (size_t i = 0, iend = vPairs.size(); i < iend; i++) {lKFs.push_front(vPairs[i].second);lWs.push_front(vPairs[i].first);}
​mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

5.2 生成树: mpParent、mspChildrens

生成树是一种稀疏连接,以最小的边数保存图中所有节点.对于含有N个节点的图,只需构造一个N-1条边的最小生成树就可以将所有节点连接起来.

下图表示含有一个10个节点,20条边的稠密图;粗黑线代表其最小生成树,只需9条边即可将所有节点连接起来.

在ORB-SLAM2中,保存所有关键帧构成的最小生成树(优先选择权重大的边作为生成树的边),在回环闭合时只需对最小生成树做BA优化就能以最小代价优化所有关键帧和地图点的位姿,相比于优化共视图大大减少了计算量.(实际上并没有对最小生成树做BA优化,而是对包含生成树的本质图做BA优化)

生成树结构由成员变量mpParent和mspChildrens维护.我们主要关注生成树结构发生改变的时机.

• 关键帧增加到生成树中的时机:

  成功创建关键帧之后会调用函数KeyFrame::UpdateConnections(),该函数第一次被调用时会将该新关键帧加入到生成树中.

  新关键帧的父关键帧会被设为其共视程度最高的共视关键帧.

void KeyFrame::UpdateConnections() {// 更新共视图信息// ...// 更新关键帧信息: 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧// 该操作会改变当前关键帧的成员变量mpParent和父关键帧的成员变量mspChildrensunique_lock<mutex> lockCon(mMutexConnections);if (mbFirstConnection && mnId != 0) {mpParent = mvpOrderedConnectedKeyFrames.front();mpParent->AddChild(this);mbFirstConnection = false;}
}

• 共视图的改变(除了删除关键帧以外)不会引发生成树的改变.
• 只有当某个关键帧删除时,与其相连的生成树结构在会发生改变.(因为生成树是个单线联系的结构,没有冗余,一旦某关键帧删除了就得更新树结构才能保证所有关键帧依旧相连).生成树结构改变的方式类似于最小生成树算法中的加边法,见后文对函数setbadflag()的分析.

5.3 关键帧的删除

与MapPoint类似,函数KeyFrame::SetBadFlag()对KeyFrame的删除过程也采取先标记再清除的方式: 先将坏帧标记mBad置为true,再依次处理其各成员变量.

5.4 参与回环检测的关键帧具有不被删除的特权: mbNotErase

参与回环检测的关键帧具有不被删除的特权,该特权由成员变量mbNotErase存储,创建KeyFrame对象时该成员变量默认被初始化为false.

若某关键帧参与了回环检测,LoopClosing线程就会就调用函数KeyFrame::SetNotErase()将该关键帧的成员变量mbNotErase设为true,标记该关键帧暂时不要被删除.

void KeyFrame::SetNotErase() {unique_lock<mutex> lock(mMutexConnections);mbNotErase = true;
}

在删除函数SetBadFlag()起始先根据成员变量mbNotErase判断当前KeyFrame是否具有豁免删除的特权.若当前KeyFrame的mbNotErase为true,则函数SetBadFlag()不能删除当前KeyFrame,但会将其成员变量mbToBeErased置为true.

void KeyFrame::SetBadFlag() {// step1. 特殊情况:豁免第一帧和具有mbNotErase特权的帧{unique_lock<mutex> lock(mMutexConnections);
​if (mnId == 0)return;else if (mbNotErase) {mbToBeErased = true;return;}}// 两步删除: 先逻辑删除,再物理删除...
}

成员变量mbToBeErased标记当前KeyFrame是否被豁免过删除特权**.LoopClosing线程不再需要某关键帧时,会调用函数KeyFrame::SetErase()剥夺该关键帧不被删除的特权**,将成员变量mbNotErase复位为false;同时检查成员变量mbToBeErased,若mbToBeErased为true就会调用函数KeyFrame::SetBadFlag()删除该关键帧.

void KeyFrame::SetErase() {{unique_lock<mutex> lock(mMutexConnections);// 若当前关键帧没参与回环检测,但其它帧与当前关键帧形成回环关系,也不应当删除当前关键帧if (mspLoopEdges.empty()) {mbNotErase = false;}}
​// mbToBeErased：删除之前记录的想要删但时机不合适没有删除的帧if (mbToBeErased) {SetBadFlag();}
}

5.5 删除关键帧时维护共视图和生成树

函数SetBadFlag()在删除关键帧的时维护其共视图和生成树结构.共视图结构的维护比较简单,这里主要关心如何维护生成树的结构.

当一个关键帧被删除时,其父关键帧和所有子关键帧的生成树信息也会受到影响,需要为其所有子关键帧寻找新的父关键帧,如果父关键帧找的不好的话,就会产生回环,导致生成树就断开.

被删除关键帧的子关键帧所有可能的父关键帧包括其兄弟关键帧和其被删除关键帧的父关键帧.以下图为例,关键帧4可能的父关键帧包括关键帧3、5、6和7.

采用类似于最小生成树算法中的加边法重新构建生成树结构: 每次循环取权重最高的候选边建立父子连接关系,并将新加入生成树的子节点到加入候选父节点集合sParentCandidates中.

void KeyFrame::SetBadFlag() {// step1. 特殊情况:豁免 第一帧 和 具有mbNotErase特权的帧{unique_lock<mutex> lock(mMutexConnections);
​if (mnId == 0)return;else if (mbNotErase) {mbToBeErased = true;return;}}
​// step2. 从共视关键帧的共视图中删除本关键帧for (auto mit : mConnectedKeyFrameWeights)mit.first->EraseConnection(this);
​// step3. 删除当前关键帧中地图点对本帧的观测for (size_t i = 0; i < mvpMapPoints.size(); i++)if (mvpMapPoints[i])mvpMapPoints[i]->EraseObservation(this);
​{// step4. 删除共视图unique_lock<mutex> lock(mMutexConnections);unique_lock<mutex> lock1(mMutexFeatures);mConnectedKeyFrameWeights.clear();mvpOrderedConnectedKeyFrames.clear();
​// step5. 更新生成树结构set<KeyFrame *> sParentCandidates;sParentCandidates.insert(mpParent);
​while (!mspChildrens.empty()) {bool bContinue = false;int max = -1;KeyFrame *pC;KeyFrame *pP;for (KeyFrame *pKF : mspChildrens) {if (pKF->isBad())continue;
​vector<KeyFrame *> vpConnected = pKF->GetVectorCovisibleKeyFrames();
​for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) {for (set<KeyFrame *>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end();spcit != spcend; spcit++) {if (vpConnected[i]->mnId == (*spcit)->mnId) {int w = pKF->GetWeight(vpConnected[i]);if (w > max) {pC = pKF;                   pP = vpConnected[i];   //目前和子关键帧具有最大权值的关键帧（将来的父关键帧）       max = w;                    bContinue = true;  //说明子节点找到了可以作为其新父关键帧的帧         }}}}}
​if (bContinue) {pC->ChangeParent(pP);sParentCandidates.insert(pC);mspChildrens.erase(pC);} elsebreak;}
​if (!mspChildrens.empty())for (set<KeyFrame *>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++) {(*sit)->ChangeParent(mpParent);}
​mpParent->EraseChild(this);mTcp = Tcw * mpParent->GetPoseInverse();// step6. 将当前关键帧的 mbBad 置为 truembBad = true;} // step7. 从地图中删除当前关键帧mpMap->EraseKeyFrame(this);mpKeyFrameDB->erase(this);
}

5.6 对地图点的观测

KeyFrame类除了像一般的Frame类那样保存二维图像特征点以外,还保存三维地图点MapPoint信息.

关键帧观测到的地图点列表由成员变量mvpMapPoints保存,下面是一些对该成员变量进行增删改查的成员函数,就是简单的列表操作,没什么值得说的地方.

值得关心的是上述函数的调用时机,也就是说参考帧何时与地图点发生关系:

• 关键帧增加对地图点观测的时机:

  1. Tracking线程和LocalMapping线程创建新地图点后,会马上调用函数KeyFrame::AddMapPoint()添加当前关键帧对该地图点的观测.
  2. LocalMapping线程处理完毕缓冲队列内所有关键帧后会调用LocalMapping::SearchInNeighbors()融合当前关键帧和共视关键帧间的重复地图点,其中调用函数ORBmatcher::Fuse()实现融合过程中会调用函数KeyFrame::AddMapPoint().
  3. LoopClosing线程闭环矫正函数LoopClosing::CorrectLoop()将闭环关键帧与其匹配关键帧间的地图进行融合,会调用函数KeyFrame::AddMapPoint().

• 关键帧替换和删除对地图点观测的时机:

  1. MapPoint删除函数MapPoint::SetBadFlag()或替换函数MapPoint::Replace()会调用KeyFrame::EraseMapPointMatch()和KeyFrame::ReplaceMapPointMatch()删除和替换关键针对地图点的观测.

  2. LocalMapping线程调用进行局部BA优化的函数Optimizer::LocalBundleAdjustment()内部调用函数KeyFrame::EraseMapPointMatch()删除对重投影误差较大的地图点的观测.

5.7 回环检测与本质图

LoopClosing线程中回环矫正函数LoopClosing::CorrectLoop()在调用本质图BA优化函数Optimizer::OptimizeEssentialGraph()之前会调用函数KeyFrame::AddLoopEdge(),在当前关键帧和其闭环匹配关键帧间添加回环关系.

在调用本质图BA优化函数Optimizer::OptimizeEssentialGraph()中会调用函数KeyFrame::GetLoopEdges()将所有闭环关系加入到本质图中进行优化.

5.8 KeyFrame`的用途

KeyFrame类的生命周期

• KeyFrame的创建:

  Tracking线程中通过函数Tracking::NeedNewKeyFrame()判断是否需要关键帧,若需要关键帧,则调用函数Tracking::CreateNewKeyFrame()创建关键帧.

• KeyFrame的销毁:

  LocalMapping线程剔除冗余关键帧函数LocalMapping::KeyFrameCulling()中若检查到某关键帧为冗余关键帧,则调用函数KeyFrame::SetBadFlag()删除关键帧.

6. ORB-SLAM2代码详解06_单目初始化器Initializer

6.1 各成员变量/函数

Initializer类仅用于单目相机初始化,双目/RGBD相机初始化不用这个类.

成员变量名中: 1代表参考帧(reference frame)中特征点编号,2代表当前帧(current frame)中特征点编号.

6.1.1 初始化函数: Initialize()

主函数Initialize()根据两帧间的匹配关系恢复帧间运动并计算地图点位姿.

bool Initializer::Initialize(const Frame &CurrentFrame,const vector<int> &vMatches12,cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D,vector<bool> &vbTriangulated) {
​// 初始化器Initializer对象创建时就已指定mvKeys1,调用本函数只需指定mvKeys2即可mvKeys2 = CurrentFrame.mvKeysUn;        // current frame中的特征点mvMatches12.reserve(mvKeys2.size());mvbMatched1.resize(mvKeys1.size());
​// step1. 将vMatches12拷贝到mvMatches12,mvMatches12只保存匹配上的特征点对for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {if (vMatches12[i] >= 0) {mvMatches12.push_back(make_pair(i, vMatches12[i]));mvbMatched1[i] = true;} elsemvbMatched1[i] = false;}
​// step2. 准备RANSAC运算中需要的特征点对const int N = mvMatches12.size();vector<size_t> vAllIndices;for (int i = 0; i < N; i++) {vAllIndices.push_back(i);}mvSets = vector<vector<size_t> >(mMaxIterations, vector<size_t>(8, 0));for (int it = 0; it < mMaxIterations; it++) {vector<size_t> vAvailableIndices = vAllIndices;for (size_t j = 0; j < 8; j++) {int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1);int idx = vAvailableIndices[randi];mvSets[it][j] = idx;vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}}
​// step3. 计算F矩阵和H矩阵及其置信程度vector<bool> vbMatchesInliersH, vbMatchesInliersF;float SH, SF;cv::Mat H, F;
​thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));threadH.join();threadF.join();
​// step4. 根据比分计算使用哪个矩阵恢复运动float RH = SH / (SH + SF); if (RH > 0.40)return ReconstructH(vbMatchesInliersH, H, mK, R21, t21, vP3D, vbTriangulated, 1.0, 50);elsereturn ReconstructF(vbMatchesInliersF, F, mK, R21, t21, vP3D, vbTriangulated, 1.0, 50);
}

6.2 计算基础矩阵F和单应矩阵H

6.2.1 RANSAC算法

• 算法流程

  1. 随机从数据集中随机抽出4个样本数据 (此4个样本之间不能共线)，计算出变换矩阵H，记为模型M；
  2. 计算数据集中所有数据与模型M的投影误差，若误差小于阈值，加入内点集 I ；
  3. 如果当前内点集 I 元素个数大于最优内点集 I_best , 则更新 I_best = I，同时更新迭代次数k ;
  4. 如果迭代次数大于k,则退出 ; 否则迭代次数加1，并重复上述步骤；

  https://guoqiang.blog.csdn.net/article/details/79775542

• 算法原理

少数外点会极大影响计算结果的准确度.随着采样数量的增加,外点数量也会同时增加,这是一种系统误差,无法通过增加采样点来解决.

RANSAC(Random sample consensus,随机采样一致性)算法的思路是少量多次重复实验,每次实验仅使用尽可能少的点来计算,并统计本次计算中的内点数.只要尝试次数足够多的话,总会找到一个包含所有内点的解.

RANSAC算法的核心是减少每次迭代所需的采样点数.从原理上来说,计算F矩阵最少只需要7对匹配点,计算H矩阵最少只需要4对匹配点;ORB-SLAM2中为了编程方便,每次迭代使用8对匹配点计算F和H.

• 算法改尽版本：渐进一致采样 PROSAC(PROgressive SAmple Consensus)

  https://guoqiang.blog.csdn.net/article/details/108661924

6.2.2 计算基础矩阵F: FindFundamental()

设点P在相机1、2坐标系下的坐标分别为、,在相机$X_1,X_2$成像平面下的像素坐标分别为$x_1$、$x_2$,有:
$$\begin{gathered} {X_2}^{T}E{X}_1=0 \\ {x}_1=K_1{X}_1 \\ {x}_2=K_2{X}_2 \end{gathered}$$
其中本质矩阵$E=t^{\wedge }R$.
$${x_2}^T{k_2}^{T}E{k_1}^T{x}_1=0$$
令$F={k_2}^{T}E{k_1}^T$可得：
$${x_2}^{T}F{x}_1=0$$

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21) {
​const int N = vbMatchesInliers.size();
​// step1. 特征点归一化vector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1, vPn1, T1);Normalize(mvKeys2, vPn2, T2);cv::Mat T2t = T2.t();       // 用于恢复原始尺度
​// step2. RANSAC循环score = 0.0;                                    // 最优解得分vbMatchesInliers = vector<bool>(N, false);      // 最优解对应的内点for (int it = 0; it < mMaxIterations; it++) {vector<cv::Point2f> vPn1i(8);vector<cv::Point2f> vPn2i(8);cv::Mat F21i;vector<bool> vbCurrentInliers(N, false);float currentScore;
​for (int j = 0; j < 8; j++) {int idx = mvSets[it][j];vPn1i[j] = vPn1[mvMatches12[idx].first];        // first存储在参考帧1中的特征点索引vPn2i[j] = vPn2[mvMatches12[idx].second];       // second存储在当前帧2中的特征点索引}
​// step3. 八点法计算单应矩阵Hcv::Mat Fn = ComputeF21(vPn1i, vPn2i);
​// step4. 恢复原始尺度F21i = T2t * Fn * T1;
​// step5. 根据重投影误差进行卡方检验currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
​// step6. 记录最优解if (currentScore > score) {F21 = F21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}
​

6.2.3 八点法计算F矩阵: ComputeF21()

F矩阵的约束:
$$（u_1,v_1,1）\left(\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=0$$
展开成:

$$u_1{u}_2{f}_{11}+u_1{v}_2{f}_{21}+u_1{f}_{31}+v_1{u}_2{f}_{12}+v_1{v}_2{f}_{22}+v_1{f}_{32}+u_2{f}_{13}+v_2{f}_{23}+f_{33}=0$$
由于F矩阵的尺度不变性,只需8对特征点即可提供足够的约束.

上图中矩阵是一个的矩阵,是一个的向量;上述方程是一个超定方程,使用SVD分解求最小二乘解.

cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {
​const int N = vP1.size();
​// step1. 构造A矩阵cv::Mat A(N, 9, CV_32F);for (int i = 0; i < N; i++) {const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;A.at<float>(i, 0) = u2 * u1;A.at<float>(i, 1) = u2 * v1;A.at<float>(i, 2) = u2;A.at<float>(i, 3) = v2 * u1;A.at<float>(i, 4) = v2 * v1;A.at<float>(i, 5) = v2;A.at<float>(i, 6) = u1;A.at<float>(i, 7) = v1;A.at<float>(i, 8) = 1;}
​// step2. 奇异值分解,取vt最后一行cv::Mat u, w, vt;cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
​// step3. 将F矩阵的秩强制置为2cv::SVDecomp(Fpre, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);w.at<float>(2) = 0;return u * cv::Mat::diag(w) * vt;
}

6.2.4 计算单应矩阵H: FindHomography()

以下两种情况更适合使用单应矩阵进行初始化:

1. 相机看到的场景是一个平面.
2. 连续两帧间没发生平移,只发生旋转.

使用八点法求解单应矩阵H的原理类似:

正常来说只用4对匹配点就可以计算单应矩阵H,但ORB-SLAM2每次RANSAC迭代取8对匹配点来计算H.个人理解这是为了和八点法计算基础矩阵H相对应,都使用8对匹配点来计算,便于后面比较分数优劣.

void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21) {
​const int N = mvMatches12.size();
​// step1. 特征点归一化vector<cv::Point2f> vPn1, vPn2;cv::Mat T1, T2;Normalize(mvKeys1, vPn1, T1);Normalize(mvKeys2, vPn2, T2);cv::Mat T2inv = T2.inv();       // 用于恢复原始尺度
​// step2. RANSAC循环score = 0.0;                                    // 最优解得分vbMatchesInliers = vector<bool>(N, false);      // 最优解对应的内点for (int it = 0; it < mMaxIterations; it++) {vector<cv::Point2f> vPn1i(8);               vector<cv::Point2f> vPn2i(8);cv::Mat H21i, H12i;vector<bool> vbCurrentInliers(N, false);float currentScore;
​for (size_t j = 0; j < 8; j++) {int idx = mvSets[it][j];vPn1i[j] = vPn1[mvMatches12[idx].first];    // first存储在参考帧1中的特征点索引vPn2i[j] = vPn2[mvMatches12[idx].second];   // second存储在当前帧2中的特征点索引}
​// step3. 八点法计算单应矩阵Hcv::Mat Hn = ComputeH21(vPn1i, vPn2i);
​// step4. 恢复原始尺度H21i = T2inv * Hn * T1;H12i = H21i.inv();
​// step5. 根据重投影误差进行卡方检验currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
​// step6. 记录最优解if (currentScore > score) {H21 = H21i.clone();vbMatchesInliers = vbCurrentInliers;score = currentScore;}}
}

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {const int N = vP1.size();
​// step1. 构造A矩阵cv::Mat A(2 * N, 9, CV_32F);for (int i = 0; i < N; i++) {const float u1 = vP1[i].x;const float v1 = vP1[i].y;const float u2 = vP2[i].x;const float v2 = vP2[i].y;A.at<float>(2 * i, 0) = 0.0;A.at<float>(2 * i, 1) = 0.0;A.at<float>(2 * i, 2) = 0.0;A.at<float>(2 * i, 3) = -u1;A.at<float>(2 * i, 4) = -v1;A.at<float>(2 * i, 5) = -1;A.at<float>(2 * i, 6) = v2 * u1;A.at<float>(2 * i, 7) = v2 * v1;A.at<float>(2 * i, 8) = v2;A.at<float>(2 * i + 1, 0) = u1;A.at<float>(2 * i + 1, 1) = v1;A.at<float>(2 * i + 1, 2) = 1;A.at<float>(2 * i + 1, 3) = 0.0;A.at<float>(2 * i + 1, 4) = 0.0;A.at<float>(2 * i + 1, 5) = 0.0;A.at<float>(2 * i + 1, 6) = -u2 * u1;A.at<float>(2 * i + 1, 7) = -u2 * v1;A.at<float>(2 * i + 1, 8) = -u2;}
​// step2. 奇异值分解,取vt最后一行cv::Mat u, w, vt;cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A |cv::SVD::FULL_UV); return vt.row(8).reshape(0, 3);
}

6.2.5 卡方检验计算置信度得分: CheckFundamental()、CheckHomography()

卡方检验通过构造检验统计量来比较期望结果和实际结果之间的差别,从而得出观察频数极值的发生概率.

**卡方检验就是统计样本的实际观测值与理论推断值之间的偏离程度，如果卡方值（χ²）越大，二者偏差程度越大；反之，二者偏差越小。**

根据重投影误差构造统计量,其值越大,观察结果和期望结果之间的差别越显著,某次计算越可能用到了外点.

统计量置信度阈值与被检验变量自由度有关: 单目特征点重投影误差的自由度为2(u,v),双目特征点重投影误差自由度为3(u,v,ur).

取95%置信度下的卡方检验统计量阈值

• 若统计量大于该阈值,则认为计算矩阵使用到了外点,将其分数设为0.
• 若统计量小于该阈值,则将统计量裕量设为该解的置信度分数.

float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12,vector<bool> &vbMatchesInliers, float sigma) {const int N = mvMatches12.size();// 取出单应矩阵H各位上的值const float h11 = H21.at<float>(0, 0);const float h12 = H21.at<float>(0, 1);const float h13 = H21.at<float>(0, 2);const float h21 = H21.at<float>(1, 0);const float h22 = H21.at<float>(1, 1);const float h23 = H21.at<float>(1, 2);const float h31 = H21.at<float>(2, 0);const float h32 = H21.at<float>(2, 1);const float h33 = H21.at<float>(2, 2);
​const float h11inv = H12.at<float>(0, 0);const float h12inv = H12.at<float>(0, 1);const float h13inv = H12.at<float>(0, 2);const float h21inv = H12.at<float>(1, 0);const float h22inv = H12.at<float>(1, 1);const float h23inv = H12.at<float>(1, 2);const float h31inv = H12.at<float>(2, 0);const float h32inv = H12.at<float>(2, 1);const float h33inv = H12.at<float>(2, 2);vbMatchesInliers.resize(N);     // 标记是否是内点float score = 0;                // 置信度得分const float th = 5.991;         // 自由度为2,显著性水平为0.05的卡方分布对应的临界阈值const float invSigmaSquare = 1.0 / (sigma * sigma);     // 信息矩阵,方差平方的倒数
​// 双向投影,计算加权投影误差for (int i = 0; i < N; i++) {bool bIn = true;
​// step1. 提取特征点对const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];const float u1 = kp1.pt.x;const float v1 = kp1.pt.y;const float u2 = kp2.pt.x;const float v2 = kp2.pt.y;
​// step2. 计算img2到img1的重投影误差const float w2in1inv = 1.0 / (h31inv * u2 + h32inv * v2 + h33inv);const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);const float chiSquare1 = squareDist1 * invSigmaSquare;
​// step3. 离群点标记上,非离群点累加计算得分if (chiSquare1 > th)bIn = false;elsescore += th - chiSquare1;
​// step4. 计算img1到img2的重投影误差const float w1in2inv = 1.0 / (h31 * u1 + h32 * v1 + h33);const float u1in2 = (h11 * u1 + h12 * v1 + h13) * w1in2inv;const float v1in2 = (h21 * u1 + h22 * v1 + h23) * w1in2inv;const float squareDist2 = (u2 - u1in2) * (u2 - u1in2) + (v2 - v1in2) * (v2 - v1in2);const float chiSquare2 = squareDist2 * invSigmaSquare;
​// step5. 离群点标记上,非离群点累加计算得分if (chiSquare2 > th)bIn = false;elsescore += th - chiSquare2;if (bIn)vbMatchesInliers[i] = true;elsevbMatchesInliers[i] = false;}return score;
}

6.2.6 归一化: Normalize()

使用均值和一阶中心矩归一化,归一化可以增强计算稳定性.

void Initializer::Normalize(const vector <cv::KeyPoint> &vKeys, vector <cv::Point2f> &vNormalizedPoints, cv::Mat &T) {// step1. 计算均值float meanX = 0;float meanY = 0;for (int i = 0; i < N; i++) {meanX += vKeys[i].pt.x;meanY += vKeys[i].pt.y;}meanX = meanX / N;meanY = meanY / N;
​// step2. 计算一阶中心矩float meanDevX = 0;float meanDevY = 0;for (int i = 0; i < N; i++) {vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;meanDevX += fabs(vNormalizedPoints[i].x);meanDevY += fabs(vNormalizedPoints[i].y);}meanDevX = meanDevX / N;meanDevY = meanDevY / N;float sX = 1.0 / meanDevX;float sY = 1.0 / meanDevY;
​// step3. 进行归一化for (int i = 0; i < N; i++) {vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;}
​// 记录归一化参数,以便后续恢复尺度T = cv::Mat::eye(3, 3, CV_32F);T.at<float>(0, 0) = sX;T.at<float>(1, 1) = sY;T.at<float>(0, 2) = -meanX * sX;T.at<float>(1, 2) = -meanY * sY;
}

6.3 使用基础矩阵F和单应矩阵H恢复运动

6.3.1 使用基础矩阵F恢复运动: ReconstructF()

使用基础矩阵F分解R、t,数学上会得到四个可能的解,因此分解后调用函数Initializer::CheckRT()检验分解结果,取相机前方成功三角化数目最多的一组解.

bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) {int N = 0;for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++)if (vbMatchesInliers[i]) N++;
​// step1. 根据基础矩阵F推算本质矩阵Ecv::Mat E21 = K.t() * F21 * K;
​// step2. 分解本质矩阵E,得到R,t    cv::Mat R1, R2, t;DecomposeE(E21, R1, R2, t);cv::Mat t1 = t;cv::Mat t2 = -t;// step3. 检验分解出的4对R,tvector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;vector<bool> vbTriangulated1, vbTriangulated2, vbTriangulated3, vbTriangulated4;float parallax1, parallax2, parallax3, parallax4;int nGood1 = CheckRT(R1, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D1, 4.0 * mSigma2, vbTriangulated1, parallax1);int nGood2 = CheckRT(R2, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D2, 4.0 * mSigma2, vbTriangulated2, parallax2);int nGood3 = CheckRT(R1, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D3, 4.0 * mSigma2, vbTriangulated3, parallax3);int nGood4 = CheckRT(R2, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D4, 4.0 * mSigma2, vbTriangulated4, parallax4);int maxGood = max(nGood1, max(nGood2, max(nGood3, nGood4)));R21 = cv::Mat();t21 = cv::Mat();int nMinGood = max(static_cast<int>(0.9 * N), minTriangulated);
​// step4. ratio test,最优分解应有区分度int nsimilar = 0;if (nGood1 > 0.7 * maxGood)nsimilar++;if (nGood2 > 0.7 * maxGood)nsimilar++;if (nGood3 > 0.7 * maxGood)nsimilar++;if (nGood4 > 0.7 * maxGood)nsimilar++;if (maxGood < nMinGood || nsimilar > 1) {return false;}
​// step5. 选择记录最佳结果,检验三角化出的特征点数和视差角if (maxGood == nGood1) {if (parallax1 > minParallax) {vP3D = vP3D1;vbTriangulated = vbTriangulated1;R1.copyTo(R21);t1.copyTo(t21);return true;}} else if (maxGood == nGood2) {if (parallax2 > minParallax) {vP3D = vP3D2;vbTriangulated = vbTriangulated2;
​R2.copyTo(R21);t1.copyTo(t21);return true;}} else if (maxGood == nGood3) {if (parallax3 > minParallax) {vP3D = vP3D3;vbTriangulated = vbTriangulated3;
​R1.copyTo(R21);t2.copyTo(t21);return true;}} else if (maxGood == nGood4) {if (parallax4 > minParallax) {vP3D = vP3D4;vbTriangulated = vbTriangulated4;
​R2.copyTo(R21);t2.copyTo(t21);return true;}}
​return false;
}

6.3.2 使用单应矩阵H恢复运动: ReconstructH()

6.3.3 检验分解结果R,t

通过成功三角化的特征点个数判断分解结果的好坏: 若某特征点的重投影误差小于4且视差角大于0.36°,则认为该特征点三角化成功

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2, const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers, const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax) {const float fx = K.at<float>(0, 0);const float fy = K.at<float>(1, 1);const float cx = K.at<float>(0, 2);const float cy = K.at<float>(1, 2);
​vbGood = vector<bool>(vKeys1.size(), false);vP3D.resize(vKeys1.size());
​vector<float> vCosParallax;vCosParallax.reserve(vKeys1.size());
​// step1. 以相机1光心为世界坐标系,计算相机的投影矩阵和光心位置cv::Mat P1(3, 4, CV_32F, cv::Scalar(0));        // P1表示相机1投影矩阵, K[I|0]K.copyTo(P1.rowRange(0, 3).colRange(0, 3));     cv::Mat O1 = cv::Mat::zeros(3, 1, CV_32F);      // O1表示世界坐标下相机1光心位置, O1=0cv::Mat P2(3, 4, CV_32F);                       // P2表示相机2投影矩阵, K[R|t]R.copyTo(P2.rowRange(0, 3).colRange(0, 3));t.copyTo(P2.rowRange(0, 3).col(3));             // O1表示世界坐标下相机2光心位置, O2=-R'*tP2 = K * P2;cv::Mat O2 = -R.t() * t;
​// 遍历所有特征点对int nGood = 0;for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {// step2. 三角化地图点const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];cv::Mat p3dC1;Triangulate(kp1, kp2, P1, P2, p3dC1);
​// step3. 检查三角化坐标点合法性: // step3.1. 正确三角化的地图点深度值应为正数且视差角足够大cv::Mat normal1 = p3dC1 - O1;float dist1 = cv::norm(normal1);cv::Mat normal2 = p3dC1 - O2;float dist2 = cv::norm(normal2);float cosParallax = normal1.dot(normal2) / (dist1 * dist2);if (p3dC1.at<float>(2) <= 0 && cosParallax < 0.99998)continue;if (p3dC2.at<float>(2) <= 0 && cosParallax < 0.99998)continue;// step3.2. 正确三角化的地图点重投影误差应足够小float im1x, im1y;float invZ1 = 1.0 / p3dC1.at<float>(2);im1x = fx * p3dC1.at<float>(0) * invZ1 + cx;im1y = fy * p3dC1.at<float>(1) * invZ1 + cy;float squareError1 = (im1x - kp1.pt.x) * (im1x - kp1.pt.x) + (im1y - kp1.pt.y) * (im1y - kp1.pt.y);if (squareError1 > th2)continue;
​float im2x, im2y;float invZ2 = 1.0 / p3dC2.at<float>(2);im2x = fx * p3dC2.at<float>(0) * invZ2 + cx;im2y = fy * p3dC2.at<float>(1) * invZ2 + cy;float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y);if (squareError2 > th2)continue;
​// step4. 记录通过检验的地图点vCosParallax.push_back(cosParallax);vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0), p3dC1.at<float>(1), p3dC1.at<float>(2));nGood++;}
​// step5. 记录三角化过程中的较小(第50个视差角)if (nGood > 0) {sort(vCosParallax.begin(), vCosParallax.end());size_t idx = min(50, int(vCosParallax.size() - 1));parallax = acos(vCosParallax[idx]) * 180 / CV_PI;} elseparallax = 0;
​return nGood;
}

SVD求解超定方程

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) {cv::Mat A(4, 4, CV_32F);A.row(0) = kp1.pt.x * P1.row(2) - P1.row(0);A.row(1) = kp1.pt.y * P1.row(2) - P1.row(1);A.row(2) = kp2.pt.x * P2.row(2) - P2.row(0);A.row(3) = kp2.pt.y * P2.row(2) - P2.row(1);cv::Mat u, w, vt;cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);x3D = vt.row(3).t();x3D = x3D.rowRange(0, 3) / x3D.at<float>(3);
}

6.4 对极几何

6.4.1 本质矩阵、基础矩阵和单应矩阵

设点P在相机1、2坐标系下的坐标分别为、,在相机$X_1,X_2$成像平面下的像素坐标分别为$x_1$、$x_2$,有:
$$\begin{gathered} E=t^{\wedge }R \\ F={K_2}^{T}E{K_1}^T \\ {x_2}^{T}F{x}_1={X_2}^{T}F{X}_1=0 \end{gathered}$$

• 矩阵的自由为:

  矩阵为大小,自由度最大为;考虑到尺度等价性约束,实际自由度为.

• 矩阵自由度为:

  待定参数各为,和的待定参数各为,共个待定参数.

  但矩阵为大小,自由度最大为;考虑到尺度等价性和行列式两个约束,实际自由度为.

• 矩阵的自由度为:

  和的自由度各为,自由度最大为,考虑到尺度等价性约束,实际自由度为.

• 矩阵的秩为,从两个方面来解释:

  • ,因此
  • 对于某对非坐标、,有成立,说明方程存在非零解,即矩阵不满秩.

6.4.2 极线与极点

• 使用基础矩阵F恢复运动 算法流程

step1: 设置RANSAC用到的带点对，这里是8对，（实际上最少使用了4对点即可，因为后面卡方检验得分，保持单一变量，拥有更高的可信度）

step2:判断2个矩阵的比例 F或者H/(H+F) ,若单应矩阵大于0.4，则使用单应矩阵恢复运动ReconstructF()使用单应矩阵H恢复运动，否则使用举出矩阵恢复运动

step3:ReconstructH()计算基础矩阵F及其卡方检验得分,FindFundamental()计算单应矩阵H及其卡方检验得分

step4:FindHomography()RANSAC循环RANSAC循环循环迭代8个分解结果循环迭代4个分解结果单应矩阵分数占比>0.4单应矩阵分数占比<0.4

===>分解得到R,t通过三角化检验R,t ===>CheckRT()三角化地图点Triangulate()分解得到R,t通过三角化检验R,tCheckRT()三角化地图点Triangulate()特征点坐标归一化

===>Normalize()八点法计算基础矩阵FComputeF21()卡方检验CheckFundamental()特征点坐标归一化Normalize()计算单应矩阵HComputeH21()

===>卡方检验CheckHomography()`设置RANSAC用到的点对判断两个矩阵得分之比若三角化得到的点数足够多且视差角足够大,则初始化成功

单应矩阵分数占比>0.4单应矩阵分数占比<0.4设置RANSAC用到的点对计算单应矩阵H及其卡方检验得分
FindHomography()计算基础矩阵F及其卡方检验得分
FindFundamental()判断两个矩阵得分之比使用单应矩阵H恢复运动
ReconstructH()使用基础矩阵F恢复运动
ReconstructF()若三角化得到的点数足够多且视差角足够大,则初始化成功

计算基础矩阵F及其卡方检验得分
FindFundamental()RANSAC循环特征点坐标归一化
Normalize()八点法计算基础矩阵F
ComputeF21()卡方检验
CheckFundamental()

计算单应矩阵H及其卡方检验得分
FindHomography()RANSAC循环特征点坐标归一化
Normalize()计算单应矩阵H
ComputeH21()卡方检验
CheckHomography()

使用基础矩阵F恢复运动
ReconstructF()循环迭代4个分解结果分解得到R,t通过三角化检验R,t
CheckRT()三角化地图点
Triangulate()

使用单应矩阵H恢复运动
ReconstructH()循环迭代8个分解结果分解得到R,t通过三角化检验R,t
CheckRT()三角化地图点
Triangulate()

7. ORB-SLAM2代码详解07_跟踪线程Tracking

7.1 各成员函数/变量

7.1.1 跟踪状态

Tracking类中定义枚举类型eTrackingState,用于表示跟踪状态,其可能的取值如下

Tracking类的成员变量mState和mLastProcessedState分别表示当前帧的跟踪状态和上一帧的跟踪状态.

跟踪状态转移图如下:

7.1.2 初始化

初始化用于SLAM系统刚开始接收到图像的几帧,初始化成功之后就进入正常的跟踪操作.

Tracking类主函数Tracking::Track()检查到当前系统的跟踪状态mState为NOT_INITIALIZED时,就会进行初始化.

void Tracking::Track() {// ...unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​// step1. 若还没初始化,则尝试初始化if (mState == NOT_INITIALIZED) {if (mSensor == System::STEREO || mSensor == System::RGBD)StereoInitialization();elseMonocularInitialization();if (mState != OK)return;} // ...
}

7.2 单目相机初始化: MonocularInitialization()

单目相机初始化条件: 连续两帧间成功三角化超过100个点,则初始化成功.

void Tracking::MonocularInitialization() {// step1. 若单目初始化器还没创建,则创建初始化器if (!mpInitializer) {if (mCurrentFrame.mvKeys.size() > 100) {mInitialFrame = Frame(mCurrentFrame);mLastFrame = Frame(mCurrentFrame);mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());for (size_t i = 0; i < mCurrentFrame.mvKeysUn.size(); i++)mvbPrevMatched[i] = mCurrentFrame.mvKeysUn[i].pt;mpInitializer = new Initializer(mCurrentFrame, 1.0, 200);fill(mvIniMatches.begin(), mvIniMatches.end(), -1);return;}} else  {// step2. 若上一帧特征点足够,但当前帧特征点太少,则匹配失败,删除初始化器if ((int) mCurrentFrame.mvKeys.size() <= 100) {delete mpInitializer;mpInitializer = static_cast<Initializer *>(NULL);fill(mvIniMatches.begin(), mvIniMatches.end(), -1);return;}
​// step3. 在mInitialFrame和mLastFrame间进行匹配搜索ORBmatcher matcher(0.9, true);int nmatches = matcher.SearchForInitialization(mInitialFrame, mCurrentFrame, mvbPrevMatched, mvIniMatches, 100);
​// step4. 匹配的特征点数目太少,则匹配失败,删除初始化器if (nmatches < 100) {delete mpInitializer;mpInitializer = static_cast<Initializer *>(NULL);return;}
​// step5. 进行单目初始化cv::Mat Rcw; cv::Mat tcw; vector<bool> vbTriangulated;if (mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) {// step6. 单目初始化成功后,删除未成功三角化的匹配点对for (size_t i = 0, iend = mvIniMatches.size(); i < iend; i++) {if (mvIniMatches[i] >= 0 && !vbTriangulated[i]) {mvIniMatches[i] = -1;nmatches--;}}// step7. 创建初始化地图,以mInitialFrame为参考系cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F);Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));tcw.copyTo(Tcw.rowRange(0, 3).col(3));mInitialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));mCurrentFrame.SetPose(Tcw);CreateInitialMapMonocular();}}
}

单目初始化成功后调用函数CreateInitialMapMonocular()创建初始化地图

void Tracking::CreateInitialMapMonocular() {// mInitialFrame 和 mCurrentFrame 是最早的两个关键帧KeyFrame *pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB);  KeyFrame *pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);  
​// step1. 计算两个关键帧的词袋pKFini->ComputeBoW();pKFcur->ComputeBoW();
​// step2. 将两个关键帧插入地图mpMap->AddKeyFrame(pKFini);mpMap->AddKeyFrame(pKFcur);
​// step3. 处理所有地图点for (size_t i = 0; i < mvIniMatches.size(); i++) {// 创建并添加地图点MapPoint *pMP = new MapPoint(mvIniP3D[i], pKFcur, mpMap);       mpMap->AddMapPoint(pMP);// 添加关键帧到地图点的观测pKFini->AddMapPoint(pMP, i);pKFcur->AddMapPoint(pMP, mvIniMatches[i]);// 添加地图点到关键帧的观测pMP->AddObservation(pKFini, i);pMP->AddObservation(pKFcur, mvIniMatches[i]);// 计算地图点描述子并更新平均观测数据pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();}
​// 基于观测到的地图点,更新关键帧共视图pKFini->UpdateConnections();pKFcur->UpdateConnections();
​// step4. 全局BA: 优化所有关键帧位姿和地图点Optimizer::GlobalBundleAdjustemnt(mpMap, 20);
​// step5. 将两帧间的平移尺度归一化(以场景的中值深度为参考)float medianDepth = pKFini->ComputeSceneMedianDepth(2);float invMedianDepth = 1.0f / medianDepth;cv::Mat Tc2w = pKFcur->GetPose();Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;pKFcur->SetPose(Tc2w);
​// step6. 将坐标点尺度也归一化vector<MapPoint *> vpAllMapPoints = pKFini->GetMapPointMatches();for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++) {if (vpAllMapPoints[iMP]) {MapPoint *pMP = vpAllMapPoints[iMP];pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);}}
​// step7. 将关键帧插入局部地图,更新归一化后的位姿和地图点坐标  mpLocalMapper->InsertKeyFrame(pKFini);mpLocalMapper->InsertKeyFrame(pKFcur);mCurrentFrame.SetPose(pKFcur->GetPose());mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKFcur;mvpLocalKeyFrames.push_back(pKFcur);mvpLocalKeyFrames.push_back(pKFini);mvpLocalMapPoints = mpMap->GetAllMapPoints();mpReferenceKF = pKFcur;mCurrentFrame.mpReferenceKF = pKFcur;mLastFrame = Frame(mCurrentFrame);mpMap->SetReferenceMapPoints(mvpLocalMapPoints);mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());mpMap->mvpKeyFrameOrigins.push_back(pKFini);// step8. 更新跟踪状态变量mStatemState = OK;
}

7.3 双目/RGBD相机初始化: StereoInitialization()

双目/RGBD相机的要求就宽松多了,只要左目图像能找到多于500个特征点,就算是初始化成功.

函数StereoInitialization()内部既完成了初始化,又构建了初始化局部地图.

void Tracking::StereoInitialization() {if (mCurrentFrame.N > 500) {// 基于当前帧构造初始关键帧mCurrentFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));KeyFrame *pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);mpMap->AddKeyFrame(pKFini);mpLocalMapper->InsertKeyFrame(pKFini);// 构造地图点for (int i = 0; i < mCurrentFrame.N; i++) {float z = mCurrentFrame.mvDepth[i];if (z > 0) {cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint *pNewMP = new MapPoint(x3D, pKFini, mpMap);pNewMP->AddObservation(pKFini, i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);pKFini->AddMapPoint(pNewMP, i);mCurrentFrame.mvpMapPoints[i] = pNewMP;}}
​// 构造局部地图mvpLocalKeyFrames.push_back(pKFini);mvpLocalMapPoints = mpMap->GetAllMapPoints();mpReferenceKF = pKFini;mCurrentFrame.mpReferenceKF = pKFini;// 更新跟踪状态变量mStatemState = OK;}
}

7.4 初始位姿估计

当Tracking线程接收到一帧图像后,会先估计其初始位姿,再根据估计出的初始位姿跟踪局部地图并进一步优化位姿.

初始位姿估计有三种手段:

• 根据恒速运动模型估计位姿TrackWithMotionModel()
• 根据参考帧估计位姿TrackReferenceKeyFrame()
• 通过重定位估计位姿Relocalization()

void Tracking::Track() {// ...unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​// step1. 若还没初始化,则尝试初始化if (mState == NOT_INITIALIZED) {// 初始化} else {// step2. 若系统已初始化,就进行跟踪(或重定位)bool bOK;
​// step2.1. 符合条件时,优先根据运动模型跟踪,如运动模型跟踪失败,就根据参考帧进行跟踪if (mState == OK) {if (mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId + 2) {     // 判断当前关键帧是否具有较稳定的速度bOK = TrackReferenceKeyFrame();} else {bOK = TrackWithMotionModel();if (!bOK)bOK = TrackReferenceKeyFrame();}} else {// step2.2. 若上一帧没跟踪丢失,则这一帧重定位bOK = Relocalization();}// ...if (bOK)mState = OK;elsemState = LOST;}// ...
}

7.5 根据恒速运动模型估计初始位姿: TrackWithMotionModel()

恒速运动模型假定连续几帧间的运动速度是恒定的;基于此假设,根据运动速度mVelocity和上一帧的位姿mLastFrame.mTcw计算出本帧位姿的估计值,再进行位姿优化.

成员变量mVelocity保存前一帧的速度,主函数Tracking::Track()中调用完函数Tracking::TrackLocalMap()更新局部地图和当前帧位姿后,就计算速度并赋值给mVelocity.

bool Tracking::TrackWithMotionModel() {ORBmatcher matcher(0.9, true);
​// step1. 更新上一帧的位姿,对于双目/RGBD相机,还会生成临时地图点UpdateLastFrame();// step2. 根据运动模型设置初始位姿估计值mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));
​// step3. 根据初始位姿估计值进行投影匹配int th;if (mSensor != System::STEREO)th = 15;//单目elseth = 7;//双目// step3.1. 寻找匹配特征点int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR);// step3.2. 匹配特征点数目太少就放宽条件重新搜索匹配if (nmatches < 20) {    fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th, mSensor == System::MONOCULAR);}// step3.3. 实在找不到足够的匹配特征点,运动模型跟踪失败if (nmatches < 20)return false;
​// step4. 位姿BA: 只优化当前帧位姿Optimizer::PoseOptimization(&mCurrentFrame);
​// step5. 剔除外点int nmatchesMap = 0;for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i]) {if (mCurrentFrame.mvbOutlier[i]) {MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);mCurrentFrame.mvbOutlier[i] = false;} else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)nmatchesMap++;}}
​// step6. 匹配的地图点数超过10个就认为是跟踪成功return nmatchesMap >= 10;
}

为保证位姿估计的准确性,对于双目/RGBD相机,为前一帧生成临时地图点.

void Tracking::UpdateLastFrame() {// step1. 根据参考关键帧确定当前帧的位姿,使用关键帧是因为参考关键帧位姿更准确KeyFrame *pRef = mLastFrame.mpReferenceKF;cv::Mat Tlr = mlRelativeFramePoses.back();mLastFrame.SetPose(Tlr * pRef->GetPose());
​// step2. 对于双目/RGBD相机,生成新的临时地图点if (mnLastKeyFrameId == mLastFrame.mnId || mSensor == System::MONOCULAR)return;// step2.1. 将上一帧种存在深度的特征点按深度从小到大排序vector<pair<float, int> > vDepthIdx;vDepthIdx.reserve(mLastFrame.N);for (int i = 0; i < mLastFrame.N; i++) {float z = mLastFrame.mvDepth[i];if (z > 0) {vDepthIdx.push_back(make_pair(z, i));}}sort(vDepthIdx.begin(), vDepthIdx.end());// step2.2. 将上一帧中没三角化的特征点三角化出来,作为临时地图点int nPoints = 0;        // 统计处理了多少特征点for (size_t j = 0; j < vDepthIdx.size(); j++) {int i = vDepthIdx[j].second;bool bCreateNew = false;MapPoint *pMP = mLastFrame.mvpMapPoints[i];if (!pMP || pMP->Observations() < 1) {bCreateNew = true;}// step2.3. 这些临时地图点在CreateNewKeyFrame之前会被删除,因此不用添加其他复杂属性if (bCreateNew) {cv::Mat x3D = mLastFrame.UnprojectStereo(i);MapPoint *pNewMP = new MapPoint(x3D, mpMap, &mLastFrame, i);mLastFrame.mvpMapPoints[i] = pNewMP;mlpTemporalPoints.push_back(pNewMP);}// step2.4. 临时地图点数目足够(多于100个)或者深度太大(深度越大位置越不准确),就停止生成临时地图点if (vDepthIdx[j].first > mThDepth && nPoints > 100)break;nPoints++;}
}

7.6 根据参考帧估计位姿: TrackReferenceKeyFrame()

成员变量mpReferenceKF保存Tracking线程当前的参考关键帧,参考关键帧有两个来源:

• 每当Tracking线程创建一个新的参考关键帧时,就将其设为参考关键帧.
• 跟踪局部地图的函数Tracking::TrackLocalMap()内部会将与当前帧共视点最多的局部关键帧设为参考关键帧.

bool Tracking::TrackReferenceKeyFrame() {// step1. 根据当前帧描述子计算词袋mCurrentFrame.ComputeBoW();// step2. 根据词袋寻找匹配ORBmatcher matcher(0.7, true);vector<MapPoint *> vpMapPointMatches;int nmatches = matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches);if (nmatches < 15)  // 词袋匹配过少则跟踪失败return false;
​// step3. 将上一帧位姿设为初始位姿估计值mCurrentFrame.SetPose(mLastFrame.mTcw); mCurrentFrame.mvpMapPoints = vpMapPointMatches;// step4. 位姿BA: 只优化当前帧位姿Optimizer::PoseOptimization(&mCurrentFrame);
​// step5. 剔除外点int nmatchesMap = 0;for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i]) {if (mCurrentFrame.mvbOutlier[i]) {MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);mCurrentFrame.mvbOutlier[i] = false;} else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)nmatchesMap++;}}// step6. 匹配的地图点数超过10个就认为是跟踪成功return nmatchesMap >= 10;
}

思考: 为什么函数Tracking::TrackReferenceKeyFrame()没有检查当前参考帧mpReferenceKF是否被LocalMapping线程删除了?

回答: 因为LocalMapping线程剔除冗余关键帧函数LocalMapping::KeyFrameCulling()不会删除最新的参考帧,有可能被删除的都是之前的参考帧.

7.7 通过重定位估计位姿: Relocalization()

当TrackWithMotionModel()和TrackReferenceKeyFrame()都失败后,就会调用函数Relocalization()通过重定位估计位姿.

bool Tracking::Relocalization() {// step1. 根据当前帧描述子计算词袋mCurrentFrame.ComputeBoW();
​// step2. 根据词袋找到当前帧的候选参考关键帧vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);    const int nKFs = vpCandidateKFs.size();vector<PnPsolver *> vpPnPsolvers;vpPnPsolvers.resize(nKFs);
​// step3. 遍历所有的候选参考关键帧,通过BOW进行快速匹配,用匹配结果初始化PnPsolvervector<vector<MapPoint *> > vvpMapPointMatches;vector<bool> vbDiscarded;ORBmatcher matcher(0.75, true);int nCandidates = 0;for (int i = 0; i < nKFs; i++) {int nmatches = matcher.SearchByBoW(pKF, mCurrentFrame, vvpMapPointMatches[i]);PnPsolver *pSolver = new PnPsolver(mCurrentFrame, vvpMapPointMatches[i]);pSolver->SetRansacParameters(0.99, 10, 300, 4, 0.5, 5.991);vpPnPsolvers[i] = pSolver;nCandidates++;}
​// step4. 使用PnPsolver估计初始位姿,并根据初始位姿获取足够的匹配特征点bool bMatch = false;ORBmatcher matcher2(0.9, true);
​while (nCandidates > 0 && !bMatch) {for (int i = 0; i < nKFs; i++) {vector<bool> vbInliers;int nInliers;bool bNoMore;// step4.1. 通过PnPsolver估计初始位姿PnPsolver *pSolver = vpPnPsolvers[i];cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers);if (bNoMore) {vbDiscarded[i] = true;nCandidates--;}// step4.2. 位姿BA: 只优化当前帧位姿Tcw.copyTo(mCurrentFrame.mTcw);set<MapPoint *> sFound;const int np = vbInliers.size();for (int j = 0; j < np; j++) {if (vbInliers[j]) {mCurrentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];sFound.insert(vvpMapPointMatches[i][j]);} elsemCurrentFrame.mvpMapPoints[j] = NULL;}int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
​// step4.3. 剔除外点for (int io = 0; io < mCurrentFrame.N; io++)if (mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io] = static_cast<MapPoint *>(NULL);// step4.4. 若匹配特征点数目太少,则尝试第2次进行特征匹配+位姿优化if (nGood < 50) {int nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 10, 100);if (nadditional + nGood >= 50) {nGood = Optimizer::PoseOptimization(&mCurrentFrame);// step4.5. 若匹配特征点数目太少,则尝试第3次进行特征匹配+位姿优化if (nGood > 30 && nGood < 50) {sFound.clear();for (int ip = 0; ip < mCurrentFrame.N; ip++)if (mCurrentFrame.mvpMapPoints[ip])sFound.insert(mCurrentFrame.mvpMapPoints[ip]);nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 3, 64);if (nGood + nadditional >= 50) {nGood = Optimizer::PoseOptimization(&mCurrentFrame);for (int io = 0; io < mCurrentFrame.N; io++)if (mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io] = NULL;}}}}// step4.6. 若最后匹配数目终于足够了,则跟踪成功if (nGood >= 50) {bMatch = true;break;}}}// step5. 返回是否跟踪成功if (!bMatch) {return false;} else {mnLastRelocFrameId = mCurrentFrame.mnId;return true;}
}

7.8 跟踪局部地图: TrackLocalMap()

成功估计当前帧的初始位姿后,基于当前位姿更新局部地图并优化当前帧位姿,主要流程:

1. 更新局部地图,包括局部关键帧列表mvpLocalKeyFrames和局部地图点列表mvpLocalMapPoints.

2. 将局部地图点投影到当前帧特征点上.

3. 进行位姿BA,优化当前帧位姿.

4. 更新地图点观测数值,统计内点个数.

   这里的地图点观测数值会被用作LocalMapping线程中LocalMapping::MapPointCulling()函数剔除坏点的标准之一.

5. 根据内点数判断是否跟踪成功.

跟踪局部地图,优化当前帧位姿
TrackLocalMap()更新局部地图
UpdateLocalMap()将局部地图点投影到当前帧特征点上
SearchLocalPoints()对当前帧位姿进行BA优化更新地图点观测根据内点数判断是否跟踪成功更新局部关键帧
UpdateLocalKeyFrames()更新局部地图点
UpdateLocalPoints()

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

bool Tracking::TrackLocalMap() {// step1. 更新局部地图,包括局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPointsUpdateLocalMap();
​// step2. 将局部地图点投影到当前帧特征点上SearchLocalPoints();
​// step3. 位姿BA: 只优化当前帧位姿Optimizer::PoseOptimization(&mCurrentFrame);
​// step4. 更新地图点观测,统计内点个数// 这里的地图点观测数值会被用作LocalMapping线程中MapPointCulling()函数剔除坏点的标准之一mnMatchesInliers = 0;for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i]) {if (!mCurrentFrame.mvbOutlier[i]) {mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); // 位姿估计用到该地图点if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)mnMatchesInliers++;                         }}}
​// step5. 判断是否跟踪成功: 若刚发生过重定位,则标准严苛一点,否则标准宽松一点.(防止误闭环)if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 50)return false;if (mnMatchesInliers < 30)return false;elsereturn true;
}

函数Tracking::UpdateLocalMap()依次调用函数Tracking::UpdateLocalKeyFrames()更新局部关键帧列表mvpLocalKeyFrames和函数Tracking::UpdateLocalPoints()更新局部地图点列表mvpLocalMapPoints.

void Tracking::UpdateLocalMap() {UpdateLocalKeyFrames();         // 更新局部关键帧列表mvpLocalKeyFramesUpdateLocalPoints();            // 更新局部地图点列表mvpLocalMapPoints
}

• 函数Tracking::UpdateLocalKeyFrames()内,局部关键帧列表mvpLocalKeyFrames会被清空并重新赋值,包括以下3部分:

  1. 当前地图点的所有共视关键帧.
  2. 1中所有关键帧的父子关键帧.
  3. 1中所有关键帧共视关系前10大的共视关键帧.

  更新完局部关键帧列表mvpLocalKeyFrames后,还将与当前帧共视关系最强的关键帧设为参考关键帧mpReferenceKF.

• 函数Tracking::UpdateLocalPoints()内,局部地图点列表mvpLocalMapPoints会被清空并赋值为局部关键帧列表mvpLocalKeyFrames的所有地图点.

函数Tracking::SearchLocalPoints()将局部地图点投影到当前帧特征点上

void Tracking::SearchLocalPoints() {// step1. 当前帧地图点已经匹配了特征点for (MapPoint *pMP: mCurrentFrame.mvpMapPoints) {if (pMP) {if (pMP->isBad()) {*vit = static_cast<MapPoint *>(NULL);} else {pMP->IncreaseVisible();pMP->mnLastFrameSeen = mCurrentFrame.mnId;pMP->mbTrackInView = false;}}}// step2. 统计视野内地图点数目 int nToMatch = 0;for (MapPoint *pMP: mvpLocalMapPoints) {if (pMP->mnLastFrameSeen == mCurrentFrame.mnId)continue;if (pMP->isBad())continue;
​if (mCurrentFrame.isInFrustum(pMP, 0.5)) {pMP->IncreaseVisible();nToMatch++;}}
​// Step 3：如果需要进行投影匹配的点的数目大于0，就进行投影匹配if (nToMatch > 0) {ORBmatcher matcher(0.8);int th = 1;if (mSensor == System::RGBD) th = 3;if (mCurrentFrame.mnId < mnLastRelocFrameId + 2)th = 5;matcher.SearchByProjection(mCurrentFrame, mvpLocalMapPoints, th);}
}

7.9 关键帧的创建

7.9.1 判断是否需要创建新关键帧: NeedNewKeyFrame()

是否生成关键帧,需要考虑以下几个方面:

1. 最近是否进行过重定位,重定位后位姿不会太准,不适合做参考帧.
2. 当前系统的工作状态: 如果LocalMapping线程还有很多KeyFrame没处理的话,不适合再给它增加负担了.
3. 距离上次创建关键帧经过的时间: 如果很长时间没创建关键帧了的话,就要抓紧创建关键帧了.
4. 当前帧的质量: 当前帧观测到的地图点要足够多,同时与参考关键帧的重合程度不能太大.

具体的代码比较乱;不看了.

总体而言,ORB-SLAM2插入关键帧的策略还是比较宽松的,因为后面LocalMapping线程的函数LocalMapping::KeyFrameCulling()会剔除冗余关键帧,因此在系统处理得过来的情况下,要尽量多创建关键帧.

7.9.2 创建新关键帧: CreateNewKeyFrame()

创建新关键帧时,对于双目/RGBD相机输入情况下也创建新地图点.

void Tracking::CreateNewKeyFrame() {// step1. 构造关键帧KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
​// step2. 将创建出的关键帧设为参考关键帧mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;
​// step3. 对于双目/RGBD相机生成新的地图点if (mSensor != System::MONOCULAR) {mCurrentFrame.UpdatePoseMatrices();
​// step3.1. 按深度从小到大排序关键点vector<pair<float, int> > vDepthIdx;vDepthIdx.reserve(mCurrentFrame.N);for (int i = 0; i < mCurrentFrame.N; i++) {float z = mCurrentFrame.mvDepth[i];if (z > 0) {vDepthIdx.push_back(make_pair(z, i));}}if (!vDepthIdx.empty()) {sort(vDepthIdx.begin(), vDepthIdx.end());// step3.2. 找出没对应地图点的特征点,并创建新地图点int nPoints = 0;for (size_t j = 0; j < vDepthIdx.size(); j++) {int i = vDepthIdx[j].second;bool bCreateNew = false;MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];if (!pMP)bCreateNew = true;else if (pMP->Observations() < 1) {bCreateNew = true;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);}if (bCreateNew) {cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint *pNewMP = new MapPoint(x3D, pKF, mpMap);pNewMP->AddObservation(pKF, i);pKF->AddMapPoint(pNewMP, i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i] = pNewMP;}nPoints++;    // step3.3. 地图点过多(多于100个)或深度太深(误差太大),则停止生成地图点if (vDepthIdx[j].first > mThDepth && nPoints > 100)break;}}}
​// step4. 插入关键帧mpLocalMapper->InsertKeyFrame(pKF);mpLocalMapper->SetNotStop(false);mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;
}

7.10 跟踪函数: Track()

主要关注成员变量mState的变化:

void Tracking::Track() {// 维护状态if (mState == NO_IMAGES_YET) {mState = NOT_INITIALIZED;}
​unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​// step1. 若还没初始化,则尝试初始化if (mState == NOT_INITIALIZED) {if (mSensor == System::STEREO || mSensor == System::RGBD)StereoInitialization();elseMonocularInitialization();if (mState != OK)return;} else {// step2. 若系统已初始化,就进行跟踪(或重定位)bool bOK;
​// step2.1. 符合条件时,优先根据运动模型跟踪,如运动模型跟踪失败,就根据参考帧进行跟踪if (mState == OK) {CheckReplacedInLastFrame();if (mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId + 2) {bOK = TrackReferenceKeyFrame();} else {bOK = TrackWithMotionModel();if (!bOK)bOK = TrackReferenceKeyFrame();}} else {// step2.2. 若上一帧没跟踪丢失,则这一帧重定位bOK = Relocalization();}// step2.3. 设置当前帧的参考关键帧  mCurrentFrame.mpReferenceKF = mpReferenceKF;
​// step3. 跟踪局部地图,进一步优化当前帧位姿//  之前的跟踪过程都是仅根据前面某一帧进行的位姿优化,TrackLocalMap()使用局部地图进行位姿优化if (bOK)bOK = TrackLocalMap();
​// step4. 根据跟踪结果判断跟踪状态if (bOK)mState = OK;elsemState = LOST;
​// step5. 跟踪成功之后的后处理if (bOK) {// step5.1. 更新恒速运动模型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;   // mVelocity = Tcl = Tcw * Twl} elsemVelocity = cv::Mat();
​// step5.2. 剔除失效地图点for (int i = 0; i < mCurrentFrame.N; i++) {MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];if (pMP)if (pMP->Observations() < 1) {mCurrentFrame.mvbOutlier[i] = false;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);}}
​// step5.3. 清除恒速模型跟踪中UpdateLastFrame创建的当前帧临时地图点(跟踪失败的话就不清空临时地图点么?)for (list<MapPoint *>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end();lit != lend; lit++) {MapPoint *pMP = *lit;delete pMP;}mlpTemporalPoints.clear();
​// step5.4. 检测并插入关键帧,双目/RGBD相机会创建新地图点if (NeedNewKeyFrame())CreateNewKeyFrame();
​// step5.5. 删除外点for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);}}
​// step6. 若系统刚启动没多久就跟踪失败的话,就直接重启if (mState == LOST) {if (mpMap->KeyFramesInMap() <= 5) {cout << "Track lost soon after initialisation, reseting..." << endl;mpSystem->Reset();return;}}// step7. 更新上一帧数据mLastFrame = Frame(mCurrentFrame);}
​// step8. 记录位姿信息if (!mCurrentFrame.mTcw.empty()) {cv::Mat Tcr = mCurrentFrame.mTcw * mCurrentFrame.mpReferenceKF->GetPoseInverse();   // 相对于参考帧Tcr = Tcw * TwrmlRelativeFramePoses.push_back(Tcr);mlpReferences.push_back(mpReferenceKF);mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);mlbLost.push_back(mState == LOST);} else {// 跟踪失败就使用上一帧数据作为当前帧记录mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState == LOST);}
}

7.11 Tracking流程中的关键问题(暗线)

7.11.1 地图点的创建与删除

1. Tracking线程中初始化过程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())会创建新的地图点.
2. Tracking线程中创建新的关键帧(Tracking::CreateNewKeyFrame())会创建新的地图点.
3. Tracking线程中根据恒速运动模型估计初始位姿(Tracking::TrackWithMotionModel())也会产生临时地图点,但这些临时地图点在跟踪成功后会被马上删除.

所有的非临时地图点都是由关键帧建立的,Tracking::TrackWithMotionModel()中由非关键帧建立的关键点被设为临时关键点,很快会被删掉,仅作增强帧间匹配用,不会对建图产生任何影响.这也不违反只有关键帧才能参与LocalMapping和LoppClosing线程的原则.

思考: 为什么跟踪失败的话不删除这些局部地图点

跟踪失败的话不会产生关键帧,这些地图点也不会被注册进地图,不会对之后的建图产生影响.

思考: 那会不会发生内存泄漏呢?

不会的,因为最后总会有一帧跟踪上,这些临时地图点都被保存在了成员变量mlpTemporalPoints中,跟踪成功后会删除所有之前的临时地图点.

7.11.2 关键帧与地图点间发生关系的时机

• 新创建出来的非临时地图点都会与创建它的关键帧建立双向连接.
• 通过ORBmatcher::SearchByXXX()函数匹配得到的帧点关系只建立单向连接:
  • 只在关键帧中添加了对地图点的观测(将地图点加入到关键帧对象的成员变量mvpMapPoints中了).
  • 没有在地图点中添加对关键帧的观测(地图点的成员变量mObservations中没有该关键帧).

这为后文中LocalMapping线程中函数LocalMapping::ProcessNewKeyFrame()对关键帧中地图点的处理埋下了伏笔.该函数通过检查地图点中是否有对关键点的观测来判断该地图点是否是新生成的.

void LocalMapping::ProcessNewKeyFrame() {
​// 遍历关键帧中的地图点const vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for (MapPoint *pMP : vpMapPointMatches) {if (!pMP->IsInKeyFrame(mpCurrentKeyFrame)) {// step3.1. 该地图点是跟踪本关键帧时匹配得到的,在地图点中加入对当前关键帧的观测pMP->AddObservation(mpCurrentKeyFrame, i);pMP->UpdateNormalAndDepth();pMP->ComputeDistinctiveDescriptors();} else {// step3.2. 该地图点是跟踪本关键帧时新生成的,将其加入容器mlpRecentAddedMapPoints待筛选mlpRecentAddedMapPoints.push_back(pMP);}}
​// ...
}

7.12 参考关键帧: mpReferenceKF

• 参考关键帧的用途:
  1. Tracking线程中函数Tracking::TrackReferenceKeyFrame()根据参考关键帧估计初始位姿.
  2. 用于初始化新创建的MapPoint的参考帧mpRefKF,函数MapPoint::UpdateNormalAndDepth()中根据参考关键帧mpRefKF更新地图点的平均观测距离.
• 参考关键帧的指定:
  1. Traking线程中函数Tracking::CreateNewKeyFrame()创建完新关键帧后,会将新创建的关键帧设为参考关键帧.
  2. Tracking线程中函数Tracking::TrackLocalMap()跟踪局部地图过程中调用函数Tracking::UpdateLocalMap(),其中调用函数Tracking::UpdateLocalKeyFrames(),将与当前帧共视程度最高的关键帧设为参考关键帧.

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

加载词典和配置文件接受新图像初始化
MonocularInitialization或StereoInitialization()``Track()跟踪成功Track()跟踪失败Relocalization()重定位成功Relocalization()重定位失败SYSTEM_NOT_READY``NO_IMAGES_YET``NOT_INITIALIZED``OK``LOST

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

mState初始化成功初始化失败NOT_INITIALIZEDOK

输入状态Y跟踪失败N跟踪失败跟踪失败跟踪成功跟踪成功跟踪成功OK``LOST

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

8. ORB-SLAM2代码详解08_局部建图线程LocalMapping

8.1 各成员函数/变量

Tracking线程创建的所有关键帧都被插入到LocalMapping线程的缓冲队列mlNewKeyFrames中.

成员函数mbAcceptKeyFrames表示当前LocalMapping线程是否愿意接收关键帧,这会被Tracking线程函数Tracking::NeedNewKeyFrame()用作是否生产关键帧的参考因素之一;但即使mbAcceptKeyFrames为false,在系统很需要关键帧的情况下Tracking线程函数Tracking::NeedNewKeyFrame()也会决定生成关键帧.

8.2 局部建图主函数: Run()

NYNYN是否请求停止建图设置暂停接收关键帧
SetAcceptKeyFrames(false)是否存在未处理的关键帧
CheckNewKeyFrames处理队列中的关键帧
ProcessNewKeyFrame剔除冗余地图点
MapPointCulling创建新地图点
CreateNewMapPoints是否处理完所有关键帧
CheckNewKeyFrames融合当前关键帧和其共视帧的地图点
SearchInNeighbors局部BA优化
Optimizer::LocalBundleAdjustment剔除冗余关键帧
KeyFrameCulling设置继续接收关键帧
SetAcceptKeyFrames(true)当前线程暂停3秒
std::this_thread::sleep_for(std::chrono::milliseconds(3))

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

函数LocalMapping::Run()是LocalMapping线程的主函数,该函数内部是一个死循环,每3毫秒查询一次当前线程缓冲队列mlNewKeyFrames.若查询到了待处理的新关键帧,就进行查询

void LocalMapping::Run() {
​while (1) {SetAcceptKeyFrames(false);      // 设置当前LocalMapping线程处于建图状态,不愿意接受Tracking线程传来的关键帧
​// step1. 检查缓冲队列内的关键帧 if (CheckNewKeyFrames()) {// step2. 处理缓冲队列中第一个关键帧ProcessNewKeyFrame();// step3. 剔除劣质地图点MapPointCulling();
​// step4. 创建新地图点CreateNewMapPoints();
​if (!CheckNewKeyFrames()) {// step5. 将当前关键帧与其共视关键帧地图点融合SearchInNeighbors();// step6. 局部BA优化: 优化局部地图mbAbortBA = false;  Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);
​// step7. 剔除冗余关键帧KeyFrameCulling();}
​// step8. 将当前关键帧加入闭环检测中mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);} SetAcceptKeyFrames(true);       // 设置当前LocalMapping线程处于空闲状态,愿意接受Tracking线程传来的关键帧
​// 线程暂停3毫秒再开启下一轮查询std::this_thread::sleep_for(std::chrono::milliseconds(3));}
}

8.3 处理队列中第一个关键帧: ProcessNewKeyFrame()

在第3步中处理当前关键点时比较有意思,通过判断该地图点是否观测到当前关键帧(pMP->IsInKeyFrame(mpCurrentKeyFrame))来判断该地图点是否是当前关键帧中新生成的.

• 若地图点是本关键帧跟踪过程中匹配得到的(Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()和Tracking::SearchLocalPoints()中调用了ORBmatcher::SearchByProjection()和ORBmatcher::SearchByBoW()方法),则是之前关键帧中创建的地图点,只需添加其对当前帧的观测即可.
• 若地图点是本关键帧跟踪过程中新生成的(包括:1.单目或双目初始化Tracking::MonocularInitialization()、Tracking::StereoInitialization();2.创建新关键帧Tracking::CreateNewKeyFrame()),则该地图点中有对当前关键帧的观测,是新生成的地图点,放入容器mlNewKeyFrames中供LocalMapping::MapPointCulling()函数筛选.

void LocalMapping::ProcessNewKeyFrame() {
​// step1. 取出队列头的关键帧 {unique_lock<mutex> lock(mMutexNewKFs);mpCurrentKeyFrame = mlNewKeyFrames.front();mlNewKeyFrames.pop_front();}
​// step2. 计算该关键帧的词向量mpCurrentKeyFrame->ComputeBoW();
​// step3. 根据地图点中是否观测到当前关键帧判断该地图是是否是新生成的const vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for (size_t i = 0; i < vpMapPointMatches.size(); i++) {MapPoint *pMP = vpMapPointMatches[i];if (pMP && !pMP->isBad()) {if (!pMP->IsInKeyFrame(mpCurrentKeyFrame)) {// step3.1. 该地图点是跟踪本关键帧时匹配得到的,在地图点中加入对当前关键帧的观测pMP->AddObservation(mpCurrentKeyFrame, i);pMP->UpdateNormalAndDepth();pMP->ComputeDistinctiveDescriptors();} else // this can only happen for new stereo points inserted by the Tracking{// step3.2. 该地图点是跟踪本关键帧时新生成的,将其加入容器mlpRecentAddedMapPoints待筛选mlpRecentAddedMapPoints.push_back(pMP);}}}// step4. 更新共视图关系mpCurrentKeyFrame->UpdateConnections();// step5. 将关键帧插入到地图中mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

8.4 剔除坏地图点: MapPointCulling()

冗余地图点的标准:满足以下其中之一就算是坏地图点

1. $$召回率=\frac{实际观测到该地图点的帧数mnFound}{理论上应当观测到该地图点的帧数mnVisible}<0.25$$

2. 在创建的3帧内观测数目少于2(双目为3)

若地图点经过了连续3个关键帧仍未被剔除,则被认为是好的地图点

void LocalMapping::MapPointCulling() {list<MapPoint *>::iterator lit = mlpRecentAddedMapPoints.begin();const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
​int nThObs;if (mbMonocular)nThObs = 2;elsenThObs = 3;const int cnThObs = nThObs;
​while (lit != mlpRecentAddedMapPoints.end()) {MapPoint *pMP = *lit;if (pMP->isBad()) {// 标准0: 地图点在其他地方被删除了lit = mlpRecentAddedMapPoints.erase(lit);} else if (pMP->GetFoundRatio() < 0.25f) {// 标准1: 召回率<0.25pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);} else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs) {// 标准2: 从创建开始连续3个关键帧内观测数目少于cnThObspMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);} else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 3)// 通过了3个关键帧的考察,认为是好的地图点lit = mlpRecentAddedMapPoints.erase(lit);elselit++;}
}

MapPoint类中关于召回率的成员函数和变量如下:

这两个成员变量主要用于Tracking线程.

• 在函数Tracking::SearchLocalPoints()中,会对所有处于当前帧视锥内的地图点调用成员函数MapPoint::IncreaseVisible().(这些点未必真的被当前帧观测到了,只是地理位置上处于当前帧视锥范围内).

void Tracking::SearchLocalPoints() {// 当前关键帧的地图点for (MapPoint *pMP : mCurrentFrame.mvpMapPoints) {pMP->IncreaseVisible();}}}
​// 局部关键帧中不属于当前帧,但在当前帧视锥范围内的地图点for (MapPoint *pMP = *vit : mvpLocalMapPoints.begin()) {if (mCurrentFrame.isInFrustum(pMP, 0.5)) {pMP->IncreaseVisible();}}
​// ...
}

在函数Tracking::TrackLocalMap()中,会对所有当前帧观测到的地图点调用MaoPoint::IncreaseFound().

bool Tracking::TrackLocalMap() {// ...for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i]) {if (!mCurrentFrame.mvbOutlier[i]) {// 当前帧观测到的地图点mCurrentFrame.mvpMapPoints[i]->IncreaseFound();// ...}}}// ...
}

8.5 创建新地图点: CreateNewMapPoints()

将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配,生成地图点.

对于双目相机的匹配特征点对,可以根据某帧特征点深度恢复地图点,也可以根据两帧间对极几何三角化地图点,这里取视差角最大的方式来生成地图点.

8.6 融合当前关键帧和其共视帧的地图点: SearchInNeighbors()

本函数将当前关键帧与其一级和二级共视关键帧做地图点融合,分两步:

1. 正向融合: 将当前关键帧的地图点融合到各共视关键帧中.
2. 反向融合: 将各共视关键帧的地图点融合到当前关键帧中.

void LocalMapping::SearchInNeighbors() {// step1. 取当前关键帧的一级共视关键帧const vector<KeyFrame *> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(10);
​// step2. 遍历一级关键帧,寻找二级关键帧vector<KeyFrame *> vpTargetKFs;for (KeyFrame *pKFi : vpNeighKFs) {if (pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi);pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;const vector<KeyFrame *> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);for (KeyFrame *pKFi2 : vpSecondNeighKFs) {if (pKFi2->isBad() || pKFi2->mnFuseTargetForKF == mpCurrentKeyFrame->mnId || pKFi2->mnId == mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi2);}}
​
​// step3. 正向融合: 将当前帧的地图点融合到各共视关键帧中vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();ORBmatcher matcher;for (KeyFrame *pKFi : vpTargetKFs) {matcher.Fuse(pKFi, vpMapPointMatches);}
​// step4. 反向融合: 将各共视关键帧的地图点融合到当前关键帧中// step4.1. 取出各共视关键帧的地图点存入vpFuseCandidatesvector<MapPoint *> vpFuseCandidates;for (KeyFrame *pKFi : vpTargetKFs) {vector<MapPoint *> vpMapPointsKFi = pKFi->GetMapPointMatches();for (MapPoint *pMP : vpMapPointsKFi.begin()) {if (!pMP || pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)continue;pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;vpFuseCandidates.push_back(pMP);}}
​// step 4.2. 进行反向融合matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates);
​// step5. 更新当前关键帧的地图点信息vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for (MapPoint *pMP : vpMapPointMatches) {if (pMP and !pMP->isBad()) {pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();}}// step6. 更新共视图mpCurrentKeyFrame->UpdateConnections();
}

ORBmatcher::Fuse()将地图点与帧中图像的特征点匹配,实现地图点融合.

在将地图点反投影到帧中的过程中,存在以下两种情况:

1. 若地图点反投影对应位置上不存在地图点,则直接添加观测.
2. 若地图点反投影位置上存在对应地图点,则将两个地图点合并到其中观测较多的那个.

int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th) {// 遍历所有的待投影地图点for(MapPoint* pMP : vpMapPoints) {// step1. 将地图点反投影到相机成像平面上const float invz = 1/p3Dc.at<float>(2);const float x = p3Dc.at<float>(0)*invz;const float y = p3Dc.at<float>(1)*invz;const float u = fx*x+cx;const float v = fy*y+cy;const float ur = u-bf*invz;const float maxDistance = pMP->GetMaxDistanceInvariance();const float minDistance = pMP->GetMinDistanceInvariance();cv::Mat PO = p3Dw-Ow;const float dist3D = cv::norm(PO);
​// step2. 地图点观测距离if(dist3D<minDistance || dist3D>maxDistance )continue;
​// step3. 地图点的观测距离和观测方向不能太离谱if (dist3D < minDistance || dist3D > maxDistance)continue;cv::Mat Pn = pMP->GetNormal();if (PO.dot(Pn) < 0.5 * dist3D)continue;
​// step4. 在投影位置寻找图像特征点int nPredictedLevel = pMP->PredictScale(dist3D, pKF);const float radius = th * pKF->mvScaleFactors[nPredictedLevel];const vector<size_t> vIndices = pKF->GetFeaturesInArea(u, v, radius);const cv::Mat dMP = pMP->GetDescriptor();int bestDist = 256;int bestIdx = -1;for (size_t idx : vIndices) {const size_t idx = *vit;const cv::KeyPoint &kp = pKF->mvKeysUn[idx];const int &kpLevel = kp.octave;// step4.1. 金字塔层级要接近if (kpLevel < nPredictedLevel - 1 || kpLevel > nPredictedLevel)continue;// step4.2. 使用卡方检验检查重投影误差,单目和双目的自由度不同if (pKF->mvuRight[idx] >= 0) {const float ex = u - kp.pt.x;const float ey = v - kp.pt.y;const float er = ur - pKF->mvuRight[idx];const float e2 = ex * ex + ey * ey + er * er;if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 7.8)continue;} else {const float ex = u - kp.pt.x;const float ey = v - kp.pt.y;const float e2 = ex * ex + ey * ey;if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 5.99)continue;}// step4.3. 检验描述子距离const cv::Mat &dKF = pKF->mDescriptors.row(idx);const int dist = DescriptorDistance(dMP, dKF);if (dist < bestDist) {bestDist = dist;bestIdx = idx;}}
​// step5. 与最近特征点的描述子距离足够小,就进行地图点融合if (bestDist <= TH_LOW) {MapPoint *pMPinKF = pKF->GetMapPoint(bestIdx);if (pMPinKF) {// step5.1. 地图点反投影位置上存在对应地图点,则将两个地图点合并到其中观测较多的那个则直接添加观测if (!pMPinKF->isBad()) {if (pMPinKF->Observations() > pMP->Observations())pMP->Replace(pMPinKF);elsepMPinKF->Replace(pMP);}} else {// step5.2. 地图点反投影对应位置上不存在地图点,pMP->AddObservation(pKF, bestIdx);pKF->AddMapPoint(pMP, bestIdx);}nFused++;}}
​return nFused;
}

8.7 局部BA优化: Optimizer::LocalBundleAdjustment()

局部BA优化当前帧的局部地图.

• 当前关键帧的一级共视关键帧位姿会被优化;二极共视关键帧会加入优化图,但其位姿不会被优化.
• 所有局部地图点位姿都会被优化.

Tracking线程中定义了局部地图成员变量mvpLocalKeyFrames和mvpLocalMapPoints,但是这些变量并没有被LocalMapping线程管理,因此在函数Optimizer::LocalBundleAdjustment()中还要重新构造局部地图变量,这种设计有些多此一举了.

8.8 剔除冗余关键帧: KeyFrameCulling()

冗余关键帧标准: 90%以上的地图点能被超过3个其他关键帧观测到.

void LocalMapping::KeyFrameCulling() {
​// step1. 遍历当前关键帧的所有共视关键帧vector<KeyFrame *> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();for (KeyFrame *pKF : vpLocalKeyFrames) {
​// step2. 遍历所有局部地图点const vector<MapPoint *> vpMapPoints = pKF->GetMapPointMatches();int nRedundantObservations = 0;int nMPs = 0;for (MapPoint *pMP : vpMapPoints) {if (pMP && !pMP->isBad()) {if (!mbMonocular) {// 双目相机只能看到不超过相机基线35倍的地图点if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)continue;}nMPs++;int nObs = 0;for (KeyFrame *pKFi : pMP->GetObservations()) {= mit->first;if (pKFi->mvKeysUn[mit->second].octave <= pKF->mvKeysUn[i].octave + 1) {nObs++;if (nObs >= 3)break;}}if (nObs >= 3) {nRedundantObservations++;}}}}
​// step3. 若关键帧超过90%的地图点能被超过3个其它关键帧观测到,则视为冗余关键帧if (nRedundantObservations > 0.9 * nMPs)pKF->SetBadFlag();
}

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

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

9. ORB-SLAM2代码详解09_闭环线程LoopClosing

9.1 各成员函数/变量

9.1.1 闭环主函数: Run()

void LoopClosing::Run() {while (1) {if (CheckNewKeyFrames()) {if (DetectLoop()) {if (ComputeSim3()) {CorrectLoop();}}}
​std::this_thread::sleep_for(std::chrono::milliseconds(5));}
}

9.2 闭环检测: DetectLoop()

LoopClosing类中定义类型ConsistentGroup,表示关键帧组.

typedef pair<set<KeyFrame *>, int> ConsistentGroup

• 第一个元素表示一组共视关键帧.
• 第二个元素表示该关键帧组的连续长度.

所谓连续,指的是两个关键帧组中存在相同的关键帧.

闭环检测原理: 若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,且这些闭环匹配关键帧组间是连续的,则认为实现闭环,

具体来说,回环检测过程如下:

1. 找到当前关键帧的闭环候选关键帧vpCandidateKFs:

   闭环候选关键帧取自于与当前关键帧具有相同的BOW向量但不存在直接连接的关键帧.

   

2. 将闭环候选关键帧和其共视关键帧组合成为关键帧组vCurrentConsistentGroups:

   

3. 在当前关键组和之前的连续关键组间寻找连续关系.

   • 若当前关键帧组在之前的连续关键帧组中找到连续关系,则当前的连续关键帧组的连续长度加1.
   • 若当前关键帧组在之前的连续关键帧组中没能找到连续关系,则当前关键帧组的连续长度为0.

   关键帧组的连续关系是指两个关键帧组间是否有关键帧同时存在于两关键帧组中.

   

   若某关键帧组的连续长度达到3,则认为该关键帧实现闭环.

bool LoopClosing::DetectLoop() {// step1. 取出缓冲队列头部的关键帧,作为当前检测闭环关键帧,设置其不被优化删除{unique_lock<mutex> lock(mMutexLoopQueue);mpCurrentKF = mlpLoopKeyFrameQueue.front();mlpLoopKeyFrameQueue.pop_front();mpCurrentKF->SetNotErase();}
​// step2. 距离上次闭环时间太短,不再检测闭环if (mpCurrentKF->mnId < mLastLoopKFid + 10) {mpKeyFrameDB->add(mpCurrentKF);mpCurrentKF->SetErase();return false;}
​// step3. 计算当前关键帧与共视关键帧间最大相似度const vector<KeyFrame *> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;float minScore = 1;for (KeyFrame *pKF : vpConnectedKeyFrames) {const DBoW2::BowVector &BowVec = pKF->mBowVec;float score = mpORBVocabulary->score(CurrentBowVec, BowVec);if (score < minScore)minScore = score;}
​// step4. 寻找当前关键帧的闭环候选关键帧vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);if (vpCandidateKFs.empty()) {mpKeyFrameDB->add(mpCurrentKF);mvConsistentGroups.clear();mpCurrentKF->SetErase();return false;}// step5. 在当前关键帧组和之前的连续关键帧组之间寻找匹配mvpEnoughConsistentCandidates.clear();vector<ConsistentGroup> vCurrentConsistentGroups;vector<bool> vbConsistentGroup(mvConsistentGroups.size(), false);   // 之前的连续关键帧组在当前关键帧组中是否存在连续
​// 遍历当前闭环候选关键帧for (KeyFrame *pCandidateKF : vpCandidateKFs) {// step5.1. 构建关键帧组,包括候选关键帧及其共视关键帧set<KeyFrame *> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();spCandidateGroup.insert(pCandidateKF);
​// step5.2. 遍历之前的连续关键帧组,寻找连续关系        bool bEnoughConsistent = false;bool bConsistentForSomeGroup = false;for (size_t iG = 0, iendG = mvConsistentGroups.size(); iG < iendG; iG++) {set<KeyFrame *> sPreviousGroup = mvConsistentGroups[iG].first;bool bConsistent = false;// step5.2. 若当前连续关键帧组中某关键帧也在前一帧的候选关键帧组中,则找到了连续关系for (KeyFrame * previousKeyFrame : spCandidateGroup.begin()) {if (sPreviousGroup.count(previousKeyFrame)) {bConsistent = true;bConsistentForSomeGroup = true;break;}}
​// step5.3. 更新当前关键帧组的连续次数if (bConsistent) {int nCurrentConsistency = mvConsistentGroups[iG].second + 1;if (!vbConsistentGroup[iG]) {ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency);vCurrentConsistentGroups.push_back(cg);vbConsistentGroup[iG] = true;}// 若当前关键帧组的连续次数达到3,则完成闭环,将其加入到mvpEnoughConsistentCandidates中if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) {mvpEnoughConsistentCandidates.push_back(pCandidateKF);bEnoughConsistent = true;}}}// 5.4. 若当前关键帧组在前一关键帧的闭环候选关键帧组中找不到连续关系,则将两虚次数置零if (!bConsistentForSomeGroup) {ConsistentGroup cg = make_pair(spCandidateGroup, 0);vCurrentConsistentGroups.push_back(cg);}}// step6. 维护循环变量mvConsistentGroups = vCurrentConsistentGroups;      // 更新连续关键帧组mpKeyFrameDB->add(mpCurrentKF);                     // 将当前关键帧加入到关键帧数据库中
​if (mvpEnoughConsistentCandidates.empty()) {mpCurrentKF->SetErase();return false;} else {return true;}
}

当前关键帧的闭环候选关键帧取自于与当前关键帧具有相同BOW向量但不直接相连的关键帧.

// 寻找当前关键帧的闭环候选关键帧
vector<KeyFrame *> KeyFrameDatabase::DetectLoopCandidates(KeyFrame *pKF, float minScore) {// step1. 找出当前关键帧的所有共视关键帧set<KeyFrame *> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
​
​// step2. 找出所有具有相同BOW但不直接相连的关键帧list<KeyFrame *> lKFsSharingWords;      // 存储闭环候选关键帧{unique_lock<mutex> lock(mMutex);// 遍历所有BOW词向量for (DBoW2::BowVector vit : pKF) {// 遍历所有含有该词向量的关键帧for (KeyFrame *pKFi : mvInvertedFile[vit.first]) {if (pKFi->mnLoopQuery != pKF->mnId) {pKFi->mnLoopWords = 0;// 若该关键帧与当前关键帧不直接相连,才能作为闭环候选if (!spConnectedKeyFrames.count(pKFi)) {pKFi->mnLoopQuery = pKF->mnId;lKFsSharingWords.push_back(pKFi);}}pKFi->mnLoopWords++;}}}
​// step3. 以最大相似度的0.8倍为阈值筛选筛选候选关键帧int maxCommonWords = 0;list<pair<float, KeyFrame *> > lScoreAndMatch;for (KeyFrame *pKFi : lKFsSharingWords) {if (*pKFi->mnLoopWords > maxCommonWords)maxCommonWords = *pKFi->mnLoopWords;}int minCommonWords = maxCommonWords * 0.8f;for (KeyFrame *pKFi : lKFsSharingWords) {if (pKFi->mnLoopWords > minCommonWords) {float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec);pKFi->mLoopScore = si;if (si >= minScore)lScoreAndMatch.push_back(make_pair(si, pKFi));}}
​// step4. 统计候选关键帧的共视关键帧组的相似度得分list<pair<float, KeyFrame *> > lAccScoreAndMatch;float bestAccScore = minScore;for (list<pair<float, KeyFrame *> >::iterator it : lScoreAndMatch) {KeyFrame *pKFi = it->second;vector<KeyFrame *> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);float bestScore = it->first; float accScore = it->first;  KeyFrame *pBestKF = pKFi;    for (vector<KeyFrame *>::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) {KeyFrame *pKF2 = *vit;if (pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) {accScore += pKF2->mLoopScore;if (pKF2->mLoopScore > bestScore) {pBestKF = pKF2;bestScore = pKF2->mLoopScore;}}}
​lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF));if (accScore > bestAccScore)bestAccScore = accScore;}
​// step5. 取相似度得分高于最高相似度0.75的组的最优匹配关键帧    float minScoreToRetain = 0.75f * bestAccScore;set<KeyFrame *> spAlreadyAddedKF;vector<KeyFrame *> vpLoopCandidates;for (list<pair<float, KeyFrame *> >::iterator it : lAccScoreAndMatch.begin()) {if (it->first > minScoreToRetain) {KeyFrame *pKFi = it->second;if (!spAlreadyAddedKF.count(pKFi)) {vpLoopCandidates.push_back(pKFi);spAlreadyAddedKF.insert(pKFi);}}}
​return vpLoopCandidates;
}

9.3 计算Sim3变换: ComputeSim3()

bool LoopClosing::ComputeSim3() {const int nInitialCandidates = mvpEnoughConsistentCandidates.size();ORBmatcher matcher(0.75, true);
​vector<Sim3Solver *> vpSim3Solvers;                 // 保存每个闭环匹配关键帧的Sim3Solvervector<vector<MapPoint *> > vvpMapPointMatches;     // 保存当前关键帧到每个闭环匹配关键帧的匹配关系vector<bool> vbDiscarded;                           // 保存每个闭环匹配关键帧是否是误匹配
​// step1. 为每个有超过20个匹配点的闭环关键帧创建Sim3Solverint nCandidates = 0;for (int i = 0; i < nInitialCandidates; i++) {KeyFrame *pKF = mvpEnoughConsistentCandidates[i];pKF->SetNotErase();int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);if (nmatches < 20) {vbDiscarded[i] = true;continue;} else {Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale);pSolver->SetRansacParameters(0.99, 20, 300);vpSim3Solvers[i] = pSolver;}nCandidates++;}
​// step2. 对每个闭环候选关键帧求解优化Sim3bool bMatch = false;        // 是否有帧通过Sim3求解while (nCandidates > 0 && !bMatch) {for (int i = 0; i < nInitialCandidates; i++) {if (vbDiscarded[i])continue;
​KeyFrame *pKF = mvpEnoughConsistentCandidates[i];vector<bool> vbInliers;int nInliers;bool bNoMore;// step2.1. 进行Sim3迭代求解Sim3Solver *pSolver = vpSim3Solvers[i];cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers);if (bNoMore) {vbDiscarded[i] = true;nCandidates--;}
​if (!Scm.empty()) {// step2.2 根据计算出的Sim3搜索匹配点vector<MapPoint *> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint *>(NULL));for (size_t j = 0, jend = vbInliers.size(); j < jend; j++) {if (vbInliers[j])vpMapPointMatches[j] = vvpMapPointMatches[i][j];}cv::Mat R = pSolver->GetEstimatedRotation();cv::Mat t = pSolver->GetEstimatedTranslation();const float s = pSolver->GetEstimatedScale();matcher.SearchBySim3(mpCurrentKF, pKF, vpMapPointMatches, s, R, t, 7.5);
​// step2.3. 根据搜索出的匹配点优化Sim3g2o::Sim3 gScm(Converter::toMatrix3d(R), Converter::toVector3d(t), s);const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);if (nInliers >= 20) {bMatch = true;mpMatchedKF = pKF;g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0);mg2oScw = gScm * gSmw;mScw = Converter::toCvMat(mg2oScw);mvpCurrentMatchedPoints = vpMapPointMatches;break;}}}}
​// step2.4. 优化失败,退出函数if (!bMatch) {for (int i = 0; i < nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();mpCurrentKF->SetErase();return false;}
​// step3. 将闭环关键帧及其共视关键帧的所有地图点 投影到 当前关键帧vector<KeyFrame *> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();vpLoopConnectedKFs.push_back(mpMatchedKF);for (KeyFrame *pKF : vpLoopConnectedKFs) {for (MapPoint *pMP : pKF->GetMapPointMatches()) {if (pMP && !pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) {mvpLoopMapPoints.push_back(pMP);pMP->mnLoopPointForKF = mpCurrentKF->mnId;}}}matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints, 10);
​// step5. 根据投影成功的地图点数判断Sim3计算的是否准确int nTotalMatches = 0;for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {if (mvpCurrentMatchedPoints[i])nTotalMatches++;}
​if (nTotalMatches >= 40) {for (int i = 0; i < nInitialCandidates; i++)if (mvpEnoughConsistentCandidates[i] != mpMatchedKF)mvpEnoughConsistentCandidates[i]->SetErase();return true;} else {for (int i = 0; i < nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();mpCurrentKF->SetErase();return false;}
}

9.4 闭环矫正: CorrectLoop()

函数LoopClosing::CorrectLoop()的主要流程:

1. Sim3位姿传播:
   • 将Sim3位姿传播到局部关键帧组上.
   • 将Sim3位姿传播到局部地图点上.
2. 地图点融合:
   • 将闭环关键帧组地图点投影到当前关键帧上.
   • 将闭环关键帧组地图点投影到局部关键帧组上.
3. BA优化
   • 本质图BA优化: 优化所有地图点和关键帧位姿,基于本质图.
   • 全局BA优化: 优化所有地图点和关键帧位姿,基于地图点到关键帧的投影关系.

void LoopClosing::CorrectLoop() {
​cout << "Loop detected!" << endl;
​// step0. 更新当前关键帧组与地图点的连接mpCurrentKF->UpdateConnections();
​// step1. Sim3位姿传播// step1.1. 构建局部关键帧组mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();mvpCurrentConnectedKFs.push_back(mpCurrentKF);map<KeyFrame*, g2o::Sim3> CorrectedSim3, NonCorrectedSim3;  // 存放局部关键帧组Sim3位姿传播前后的位姿CorrectedSim3[mpCurrentKF] = mg2oScw;    cv::Mat Twc = mpCurrentKF->GetPoseInverse();{unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​// step1.2 将Sim3位姿传播到局部关键帧组中for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {cv::Mat Tiw = pKFi->GetPose();if (pKFi != mpCurrentKF) {cv::Mat Tic = Tiw * Twc;cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3);cv::Mat tic = Tic.rowRange(0, 3).col(3);g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0);g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;CorrectedSim3[pKFi] = g2oCorrectedSiw;}cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);cv::Mat tiw = Tiw.rowRange(0, 3).col(3);g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);NonCorrectedSim3[pKFi] = g2oSiw;}// step1.3. 将Sim3位姿传播到局部地图点上for (pair<KeyFrame*, g2o::Sim3> mit : CorrectedSim3) {KeyFrame *pKFi = mit.first;g2o::Sim3 g2oCorrectedSiw = mit.second;g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];
​for (MapPoint *pMPi : pKFi->GetMapPointMatches()) {if (!pMPi || pMPi->isBad())continue;if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId)     // 标记,防止重复矫正continue;cv::Mat P3Dw = pMPi->GetWorldPos();Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);pMPi->SetWorldPos(cvCorrectedP3Dw);pMPi->mnCorrectedByKF = mpCurrentKF->mnId;pMPi->mnCorrectedReference = pKFi->mnId;pMPi->UpdateNormalAndDepth();}
​// 将更新后的Sim3位姿赋值给关键帧变量Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();Eigen::Vector3d eigt = g2oCorrectedSiw.translation();double s = g2oCorrectedSiw.scale();eigt *= (1. / s);cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt);pKFi->SetPose(correctedTiw);pKFi->UpdateConnections();}// step2. 地图点融合// step2.1 将闭环关键帧组地图点融合到当前关键帧上for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {if (mvpCurrentMatchedPoints[i]) {MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);if (pCurMP)pCurMP->Replace(pLoopMP);       // 闭环关键帧组地图点在地图中的时间更长,位姿更准确else {mpCurrentKF->AddMapPoint(pLoopMP, i);pLoopMP->AddObservation(mpCurrentKF, i);pLoopMP->ComputeDistinctiveDescriptors();}}}
​}
​// step2.2 将闭环关键帧组地图点融合到局部关键帧组上SearchAndFuse(CorrectedSim3);
​
​// step3. BA优化// step3.0. 查找回环连接边,用于和生成树共同组成本质图map<KeyFrame *, set<KeyFrame *> > LoopConnections;for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();pKFi->UpdateConnections();// 闭环矫正后的共视关系 - 闭环矫正前的共视关系 = 闭环带来的新共视关系for (KeyFrame* prev_KFi : vpPreviousNeighbors) {LoopConnections[pKFi].erase(*prev_KFi);}for (KeyFrame* prev_KFi : mvpCurrentConnectedKFs) {LoopConnections[pKFi].erase(prev_KFi);}}
​// step3.1. 本质图BA优化mpMatchedKF->AddLoopEdge(mpCurrentKF);mpCurrentKF->AddLoopEdge(mpMatchedKF);Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
​// step3.2. 全局BA优化mbRunningGBA = true;mbFinishedGBA = false;mbStopGBA = false;mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);
​cout << "Loop Closed!" << endl;mLastLoopKFid = mpCurrentKF->mnId;
}

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

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

10. ORB-SLAM2代码详解十大trick

10.1. 关键帧与关键点的删除

由于关键帧与关键点的作用各个地方都要用到，所以对关键帧和地图点的操作就属于一种“重操作”，时间消耗很严重，这里作者采用了很巧妙的办法来处理，以防止系统卡顿或者加锁计算对系统的数据资源的共享。总的来说是采用一种先标记后处理的方式，标志该地图点或者关键帧社会性死亡。当然这带来的副作用就是每一个地图点或者关键帧都多了一个状态值，每次访问之前都需要判断关键帧或者地图点的状态以判断该地图点或者关键帧的状态还正常。

10.2 ORB特征点提取过程中的超像素处理

首先作者通过很巧妙的方法（极大值抑制，分区处理，递归等方法）获取了均匀分布的特征点。随后在极线方向进行搜索，获取对应的匹配点。接着在对应的匹配点的位置左右俩个侧，取三个点，根据极限上三个点的位置拟合出一条高斯曲线，获取你和的高斯曲线的最值所在的点，也即最终的匹配点在极限上的坐标，从而完成了从像素到超像素级别的求解，这个设计很nice!

10.3 最小生成树的维护

10.4 不同高斯金字塔下的视差与距离的约束关系的增加

10.5 对地图点的处理

采用先标记后清除的方式，由于地图点的操作，可能会影响到大部分的操作，如果直接删除地图点，会涉及到所有地图点的有关的操作，属于一个”重操作“，可能会造成程序的阻塞，这里迂回的采用先标记后清除的方式，使得对地图点的操作可以等到操作系统相对空闲时候去做，这种思想值得我们学习。

使用类似质量评估的概念，nObs记录当前地图点被（多少）相机观测到的次数（质量评估）。单目帧每次观测加1,双目帧每次观测加2.同时采用不同金字塔下的最近距离和最远距离

11. ORB-SLAM2代码详解之十大缺点及待优化空间

12. 关注点

基于对地图点的观测重新构造共视图: UpdateConnections 见5.1.2