😀 ORB-SLAM2 代码解读（二）：可视化线程

0. 可视化线程介绍

可视化线程用于显示 3D 地图绘制器和 2D 图像帧绘制器，还包括一些运行模式的开关，不涉及到具体的算法，只负责接受、传递和显示数据，不影响 SLAM 系统的运行，可以再初始化 SLAM 系统时选择开启或者关闭。但需要注意的是线程锁的问题，不严谨的数据共享可能会引起多线程的段错误。

1 创建可视化线程

可视化线程的创建和运行在初始化 SLAM 系统时进行

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17


// 创建帧绘制器和地图绘制器
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
// 创建并运行可视化线程
if(bUseViewer)
{
    // 新建viewer
    mpViewer = new Viewer(  this, 
                            mpFrameDrawer,	//帧绘制器
                            mpMapDrawer,		//地图绘制器
                            mpTracker,		//追踪器
                            strSettingsFile);	//配置文件的访问路径
    // 运行viewer线程
    mptViewer = new thread(&Viewer::Run, mpViewer);
    // 给运动追踪器设置其查看器
    mpTracker->SetViewer(mpViewer);
}

可视化线程构造函数 ==Viewer::Viewer()==
- 用于初始化可视化线程，读取相机的配置参数，包括：
  - 图像采集的帧率，计算出每帧的耗时 double mT;
  - 读取图像的长款，默认值为 640 × 480；
  - 可视化视角和相机的焦距。

2 运行可视化线程

可视化线程与系统其他线程并行运行在 ==Viewer::Run()== 函数中

步骤一： 创建 pangolin 窗口并设置大小

1
2
3
4
5
6
7


pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768);
// 启动深度测试，OpenGL只绘制最前面的一层，绘制时检查当前像素前面是否有别的像素，如果别的像素挡住了它，那它就不会绘制
glEnable(GL_DEPTH_TEST);
// 在OpenGL中使用颜色混合
glEnable(GL_BLEND);
// 选择混合选项
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

步骤二： 创建按钮和选择框

1
2
3
4
5
6
7
8


// 新建按钮和选择框，第一个参数为按钮的名字，第二个为默认状态，第三个为是否有选择框
pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
pangolin::Var<bool> menuFollowCamera("menu.Follow Camera",true,true);
pangolin::Var<bool> menuShowPoints("menu.Show Points",true,true);
pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames",true,true);
pangolin::Var<bool> menuShowGraph("menu.Show Graph",true,true);
pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
pangolin::Var<bool> menuReset("menu.Reset",false,false);

步骤三： 设置 pangolin 相机的投影模型和观测方向

1
2
3
4
5
6
7
8


// 定义相机投影模型：ProjectionMatrix(w, h, fu, fv, u0, v0, zNear, zFar)
// 定义观测方位向量：观测点位置：(mViewpointX mViewpointY mViewpointZ)
//                观测目标位置：(0, 0, 0)
//                观测的方位向量：(0.0,-1.0, 0.0)
pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
            pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)
            );

步骤四： 定义显示面板大小

1
2
3
4
5
6
7


// 定义显示面板大小，orbslam中有左右两个面板，左边显示一些按钮，右边显示图形
// 前两个参数（0.0, 1.0）表明宽度和面板纵向宽度和窗口大小相同
// 中间两个参数（pangolin::Attach::Pix(175), 1.0）表明右边所有部分用于显示图形
// 最后一个参数（-1024.0f/768.0f）为显示长宽比
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));

步骤五：更新绘制内容（详见下一节）

3. 更新可视化内容

步骤一： 地图绘制器获取最新的相机位姿，在函数 ==MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M)== 中实现；
- 检查地图绘制器的 cv::Mat mCameraPose 是否为空，若不为空，则将其转换为 cv::Mat 格式转换为 pangolin::OpenGlMatrix &M 格式，否则则设置为单位矩阵；
- 检查地图绘制器中的相机位姿 cv::Mat mCameraPose 是通过 ==MapDrawer::SetCurrentCameraPose(const cv::Mat &Tcw)== 函数从跟踪线程传递过来的，该函数在跟踪线程中有两个地方会更新：
  - 创建初始化地图时
    1
    
    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
  - 跟踪线程每次跟踪成功之后
    1 2
    
    if(bOK) mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
步骤二： 根据可视化界面的按钮和相机位姿情况选择调整视角；
步骤三： 判断模式是否有边，定位模式还是 SLAM 模式；

步骤四：绘制 3D 地图

步骤 1： 根据相机位姿在 pangolin 界面中绘制相机框，在函数 ==MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc)== 中实现；
- 该步骤需要的参数是相机位姿，在步骤一中已获取；
步骤 2： 在地图中绘制关键帧及其共视关系线，在函数 ==MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph)== 中实现；
- 该步骤需要获取地图的所有关键帧，通过函数 ==vector<KeyFrame*> Map::GetAllKeyFrames()== 来获取关键帧 mspKeyFrames；
  1 2
  
  // 取出所有的关键帧 const vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
- 关键帧 mspKeyFrames 通过函数 ==Map::AddKeyFrame(KeyFrame *pKF)== 来更新，该函数在三个地方被触发
  - ①：单目相机创建初始地图时；
  - ②：双目和 RGBD 相机初始化时；
  - ③：局部建图线程处理待处理关键帧列表时 ==LocalMapping::ProcessNewKeyFrame()==

步骤 3：绘制地图点，在函数 ==MapDrawer::DrawMapPoints()== 中实现，局部地图点用红色表示，出局部地图点外的地图点用黑色表示；

该步骤需要用到两种地图点：所有地图点和局部地图点

1
2
3
4


// 取出所有的地图点
const vector<MapPoint*> &vpMPs = mpMap->GetAllMapPoints();
// 取出mvpReferenceMapPoints，也即局部地图d点
const vector<MapPoint*> &vpRefMPs = mpMap->GetReferenceMapPoints();

所有地图点通过函数 ==Map::GetAllMapPoints()== 获取 mspMapPoints ， mspMapPoints 的更新情况与步骤 2 的关键帧一样；
局部地图点在地图绘制器中通过函数 ==Map::GetReferenceMapPoints()== 来获取，局部地图点在地图中通过 ==Map::SetReferenceMapPoints(const vector<MapPoint *> &vpMPs)== 来更新，该函数在三个地方被触发
- ①：单目相机创建初始地图时；
- ②：双目和 RGBD 相机初始化时；
- ③：==Tracking::UpdateLocalMap()== 函数中，也就是跟踪局部地图进行局部优化时；

步骤四：绘制带有检测结果的 2D 图像帧，通过函数 ==FrameDrawer::DrawFrame()== 来绘制

绘制时需要用到的参数信息需要在进入该函数时加互斥锁先获取，后面才可以放心使用，否则容易出现段错误。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32


// step 1：将成员变量赋值给局部变量（包括图像、状态、其它的提示）
//NOTICE 加互斥锁，避免与FrameDrawer::Update函数中图像拷贝发生冲突
{
    unique_lock<mutex> lock(mMutex);
    state=mState;
    if(mState==Tracking::SYSTEM_NOT_READY)
    mState=Tracking::NO_IMAGES_YET;

    // 这里使用copyTo进行深拷贝是因为后面会把单通道灰度图像转为3通道图像
    mIm.copyTo(im);

    // 没有初始化的时候
     if(mState==Tracking::NOT_INITIALIZED)
    {
        //获取当前帧\参考帧的特征点,并且得到他们的匹配关系
        vCurrentKeys = mvCurrentKeys; // 当前帧的特征点.
        vIniKeys = mvIniKeys;         // 参考帧的特征点.
        vMatches = mvIniMatches;      // 之间的匹配关系.
    }
    // 初始化完成跟踪正常时
    else if(mState==Tracking::OK)
    {
        vCurrentKeys = mvCurrentKeys; // 当前帧的特征点.
        vbVO = mvbVO;
        vbMap = mvbMap;
    }
    else if(mState==Tracking::LOST)
    {
        // 跟丢的时候就之获得当前帧的特征点就可以了
        vCurrentKeys = mvCurrentKeys;
    }
}

步骤五： 响应复位停止等请求。

【Q】问题

2019.05.28
wuyanminmax@gmail.com

文章目录

0. 可视化线程介绍

1 创建可视化线程

2 运行可视化线程

3. 更新可视化内容

【Q】问题