From 39c6c22aace30b0ff75fbdad6595c05ba158110e Mon Sep 17 00:00:00 2001 From: zzx9636 Date: Sat, 9 Sep 2017 14:18:31 -0500 Subject: [PATCH 1/2] add drawer for octomap --- include/MapDrawer.h | 6 ++++++ src/MapDrawer.cc | 27 +++++++++++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/include/MapDrawer.h b/include/MapDrawer.h index 5591476..46e6714 100644 --- a/include/MapDrawer.h +++ b/include/MapDrawer.h @@ -47,12 +47,16 @@ class MapDrawer void SetCurrentPath(const std::vector> &bPath); void DrawPath(); void SetCurrentCounter(int counter); + void DrawMapCollision(); + void SetCurrentCollision(const std::vector> &bCollision); + private: std::vector> mPath; + std::vector> mCollisionPts; float mKeyFrameSize; float mKeyFrameLineWidth; float mGraphLineWidth; @@ -78,6 +82,8 @@ class MapDrawer std::mutex mMutexPath; std::mutex mMutexCounter; + + std::mutex mMutexCollision; }; } //namespace ORB_SLAM diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc index 48bea4d..437cfcd 100644 --- a/src/MapDrawer.cc +++ b/src/MapDrawer.cc @@ -80,6 +80,33 @@ void MapDrawer::DrawMapPoints() glEnd(); } +void MapDrawer::SetCurrentCollision(const std::vector> &bCollision) +{ + unique_lock lock(mMutexCollision); + mCollisionPts=bCollision; +} + +void MapDrawer::DrawMapCollision() +{ + //const vector &vpMPs = mpMap->GetAllMapPoints(); + //const vector &vpRefMPs = mpMap->GetReferenceMapPoints(); + + //set spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); + unique_lock lock(mMutexCollision); + + if(mCollisionPts.empty()) + return; + + glPointSize(3*mPointSize); + glBegin(GL_POINTS); + glColor3f(0.0,0.0,1.0); + + for(auto & it : mCollisionPts) + { + glVertex3f(it[0],it[1],it[2]); + } + glEnd(); +} void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) { From 1f90b9a5db70258f155f72b3bfdc915149145f84 Mon Sep 17 00:00:00 2001 From: zzx9636 Date: Sat, 9 Sep 2017 15:35:25 -0500 Subject: [PATCH 2/2] add visualization of collision --- include/MapDrawer.h | 6 ++++-- include/Tracking.h | 2 ++ src/MapDrawer.cc | 17 ++++++++++++----- src/System.cc | 1 + src/Tracking.cc | 10 ++++++++-- src/Viewer.cc | 1 + 6 files changed, 28 insertions(+), 9 deletions(-) diff --git a/include/MapDrawer.h b/include/MapDrawer.h index 46e6714..f7e5df7 100644 --- a/include/MapDrawer.h +++ b/include/MapDrawer.h @@ -48,7 +48,7 @@ class MapDrawer void DrawPath(); void SetCurrentCounter(int counter); void DrawMapCollision(); - void SetCurrentCollision(const std::vector> &bCollision); + void SetCurrentCollision(const std::vector> &bCollision); @@ -56,7 +56,7 @@ class MapDrawer private: std::vector> mPath; - std::vector> mCollisionPts; + std::vector> mCollisionPts; float mKeyFrameSize; float mKeyFrameLineWidth; float mGraphLineWidth; @@ -73,6 +73,8 @@ class MapDrawer 0,-1, 0, 0, 0, 0, 0, 1); + cv::Mat T_sw_mat=(T_ws_mat.inv()); + cv::Mat T_cb_mat = (cv::Mat_(4,4) << 0, -1, 0, -0.1, //-0.1, 0, 0, -1, 0, 1,0, 0, -0.22, //-0.22, diff --git a/include/Tracking.h b/include/Tracking.h index 737bdb7..4b2f4fc 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -83,6 +83,8 @@ class Tracking // Get Scale Factor float GetDepthScaleFactor(); + void UpdateCollision(const std::vector> &bCollision); + public: diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc index 437cfcd..46487b0 100644 --- a/src/MapDrawer.cc +++ b/src/MapDrawer.cc @@ -80,10 +80,11 @@ void MapDrawer::DrawMapPoints() glEnd(); } -void MapDrawer::SetCurrentCollision(const std::vector> &bCollision) +void MapDrawer::SetCurrentCollision(const std::vector> &bCollision) { unique_lock lock(mMutexCollision); mCollisionPts=bCollision; + //std::cout<<"Octomap Updated in Drawer"<(mCollisionPts.size()); + glPointSize(9*mPointSize); glBegin(GL_POINTS); glColor3f(0.0,0.0,1.0); - for(auto & it : mCollisionPts) - { - glVertex3f(it[0],it[1],it[2]); + for(int i=0; i(4,1) << mCollisionPts[i][0], + mCollisionPts[i][1], + mCollisionPts[i][2], + 1); + cv::Mat P_s=T_sw_mat*P_w; + glVertex3f(P_s.at(0,0),P_s.at(1,0),P_s.at(2,0)); } glEnd(); } diff --git a/src/System.cc b/src/System.cc index 829ad64..db2613b 100644 --- a/src/System.cc +++ b/src/System.cc @@ -347,6 +347,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub if (mpTracker->mbKeyframe || !octomapInitialize) { mpOctomapBuilder->UpdateOctomap(depthmap, currPose); vector> occupiedPoints = mpOctomapBuilder->getOccupiedPoints(); + mpTracker->UpdateCollision(occupiedPoints); //cout << occupiedPoints.size() << endl; if(occupiedPoints.size()!=0) octomapInitialize = true; diff --git a/src/Tracking.cc b/src/Tracking.cc index fa2e7cf..3e0fe4e 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -261,14 +261,14 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d } } - +/* // recover from the LOST state if(mState==LOST){ recoverCounter++; if(recoverCounter > 100){ recoverMode = true; - cout << "start recover mode" << endl; + //cout << "start recover mode" << endl; } }else if (mState==OK) { @@ -303,6 +303,8 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d recoverMode = false; //cout << "not in recover mode" << endl; } + + */ return mCurrentFrame.mTcw.clone(); } @@ -1957,6 +1959,10 @@ bool Tracking::checkWayPointRecover(){ } +void Tracking::UpdateCollision(const std::vector> &bCollision) +{ + this->mpMapDrawer->SetCurrentCollision(bCollision); +} } //namespace ORB_SLAM diff --git a/src/Viewer.cc b/src/Viewer.cc index 3c2a42c..854501b 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -133,6 +133,7 @@ void Viewer::Run() mpMapDrawer->DrawMapPoints(); mpMapDrawer->DrawPath(); + mpMapDrawer->DrawMapCollision(); pangolin::FinishFrame();