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 needglEnable (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//创建一个窗口，也就是打开相机后相机有一个成像平面，即视口viewportpangolin::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;}elseM.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_GLESglMultMatrixf(Twc.m);
#elseglMultMatrixd(Twc.m);//把m指定的16个值作为一个矩阵，与当前矩阵相乘，并把结果存储在当前矩阵中
#endifglLineWidth(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 treeKeyFrame* 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 framevector<int> vMatches; // Initialization: correspondeces with reference keypointsvector<cv::KeyPoint> vCurrentKeys; // KeyPoints in current framevector<bool> vbVO, vbMap; // Tracked MapPoints in current frameint 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 mutexif(im.channels()<3) //this should be always truecvtColor(im,im,CV_GRAY2BGR);//Drawif(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 mapif(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的坐标值的估计的准确程度直接在可视化窗口中能够得到反应。这部分的主要功能和相应函数如下：

ORB-SLAM2 窗口显示Viewer线程相关推荐

PCL基础4：PCLVisualizer可视化窗口显示
1.单视口显示. //可视化结果 pcl::visualization::CloudViewer viewer("边界提取"); viewer.showCloud(cloud_ou ...
ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
Ubuntu下使用单目相机运行ORB SLAM2
环境:Ubuntu16.04+ROS Kinetic+USB单目摄像头虽然ORB SLAM2的官方说明中表示没有ROS也可以编译运行,但要实时的跑ORB SLAM2还是需要ROS平台的,所以之前没有 ...
activity及其窗口显示流程
时序图预览 activity及其窗口显示流程图 activity的启动流程可参考:activity启动流程图关键代码分析 ActivityThread 接收框架 (ActivityTaskManag ...
多线程试验。模拟航班售票系统，实现4个售票窗口发售某班次航班的100张机票，一个售票窗口用一个线程表示。要求不能重复发售。
五一假期要到了,希望大家都都能开开心心代码 import javax.swing.*; import java.awt.*; import java.awt.event.ActionEvent; i ...
【实用】几个实用的webstorm、IDEA编辑器窗口快捷键设置，Alt+V垂直复制当前窗口，Alt+Shift+V将当前窗口复制到另一边的分割窗口显示，Alt+Shift+M移动当前活动窗口到另一边
Alt+V垂直复制当前窗口 Alt+Shift+V将当前窗口复制到另一边的分割窗口显示,Alt+Shift+M移动当前活动窗口到另一边
【OpenGL】七、桌面窗口搭建 ( 导入头文件 | 桌面程序入口函数 | 注册窗口 | 创建窗口 | 显示窗口 )
文章目录一.导入头文件二.桌面程序入口函数三.注册窗口四.创建窗口五.显示窗口六.完整代码示例七.相关资源基于 [OpenGL]一.Visual Studio 2019 创建 Wind ...
OpenCV学习笔记（五十六）——InputArray和OutputArray的那些事core OpenCV学习笔记（五十七）——在同一窗口显示多幅图片 OpenCV学习笔记（五十八）——读《Mast
OpenCV学习笔记(五十六)--InputArray和OutputArray的那些事core 看过OpenCV源代码的朋友,肯定都知道很多函数的接口都是InputArray或者OutputArray ...
dos窗口显示中文的一种方法
dos窗口显示中文的一种方法 dos窗口显示中文的一种方法 (2011-09-13 11:20:41)转载▼ 标签: 杂谈中文代码页 936 UTF8 65001 CHCP 936 # 使用简体中文 ...
python PyQt5中文教程☞【第二节】PyQt5基本功能（创建窗口、应用程序图标、显示提示语、通过按钮关闭窗口、消息框（关闭窗口确认框）、窗口显示在屏幕中间【居中显示】）
引用文章:http://code.py40.com/pyqt5/ 文章目录简单的例子:创建一个小窗口应用程序的图标显示提示语通过按钮关闭窗口消息框(关闭窗口确认框) 窗口显示在屏幕的中间[居 ...

ORB-SLAM2 窗口显示Viewer线程

1.View线程都做了些什么

2.View线程代码

1）View线程创建

2）Run函数代码

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

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

5）绘制地图中的MapPoint

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

3.小结

ORB-SLAM2 窗口显示Viewer线程相关推荐

最新文章

热门文章