【ORB_SLAM2源码解读】System::TrackRGBD Tracking::GrabImageRGBD Frame::Frame Track

函数System::TrackRGBD的执行流程

ORB_SLAM2实际上是四个线程，我觉得可视化Viewer线程可以实时检验Tracking、LocalMapping、LoopClosing的结果这很重要不是吗？
我以线程的启动和停止为主线，模拟了一下程序执行的过程中函数的调用和变量值的改变，这个过程可以干掉很多ORB_SLAM2的代码。

接下来执行System::TrackRGBD函数, 依次执行Tracking::GrabImageRGBD Frame::Frame Frame::ExtractORB ORBextractor::operator()等函数 。添加微信 slamshizhanjiaocheng 邀请加入 SLAM实战教程 微信交流群

给SLAM对象的TrackRGBD函数传入初始值分别是：彩色图 深度图 时间戳

// Pass the image to the SLAM system
SLAM.TrackRGBD(imRGB,imD,tframe);

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
{
    if(mSensor!=RGBD)
    {
        cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
        exit(-1);
    }    

    // Check mode change 这一部分主要是对局部地图线程进行操作
    // mbActivateLocalizationMode为true会关闭局部地图线程,在显示窗口通过按键赋值 pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true);
    {
        // 独占锁
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)// 纯定位模式
        {
            mpLocalMapper->RequestStop();//关闭局部地图的线程

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())// 局部地图线程是否已经关闭
            {
                usleep(1000);
            }

            mpTracker->InformOnlyTracking(true);//只定位
            mbActivateLocalizationMode = false;// 复位标记防止重复执行
        }
        if(mbDeactivateLocalizationMode)// 关闭纯定位模式
        {
            mpTracker->InformOnlyTracking(false);// 局部地图线程开始工作
            mpLocalMapper->Release();// 关键帧从局部地图中删除
            mbDeactivateLocalizationMode = false;// 复位标记防止重复执行
        }
    }

    // Check reset
    {
        unique_lock<mutex> lock(mMutexReset);// mbReset加锁
        if(mbReset)
        {
            mpTracker->Reset();// mpViwer可视化视图停止更新, mpLocalMapper局部地图停止, mpLoopClosing闭环检测停止, mpMap和mpKeyFrameDB清空
            mbReset = false;
        }
    }

    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}

ORB_SLAM2/include/System.h文件中定义下面两个bool变量，

ORB_SLAM2/src/System.cc文件中的构造函数对这两个变量进行初始化，均为false。
如果想改变这两个变量的值，可以调用下面两个函数
最终发现是在可视化线程中调用上面两个函数的
通过可视化界面点击按钮和bLocalizationMode的值进行触发定位和定位建图两种模式的切换

pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
menuLocalizationMode && !bLocalizationMode

来看下面代码
当触发这个的bool变量mbActivateLocalizationMode的值为true时，
对建立地图线程发送停止请求mpLocalMapper->RequestStop();

mbActivateLocalizationMode为true会关闭局部地图线程

LocalMapping涉及到的变量如下图所示，在构造函数中默认值是false

执行流程：
mbAbortBA(false), 和mbStopRequested(false)在构造函数中初始值是false
LocalMapping::RequestStop函数设置为true->mbStopRequested = true;、mbAbortBA = true;
LocalMapping::Stop()函数中mbNotStop初始化是(false)，这个判断if(mbStopRequested && !mbNotStop)满足使得mbStopped = true；

判断mbReset这个变量的值，mbReset它是一个私有的布尔类型变量，就是说它只会在System.cc中使用，flase很常见是初始状态，重要的是什么条件会触发这个变量的值变为true呢？下面我们一起来看看吧！

一个程序有很多的变量，程序跑起来的时候这些变量的值就会发生变化，我们的程序之所以呈现不同的结果全是通过变量的值驱动的，就像一个水龙头的开关，变量，局部变量，全局变量，私有变量，公有变量，宏定义，条件编译等都可以实现类似功能，不论是程序还是算法，我们操作本质都是数据，或者都是变量，操作着变量与变量之间的关系。

这是唯一一个让mbReset = true的地方，那又是谁调用了System::Reset()这个函数呢？ src/Tracking.cc:   mpSystem->Reset();

void System::Reset()
{
    unique_lock<mutex> lock(mMutexReset);
    mbReset = true;
}

可以看到就是 Track lost 跟踪丢失的时候就会调用 mpSystem->Reset()这个函数，接下来看看mbReset = false;系统都做了什么！
// Reset if the camera get lost soon after initialization
if(mState==LOST){
  if(mpMap->KeyFramesInMap()<=5){
    cout << "Track lost soon after initialisation, reseting..." << endl;
      mpSystem->Reset(); 
        return;
  }
}

会调用Tracking::Reset()函数
void Tracking::Reset()
{
    cout << "System Reseting" << endl;
    // 关闭可视化
    if(mpViewer)
    {
        mpViewer->RequestStop();
        while(!mpViewer->isStopped())
            usleep(3000);
    }

    // Reset Local Mapping 停止局部建立地图
    cout << "Reseting Local Mapper...";
    mpLocalMapper->RequestReset();
    cout << " done" << endl;

    // Reset Loop Closing 停止回环检测
    cout << "Reseting Loop Closing...";
    mpLoopClosing->RequestReset();
    cout << " done" << endl;

    // Clear BoW Database 清除词典
    cout << "Reseting Database...";
    mpKeyFrameDB->clear();
    cout << " done" << endl;

    // Clear Map (this erase MapPoints and KeyFrames) 清除地图
    mpMap->clear();

    KeyFrame::nNextId = 0; // 关键帧ID置零
    Frame::nNextId = 0; // 普通帧ID置零
    mState = NO_IMAGES_YET; // mState 

    // 删除初始化指针
    if(mpInitializer)
    {
        delete mpInitializer;
        mpInitializer = static_cast<Initializer*>(NULL);
    }
    // Lists used to recover the full camera trajectory at the end of the execution.
    // Basically we store the reference keyframe for each frame and its relative transformation
    mlRelativeFramePoses.clear();// 清空参RelativeFrame相对位姿 list
    mlpReferences.clear();// 清空参RelativeFrame参考关键帧的位姿 list
    mlFrameTimes.clear(); // 清空帧的时间戳 list
    mlbLost.clear(); // 清空是否跟丢的标志 list

    if(mpViewer)
        mpViewer->Release();
}

函数System::TrackStereo的执行流程

// Pass the images to the SLAM system
//执行ORB_SLAM2::System构造函数,创建ORB_SLAM2::System对象,依次执行 System::System Tracking::Tracking ORBextractor::ORBextractor 等构造函数 
//   a. 金字塔每层图像缩放系数 
//   b. 金字塔每层图像提取特征点数量 
//   c. 计算umax --> 添加微信 slamshizhanjiaocheng 邀请加入 SLAM实战教程 微信交流群
SLAM.TrackStereo(imLeftRect,imRightRect,tframe);
cv::Mat System::TrackStereo
Tracking::GrabImageStereo
Frame::Frame
Frame::ExtractORB
ORBextractor::ORBextractor

函数System::TrackMonocular的执行流程

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
...
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

GrabImageMonocular做的工作有 彩色图像转换为灰度图

判断这两个变量的值

mState==NOT_INITIALIZED || mState==NO_IMAGES_YET

构造mCurrentFrame

主要的区别就是没有初始化会调用创建一个特征点的ORBextractor* mpIniORBextractor;提取两倍的特征点
初始化正常运行之后只提取一倍的特征点ORBextractor* mpORBextractorLeft,

函数Tracking::GrabImageRGBD的执行流程

mImGray = imRGB;
cv::Mat imDepth = imD;

// 将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
    if(mbRGB)// 图像的颜色顺序
        cvtColor(mImGray,mImGray,CV_RGB2GRAY);
    else
        cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
    if(mbRGB)
        cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
    else
        cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}

首先是把彩色图imRGB图像传给在Tracking.h头文件中创建的变量cv::Mat mImGray;，
再把深度图imD传递给cv::Mat imDepth；
然后判断mImGray是三通道还是四通道图像，再通过变量bool mbRGB;判断图像的颜色顺序 (true RGB, false BGR, ignored if grayscale)
然后调用不同的函数将彩色图转换程灰度图；

下面这段代码的含义将深度图的深度信息转换成实际的深度，这样imDepth代表的就是三维点的真实的Ｚ值了。

if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
    imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);

要理解这个首先要理解一下深度图的生成原理，深度图是由相机拍摄的，每个像素值代表的是物体到相机xy平面的距离，单位为mm。
同一个物体的平面距离是相同的，所以颜色也相同，深度图通过颜色来表示距离，所以在原有深度图的基础上，再乘上一个系数mDepthMapFactor就可以和真实的距离对应上。深度图图像如下图所示：

下面补充一下参数文件中参数ThDepth、DepthMapFactor、和函数cv::Mat.convertTo的含义

mbf = fSettings["Camera.bf"];
# IR projector baseline times fx (aprox.)
# D455的基线Baseline	95 mm 乘　Camera.fx: 385.5836486816406
Camera.bf: 36.630446624756

# Close/Far threshold. Baseline times.
ThDepth: 50.0

float fx = fSettings["Camera.fx"];
Camera.fx: 385.5836486816406

# Depth Threshold (Close/Far Points): 4.75单位是ｍ
if(sensor==System::STEREO || sensor==System::RGBD)
{   
    // 远近点阈值，就是基线的多少倍，50 
    mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
    cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
}

# Deptmap values factor 深度图中的DepthMapFactor为1000.0，代表深度图中的1000.0个单位为1米。
DepthMapFactor: 1000.0
if(sensor==System::RGBD)
{
    mDepthMapFactor = fSettings["DepthMapFactor"];
    if(fabs(mDepthMapFactor)<1e-5)// 0.00001
        mDepthMapFactor=1;
    else
        mDepthMapFactor = 1.0f/mDepthMapFactor;
        // 1.0f/mDepthMapFactor: 0.001
}

// 将深度图数据类型变换到CV_32F，如果数据集中的数据有缩放的话需要进行缩放
    //#define CV_8U   0
    //#define CV_8S   1
    //#define CV_16U  2
    //#define CV_16S  3
    //#define CV_32S  4
    //#define CV_32F  5
    //#define CV_64F  6
    //#define CV_16F  7
    std::cout << "imDepth.type() " << imDepth.type() << std::endl;// CV_16U  2 16 位无符号整数（0-65535）
    std::cout << "imDepth.dims " << imDepth.dims << std::endl;//
    std::cout << "imDepth.dims " << imDepth.dims << std::endl;//
    std::cout << "imDepth.dims " << imDepth.dims << std::endl;//
    std::cout << "imDepth.channels() " << imDepth.channels() << std::endl;//
//    data：Mat对象中的一个指针，指向内存中存放矩阵数据的一块内存 (uchar* data)
//    dims：Mat所代表的矩阵的维度，如 3 * 4 的矩阵为 2 维， 3 * 4 * 5 的为3维
//    channels：通道，矩阵中的每一个矩阵元素拥有的值的个数，比如说 3 * 4 矩阵中一共 12 个元素，如果每个元素有三个值，那么就说这个矩阵是 3 通道的，即 channels = 3。常见的是一张彩色图片有红、绿、蓝三个通道。
//    depth：深度，即每一个像素的位数(bits)，在opencv的Mat.depth()中得到的是一个 0 – 6 的数字，分别代表不同的位数：enum { CV_8U=0, CV_8S=1, CV_16U=2, CV_16S=3, CV_32S=4, CV_32F=5, CV_64F=6 }; 可见 0和1都代表8位， 2和3都代表16位，4和5代表32位，6代表64位；
//    step：是一个数组，定义了矩阵的布局，另外注意 step1 (step / elemSize1)，M.step[m-1] 总是等于 elemSize，M.step1(m-1)总是等于 channels；
//    elemSize : 矩阵中每一个元素的数据大小，如果Mat中的数据的数据类型是 CV_8U 那么 elemSize = 1，CV_8UC3 那么 elemSize = 3，CV_16UC2 那么 elemSize = 4；记住另外有个 elemSize1 表示的是矩阵中数据类型的大小，即 elemSize / channels 的大小

//    Image.shape ：返回图像Image宽度、高度以及通道数，若为灰度图像，则不返回通道数
//    Image.size： 返回图像Image的像素大小，没有通道数；
//    Image.dtype： datatype的缩写，返回图像的数据类型，通常返回unit8，无符号8位整型

    cout << "mDepthMapFactor " << mDepthMapFactor << endl;// 0.0002
    cout << "mDepthMapFactor-1.0f " << mDepthMapFactor-1.0f << endl;// -0.9998
    cout << "(fabs(mDepthMapFactor-1.0f) " << fabs(mDepthMapFactor-1.0f) << endl;// 0.9998
    cout << "(fabs(mDepthMapFactor-1.0f)>1e-5) " << (fabs(mDepthMapFactor-1.0f)>1e-5) << endl;// 1

    // 将灰度图数据类型CV_16U转到CV_32F,如果数据集中的数据有缩放的话需要缩放 mDepthMapFactor
    // https://blog.csdn.net/jgj123321/article/details/95057025
    // https://blog.csdn.net/mars_xiaolei/article/details/88637099
    if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
        imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);

    std::cout << "imDepth.type() " << imDepth.type() << std::endl;// CV_32F  5  32 位浮点数 1.18*10-38 3.40*10-38
    std::cout << "imDepth.dims " << imDepth.dims << std::endl;//
    std::cout << "imDepth.channels() " << imDepth.channels() << std::endl;//


深度图的距离值：imDepth.z 1.194

3D点坐标：
[0.31058097;
 -0.57546705;
 1.194]
 
z 1.212

下面是可以验证上述代码的可执行程序

#include 

#include
#include 

using namespace std;


int main() {
    std::cout << "Hello, World!" << std::endl;
    cv::Mat imDepth;
    imDepth = cv::imread("/home/xq/CLionProjects/untitled2/1305031102.160407.png", -1);

    cv::imshow("imDepth", imDepth);
    cv::waitKey(0);

    float mDepthMapFactor;
    mDepthMapFactor = 5000.0;
    if(fabs(mDepthMapFactor)<1e-5)
        mDepthMapFactor=1;
    else
        mDepthMapFactor = 1.0f/mDepthMapFactor;

    // CV_8UC3: 8U表示颜色范围, 8位二进制 无符号字符型 最大为255, C3代表RGB是3通道
    // CV_16U 或者 CV_16UC1:  16位无符号整数（0-65535） 灰度图C1, 只有一个通道
    // C4: 4通道 带alph通道的图像
    std::cout << "imDepth.type() " << imDepth.type() << std::endl;// 2 CV_16U - 16位无符号整数（0-65535）
    std::cout << "imDepth.dims " << imDepth.dims << std::endl;// 维度
    std::cout << "imDepth.channels() " << imDepth.channels() << std::endl;// 通道

    cout << "mDepthMapFactor " << mDepthMapFactor << endl;// 深度系数
    cout << "mDepthMapFactor-1.0f " << mDepthMapFactor-1.0f << endl;
    cout << "(fabs(mDepthMapFactor-1.0f) " << fabs(mDepthMapFactor-1.0f) << endl;
    cout << "(fabs(mDepthMapFactor-1.0f)>1e-5) " << (fabs(mDepthMapFactor-1.0f)>1e-5) << endl;

    // 许多函数例如Sobel或convertTo都需要目标深度而不是通道数因此您需要, CV_32F 32位浮点数
    if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
        imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);

    cv::imwrite("../CV_32F_imDepth.png", imDepth);

    std::cout << "imDepth.type() " << imDepth.type() << std::endl;// 5 CV_32F定义矩阵中每个元素的深度, 而CV_32FC1定义每个元素的深度和通道数
    std::cout << "imDepth.dims " << imDepth.dims << std::endl;//
    std::cout << "imDepth.channels() " << imDepth.channels() << std::endl;//

    std::cout <<  CV_32F << std::endl;// 5
    std::cout <<  CV_32FC1 << std::endl;// 5

    return 0;
}


CMakeLists.txt

cmake_minimum_required(VERSION 3.22)
project(untitled2)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV)

add_executable(untitled2 main.cpp)
target_link_libraries(untitled2 ${OpenCV_LIBS})

Frame::Frame()

生成帧会传入彩色图mImGray、深度图imDepth、时间戳timestamp、左目提提取特征的指针mpORBextractorLeft、词典指针mpORBVocabulary、相机内参mK、畸变系数mDistCoef、基线×焦距mbf、深度因子mThDepth

帧有一个全局的唯一IDmnId = nNextId++
然后传入彩色图提取特征ExtractORB(0, imGray)

这里使用mpORBextractorLeft提取图像特征，然后将特征存储在std::vector mvKeys,、描述子存储在cv::Mat mDescriptors

特征点点和描述子生成代码参考下面文章

详细讲解ORB_SLAM2源码每个函数功能|umax
详细讲解ORB_SLAM2源码每个函数功能|图像金字塔
【ORB_SLAM2源码解读】金字塔图像分块并提取Fast特征点（６）
【ORB_SLAM2源码解读】图像初始节点划分并将特征点存储到对应的节点数据结构中（7）
【 ORB_SLAM2源码解读】特征点均匀化之四叉树节点分裂（８）
【ORB_SLAM2源码解读】根据提取的特征点数目采用四叉树实现特征点均匀化操作（９）

获取特征点的数目N = mvKeys.size();用于判断数量是否满足生成帧的要求

然后对提取到的特征点去畸变，而不是对全图像素去畸变，降低计算量， 单目图像不会去畸变。

在Tracking的构造函数里会从参数文件里面读取相机的内参、和畸变系数存储到Tracking的成员变量mDistCoef
然后在帧的构造函数创建帧的时候传递给Frame的成员变量mDistCoef，这样Frame的成员函数UndistortKeyPoints就可以访问这个变量的值，因为畸变系数如果没有值第一个就会是"0"，所以没有畸变系数那就不用对特征点进行去畸变了，直接让mvKeysUn = mvKeys。
第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0
变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)

执行去畸变操作用的是Opencv的函数接口，关于Opencv图像去畸变我写过一系列文章，想了解可以点击下面链接

「OpenCV」OpenCV3 模拟图像、数字图像、表示图像、opencv（零）
「OpenCV」OpenCV3 图像创建、像素遍历、像素显示、Vec4b、Vec3b、ptr、at、uchar（一）
「OpenCV」OpenCV3 图像旋转、平移、仿射变换、透视变换（二）
「OpenCV」OpenCV3 OpenCV3 图像多通道混合addWeighted、resize、copyTo、cvtColor、rectangle（三）
「OpenCV」OpenCV3 图像GaussianBlur、medianBlur、bilateralFilter（四）
「OpenCV」OpenCV3 图像line、putText、ellipse、rectangle（五）
「OpenCV」OpenCV3 图像批量重命名（六）
「OpenCV」OpenCV3 图像去畸变（九）

参文件中参数的理解

float mbf;// mbf: Stereo baseline multiplied by fx.
float mb;// mb: Stereo baseline in meters. 双目相机基线长度,单位为米

mb = mbf / fx;// 计算假想的基线长度 baseline= mbf/fx 后面要对从RGBD相机输入的特征点,结合相机基线长度,焦距,以及点的深度等信息来计算其在假想的"右侧图像"上的匹配点

mbf(bf),
float mbf;//Stereo baseline multiplied by fx.
mb = mbf / fx;

float mb;//Stereo baseline in meters. 双目相机基线长度,单位为米
对于立体矫正后的两张图，在列方向(x)存在最大视差maxd和最小视差mind，也即是左图中任何一点p，在右图上的匹配点的范围为应该是[p - maxd, p - mind]，而不需要遍历每一行所有的像素
maxd = baseline * length_focal / minZ
mind = baseline * length_focal / maxZ

const float minZ = mb;
const float minD = 0;
const float maxD = mbf / minZ;

根据视差值计算深度信息
保存最相似点的列坐标(x)信息
保存归一化sad最小相似度
最优视差值/深度选择.　mvDepth[iL] = mbf / disparity;

如果获取到的深度点合法(d>0)，那么就保存这个特征点的深度，并且计算出等效的在假想的右图中该特征点所匹配的特征点的横坐标，这个横坐标的计算是 x-mbf/d 其中的x使用的是矫正后的特征点的图像坐标

根据这个公式就可以构建点云

单幅点云，认为相机没有旋转和平移。所以，把设成单位矩阵，把设成了零。
是scaling factor，即深度图里给的数据与实际距离的比例。由于深度图给的都是short (mm单位)，通常为1000。
深度图是单通道的图像，每个像素由16个bit组成（也就是C++里的unsigned short），深度图的像素值代表该点离传感器的距离。
通常1000的值代表1米，所以我们把camera_factor设置成1000. 这样，深度图里每个像素点的读数除以1000，就是它离你的真实距离了。

这里要注意一点就是　深度图的的含义是什么？
深度图只是给我们提供一个尺度，借助这个并且这个尺度的单位是毫米，而三维坐标的单位是米，所以我们要把这个深度值除以１０００使得毫米变成米。

实际上我们完全抽象出两个坐标系
像素坐标系　u v d/1000
相机坐标系 x y z
其他的那些参数都只是不同的投影矩阵、映射矩阵、矩阵、参数。

Frame::ComputeStereoFromRGBD

大体思路就是根据基线和焦距还有Ｚ计算视差ｄ，进而求出像素点在右侧图像中的x坐标是多少。

// RGBD图像可以直接从深度图像上获得特征点的深度，为了处理上的一致使用这个深度计算了彩色图像中的特征点在虚拟的右目中的坐标
// 其他部分中不再区分双目特征点和RGBD特征点，它们以双目特征点的形式被处理，仅通过mvuRight[idx]判断某特征点是否有深度。
// 虚拟右影像上的 u 坐标值：mvuRight[i] = kpU.pt.x-mbf/d;
    void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)
{
    // 初始化存储右目匹配特征点横坐标和特征点深度值的vector
    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 &v = kp.pt.y;
        const float &u = kp.pt.x;

        //　从深度图像中获取这个特征点对应的深度点
        const float d = imDepth.at<float>(v,u);

        if(d>0)
        {
            mvDepth[i] = d;
            // 根据深度计算出等效的、在虚拟右图中的该特征点的横坐标、计算公式是： x-mbf/d
            mvuRight[i] = kpU.pt.x-mbf/d;// ur = ul-bf/d 其中f是u方向上的焦距fx，d是左影像上的深度，b是左右影像基线的长度。
        }
    }
}

针孔相机成像原理双目图像生成视差图和点云图

SLAM (Simultaneous Localization and Mapping) 同时定位与建图，系统的三部曲 “图像前端，优化后端，闭环检测”
2D SLAM，机器人位置就是x, y加一个转角
3D SLAM，机器人位置就是x,y,z加一个四元数姿态（或者rpy姿态）

从图像到点云

根据这个图我们就知道了相机内参和外参数的含义
透视投影对应投影矩阵
畸变矫正和数字化图像对应相机的内参
刚体变换对应相机外参数
下面是双目的相机参数截图

双目相机模型 双目采集图片 视差图(disparity map) UV-视差(UV-disparity)

使用双目相机，利用左右目的视差计算像素的距离（多目原理相同

https://www.mathworks.com/help/vision/ref/disparity.html

根据相机成像原理可知透视投影是多对一的关系，无法根据成像平面上的点确定三维空间中对应的点。

为了消除这种多对一的关系，可以利用双目摄像机：

将左相机放在世界坐标系原点，两个相机参数都一致，结构图如下：

UV-disparity
在这部分中，所有的f都等于fx/dx的值。世界坐标系位于两个相机的中间，且相机pitch角度为theta。
根据成像原理，世界坐标系中的一点在左右相机成像平面上的投影点为右侧的公式。

同时，以成像平面上的principal point为原点建立一个新的坐标系UV，其实不用纠结这个坐标系，可以理解为：后面的所有公式都是基于u-u0和v-v0，相当于做了一个代换而已，从U-V视差还原到原图时，再做一次逆变换就行了。

将图像中需要检测的一些目标，抽象成一些平面来描述，见下图：

每个平面在三维坐标系中的表达式，以及在UV-disparity mapping中的表达式如下：

所以，如果我们再UV-disparity mapping中能找到一条直线，那么就可以逆向找到该条直线所对应的目标，在U-disparity mapping中能找到目标在图像中所对应的行，V-disparity mapping中能找到目标在图像中对应的列。
那么如何构建UV-disparity mapping呢？
U-disparity mapping的行数 = V-disparity mapping 的列数 = 视差最大值+1；
U-disparity mapping的列对应于原始视差图的列；
V-disparity mapping的行对应于原始视差图的行；
V-disparity mapping中(v,d) = 原始视差图中第v行，视差为d的个数。
例如V-disparity mapping的第i行，分别统计原始视差图的第一行中视差为0,1,2,3,4,5 的个数：0，2，0，1，1，1，以此类推。
U-disparity mapping中(d,u) = 原始视差图中第u列，视差为d的个数。

数据集来自KITTI。目前只是做了：左图+右图->视差图->UV-disparity mapping。后面，需要在UV-disparity mapping中进行拟合得到直线，然后逆向获取一些障碍目标的信息。

https://blog.csdn.net/chentravelling/article/details/53671279

Depth Map from Stereo Images

   import numpy as np
   import cv2
   from matplotlib import pyplot as plt
   imgL = cv2.imread('tsukuba_l.png',0)
   imgR = cv2.imread('tsukuba_r.png',0) 
   stereo = cv2.StereoBM_create(numDisparities=16, blockSize=15)
   disparity = stereo.compute(imgL,imgR)
   plt.imshow(disparity,'gray')
   plt.show()

depth = f*baseline/disparity

针孔相机成像原理双目图像生成视差图和点云图源码解读

cmake_minimum_required(VERSION 2.8)

set(CMAKE_BUILD_TYPE "Release")
# 添加c++ 11标准支持
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")

# Eigen
include_directories("/usr/include/eigen3")

# 寻找OpenCV库
find_package(OpenCV REQUIRED)
# 添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

find_package(Pangolin REQUIRED)

add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})


#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "/home/q/projects/slambook2/ch5/stereo/left.png";
string right_file = "/home/q/projects/slambook2/ch5/stereo/right.png";

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    // semi-global matching（SGM）是一种用于计算双目视觉中视差（disparity）的半全局匹配算法，
    // 在OpenCV中的实现为semi-global block matching（SGBM）；
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
        0,
        96,
        9,
        8 * 9 * 9,
        32 * 9 * 9,
        1,
        63,
        10,
        100,
        32);    // 神奇的参数
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

Frame::AssignFeaturesToGrid()

前面已经将特征点进行了去畸变，这部分是对全图像进行去畸变，得到去畸变的图像区域，将特征点存储到对应图像分块中。

void Tracking::Track()

终于到了这个大boos了 先来分析这句代码

if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }


// Tracking states 这是枚举类型，数字和字符串是等价
enum eTrackingState{
    SYSTEM_NOT_READY=-1,
    NO_IMAGES_YET=0,
    NOT_INITIALIZED=1,
    OK=2,
    LOST=3
};

eTrackingState mState;// 都是枚举类型的变量
eTrackingState mLastProcessedState;

void Tracking::Reset()
mState = NO_IMAGES_YET;

if(mState==NO_IMAGES_YET){ mState = NOT_INITIALIZED;}

void Tracking::CreateInitialMapMonocular()
mState=OK;

void Tracking::StereoInitialization()
mState=OK

这是通过这个变量的值来判断track的不同状态的，因为不同的状态执行不同函数，这是显示界面的截图，可以里看到这个状态。

因为mState==NO_IMAGES_YET所以设置mState = NOT_INITIALIZED并且让mLastProcessedState=mState
说人话就是，还没有图片数据传进来，所以要先初始化，并且把这个状态做一个记录，供其它函数访问，进而执行不同的操作。

接下来上锁：unique_lock lock(mpMap->mMutexMapUpdate);
说人话就是将mpMap地图相关的变量其它函数暂时不能使用，因为track过程中会对这些变量进行更改。

然后因为mState==NOT_INITIALIZED所以调用这个函数StereoInitialization()进行双目和RGBD初始化。

Tracking.h中定义了一些变量用来传递跟踪的状态，可以理解为函数的返回值，只不过这里没有返回值，而是通过变量来传递程序的状态，这样整个程序都可以获取当前状态量的值，进而控制程序的执行流程。

下面来看看这几个状态量是怎么变换并且都代表什么流程

SYSTEM_NOT_READY是ORB_SLAM2刚刚启动运行后的状态，被FrameDrawer.cc使用。
NO_IMAGES_YET是ORB_SLAM2第一次运行tracking构造函数初始化或者tracking复位后的状态，被Tracking.cc和FrameDrawer.cc使用。

ORB_SLAM2根据NOT_INITIALIZED的值判断是否进行初始化，针对是否加载地图以及传感器类型选择不同的初始化方式，
初始化的目的就是通过初始两帧计算初始位姿，单目初始化是先后顺序的两帧通过对极几何和三角化计算位姿，
双目初始化是同一时刻的两帧通过视差计算位姿，而RGB-D相机自带深度信息。
当然单目初始化会涉及尺度的问题可以借助其他传感器辅助初始化。
当ORB_SLAM2的mState != OK的时候会令mState = NOT_INITIALIZED重新初始化。
当ORB_SLAM2的mState = LOST的时候会执行重定位bOK = Relocalization()操作。

红色框中就是程序的走向，这里有一个变量需要注意mLastProcessedState

接下来程序执行过程中，程序都要根据mState的值判断程序的不同走向。

void Tracking::Track()函数通过三个选择分支进入不同的程序
mState==NO_IMAGES_YET说明没有初始化，设置mState = NOT_INITIALIZED
进入第二个选择分支调用StereoInitialization() MonocularInitialization()进行初始化。
初始化完成之后进入if(!mbOnlyTracking)跟踪+建立地图模式或者只是跟踪模式，通过读取相关变量的值进入不同的执行代码，

Tracking线程中首先加载图片帧数据集，接着从图片帧中提取ORB特征点，后边就是初始化部分，初始化完成后就是跟踪部分。

初始化

1. 首次进入初始化
   对于当前帧中特征点个数>100的，用来构造初始化帧和上一个帧；
   并记录当前帧中特征点的坐标值，用于下一帧进来的时候进行特征点匹配；
   使用当前帧构造初始化器；

2. 第二次进入初始化
   如果当前传入帧的特征点个数<=100的，删除初始化器，直接返回；
   对当前传入的帧和初始化帧之间进行特征点匹配，调用matcher.SearchForInitialization函数；

如果匹配的特征点个数<100,则释放初始化器并直接返回；
使用初始化器进行初始化调用Tracking.cc:MonocularInitialization–>mpInitializer->Initialize函数
2d-2d对极几何求解基础矩阵和单应矩阵；
从基础矩阵或者单应矩阵中分解出旋转矩阵和平移变量，并进行三角化计算出了特征点的3D世界坐标；
对于Initialize函数初始化完成后不能三角化的匹配特征点都进行删除；
为当前帧设置位姿也就是Initialize中求出的旋转矩阵和平移变量组成的变换矩阵；

初始化Map
CreateInitialMapMonocular中为关键帧初始化创建了地图点，并将关键帧插入到地图当中，并更新地图中关键帧和地图点的观测关系。
之后对地图做了全局BA优化GlobalBundleAdjustemnt–>BundleAdjustment
添加顶点：
BundleAdjustment当中进行g2o优化，遍历关键帧并将关键帧位姿（变换矩阵）作为图的顶点；
遍历地图点并将地图点作为图的顶点，估计量为地图点的3d坐标；
添加优化边：通过遍历每一个地图点对应的关键帧，将关键帧的位姿和地图点的3d坐标构成的点作为边的两个顶点来构造优化边。
获取优化后关键帧的位姿和地图点对应的3d坐标，更新关键帧和地图点中的相关值。

初始化完成
跟踪参考关键帧TrackReferenceKeyFrame：

1. 用当前帧追踪参考帧通过特征匹配：Tracking::TrackReferenceKeyFrame()-->matcher.SearchByBoW
   将当前传入的帧中特征点的描述子和参考关键帧中地图点的描述子进行比较，找到匹配关系；
2. 用和当前帧中特征点的描述子匹配的参考关键帧的地图点更新当前帧的地图点列表；
3. 进行当前帧位姿的优化：Optimizer::PoseOptimization,当前帧的初始位姿为参考关键帧的位姿，然后根据1当中的匹配关系进行优化，
   那么对应关系就变成了3D-2D，使用PnP(这里是P3P)来优化当前帧的位姿;
4. 在2中优化完当前帧位姿之后，对于当前帧对应的地图点属于outlier的，进行剔除不在地图当中显示。

使用运动模型进行跟踪TrackWithMotionModel：

1. 更新上一帧信息UpdateLastFrame：
2. 设置当前帧的位姿；
3. matcher.SearchByProjection：
   1）获取当前帧和上一帧的旋转和平移矩阵；
   2）遍历上一帧的地图点，对于非outlier的地图点将其三维坐标转换为当前帧中的像素坐标（三维坐标–>相机坐标–>相机归一化坐标–>像素坐标），并判断所得的像素坐标是否在当前帧的有效范围内；
   3）根据像素坐标值(u,v)在当前帧中的有效半径内进行特征点匹配；

4. 遍历3）中的匹配列表，用上一帧地图点的描述子和匹配的特征点的描述子计算距离，获取描述子距离最小的特征点为和该地图点匹配的特征点，并计算匹配的特征点数量；

4. 如果3中匹配的特征点数量<20，则扩大匹配的有效半径为原来的2倍，再次调用matcher.SearchByProjection进行投影匹配，如果匹配的特征点数量还是小于20则跟踪失败做返回处理；
5. 优化当前帧的位姿Optimizer::PoseOptimization(&mCurrentFrame)；
6. 优化位姿后剔除outlier的mvpMapPoints抛弃杂点，并记录匹配成功的特征点个数。在仅进行跟踪的情况下匹配点数大于20则跟踪成功；在跟踪和定位同时进行的情况下匹配点数大于等于10则成功。

重定位Relocalization（有三种情况需要进行重定位：1.初始化未成功；2.跟踪丢失；3.仅跟踪的情况下，跟踪匹配的特征点数小于10；）：

1. 计算当前帧中特征点的BOW；
2. 在关键帧数据库中检测当前帧的候选关键帧mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
3. 遍历2中检测的候选关键帧列表，调用matcher.SearchByBoW将候选关键帧和当前帧进行BoW匹配。
   匹配特征点数小于15则继续匹配下一个候选关键帧；
   匹配点数>=15则构造PnP(调用PnPsolver)求解器，并设置Ransac(调用pSolver->SetRansacParameters)参数；
   注意：这个PnP求解器中的3D point为候选关键帧中对应的MapPoint，2D点为mCurrentFrame中的关键点
   因为是重定位，所以就是求重定位的候选帧对应的MapPoint到当前帧的关键点之间的投影关系，通过投影关系确定当前帧的位姿，也就进行了重定位。
4. 遍历和当前帧匹配点数>=15的候选关键帧，通过3中构造的PnP求解器进行迭代优化求解；
5. 对于4中优化求解得到的当前帧的位姿进行优化，（调用Optimizer::PoseOptimization(&mCurrentFrame)进行优化），优化完成后对当前帧中的outlier的地图点设置为NULL;
6. 如果第5步中位姿优化求解获得的内点数少于50个，则在一个更大半径的区域中去进行匹配（调用函数为matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);）；
   6.1.对于第6步中匹配的特征点数加上第5步优化后的内点数总个数大于50的情况下，再次对当前帧的位姿进行优化（Optimizer::PoseOptimization(&mCurrentFrame)）；
   6.2.对于第6.1步位姿优化后的inlier点数>30并且<50的情况，再次在一个更小的窗口中进行投影；
   如果投影后的匹配点数加上第7步位姿优化的inlier点数之和>=50，则再次进行位姿优化（Optimizer::PoseOptimization(&mCurrentFrame)）；
7. 如果第5步或者6.2步中所获得的inlier点数>=50，则停止Ransac操作并更新当前帧id为上一次重定位帧的id。
   至此，当前帧就在历史关键帧中找到了匹配的关键帧，该关键帧的位姿也就是当前帧的位姿，完成了定位。如果遍历完了所有的候选关键帧后，还是没有找到足够的inlier，那么重定位失败。

更新和当前帧进行匹配的参考关键帧为当前帧的参考关键帧；
跟踪局部地图（TrackLocalMap）：

1. 更新局部地图（UpdateLocalMap）:
   1)更新局部关键帧：局部关键帧是指能看到当前帧对应的地图点的所有关键帧组成的关键帧列表；
   在局部关键帧列表中的关键帧数不够80个的时候，这时候需要通过下面3个策略来给局部关键帧列表中添加关键帧：
   a)遍历局部关键帧列表中的关键帧，获取和关键帧共视程度最高的前10个（少于10个的有几个就加入几个）关键帧加入局部关键帧列表当b)遍历局部关键帧列表中的关键帧，将关键帧的孩子关键帧加入到局部关键帧列表当中； （什么是孩子关键帧？孩子关键帧就是和当前关键帧有共视关系的关键帧）
   c)遍历局部关键帧列表中的关键帧，将关键帧的父亲关键帧加入到局部关键帧列表当中； （什么是父亲关键帧呢？父亲关键帧是和当前关键帧共视MapPoint个数最多的关键帧）
   2）更新局部地图点：
   a)遍历局部关键帧，获取每一帧中的地图点列表，将有用的地图点存入局部地图点列表当中，并更新地图点的跟踪参考帧为当前帧的id;
2. 在局部地图中查找与当前帧匹配的MapPoints，并建立关联（SearchLocalPoints）：
   1）遍历局部地图点，对当前帧对应的地图点之外的地图点判断其是否在可视范围内（isInFrustum），对于在可视范围内的地图点的观测帧数目+1操作，并统计可视范围内的地图点总体个数；
   2）对当前帧视野范围内的局部地图点通过投影和当前帧中的特征点进行匹配。
3. 对当前帧的位姿进行优化：Optimizer::PoseOptimization。在更新了局部地图点并将当前帧和局部地图点建立关联后，再次调用优化方法对当前帧的位姿进行优化。
4. 统计当前帧的地图点被局部地图中其他关键帧观测情况（匹配情况），如果匹配成功的数量>=30则为跟踪布局地图成功。
   更新绘制帧的窗口：mpFrameDrawer->Update(this)
   在绘制地图的窗口中设置当前相机的位置：mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw)；
   清理当前帧的地图点中没有被其他关键帧观测到的地图点，将该地图点设置为NULL;

创建新的关键帧CreateNewKeyFrame：

1. 使用当前帧创建新的关键帧，并设置新创建的关键帧为当前帧的参考关键帧；
2. 对于非单目相机的情况处理：
   1）更新当前帧的旋转矩阵、平移和相机中心等变量；
   2）创建地图点，并将地图点插入到地图当中；
3. 往局部地图中插入新创建的关键帧：mpLocalMapper->InsertKeyFrame(pKF)；至此LocalMapping线程有了关键帧插入就运行起来了，关键帧巧妙的将各个线程之间串联了起来。
4. 更新记录的上一个关键帧和关键帧id：
   mnLastKeyFrameId = mCurrentFrame.mnId;
   mpLastKeyFrame = pKF;
   剔除当前帧中为outlier的地图点；
   如果初始化完成后很快状态跟丢了，则将系统进行重置；
   用当前帧来更新上一个帧；
   存储帧的位姿信息，稍后用来重现完整的相机运动轨迹；
   本篇笔记为笔者梳理的Tracking线程的主要流程，方便在脑海中进行推演，我们可以描述出Tracking线程中跟踪是怎么做的。
   上边已经用红色加粗标注出了Tracking线程中进行全局BA优化的地方，也就是在初始化中。
   也用红色加粗字体标注除了Tracking线程中进行位姿优化的地方，这样方便进行梳理。

系统的第一步是初始化，同时计算两个模型：用于平面场景的单应性矩阵H和用于非平面场景的基础矩阵F，然后通过一个评分规则来选择合适的模型，恢复相机的旋转矩阵R和平移向量t。
为了计算R和t，ORB_SLAM为了针对平面和非平面场景选择最合适的模型，同时开启了两个线程，分别计算单应性矩阵Hcr和基础矩阵Fcr。

线程threadH调用Initializer::FindHomography函数，计算单应性矩阵H，采用归一化的直接线性变换（normalized DLT）。
线程threadF调用Initializer::FindFundamental函数计算基础矩阵F，使用归一化8点法。
为了评估哪个模型更合适，文中使用SH和SF来计算各自的分值。

文中认为，当场景是一个平面、或近似为一个平面、或者视差较小的时候，可以使用单应性矩阵H，而使用基础矩阵F恢复运动，需要场景是一个非平面、视差大的场景。

选择好模型后，就可以恢复运动。
（1）从单应性变换矩阵H中恢复
  在求得单应性变化H后，本文使用FAUGERAS的论文[1]的方法，提取8种运动假设。这个方法通过可视化约束来测试选择合理的解。但是如果在低视差的情况下，点云会跑到相机的前面或后面，测试就会出现错误从而选择一个错误的解。文中使用的是直接三角化8种方案，检查两个相机前面具有较少的重投影误差情况下，在视图低视差情况下是否大部分云点都可以看到。如果没有一个解很合适，就不执行初始化，重新从第一步开始。这种方法在低视差和两个交叉的视图情况下，初始化程序更具鲁棒性。程序中调用Initializer::ReconstructH函数恢复运动。

(2)从基础矩阵F中恢复
  在得到基础矩阵F，并且一直摄像机内参K的情况下，可以计算得到本质矩阵E，然后使用[2]中的方法，恢复出4个运动假设的解。这一部分的理论推导在之前博客做过介绍。其中基础矩阵F得到本质矩阵E的公式如下所示：

同样的，这4个解中只有一个是合理的，可以使用可视化约束来选择，本文使用与单应性矩阵做sfm一样的方法，即将4种解都进行三角化，然后从中选择出最合适的解。这里使用的是Initializer::ReconstructF函数。

最后使用一个全局集束调整（BA)，优化初始化结果。

Tracking线程运行在主线程中，主要思路是在当前帧和（局部）地图之间寻找尽可能多的对应关系，来优化当前帧的位姿。每次新采集到一帧图像，就是用下列接口将图像传入SLAM系统就行处理。

匹配的过程用的都是ORB特征描述子。先在8层图像金字塔中，提取FAST特征点。提取特征点的个数根据图像分辨率不同而不同，高分辨率的图像提取更多的角点。然后对检测到的特征点用ORB来描述，用于之后的匹配和识别。跟踪这部分主要用了几种模型：运动模型（Tracking with motion model）、关键帧（Tracking with reference keyframe）和重定位（Relocalization）。

在成功与前面帧跟踪上之后，为了提高速率，使用与之前速率相同的运动模式来预测相机姿态，并搜索上一帧观测到的地图点。这个模型是假设物体处于匀速运动，例如匀速运动的汽车、机器人、行人等，就可以用上一帧的位姿和速度来估计当前帧的位姿。使用的函数为TrackWithMotionModel()。这里匹配是通过投影来与上一帧看到的地图点匹配，使用的是matcher.SearchByProjection()。

当使用运动模式匹配到的特征点数较少时，就会选用关键帧模式。即尝试和最近一个关键帧去做匹配。为了快速匹配，本文利用了bag of words（BoW）来加速。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配，使用函数matcher.SearchByBoW()；最后，利用匹配的特征优化位姿。

当前帧与最近邻关键帧的匹配也失败了，那么意味着需要重新定位才能继续跟踪。重定位的入口如下：bOK = Relocalization();

此时，只有去和所有关键帧匹配，看能否找到合适的位置。首先，计算当前帧的BOW向量，在关键帧词典数据库中选取若干关键帧作为候选。

其次，寻找有足够多的特征点匹配的关键帧；最后，利用RANSAC迭代，然后使用PnP算法求解位姿。这一部分也在Tracking::Relocalization() 里。

通过之前的计算，已经得到一个对位姿的初始估计，我们就能透过投影，从已经生成的地图点中找到更多的对应关系，来精确结果。函数入口为：bOK = TrackLocalMap();
为了降低复杂度，这里只是在局部图中做投影。局部地图中与当前帧有相同点的关键帧序列成为K1，在covisibility graph中与K1相邻的称为K2。局部地图有一个参考关键帧Kref∈K1，它与当前帧具有最多共同看到的地图云点。针对K1, K2可见的每个地图云点，通过如下步骤，在当前帧中进行搜索:
（1）将地图点投影到当前帧上，如果超出图像范围，就将其舍弃；
（2）计算当前视线方向向量v与地图点云平均视线方向向量n的夹角，舍弃n·v < cos(60°)的点云；
（3）计算地图点到相机中心的距离d，认为[dmin, dmax]是尺度不变的区域，若d不在这个区域，就将其舍弃；
（4）计算图像的尺度因子，为d/dmin；
（5）将地图点的特征描述子D与还未匹配上的ORB特征进行比较，根据前面的尺度因子，找到最佳匹配。
  这样，相机位姿就能通过匹配所有地图点，最终被优化。

最后一步是确定是否将当前帧定为关键帧，由于在Local Mapping中，会剔除冗余关键帧，所以我们要尽快插入新的关键帧，这样才能更鲁棒。这个部分代码为：

Check if we need to insert a new keyframe
if(NeedNewKeyFrame())
    CreateNewKeyFrame();

确定关键帧的标准如下：
（1）在上一个全局重定位后，又过了20帧；
（2）局部建图闲置，或在上一个关键帧插入后，又过了20帧；
（3)当前帧跟踪到大于50个点；
（4）当前帧跟踪到的比参考关键帧少90%。

一个主线程就是Tracking线程，三个子线程LocalMapping、LoopClosing、Viewer

所以后面涉及到这几个线程可以共同访问的变量的时候要加锁，就是只能有一个线程操作数据。
锁定锁所在的作用域内的变量，防止其它代码对这一部分变量同时进行操作

ORB_SLAM2程序结束时首先是通过ctrl+c按键调用void System::Shutdown()结束三个子线程LocalMapping、LoopClosing、Viewer，然后保存轨迹，最后结束Tracking主线程。

SLAM.Shutdown(); SLAM对象调用Shutdown()函数，结束其它线程
mpLocalMapper->RequestFinish(); 调用局部建图线程的终止函数
mpLoopCloser->RequestFinish(); 调用回环检测线程的终止函数
mpViewer->RequestFinish(); 调用可视化窗口发送终止函数
while(!mpViewer->isFinished()) 判断可视化窗口是否关闭，如果没有关闭睡眠等待
while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA()) Wait until all thread have effectively stopped

如果你运行的是ROS程序，在结束之前会一直循环处理接收的图像，通过调用函数ros::spin()实现，这个ros::spin() 调用后不会再返回，主程序到这儿就不往下执行了，直到外部触发程序终止信号（ctrl+c按键），而 ros::spinOnce()在调用后还可以继续执行之后的程序。

结束线程RequestFinish->isFinished

System::Shutdown()函数请求终止当前线程，通过设置变量mbFinishRequested = true;
然后CheckFinish函数检测到mbFinishRequested = true;继续传递这个变量给void LocalMapping::Run()、LoopClosing::Run()、 Viewer::Run()函数

启动线程LocalMapping::Run->CheckFinish->SetFinish

检查是否已经有外部线程请求终止mpLocalMapper、mpLoopCloser、mptViewer三个平行子线程。
首先是设置终止线程SetFinish，然后主要是System::Shutdown()函数调用RequestFinish，请求终止当前线程、然后调用isFinished判断是否成功终止当前线程、CheckFinish检测是否有外部程序请求终止线程。

程序结束后mpTracker主线程要等待mpLocalMapper、mpLoopCloser、mptViewer三个平行线程结束，所以创建的了下面三个SetFinish()函数接口，供外部程序调用。

重置Tracking::Reset()->RequestReset()->ResetIfRequested()

请求当前线程复位、由Tracking::Reset()调用
除了结束线程还有复位线程的需求，结束就是释放线程创建的变量和所占用的系统资源，复位就是重置变量。
这里复位线程通过调用函数RequestReset再通过全局变量mbResetRequested实现把mbResetRequested这个变量设置为true， 调用mlpLoopKeyFrameQueue.clear()函数、并设置mLastLoopKFid=0实现复位操作。

停止线程RequestStop、isStopped、Stop、Release

被System、Tracking、LoopClosing线程调用停止LocalMapping、Viewer线程工作;

下面是停止线程的用法

unique_lock<mutex> lock(mMutexMode);// 锁定当前程序运行相关的变量，禁止其他线程读写操作。
// 判断变量的值，从界面获取，代码是：pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true);
if(mbActivateLocalizationMode)
{
    // 启动定位模式，发布停止建立地图模式的请求，建立地图的run函数运行过程中会检测这个变量的值， 
    mpLocalMapper->RequestStop();

    // Wait until Local Mapping has effectively stopped　
    // 睡眠等待真正停止
    while(!mpLocalMapper->isStopped())
    {
        usleep(1000);
    }
　　// 告诉tracking线程启动定位模式
    mpTracker->InformOnlyTracking(true);
    // 重置模式切换的变量的值，因为这个信号已经传递给了tracking线程的了。
    mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
{
    mpTracker->InformOnlyTracking(false);
    mpLocalMapper->Release();// 清空关键帧列表std::list mlNewKeyFrames;　mlNewKeyFrames.clear();
    mbDeactivateLocalizationMode = false;
}

释放Release()

LocalMapping::Release()主要也是调用上面的一系列流程，清空新创建的关键帧列表
LoopClosing.cc调用的是mpLocalMapper->Release();
Viewer是通过mbStopped = false触发其他函数。

LoopClosing::CorrectLoop、LoopClosing::RunGlobalBundleAdjustment

回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
优化所有的关键帧位姿和地图中地图点
上面两部分工作都要求停止mpLocalMapper线程

Tracking::Reset、Viewer

在运行界面会调用System::Reset()设置mbReset = true;

pangolin::Var<bool> menuReset("menu.Reset",false,false);

在跟踪丢失的时候
mState==LOST
如果地图中的关键帧信息过少
mpMap->KeyFramesInMap()<=5)
会调用System::Reset()设置mbReset = true;

System::Reset()设置mbReset = true;
cv::Mat System::TrackRGBD()调用mpTracker->Reset()执行mpLocalMapper->RequestReset()、mpLoopClosing->RequestReset()、mpKeyFrameDB->clear()、mpMap->clear()、KeyFrame::nNextId = 0、Frame::nNextId = 0、mState = NO_IMAGES_YET、 mpInitializer = static_cast<Initializer*>(NULL)、mlRelativeFramePoses.clear()、mlpReferences.clear()、mlFrameTimes.clear()、mlbLost.clear()、mpViewer->Release()

执行流程是：

• 变量在初始化中的值是true，mbStopped(true)
• Viewer::Run函数改变false
  
• Viewer::RequestStop函数通过mbStopped = false使得mbStopRequested = true, mbStopRequested的初始值是mbStopRequested(false)
  
• Viewer::Stop函数通过mbStopRequested = true
  
  使得mbStopped = true;、mbStopRequested = false;回到构造函数初始化值状态，Viewer线程得到停止，至此形成闭环。
  mbStopRequested
  其实是回环检测线程调用,来避免在进行全局优化的过程中局部建图线程添加新的关键帧

Tracking::Replace

void MapPoint::Replace(MapPoint *pMP){
   if (pMP->mnId == this->mnId)
       return;
   int nvisible, nfound;
   map<KeyFrame *, size_t> obs;
   {
       obs = mObservations;
       mObservations.clear();
       mbBad = true;
       nvisible = mnVisible;
       nfound = mnFound;
       mpReplaced = pMP;
   }

   for (map<KeyFrame *, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++){
       KeyFrame *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();
   mpMap->EraseMapPoint(this);
}

上面这张调试图像可以解释这句代码pMP->mnId == this->mnId

bool MapPoint::IsInKeyFrame(KeyFrame *pKF){
    return (mObservations.count(pKF));
}


void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP){
    mvpMapPoints[idx]=pMP;
}


void MapPoint::AddObservation(KeyFrame* pKF, size_t idx){
    if(mObservations.count(pKF)) 
        return;
    mObservations[pKF]=idx;

    if(pKF->mvuRight[idx]>=0)
        nObs+=2;
    else
        nObs++;
}


void KeyFrame::EraseMapPointMatch(const size_t &idx){
    mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}


void MapPoint::IncreaseFound(int n){
    mnFound+=n;
}


void MapPoint::IncreaseVisible(int n){
    mnVisible+=n;
}


void Map::EraseMapPoint(MapPoint *pMP){
    mspMapPoints.erase(pMP);
}

Tracking::CheckReplacedInLastFrame

void Tracking::CheckReplacedInLastFrame()
{
    for(int i =0; i<mLastFrame.N; i++)
    {
        MapPoint* pMP = mLastFrame.mvpMapPoints[i];

        if(pMP)
        {
            MapPoint* pRep = pMP->GetReplaced();
            if(pRep)
            {
                mLastFrame.mvpMapPoints[i] = pRep;
            }
        }
    }
}


Frame mLastFrame;

void Tracking::StereoInitialization()
mLastFrame = Frame(mCurrentFrame)

bool Tracking::TrackReferenceKeyFrame()
mCurrentFrame.SetPose(mLastFrame.mTcw);

bool Tracking::TrackWithMotionModel()
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

std::vector<MapPoint*> mvpMapPoints;

MapPoint *MapPoint::GetReplaced(){
    return mpReplaced;
}
MapPoint* mpReplaced;

mpFrameDrawer-＞Update(this)函数功能

Track()是跟踪的主要接口函数

mpFrameDrawer->Update(this);

将tracking对象指针传递给 FrameDrawer对象，进而将一系列的变量和函数(将跟踪线程的图像、特征点、地图点、跟踪状态传递至绘图线程)传递给FrameDrawer对象

类间数据共享

做法是

在A.h中
#inclue “B.h”
在A.h中A类的定义前面添加这样一句：
Class B; 

在B.h中
#inclue “A.h”
在B.h中B类的定义前面添加这样一句：
Class A; 

这样就可以在Tracking和FrameDrawer两个类之间共享变量和函数了。

执行流程

pTracker->mImGray.copyTo(mIm)将Tracking的图像赋值给cv::Mat mIm关于这句代码，

mvCurrentKeys=pTracker->mCurrentFrame.mvKeys赋值给vector mvCurrentKeys

继续执行是一个if判断结构，前面已经成功初始化所以进入else分支

如果上一帧跟踪状态是没有初始化

if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED)
{
    // 那么首先要获取初始帧的特征点和匹配信息
    mvIniKeys=pTracker->mInitialFrame.mvKeys;
    mvIniMatches=pTracker->mvIniMatches;
}

std::vector<int> mvIniMatches;
代表初始化阶段,当前帧中的特征点和参考帧中的特征点的匹配关系
int nmatches = matcher.SearchForInitialization(
            mInitialFrame,mCurrentFrame,    //初始化时的参考帧和当前帧
            mvbPrevMatched,                 //在初始化参考帧中提取得到的特征点
            mvIniMatches,                   //保存匹配关系
            100);                           //搜索窗口大小

记录地图点是否为外点，初始化均为外点false
mvbOutlier = vector(N,false)

N = mvCurrentKeys.size();
for(int i=0;i<N;i++)
        {
            MapPoint* pMP = pTracker->mCurrentFrame.mvpMapPoints[i];
            if(pMP)
            {
                if(!pTracker->mCurrentFrame.mvbOutlier[i])
执行的流程，先通过当前帧获得当前帧的特征点数目Ｎ
然遍历每一个特征点，因为之前生成地图点的时候也是这生成的
因为不是每一个特征点都会生成地图点，所以要先判断这个地图点是否存在
接着判断这个地图点是否是外点，外点就是不符合要求的无效的地图点。
当上面条件都满足的时候就可以显示了，但是为了区分地图点是当前两帧生成的还是地图里面之前保存的所以有下面两个vector去标记
//　如果地图点被多帧观测则为地图点
if(pMP->Observations()>0)
    mvbMap[i]=true;
else
//　否则表示这个地图点是在当前两帧中提取得到的
    mvbVO[i]=true;
    
mvbVO = vector<bool>(N,false);
mvbMap = vector<bool>(N,false);

下面的代码中红色框位置可以看到针对上面的状态做了区分显示

这是调整显示时间截到的图片，显示正在加载词典

这是第一个关键帧，还没有生成第二个关键帧。可以看到地图点的数目是没有变化的，变化的只是当前帧和前一帧之间匹配生成地图点的数目。

FrameDrawer.cc的主要功能是一帧一帧的显示图像，并且在图像上画出可以恢复出有效的3D点的地图点，并用绿色方框画出，如果是初始化的帧用蓝色的方框，如果是单目还会画出绿色的线连接两个相似度很高的特征点。图像的下方黑色条状区域会显示当前的跟踪状态，当前显示的关键帧ID，当前地图中地图点的数目，当前跟踪到的地图点数目。