octomap visualization

include/MapDrawer.h

@@ -47,12 +47,16 @@ class MapDrawer
     void SetCurrentPath(const std::vector<std::vector<double>> &bPath);
     void DrawPath();
     void SetCurrentCounter(int counter);
+    void DrawMapCollision();
+    void SetCurrentCollision(const std::vector<std::vector<float>> &bCollision);
+
 
 
 
 
 private:
     std::vector<std::vector<double>> mPath;
+    std::vector<std::vector<float>> mCollisionPts;
     float mKeyFrameSize;
     float mKeyFrameLineWidth;
     float mGraphLineWidth;
@@ -69,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_<float>(4,4) << 0, -1, 0, -0.1, //-0.1,
                         0, 0, -1, 0,
                         1,0, 0, -0.22, //-0.22,
@@ -78,6 +84,8 @@ class MapDrawer
     std::mutex mMutexPath;
 
     std::mutex mMutexCounter;
+
+    std::mutex mMutexCollision;
 };
 
 } //namespace ORB_SLAM

include/Tracking.h

@@ -83,6 +83,8 @@ class Tracking
     // Get Scale Factor
     float GetDepthScaleFactor();
 
+    void UpdateCollision(const std::vector<std::vector<float>> &bCollision);
+
 
 public:
 

src/MapDrawer.cc

@@ -80,6 +80,40 @@ void MapDrawer::DrawMapPoints()
 
     glEnd();
 }
+void MapDrawer::SetCurrentCollision(const std::vector<std::vector<float>> &bCollision)
+{
+    unique_lock<mutex> lock(mMutexCollision);
+    mCollisionPts=bCollision;
+    //std::cout<<"Octomap Updated in Drawer"<<std::endl;
+}
+
+void MapDrawer::DrawMapCollision()
+{
+    //const vector<MapPoint*> &vpMPs = mpMap->GetAllMapPoints();
+    //const vector<MapPoint*> &vpRefMPs = mpMap->GetReferenceMapPoints();
+
+    //set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
+    unique_lock<mutex> lock(mMutexCollision);
+
+    if(mCollisionPts.empty())
+        return;
+
+    int rows = static_cast<int>(mCollisionPts.size());
+    glPointSize(9*mPointSize);
+    glBegin(GL_POINTS);
+    glColor3f(0.0,0.0,1.0);
+
+    for(int i=0; i<rows; i++)
+    {   
+        cv::Mat P_w=(cv::Mat_<float>(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<float>(0,0),P_s.at<float>(1,0),P_s.at<float>(2,0));
+    }
+    glEnd();
+}
 
 void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph)
 {

src/System.cc

@@ -348,6 +348,8 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub
             mpOctomapBuilder->UpdateOctomap(depthmap, currPose);
             vector<vector<float>> occupiedPoints = mpOctomapBuilder->getOccupiedPoints();
             cout << "occupancy grid size = " << occupiedPoints.size() << endl;
+
+            mpTracker->UpdateCollision(occupiedPoints);
             if(occupiedPoints.size()!=0)
                 octomapInitialize = true;
         }

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<std::vector<float>> &bCollision)
+{
+    this->mpMapDrawer->SetCurrentCollision(bCollision);
+}
 
 
 } //namespace ORB_SLAM

src/Viewer.cc

@@ -133,6 +133,7 @@ void Viewer::Run()
             mpMapDrawer->DrawMapPoints();
 
         mpMapDrawer->DrawPath();
+        mpMapDrawer->DrawMapCollision();
 
         pangolin::FinishFrame();