Skip to content

Instantly share code, notes, and snippets.

@RomanVolkov

RomanVolkov/patch.patch

Last active September 23, 2021 11:33
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save RomanVolkov/c93bd36c4744286dd66b15a72399d7ac to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save RomanVolkov/c93bd36c4744286dd66b15a72399d7ac to your computer and use it in GitHub Desktop.
Download ZIP
Raw
patch.patch
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