ORB-SLAM2代码阅读笔记（九）：进行窗口显示的Viewer线程

Table of Contents

1.View线程都做了些什么

2.View线程代码

1）View线程创建

2）Run函数代码

3）在地图中画出当前相机位姿

4）绘制地图中当前相机之前的关键帧

5）绘制地图中的MapPoint

6）窗口中显示标注有特征点的图像帧

3.小结

---

ORB-SLAM2的可视化主要是使用Pangolin这个可视化库来实现的，其中也有用到opencv的接口。所以，其实了解View线程做了哪些事情最好的办法就是照着ORB-SLAM2的运行结果来和代码中的Pangolin接口进行对应。

1.View线程都做了些什么

ORB-SLAM2代码阅读笔记（一）：从mono_kitti单目运行开始 中用KITTI的单目数据集运行ORB-SLAM2系统的运行效果，我们看到的地图显示界面就是VIew线程来负责显示的。如下图所示：

这个图中有两个窗口，上边的窗口一直在显示一张一张的图片，图片中绿色方形和圆圈标注的就是该图像中提取的ORB特征点。下边的窗口用于显示相机的位姿（也就是关键帧）和地图点（每个地图点和图像中的特征点有对应关系），其中绿色的为当前相机的展示，蓝色的是历史相机位姿展示。缩小一下窗口，可以看到相机走过的轨迹，如下图所示：

图像更新的地方在Tracking线程中，在Track()函数中有下面的语句用来更新图像显示：

mpFrameDrawer->Update(this);

2.View线程代码

1）View线程创建

View线程是在SLAM系统初始化的时候创建的，在Sytem.cc中System构造函数中进行创建：

if(bUseViewer)
{
   //6.初始化显示窗口对象，并启动线程用于显示图像和地图点
   mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
   mptViewer = new thread(&Viewer::Run, mpViewer);
   mpTracker->SetViewer(mpViewer);
}

2）Run函数代码

void Viewer::Run()
{
    mbFinished = false;
    mbStopped = false;
    //创建显示相机位姿的地图窗口
    pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768);

    // 3D Mouse handler requires depth testing to be enabled
    //启动深度测试：当场景中出现一个物体遮挡另一个物体时，为了看清楚到底谁遮挡了谁，需要启动深度检测。
    glEnable(GL_DEPTH_TEST);

    // Issue specific OpenGl we might need
    glEnable (GL_BLEND);
    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

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

    // Define Camera Render Object (for view / scene browsing)
    /**
     * Camera Axis:
     * X - Right, Y - Up, Z - Back
     * Image Origin:
     * Bottom Left
     * ProjectionMatrix为投影矩阵
     * void gluLookAt(GLdouble eyeX,  GLdouble eyeY,  GLdouble eyeZ,  GLdouble centerX,  GLdouble centerY,  GLdouble centerZ,  GLdouble upX,  GLdouble upY,  GLdouble upZ);
     * 第一组eyex, eyey,eyez 相机在世界坐标的位置
     * 第二组centerx,centery,centerz 相机镜头对准的物体在世界坐标的位置
     * 第三组upx,upy,upz 相机向上的方向在世界坐标中的方向
     * 你把相机想象成为你自己的脑袋：
     * 第一组数据就是脑袋的位置
     * 第二组数据就是眼睛看的物体的位置
     * 第三组就是头顶朝向的方向（因为你可以歪着头看同一个物体）。
     * 这段解释参考自：https://www.cnblogs.com/Anita9002/p/4386472.html
     * mViewpointX: 0
     * mViewpointY: -0.7
     * mViewpointZ: -1.8
    */
    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)
                );

    // Add named OpenGL viewport to window and provide 3D Handler
    //创建一个窗口，也就是打开相机后相机有一个成像平面，即视口viewport
    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));

    pangolin::OpenGlMatrix Twc;
    Twc.SetIdentity();//将Twc中值全部设置为1
    //cv::namedWindow函数用于创建一个窗口，第一个参数是窗口名称，第二个参数是窗口大小，默认是图片自适应
    cv::namedWindow("ORB-SLAM2: Current Frame");

    bool bFollow = true;
    bool bLocalizationMode = false;
    //这个大循环里边是绘图的代码
    while(1)
    {
        //清除颜色缓冲和深度缓冲
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        //一定要注意：Twc.m中以列优先存放的16个数值为相机的旋转和平移矩阵值
        mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc);

        if(menuFollowCamera && bFollow)
        {
            //相机跟随Twc位置的设置
            s_cam.Follow(Twc);
        }
        else if(menuFollowCamera && !bFollow)
        {
            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
            s_cam.Follow(Twc);
            bFollow = true;
        }
        else if(!menuFollowCamera && bFollow)
        {
            bFollow = false;
        }

        if(menuLocalizationMode && !bLocalizationMode)
        {
            mpSystem->ActivateLocalizationMode();
            bLocalizationMode = true;
        }
        else if(!menuLocalizationMode && bLocalizationMode)
        {
            mpSystem->DeactivateLocalizationMode();
            bLocalizationMode = false;
        }

        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        //画当前相机的在地图中的位姿
        mpMapDrawer->DrawCurrentCamera(Twc);
        if(menuShowKeyFrames || menuShowGraph)
            mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph);
        if(menuShowPoints)
            mpMapDrawer->DrawMapPoints();
        //交换帧和处理事件
        pangolin::FinishFrame();
        //获取标注有特征点的图像帧
        cv::Mat im = mpFrameDrawer->DrawFrame();
        //将图像im在名称为ORB-SLAM2: Current Frame的窗口中显示
        cv::imshow("ORB-SLAM2: Current Frame",im);
        //在mT时间内等待用户按键触发，设置waitKey(0),则表示程序会无限制的等待用户的按键事件
        cv::waitKey(mT);
        //如果显示关键帧相机位姿的窗口中reset按钮被按了，需要重置状态
        if(menuReset)
        {
            menuShowGraph = true;
            menuShowKeyFrames = true;
            menuShowPoints = true;
            menuLocalizationMode = false;
            if(bLocalizationMode)
                mpSystem->DeactivateLocalizationMode();
            bLocalizationMode = false;
            bFollow = true;
            menuFollowCamera = true;
            mpSystem->Reset();
            menuReset = false;
        }

        if(Stop())
        {
            while(isStopped())
            {
                usleep(3000);
            }
        }

        if(CheckFinish())
            break;
    }

    SetFinish();
}

获取当前相机的位姿代码如下：

/**
 * 将当前帧相机的旋转和平移矩阵按照列优先的顺序存入M矩阵中
 * 后边用于进行相机位姿调整
*/
void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M)
{
    if(!mCameraPose.empty())
    {
        cv::Mat Rwc(3,3,CV_32F);
        cv::Mat twc(3,1,CV_32F);
        {
            unique_lock<mutex> lock(mMutexCamera);
            //获取相机位姿的旋转矩阵
            Rwc = mCameraPose.rowRange(0,3).colRange(0,3).t();
            //获取相机位姿的平移向量
            twc = -Rwc*mCameraPose.rowRange(0,3).col(3);
        }

        M.m[0] = Rwc.at<float>(0,0);
        M.m[1] = Rwc.at<float>(1,0);
        M.m[2] = Rwc.at<float>(2,0);
        M.m[3]  = 0.0;

        M.m[4] = Rwc.at<float>(0,1);
        M.m[5] = Rwc.at<float>(1,1);
        M.m[6] = Rwc.at<float>(2,1);
        M.m[7]  = 0.0;

        M.m[8] = Rwc.at<float>(0,2);
        M.m[9] = Rwc.at<float>(1,2);
        M.m[10] = Rwc.at<float>(2,2);
        M.m[11]  = 0.0;

        M.m[12] = twc.at<float>(0);
        M.m[13] = twc.at<float>(1);
        M.m[14] = twc.at<float>(2);
        M.m[15]  = 1.0;
    }
    else
        M.SetIdentity();
}

3）在地图中画出当前相机位姿

/**
 * 画出的是当前的相机位姿，在图像窗口中显示为绿色的相机
 * 这里主要是根据Twc.m的值进行位姿调整，Twc.m中存储的是列优先的相机旋转和平移矩阵
*/
void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc)
{
    const float &w = mCameraSize;
    const float h = w*0.75;
    const float z = w*0.6;
    /**
     * glPushMatrix、glPopMatrix实际上是做入栈和出栈的操作。因为旋转和平移都是在glPushMatrix、glPopMatrix之间进行的。
     * 在绘制之前先把当前的位置保存下来，后边调用glPopMatrix可再次回到当前的位置。下次再绘制的时候就不会受当前的旋转或者平移的影响。
     * glPushMatrix()和glPopMatrix()的配对使用能够消除上一次的变换对本次变换的影响。使本次变换是以世界坐标系的原点为參考点进行。
     * */
    glPushMatrix();

#ifdef HAVE_GLES
        glMultMatrixf(Twc.m);
#else
        glMultMatrixd(Twc.m);//把m指定的16个值作为一个矩阵，与当前矩阵相乘，并把结果存储在当前矩阵中
#endif

    glLineWidth(mCameraLineWidth);
    //相机为绿色
    glColor3f(0.0f,1.0f,0.0f);

    glBegin(GL_LINES);
    glVertex3f(0,0,0);
    glVertex3f(w,h,z);
    glVertex3f(0,0,0);
    glVertex3f(w,-h,z);
    glVertex3f(0,0,0);
    glVertex3f(-w,-h,z);
    glVertex3f(0,0,0);
    glVertex3f(-w,h,z);

    glVertex3f(w,h,z);
    glVertex3f(w,-h,z);

    glVertex3f(-w,h,z);
    glVertex3f(-w,-h,z);

    glVertex3f(-w,h,z);
    glVertex3f(w,h,z);

    glVertex3f(-w,-h,z);
    glVertex3f(w,-h,z);
    glEnd();

    glPopMatrix();
}

这里画出的相机位姿如下：

图中所画的相机位姿图由五个点每两个点连一条线组成，点的顺序和坐标如下：

4）绘制地图中当前相机之前的关键帧

//绘制后边跟随的关键帧
void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph)
{
    const float &w = mKeyFrameSize;
    const float h = w*0.75;
    const float z = w*0.6;

    const vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    //判断是否要画出关键帧
    if(bDrawKF)
    {
        //遍历关键帧
        for(size_t i=0; i<vpKFs.size(); i++)
        {
            KeyFrame* pKF = vpKFs[i];
            //获取关键帧位姿的转置（因为pangolin是列优先存放的）
            cv::Mat Twc = pKF->GetPoseInverse().t();

            glPushMatrix();
            //通过ptr获取第0行
            glMultMatrixf(Twc.ptr<GLfloat>(0));

            glLineWidth(mKeyFrameLineWidth);
            //颜色设置为黑色
            glColor3f(0.0f,0.0f,1.0f);
            glBegin(GL_LINES);
            glVertex3f(0,0,0);
            glVertex3f(w,h,z);
            glVertex3f(0,0,0);
            glVertex3f(w,-h,z);
            glVertex3f(0,0,0);
            glVertex3f(-w,-h,z);
            glVertex3f(0,0,0);
            glVertex3f(-w,h,z);

            glVertex3f(w,h,z);
            glVertex3f(w,-h,z);

            glVertex3f(-w,h,z);
            glVertex3f(-w,-h,z);

            glVertex3f(-w,h,z);
            glVertex3f(w,h,z);

            glVertex3f(-w,-h,z);
            glVertex3f(w,-h,z);
            glEnd();

            glPopMatrix();
        }
    }
    //是否要将关键帧的相机中心点连起来，这里采用绿色线条连接，窗口中可以看到
    if(bDrawGraph)
    {
        glLineWidth(mGraphLineWidth);
        glColor4f(0.0f,1.0f,0.0f,0.6f);
        glBegin(GL_LINES);

        for(size_t i=0; i<vpKFs.size(); i++)
        {
            // Covisibility Graph 获取和vpKFs共视的所有关键帧
            const vector<KeyFrame*> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100);
            cv::Mat Ow = vpKFs[i]->GetCameraCenter();
            //将vpKFs和它所有的共视关键帧连接
            if(!vCovKFs.empty())
            {
                for(vector<KeyFrame*>::const_iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++)
                {
                    if((*vit)->mnId<vpKFs[i]->mnId)
                        continue;
                    cv::Mat Ow2 = (*vit)->GetCameraCenter();
                    glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
                    glVertex3f(Ow2.at<float>(0),Ow2.at<float>(1),Ow2.at<float>(2));
                }
            }

            // Spanning tree
            KeyFrame* pParent = vpKFs[i]->GetParent();
            //将vpKFs和其父亲关键帧的相机中心点连接
            if(pParent)
            {
                cv::Mat Owp = pParent->GetCameraCenter();
                glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
                glVertex3f(Owp.at<float>(0),Owp.at<float>(1),Owp.at<float>(2));
            }

            // Loops
            //将vpKFs和与它产生闭环的关键帧的相机中心点连接
            set<KeyFrame*> sLoopKFs = vpKFs[i]->GetLoopEdges();
            for(set<KeyFrame*>::iterator sit=sLoopKFs.begin(), send=sLoopKFs.end(); sit!=send; sit++)
            {
                if((*sit)->mnId<vpKFs[i]->mnId)
                    continue;
                cv::Mat Owl = (*sit)->GetCameraCenter();
                glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
                glVertex3f(Owl.at<float>(0),Owl.at<float>(1),Owl.at<float>(2));
            }
        }

        glEnd();
    }
}

5）绘制地图中的MapPoint

/**
 * 在窗口中画出那些被检测出的关键点
 * 
*/
void MapDrawer::DrawMapPoints()
{
    const vector<MapPoint*> &vpMPs = mpMap->GetAllMapPoints();
    //获取参考帧中的关键点
    const vector<MapPoint*> &vpRefMPs = mpMap->GetReferenceMapPoints();

    set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());

    if(vpMPs.empty())
        return;

    glPointSize(mPointSize);
    //glBegin和glEnd配合使用，GL_POINTS：把每个顶点作为一个点进行处理，顶点n定义了点n，绘制N个点
    glBegin(GL_POINTS);
    //黑色
    glColor3f(0.0,0.0,0.0);

    for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
    {
        if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
            continue;
        cv::Mat pos = vpMPs[i]->GetWorldPos();
        glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));
    }
    glEnd();
    /**
     * 以下要画出的点为参考关键帧中的特征点，在窗口中标记为红色
     * 表示这些点在接下来输入新的帧的时候用来进行匹配
    */
    glPointSize(mPointSize);
    glBegin(GL_POINTS);
    //红色
    glColor3f(1.0,0.0,0.0);

    for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
    {
        if((*sit)->isBad())
            continue;
        cv::Mat pos = (*sit)->GetWorldPos();
        glVertex3f(pos.at<float>(0),pos.at<float>(1),pos.at<float>(2));

    }

    glEnd();
}

6）窗口中显示标注有特征点的图像帧

DrawFrame函数是在图像帧中用绿色的方框和绿色的小圆圈标注出特征点的图像帧。

/**
 * 根据传入的图片帧，在其中标注计算出来的特征点
 * 
*/
cv::Mat FrameDrawer::DrawFrame()
{
    cv::Mat im;
    vector<cv::KeyPoint> vIniKeys; // Initialization: KeyPoints in reference frame
    vector<int> vMatches; // Initialization: correspondeces with reference keypoints
    vector<cv::KeyPoint> vCurrentKeys; // KeyPoints in current frame
    vector<bool> vbVO, vbMap; // Tracked MapPoints in current frame
    int state; // Tracking state

    //Copy variables within scoped mutex
    {
        unique_lock<mutex> lock(mMutex);
        state=mState;
        if(mState==Tracking::SYSTEM_NOT_READY)
            mState=Tracking::NO_IMAGES_YET;
        //将mIm图像拷贝到im中
        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;
        }
    } // destroy scoped mutex -> release mutex

    if(im.channels()<3) //this should be always true
        cvtColor(im,im,CV_GRAY2BGR);

    //Draw
    if(state==Tracking::NOT_INITIALIZED) //INITIALIZING
    {
        for(unsigned int i=0; i<vMatches.size(); i++)
        {
            if(vMatches[i]>=0)
            {
                cv::line(im,vIniKeys[i].pt,vCurrentKeys[vMatches[i]].pt,
                        cv::Scalar(0,255,0));
            }
        }        
    }
    else if(state==Tracking::OK) //TRACKING
    {
        mnTracked=0;
        mnTrackedVO=0;
        const float r = 5;
        const int n = vCurrentKeys.size();
        for(int i=0;i<n;i++)
        {
            if(vbVO[i] || vbMap[i])
            {
                cv::Point2f pt1,pt2;
                pt1.x=vCurrentKeys[i].pt.x-r;
                pt1.y=vCurrentKeys[i].pt.y-r;
                pt2.x=vCurrentKeys[i].pt.x+r;
                pt2.y=vCurrentKeys[i].pt.y+r;

                // This is a match to a MapPoint in the map
                if(vbMap[i])
                {
                    /**
                     * 这个是画矩形框的函数
                     * im 图像
                     * pt1和pt2为矩形对角线上的两个顶点坐标
                     * cv::Scalar(0,255,0) 为线条颜色
                    */
                    cv::rectangle(im,pt1,pt2,cv::Scalar(0,255,0));
                    /**
                     * void circle(CV_IN_OUT Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0); 
                     * img为图像，单通道多通道都行，不需要特殊要求
                     * center为画圆的圆心坐标，这里是关键点的坐标值
                     * radius为圆的半径
                     * color为设定圆的颜色，比如用CV_RGB(255, 0,0)设置为红色， CV_RGB(255, 255,255)设置为白色，CV_RGB(0, 0,0)设置为黑色 
                     * thickness为设置圆线条的粗细，值越大则线条越粗，为负数则是填充效果
                    */
                    cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(0,255,0),-1);
                    mnTracked++;
                }
                else // This is match to a "visual odometry" MapPoint created in the last frame
                {
                    cv::rectangle(im,pt1,pt2,cv::Scalar(255,0,0));
                    cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(255,0,0),-1);
                    mnTrackedVO++;
                }
            }
        }
    }

    cv::Mat imWithInfo;
    //图像帧内文字的显示
    DrawTextInfo(im,state, imWithInfo);

    return imWithInfo;
}

使用opencv提供的函数进行显示：

//将图像im在名称为ORB-SLAM2: Current Frame的窗口中显示
cv::imshow("ORB-SLAM2: Current Frame",im);

3.小结

从地图的显示线程代码流程可以看出，地图中显示出的相机的位姿是Tracking线程中估计出的相机位姿（旋转矩阵和平移向量）形象的可视化。关键帧的位姿和MapPoint的坐标值的估计的准确程度直接在可视化窗口中能够得到反应。这部分的主要功能和相应函数如下：