Last active
September 23, 2021 11:33
-
-
Save RomanVolkov/c93bd36c4744286dd66b15a72399d7ac to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
diff --git a/CMakeLists.txt b/CMakeLists.txt | |
index 1f7c65b..950068c 100644 | |
--- a/CMakeLists.txt | |
+++ b/CMakeLists.txt | |
@@ -45,7 +45,7 @@ MESSAGE("OPENCV VERSION:") | |
MESSAGE(${OpenCV_VERSION}) | |
find_package(Eigen3 3.1.0 REQUIRED) | |
-find_package(Pangolin REQUIRED) | |
+#find_package(Pangolin REQUIRED) | |
include_directories( | |
${PROJECT_SOURCE_DIR} | |
diff --git a/build.sh b/build.sh | |
index 9aadf59..343b50b 100755 | |
--- a/build.sh | |
+++ b/build.sh | |
@@ -4,7 +4,7 @@ cd Thirdparty/DBoW2 | |
mkdir build | |
cd build | |
cmake .. -DCMAKE_BUILD_TYPE=Release | |
-make -j | |
+make | |
cd ../../g2o | |
@@ -13,7 +13,7 @@ echo "Configuring and building Thirdparty/g2o ..." | |
mkdir build | |
cd build | |
cmake .. -DCMAKE_BUILD_TYPE=Release | |
-make -j | |
+make | |
cd ../../../ | |
@@ -28,4 +28,4 @@ echo "Configuring and building ORB_SLAM3 ..." | |
mkdir build | |
cd build | |
cmake .. -DCMAKE_BUILD_TYPE=Release | |
-make -j | |
+make | |
diff --git a/include/LoopClosing.h b/include/LoopClosing.h | |
index 215b80a..5a3ffc8 100644 | |
--- a/include/LoopClosing.h | |
+++ b/include/LoopClosing.h | |
@@ -47,8 +47,7 @@ class LoopClosing | |
public: | |
typedef pair<set<KeyFrame*>,int> ConsistentGroup; | |
- typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>, | |
- Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose; | |
+ typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>> KeyFrameAndPose; | |
public: | |
diff --git a/include/Map.h b/include/Map.h | |
index 00396ba..780c17a 100644 | |
--- a/include/Map.h | |
+++ b/include/Map.h | |
@@ -24,7 +24,7 @@ | |
#include "KeyFrame.h" | |
#include <set> | |
-#include <pangolin/pangolin.h> | |
+// #include <pangolin/pangolin.h> | |
#include <mutex> | |
#include <boost/serialization/base_object.hpp> | |
@@ -186,8 +186,8 @@ protected: | |
int mnBigChangeIdx; | |
- // View of the map in aerial sight (for the AtlasViewer) | |
- GLubyte* mThumbnail; | |
+ // // View of the map in aerial sight (for the AtlasViewer) | |
+ // GLubyte* mThumbnail; | |
bool mIsInUse; | |
bool mHasTumbnail; | |
diff --git a/include/MapDrawer.h b/include/MapDrawer.h | |
index 88f256a..8e8caf2 100644 | |
--- a/include/MapDrawer.h | |
+++ b/include/MapDrawer.h | |
@@ -23,7 +23,7 @@ | |
#include"Atlas.h" | |
#include"MapPoint.h" | |
#include"KeyFrame.h" | |
-#include<pangolin/pangolin.h> | |
+// #include<pangolin/pangolin.h> | |
#include<mutex> | |
@@ -39,11 +39,11 @@ public: | |
void DrawMapPoints(); | |
void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph); | |
- void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); | |
+ // void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); | |
void SetCurrentCameraPose(const cv::Mat &Tcw); | |
void SetReferenceKeyFrame(KeyFrame *pKF); | |
- void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw); | |
- void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp); | |
+ // void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw); | |
+ // void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp); | |
private: | |
diff --git a/src/Map.cc b/src/Map.cc | |
index 9e5cf7b..76c5eed 100644 | |
--- a/src/Map.cc | |
+++ b/src/Map.cc | |
@@ -30,7 +30,7 @@ Map::Map():mnMaxKFid(0),mnBigChangeIdx(0), mbImuInitialized(false), mnMapChange( | |
mbFail(false), mIsInUse(false), mHasTumbnail(false), mbBad(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false) | |
{ | |
mnId=nNextId++; | |
- mThumbnail = static_cast<GLubyte*>(NULL); | |
+ //mThumbnail = static_cast<GLubyte*>(NULL); | |
} | |
Map::Map(int initKFid):mnInitKFid(initKFid), mnMaxKFid(initKFid),mnLastLoopKFid(initKFid), mnBigChangeIdx(0), mIsInUse(false), | |
@@ -38,7 +38,7 @@ Map::Map(int initKFid):mnInitKFid(initKFid), mnMaxKFid(initKFid),mnLastLoopKFid( | |
mnMapChange(0), mbFail(false), mnMapChangeNotified(0), mbIsInertial(false), mbIMU_BA1(false), mbIMU_BA2(false) | |
{ | |
mnId=nNextId++; | |
- mThumbnail = static_cast<GLubyte*>(NULL); | |
+ //mThumbnail = static_cast<GLubyte*>(NULL); | |
} | |
Map::~Map() | |
@@ -49,9 +49,9 @@ Map::~Map() | |
//TODO: erase all keyframes from memory | |
mspKeyFrames.clear(); | |
- if(mThumbnail) | |
- delete mThumbnail; | |
- mThumbnail = static_cast<GLubyte*>(NULL); | |
+ //if(mThumbnail) | |
+ // delete mThumbnail; | |
+ //mThumbnail = static_cast<GLubyte*>(NULL); | |
mvpReferenceMapPoints.clear(); | |
mvpKeyFrameOrigins.clear(); | |
diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc | |
index 1331a9b..146c099 100644 | |
--- a/src/MapDrawer.cc | |
+++ b/src/MapDrawer.cc | |
@@ -20,7 +20,7 @@ | |
#include "MapDrawer.h" | |
#include "MapPoint.h" | |
#include "KeyFrame.h" | |
-#include <pangolin/pangolin.h> | |
+// #include <pangolin/pangolin.h> | |
#include <mutex> | |
namespace ORB_SLAM3 | |
@@ -122,309 +122,309 @@ bool MapDrawer::ParseViewerParamFile(cv::FileStorage &fSettings) | |
void MapDrawer::DrawMapPoints() | |
{ | |
- const vector<MapPoint*> &vpMPs = mpAtlas->GetAllMapPoints(); | |
- const vector<MapPoint*> &vpRefMPs = mpAtlas->GetReferenceMapPoints(); | |
+ // const vector<MapPoint*> &vpMPs = mpAtlas->GetAllMapPoints(); | |
+ // const vector<MapPoint*> &vpRefMPs = mpAtlas->GetReferenceMapPoints(); | |
- set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); | |
+ // set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); | |
- if(vpMPs.empty()) | |
- return; | |
+ // if(vpMPs.empty()) | |
+ // return; | |
- glPointSize(mPointSize); | |
- glBegin(GL_POINTS); | |
- glColor3f(0.0,0.0,0.0); | |
+ // glPointSize(mPointSize); | |
+ // 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(); | |
+ // 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); | |
+ // 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)); | |
+ // 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(); | |
+ // glEnd(); | |
} | |
void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph) | |
{ | |
- const float &w = mKeyFrameSize; | |
- const float h = w*0.75; | |
- const float z = w*0.6; | |
- | |
- const vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames(); | |
- | |
- if(bDrawKF) | |
- { | |
- for(size_t i=0; i<vpKFs.size(); i++) | |
- { | |
- KeyFrame* pKF = vpKFs[i]; | |
- cv::Mat Twc = pKF->GetPoseInverse().t(); | |
- unsigned int index_color = pKF->mnOriginMapId; | |
- | |
- glPushMatrix(); | |
- | |
- glMultMatrixf(Twc.ptr<GLfloat>(0)); | |
- | |
- if(!pKF->GetParent()) // It is the first KF in the map | |
- { | |
- glLineWidth(mKeyFrameLineWidth*5); | |
- glColor3f(1.0f,0.0f,0.0f); | |
- glBegin(GL_LINES); | |
- | |
- //cout << "Initial KF: " << mpAtlas->GetCurrentMap()->GetOriginKF()->mnId << endl; | |
- //cout << "Parent KF: " << vpKFs[i]->mnId << endl; | |
- } | |
- else | |
- { | |
- glLineWidth(mKeyFrameLineWidth); | |
- //glColor3f(0.0f,0.0f,1.0f); | |
- glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]); | |
- 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(); | |
- | |
- //Draw lines with Loop and Merge candidates | |
- /*glLineWidth(mGraphLineWidth); | |
- glColor4f(1.0f,0.6f,0.0f,1.0f); | |
- glBegin(GL_LINES); | |
- cv::Mat Ow = pKF->GetCameraCenter(); | |
- const vector<KeyFrame*> vpLoopCandKFs = pKF->mvpLoopCandKFs; | |
- if(!vpLoopCandKFs.empty()) | |
- { | |
- for(vector<KeyFrame*>::const_iterator vit=vpLoopCandKFs.begin(), vend=vpLoopCandKFs.end(); vit!=vend; vit++) | |
- { | |
- 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)); | |
- } | |
- } | |
- const vector<KeyFrame*> vpMergeCandKFs = pKF->mvpMergeCandKFs; | |
- if(!vpMergeCandKFs.empty()) | |
- { | |
- for(vector<KeyFrame*>::const_iterator vit=vpMergeCandKFs.begin(), vend=vpMergeCandKFs.end(); vit!=vend; vit++) | |
- { | |
- 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)); | |
- } | |
- }*/ | |
- | |
- glEnd(); | |
- } | |
- } | |
+ // const float &w = mKeyFrameSize; | |
+ // const float h = w*0.75; | |
+ // const float z = w*0.6; | |
+ | |
+ // const vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames(); | |
+ | |
+ // if(bDrawKF) | |
+ // { | |
+ // for(size_t i=0; i<vpKFs.size(); i++) | |
+ // { | |
+ // KeyFrame* pKF = vpKFs[i]; | |
+ // cv::Mat Twc = pKF->GetPoseInverse().t(); | |
+ // unsigned int index_color = pKF->mnOriginMapId; | |
+ | |
+ // glPushMatrix(); | |
+ | |
+ // glMultMatrixf(Twc.ptr<GLfloat>(0)); | |
+ | |
+ // if(!pKF->GetParent()) // It is the first KF in the map | |
+ // { | |
+ // glLineWidth(mKeyFrameLineWidth*5); | |
+ // glColor3f(1.0f,0.0f,0.0f); | |
+ // glBegin(GL_LINES); | |
+ | |
+ // //cout << "Initial KF: " << mpAtlas->GetCurrentMap()->GetOriginKF()->mnId << endl; | |
+ // //cout << "Parent KF: " << vpKFs[i]->mnId << endl; | |
+ // } | |
+ // else | |
+ // { | |
+ // glLineWidth(mKeyFrameLineWidth); | |
+ // //glColor3f(0.0f,0.0f,1.0f); | |
+ // glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]); | |
+ // 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(); | |
+ | |
+ // //Draw lines with Loop and Merge candidates | |
+ // /*glLineWidth(mGraphLineWidth); | |
+ // glColor4f(1.0f,0.6f,0.0f,1.0f); | |
+ // glBegin(GL_LINES); | |
+ // cv::Mat Ow = pKF->GetCameraCenter(); | |
+ // const vector<KeyFrame*> vpLoopCandKFs = pKF->mvpLoopCandKFs; | |
+ // if(!vpLoopCandKFs.empty()) | |
+ // { | |
+ // for(vector<KeyFrame*>::const_iterator vit=vpLoopCandKFs.begin(), vend=vpLoopCandKFs.end(); vit!=vend; vit++) | |
+ // { | |
+ // 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)); | |
+ // } | |
+ // } | |
+ // const vector<KeyFrame*> vpMergeCandKFs = pKF->mvpMergeCandKFs; | |
+ // if(!vpMergeCandKFs.empty()) | |
+ // { | |
+ // for(vector<KeyFrame*>::const_iterator vit=vpMergeCandKFs.begin(), vend=vpMergeCandKFs.end(); vit!=vend; vit++) | |
+ // { | |
+ // 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)); | |
+ // } | |
+ // }*/ | |
+ | |
+ // glEnd(); | |
+ // } | |
+ // } | |
+ | |
+ // if(bDrawGraph) | |
+ // { | |
+ // glLineWidth(mGraphLineWidth); | |
+ // glColor4f(0.0f,1.0f,0.0f,0.6f); | |
+ // glBegin(GL_LINES); | |
+ | |
+ // // cout << "-----------------Draw graph-----------------" << endl; | |
+ // for(size_t i=0; i<vpKFs.size(); i++) | |
+ // { | |
+ // // Covisibility Graph | |
+ // const vector<KeyFrame*> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); | |
+ // cv::Mat Ow = vpKFs[i]->GetCameraCenter(); | |
+ // 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(); | |
+ // 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 | |
+ // 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(); | |
+ // } | |
+ | |
+ // if(bDrawInertialGraph && mpAtlas->isImuInitialized()) | |
+ // { | |
+ // glLineWidth(mGraphLineWidth); | |
+ // glColor4f(1.0f,0.0f,0.0f,0.6f); | |
+ // glBegin(GL_LINES); | |
+ | |
+ // //Draw inertial links | |
+ // for(size_t i=0; i<vpKFs.size(); i++) | |
+ // { | |
+ // KeyFrame* pKFi = vpKFs[i]; | |
+ // cv::Mat Ow = pKFi->GetCameraCenter(); | |
+ // KeyFrame* pNext = pKFi->mNextKF; | |
+ // if(pNext) | |
+ // { | |
+ // cv::Mat Owp = pNext->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)); | |
+ // } | |
+ // } | |
+ | |
+ // glEnd(); | |
+ // } | |
+ | |
+ // vector<Map*> vpMaps = mpAtlas->GetAllMaps(); | |
+ | |
+ // if(bDrawKF) | |
+ // { | |
+ // for(Map* pMap : vpMaps) | |
+ // { | |
+ // if(pMap == mpAtlas->GetCurrentMap()) | |
+ // continue; | |
+ | |
+ // vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames(); | |
+ | |
+ // for(size_t i=0; i<vpKFs.size(); i++) | |
+ // { | |
+ // KeyFrame* pKF = vpKFs[i]; | |
+ // cv::Mat Twc = pKF->GetPoseInverse().t(); | |
+ // unsigned int index_color = pKF->mnOriginMapId; | |
+ | |
+ // glPushMatrix(); | |
+ | |
+ // glMultMatrixf(Twc.ptr<GLfloat>(0)); | |
+ | |
+ // if(!vpKFs[i]->GetParent()) // It is the first KF in the map | |
+ // { | |
+ // glLineWidth(mKeyFrameLineWidth*5); | |
+ // glColor3f(1.0f,0.0f,0.0f); | |
+ // glBegin(GL_LINES); | |
+ // } | |
+ // else | |
+ // { | |
+ // glLineWidth(mKeyFrameLineWidth); | |
+ // //glColor3f(0.0f,0.0f,1.0f); | |
+ // glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]); | |
+ // 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); | |
+// void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc) | |
+// { | |
+// const float &w = mCameraSize; | |
+// const float h = w*0.75; | |
+// const float z = w*0.6; | |
- // cout << "-----------------Draw graph-----------------" << endl; | |
- for(size_t i=0; i<vpKFs.size(); i++) | |
- { | |
- // Covisibility Graph | |
- const vector<KeyFrame*> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); | |
- cv::Mat Ow = vpKFs[i]->GetCameraCenter(); | |
- 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(); | |
- 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 | |
- 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)); | |
- } | |
- } | |
+// glPushMatrix(); | |
- glEnd(); | |
- } | |
+// #ifdef HAVE_GLES | |
+// glMultMatrixf(Twc.m); | |
+// #else | |
+// glMultMatrixd(Twc.m); | |
+// #endif | |
- if(bDrawInertialGraph && mpAtlas->isImuInitialized()) | |
- { | |
- glLineWidth(mGraphLineWidth); | |
- glColor4f(1.0f,0.0f,0.0f,0.6f); | |
- glBegin(GL_LINES); | |
+// 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); | |
- //Draw inertial links | |
- for(size_t i=0; i<vpKFs.size(); i++) | |
- { | |
- KeyFrame* pKFi = vpKFs[i]; | |
- cv::Mat Ow = pKFi->GetCameraCenter(); | |
- KeyFrame* pNext = pKFi->mNextKF; | |
- if(pNext) | |
- { | |
- cv::Mat Owp = pNext->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)); | |
- } | |
- } | |
+// glVertex3f(w,h,z); | |
+// glVertex3f(w,-h,z); | |
- glEnd(); | |
- } | |
+// glVertex3f(-w,h,z); | |
+// glVertex3f(-w,-h,z); | |
- vector<Map*> vpMaps = mpAtlas->GetAllMaps(); | |
+// glVertex3f(-w,h,z); | |
+// glVertex3f(w,h,z); | |
- if(bDrawKF) | |
- { | |
- for(Map* pMap : vpMaps) | |
- { | |
- if(pMap == mpAtlas->GetCurrentMap()) | |
- continue; | |
- | |
- vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames(); | |
- | |
- for(size_t i=0; i<vpKFs.size(); i++) | |
- { | |
- KeyFrame* pKF = vpKFs[i]; | |
- cv::Mat Twc = pKF->GetPoseInverse().t(); | |
- unsigned int index_color = pKF->mnOriginMapId; | |
- | |
- glPushMatrix(); | |
- | |
- glMultMatrixf(Twc.ptr<GLfloat>(0)); | |
- | |
- if(!vpKFs[i]->GetParent()) // It is the first KF in the map | |
- { | |
- glLineWidth(mKeyFrameLineWidth*5); | |
- glColor3f(1.0f,0.0f,0.0f); | |
- glBegin(GL_LINES); | |
- } | |
- else | |
- { | |
- glLineWidth(mKeyFrameLineWidth); | |
- //glColor3f(0.0f,0.0f,1.0f); | |
- glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]); | |
- 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(); | |
- } | |
- } | |
- } | |
-} | |
+// glVertex3f(-w,-h,z); | |
+// glVertex3f(w,-h,z); | |
+// glEnd(); | |
-void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc) | |
-{ | |
- const float &w = mCameraSize; | |
- const float h = w*0.75; | |
- const float z = w*0.6; | |
- | |
- glPushMatrix(); | |
- | |
-#ifdef HAVE_GLES | |
- glMultMatrixf(Twc.m); | |
-#else | |
- glMultMatrixd(Twc.m); | |
-#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(); | |
-} | |
+// glPopMatrix(); | |
+// } | |
void MapDrawer::SetCurrentCameraPose(const cv::Mat &Tcw) | |
@@ -433,112 +433,112 @@ void MapDrawer::SetCurrentCameraPose(const cv::Mat &Tcw) | |
mCameraPose = Tcw.clone(); | |
} | |
-void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw) | |
-{ | |
- 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; | |
- | |
- MOw.SetIdentity(); | |
- MOw.m[12] = twc.at<float>(0); | |
- MOw.m[13] = twc.at<float>(1); | |
- MOw.m[14] = twc.at<float>(2); | |
- } | |
- else | |
- { | |
- M.SetIdentity(); | |
- MOw.SetIdentity(); | |
- } | |
-} | |
- | |
-void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp) | |
-{ | |
- if(!mCameraPose.empty()) | |
- { | |
- cv::Mat Rwc(3,3,CV_32F); | |
- cv::Mat twc(3,1,CV_32F); | |
- cv::Mat Rwwp(3,3,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; | |
- | |
- MOw.SetIdentity(); | |
- MOw.m[12] = twc.at<float>(0); | |
- MOw.m[13] = twc.at<float>(1); | |
- MOw.m[14] = twc.at<float>(2); | |
- | |
- MTwwp.SetIdentity(); | |
- MTwwp.m[0] = Rwwp.at<float>(0,0); | |
- MTwwp.m[1] = Rwwp.at<float>(1,0); | |
- MTwwp.m[2] = Rwwp.at<float>(2,0); | |
- | |
- MTwwp.m[4] = Rwwp.at<float>(0,1); | |
- MTwwp.m[5] = Rwwp.at<float>(1,1); | |
- MTwwp.m[6] = Rwwp.at<float>(2,1); | |
- | |
- MTwwp.m[8] = Rwwp.at<float>(0,2); | |
- MTwwp.m[9] = Rwwp.at<float>(1,2); | |
- MTwwp.m[10] = Rwwp.at<float>(2,2); | |
- | |
- MTwwp.m[12] = twc.at<float>(0); | |
- MTwwp.m[13] = twc.at<float>(1); | |
- MTwwp.m[14] = twc.at<float>(2); | |
- } | |
- else | |
- { | |
- M.SetIdentity(); | |
- MOw.SetIdentity(); | |
- MTwwp.SetIdentity(); | |
- } | |
- | |
-} | |
+// void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw) | |
+// { | |
+// 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; | |
+ | |
+// MOw.SetIdentity(); | |
+// MOw.m[12] = twc.at<float>(0); | |
+// MOw.m[13] = twc.at<float>(1); | |
+// MOw.m[14] = twc.at<float>(2); | |
+// } | |
+// else | |
+// { | |
+// M.SetIdentity(); | |
+// MOw.SetIdentity(); | |
+// } | |
+// } | |
+ | |
+// void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp) | |
+// { | |
+// if(!mCameraPose.empty()) | |
+// { | |
+// cv::Mat Rwc(3,3,CV_32F); | |
+// cv::Mat twc(3,1,CV_32F); | |
+// cv::Mat Rwwp(3,3,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; | |
+ | |
+// MOw.SetIdentity(); | |
+// MOw.m[12] = twc.at<float>(0); | |
+// MOw.m[13] = twc.at<float>(1); | |
+// MOw.m[14] = twc.at<float>(2); | |
+ | |
+// MTwwp.SetIdentity(); | |
+// MTwwp.m[0] = Rwwp.at<float>(0,0); | |
+// MTwwp.m[1] = Rwwp.at<float>(1,0); | |
+// MTwwp.m[2] = Rwwp.at<float>(2,0); | |
+ | |
+// MTwwp.m[4] = Rwwp.at<float>(0,1); | |
+// MTwwp.m[5] = Rwwp.at<float>(1,1); | |
+// MTwwp.m[6] = Rwwp.at<float>(2,1); | |
+ | |
+// MTwwp.m[8] = Rwwp.at<float>(0,2); | |
+// MTwwp.m[9] = Rwwp.at<float>(1,2); | |
+// MTwwp.m[10] = Rwwp.at<float>(2,2); | |
+ | |
+// MTwwp.m[12] = twc.at<float>(0); | |
+// MTwwp.m[13] = twc.at<float>(1); | |
+// MTwwp.m[14] = twc.at<float>(2); | |
+// } | |
+// else | |
+// { | |
+// M.SetIdentity(); | |
+// MOw.SetIdentity(); | |
+// MTwwp.SetIdentity(); | |
+// } | |
+ | |
+// } | |
} //namespace ORB_SLAM | |
diff --git a/src/System.cc b/src/System.cc | |
index ab66b7c..2b2f189 100644 | |
--- a/src/System.cc | |
+++ b/src/System.cc | |
@@ -21,7 +21,7 @@ | |
#include "System.h" | |
#include "Converter.h" | |
#include <thread> | |
-#include <pangolin/pangolin.h> | |
+//include <pangolin/pangolin.h> | |
#include <iomanip> | |
#include <openssl/md5.h> | |
#include <boost/serialization/base_object.hpp> | |
@@ -471,8 +471,8 @@ void System::Shutdown() | |
usleep(5000); | |
} | |
- if(mpViewer) | |
- pangolin::BindToContext("ORB-SLAM2: Map Viewer"); | |
+ //if(mpViewer) | |
+ // pangolin::BindToContext("ORB-SLAM2: Map Viewer"); | |
} | |
diff --git a/src/Viewer.cc b/src/Viewer.cc | |
index 6ac81e1..1998cae 100644 | |
--- a/src/Viewer.cc | |
+++ b/src/Viewer.cc | |
@@ -18,7 +18,7 @@ | |
#include "Viewer.h" | |
-#include <pangolin/pangolin.h> | |
+// #include <pangolin/pangolin.h> | |
#include <mutex> | |
@@ -132,207 +132,207 @@ void Viewer::Run() | |
mbFinished = false; | |
mbStopped = false; | |
- pangolin::CreateWindowAndBind("ORB-SLAM3: 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> menuCamView("menu.Camera View",false,false); | |
- pangolin::Var<bool> menuTopView("menu.Top View",false,false); | |
- // pangolin::Var<bool> menuSideView("menu.Side View",false,false); | |
- 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",false,true); | |
- pangolin::Var<bool> menuShowInertialGraph("menu.Show Inertial Graph",true,true); | |
- pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true); | |
- pangolin::Var<bool> menuReset("menu.Reset",false,false); | |
- pangolin::Var<bool> menuStepByStep("menu.Step By Step",false,true); // false, true | |
- pangolin::Var<bool> menuStep("menu.Step",false,false); | |
- | |
- // Define Camera Render Object (for view / scene browsing) | |
- 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 | |
- 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, Twr; | |
- Twc.SetIdentity(); | |
- pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis | |
- Ow.SetIdentity(); | |
- pangolin::OpenGlMatrix Twwp; // Oriented with g in the z axis, but y and x from camera | |
- Twwp.SetIdentity(); | |
- cv::namedWindow("ORB-SLAM3: Current Frame"); | |
- | |
- bool bFollow = true; | |
- bool bLocalizationMode = false; | |
- bool bStepByStep = false; | |
- bool bCameraView = true; | |
- | |
- if(mpTracker->mSensor == mpSystem->MONOCULAR || mpTracker->mSensor == mpSystem->STEREO || mpTracker->mSensor == mpSystem->RGBD) | |
- { | |
- menuShowGraph = true; | |
- } | |
- | |
- while(1) | |
- { | |
- glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); | |
- | |
- mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc,Ow,Twwp); | |
- | |
- if(mbStopTrack) | |
- { | |
- menuStepByStep = true; | |
- mbStopTrack = false; | |
- } | |
- | |
- if(menuFollowCamera && bFollow) | |
- { | |
- if(bCameraView) | |
- s_cam.Follow(Twc); | |
- else | |
- s_cam.Follow(Ow); | |
- } | |
- else if(menuFollowCamera && !bFollow) | |
- { | |
- if(bCameraView) | |
- { | |
- s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000)); | |
- s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); | |
- s_cam.Follow(Twc); | |
- } | |
- else | |
- { | |
- s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); | |
- s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0)); | |
- s_cam.Follow(Ow); | |
- } | |
- bFollow = true; | |
- } | |
- else if(!menuFollowCamera && bFollow) | |
- { | |
- bFollow = false; | |
- } | |
- | |
- if(menuCamView) | |
- { | |
- menuCamView = false; | |
- bCameraView = true; | |
- s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,10000)); | |
- s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); | |
- s_cam.Follow(Twc); | |
- } | |
- | |
- if(menuTopView && mpMapDrawer->mpAtlas->isImuInitialized()) | |
- { | |
- menuTopView = false; | |
- bCameraView = false; | |
- /*s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); | |
- s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));*/ | |
- s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); | |
- s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0)); | |
- s_cam.Follow(Ow); | |
- } | |
- | |
- /*if(menuSideView && mpMapDrawer->mpAtlas->isImuInitialized()) | |
- { | |
- s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); | |
- s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0.0,0.1,30.0,0,0,0,0.0,0.0,1.0)); | |
- s_cam.Follow(Twwp); | |
- }*/ | |
- | |
- | |
- if(menuLocalizationMode && !bLocalizationMode) | |
- { | |
- mpSystem->ActivateLocalizationMode(); | |
- bLocalizationMode = true; | |
- } | |
- else if(!menuLocalizationMode && bLocalizationMode) | |
- { | |
- mpSystem->DeactivateLocalizationMode(); | |
- bLocalizationMode = false; | |
- } | |
- | |
- if(menuStepByStep && !bStepByStep) | |
- { | |
- mpTracker->SetStepByStep(true); | |
- bStepByStep = true; | |
- } | |
- else if(!menuStepByStep && bStepByStep) | |
- { | |
- mpTracker->SetStepByStep(false); | |
- bStepByStep = false; | |
- } | |
- | |
- if(menuStep) | |
- { | |
- mpTracker->mbStep = true; | |
- menuStep = false; | |
- } | |
- | |
- | |
- d_cam.Activate(s_cam); | |
- glClearColor(1.0f,1.0f,1.0f,1.0f); | |
- mpMapDrawer->DrawCurrentCamera(Twc); | |
- if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph) | |
- mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph); | |
- if(menuShowPoints) | |
- mpMapDrawer->DrawMapPoints(); | |
- | |
- pangolin::FinishFrame(); | |
- | |
- cv::Mat toShow; | |
- cv::Mat im = mpFrameDrawer->DrawFrame(true); | |
- | |
- if(both){ | |
- cv::Mat imRight = mpFrameDrawer->DrawRightFrame(); | |
- cv::hconcat(im,imRight,toShow); | |
- } | |
- else{ | |
- toShow = im; | |
- } | |
- | |
- cv::imshow("ORB-SLAM3: Current Frame",toShow); | |
- cv::waitKey(mT); | |
- | |
- if(menuReset) | |
- { | |
- menuShowGraph = true; | |
- menuShowInertialGraph = true; | |
- menuShowKeyFrames = true; | |
- menuShowPoints = true; | |
- menuLocalizationMode = false; | |
- if(bLocalizationMode) | |
- mpSystem->DeactivateLocalizationMode(); | |
- bLocalizationMode = false; | |
- bFollow = true; | |
- menuFollowCamera = true; | |
- //mpSystem->Reset(); | |
- mpSystem->ResetActiveMap(); | |
- menuReset = false; | |
- } | |
- | |
- if(Stop()) | |
- { | |
- while(isStopped()) | |
- { | |
- usleep(3000); | |
- } | |
- } | |
- | |
- if(CheckFinish()) | |
- break; | |
- } | |
+ // pangolin::CreateWindowAndBind("ORB-SLAM3: 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> menuCamView("menu.Camera View",false,false); | |
+ // pangolin::Var<bool> menuTopView("menu.Top View",false,false); | |
+ // // pangolin::Var<bool> menuSideView("menu.Side View",false,false); | |
+ // 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",false,true); | |
+ // pangolin::Var<bool> menuShowInertialGraph("menu.Show Inertial Graph",true,true); | |
+ // pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true); | |
+ // pangolin::Var<bool> menuReset("menu.Reset",false,false); | |
+ // pangolin::Var<bool> menuStepByStep("menu.Step By Step",false,true); // false, true | |
+ // pangolin::Var<bool> menuStep("menu.Step",false,false); | |
+ | |
+ // // Define Camera Render Object (for view / scene browsing) | |
+ // 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 | |
+ // 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, Twr; | |
+ // Twc.SetIdentity(); | |
+ // pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis | |
+ // Ow.SetIdentity(); | |
+ // pangolin::OpenGlMatrix Twwp; // Oriented with g in the z axis, but y and x from camera | |
+ // Twwp.SetIdentity(); | |
+ // cv::namedWindow("ORB-SLAM3: Current Frame"); | |
+ | |
+ // bool bFollow = true; | |
+ // bool bLocalizationMode = false; | |
+ // bool bStepByStep = false; | |
+ // bool bCameraView = true; | |
+ | |
+ // if(mpTracker->mSensor == mpSystem->MONOCULAR || mpTracker->mSensor == mpSystem->STEREO || mpTracker->mSensor == mpSystem->RGBD) | |
+ // { | |
+ // menuShowGraph = true; | |
+ // } | |
+ | |
+ // while(1) | |
+ // { | |
+ // glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); | |
+ | |
+ // mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc,Ow,Twwp); | |
+ | |
+ // if(mbStopTrack) | |
+ // { | |
+ // menuStepByStep = true; | |
+ // mbStopTrack = false; | |
+ // } | |
+ | |
+ // if(menuFollowCamera && bFollow) | |
+ // { | |
+ // if(bCameraView) | |
+ // s_cam.Follow(Twc); | |
+ // else | |
+ // s_cam.Follow(Ow); | |
+ // } | |
+ // else if(menuFollowCamera && !bFollow) | |
+ // { | |
+ // if(bCameraView) | |
+ // { | |
+ // s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000)); | |
+ // s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); | |
+ // s_cam.Follow(Twc); | |
+ // } | |
+ // else | |
+ // { | |
+ // s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); | |
+ // s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0)); | |
+ // s_cam.Follow(Ow); | |
+ // } | |
+ // bFollow = true; | |
+ // } | |
+ // else if(!menuFollowCamera && bFollow) | |
+ // { | |
+ // bFollow = false; | |
+ // } | |
+ | |
+ // if(menuCamView) | |
+ // { | |
+ // menuCamView = false; | |
+ // bCameraView = true; | |
+ // s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,10000)); | |
+ // s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); | |
+ // s_cam.Follow(Twc); | |
+ // } | |
+ | |
+ // if(menuTopView && mpMapDrawer->mpAtlas->isImuInitialized()) | |
+ // { | |
+ // menuTopView = false; | |
+ // bCameraView = false; | |
+ // /*s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); | |
+ // s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));*/ | |
+ // s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); | |
+ // s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0)); | |
+ // s_cam.Follow(Ow); | |
+ // } | |
+ | |
+ // /*if(menuSideView && mpMapDrawer->mpAtlas->isImuInitialized()) | |
+ // { | |
+ // s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); | |
+ // s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0.0,0.1,30.0,0,0,0,0.0,0.0,1.0)); | |
+ // s_cam.Follow(Twwp); | |
+ // }*/ | |
+ | |
+ | |
+ // if(menuLocalizationMode && !bLocalizationMode) | |
+ // { | |
+ // mpSystem->ActivateLocalizationMode(); | |
+ // bLocalizationMode = true; | |
+ // } | |
+ // else if(!menuLocalizationMode && bLocalizationMode) | |
+ // { | |
+ // mpSystem->DeactivateLocalizationMode(); | |
+ // bLocalizationMode = false; | |
+ // } | |
+ | |
+ // if(menuStepByStep && !bStepByStep) | |
+ // { | |
+ // mpTracker->SetStepByStep(true); | |
+ // bStepByStep = true; | |
+ // } | |
+ // else if(!menuStepByStep && bStepByStep) | |
+ // { | |
+ // mpTracker->SetStepByStep(false); | |
+ // bStepByStep = false; | |
+ // } | |
+ | |
+ // if(menuStep) | |
+ // { | |
+ // mpTracker->mbStep = true; | |
+ // menuStep = false; | |
+ // } | |
+ | |
+ | |
+ // d_cam.Activate(s_cam); | |
+ // glClearColor(1.0f,1.0f,1.0f,1.0f); | |
+ // mpMapDrawer->DrawCurrentCamera(Twc); | |
+ // if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph) | |
+ // mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph); | |
+ // if(menuShowPoints) | |
+ // mpMapDrawer->DrawMapPoints(); | |
+ | |
+ // pangolin::FinishFrame(); | |
+ | |
+ // cv::Mat toShow; | |
+ // cv::Mat im = mpFrameDrawer->DrawFrame(true); | |
+ | |
+ // if(both){ | |
+ // cv::Mat imRight = mpFrameDrawer->DrawRightFrame(); | |
+ // cv::hconcat(im,imRight,toShow); | |
+ // } | |
+ // else{ | |
+ // toShow = im; | |
+ // } | |
+ | |
+ // cv::imshow("ORB-SLAM3: Current Frame",toShow); | |
+ // cv::waitKey(mT); | |
+ | |
+ // if(menuReset) | |
+ // { | |
+ // menuShowGraph = true; | |
+ // menuShowInertialGraph = true; | |
+ // menuShowKeyFrames = true; | |
+ // menuShowPoints = true; | |
+ // menuLocalizationMode = false; | |
+ // if(bLocalizationMode) | |
+ // mpSystem->DeactivateLocalizationMode(); | |
+ // bLocalizationMode = false; | |
+ // bFollow = true; | |
+ // menuFollowCamera = true; | |
+ // //mpSystem->Reset(); | |
+ // mpSystem->ResetActiveMap(); | |
+ // menuReset = false; | |
+ // } | |
+ | |
+ // if(Stop()) | |
+ // { | |
+ // while(isStopped()) | |
+ // { | |
+ // usleep(3000); | |
+ // } | |
+ // } | |
+ | |
+ // if(CheckFinish()) | |
+ // break; | |
+ // } | |
SetFinish(); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment