Merge pull request raulmur#3 from alexmillane/feature/keyframe_publis…

…hing

Feature/keyframe publishing

include/orb_slam_2/DenseMappingInterface.h

@@ -14,6 +14,13 @@ namespace ORB_SLAM2 {
 // A type of a stamped pose
 typedef std::pair<cv::Mat, double> PoseStamped;
 
+// A type containing a keyframe pose, a timestampe and keyframe ID
+struct PoseWithID {
+  cv::Mat pose;
+  double timestamp;
+  long unsigned int id;
+};
+
 class Map;
 
 class DenseMappingInterface {
@@ -22,27 +29,38 @@ class DenseMappingInterface {
 
   // Tells the interface that a global bundle adjustment has occurred.
   void notifyFinishedGBA();
-
   // Functions for getting loop closed trajectories.
   bool isUpdatedTrajectoryAvailable();
-  std::vector<PoseStamped> getUpdatedTrajectory();
+  //std::vector<PoseStamped> getUpdatedTrajectory();
+  std::vector<PoseWithID> getUpdatedTrajectory();
+
+  // Tells interface if the last frame was a keyframe
+  void notifyKeyFrameStatusAvailable(bool keyframe_flag);
+  // Functions for getting last keyframe status
+  bool isKeyFrameStatusAvailable();
+  bool getKeyFrameStatus();
 
  protected:
 
+  // TODO(alexmillane): Replace this with a lambda in the trajectory function
   // Compares two keyframes, returning true if KF1 ID is > KF2 ID. For sorting.
   static bool compareKeyframes(KeyFrame* keyframe1, KeyFrame* keyframe2);
 
   // Map
   Map* mpMap;
 
-  // Flag indicating if a new (unfetched) trajectory is available
+  // Members to do with bundle adjusted trajectory
   bool mbUpdatedTrajectoryAvailable;
+  std::mutex mMutexTrajectory;
+  //std::vector<PoseStamped> mvPoseTrajectory;
+  std::vector<PoseWithID> mvPoseTrajectory;
 
-  // The latest loop closed trajectory stored here
-  std::vector<PoseStamped> mvPoseTrajectory;
 
-  // A mutex which locks the trajectory for read/write
-  std::mutex mMutexTrajectory;
+  // Members to do with keyframe status
+  bool mbKeyFrameStatusAvailable;
+  std::mutex mMutexKeyFrameStatus;
+  bool mbKeyFrameStatus;
+
 };
 
 }  // namespace ORB_SLAM

include/orb_slam_2/System.h

@@ -65,21 +65,22 @@ class System {
   // grayscale.
   // Returns the camera pose (empty if tracking fails).
   cv::Mat TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
-                      const double& timestamp);
+                      const double& timestamp, bool* keyframe_flag,
+                      bool* big_change_flag);
 
   // Process the given rgbd frame. Depthmap must be registered to the RGB frame.
   // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to
   // grayscale.
   // Input depthmap: Float (CV_32F).
   // Returns the camera pose (empty if tracking fails).
   cv::Mat TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
-                    const double& timestamp);
+                    const double& timestamp, bool* keyframe_flag);
 
   // Proccess the given monocular frame
   // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to
   // grayscale.
   // Returns the camera pose (empty if tracking fails).
-  cv::Mat TrackMonocular(const cv::Mat& im, const double& timestamp);
+  cv::Mat TrackMonocular(const cv::Mat& im, const double& timestamp, bool* keyframe_flag);
 
   // This stops local mapping thread (map building) and performs only camera
   // tracking.
@@ -130,7 +131,14 @@ class System {
 
   // Functions related to trajectory publishing
   bool isUpdatedTrajectoryAvailable();
-  std::vector<PoseStamped> GetUpdatedTrajectory();
+  std::vector<PoseWithID> GetUpdatedTrajectory();
+
+  // Functions related to trajectory publishing
+  bool isKeyFrameStatusAvailable();
+  bool getKeyFrameStatus();
+
+  // Returns the last frame 
+  long unsigned int getLastKeyFrameID();
 
  private:
   // Input sensor

include/orb_slam_2/Tracking.h

@@ -58,9 +58,9 @@ class Tracking
              KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
 
     // Preprocess the input and call Track(). Extract features and performs stereo matching.
-    cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double &timestamp);
-    cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp);
-    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);
+    cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double &timestamp, bool* keyframe_indicator, bool* big_change_indicator);
+    cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp, bool* keyframe_indicator);
+    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp, bool* keyframe_indicator);
 
     void SetLocalMapper(LocalMapping* pLocalMapper);
     void SetLoopClosing(LoopClosing* pLoopClosing);
@@ -113,12 +113,15 @@ class Tracking
     // True if local mapping is deactivated and we are performing only localization
     bool mbOnlyTracking;
 
+    // Returns the last frame 
+    long unsigned int getLastKeyFrameID();
+
     void Reset();
 
 protected:
 
     // Main tracking function. It is independent of the input sensor.
-    void Track();
+    void Track(bool* keyframe_indicator);
 
     // Map initialization for stereo and RGB-D
     void StereoInitialization();

src/DenseMappingInterface.cc

@@ -19,23 +19,23 @@ void DenseMappingInterface::notifyFinishedGBA() {
   unique_lock<mutex> lock(mMutexTrajectory);
   // Getting the keyframes in the map
   vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
-
   // Sort the keyframes according to their ID
+  // TODO(alex.millane:) Do this with a lambda.
   std::sort(vpKFs.begin(), vpKFs.end(), compareKeyframes);
-
   // Storing the keyframe poses and timestamps
   mvPoseTrajectory.clear();
   mvPoseTrajectory.reserve(vpKFs.size());
   for (size_t i = 0; i < vpKFs.size(); i++) {
     // Extracting keyframe
     KeyFrame* pKF = vpKFs[i];
-    // Extracting and storing pose
-    cv::Mat Twc = pKF->GetPose();
-    double timestamp = pKF->mTimeStamp;
-    PoseStamped post_stamped(Twc, timestamp);
-    mvPoseTrajectory.push_back(post_stamped);
+    // Extracting and storing pose with ID
+    PoseWithID pose_with_id;
+    pose_with_id.pose = pKF->GetPose();
+    pose_with_id.timestamp = pKF->mTimeStamp;
+    pose_with_id.id = pKF->mnId;
+    // Storing this pose
+    mvPoseTrajectory.push_back(pose_with_id);
   }
-
   // Indicating the trajectory is ready for retreival
   mbUpdatedTrajectoryAvailable = true;
 }
@@ -44,16 +44,30 @@ bool DenseMappingInterface::isUpdatedTrajectoryAvailable() {
   return mbUpdatedTrajectoryAvailable;
 }
 
-std::vector<PoseStamped> DenseMappingInterface::getUpdatedTrajectory() {
-  // Locking the trajectory to prevent modification on copy
+std::vector<PoseWithID> DenseMappingInterface::getUpdatedTrajectory() {
   mbUpdatedTrajectoryAvailable = false;
   unique_lock<mutex> lock(mMutexTrajectory);
   return mvPoseTrajectory;
 }
 
+void DenseMappingInterface::notifyKeyFrameStatusAvailable(bool keyframe_flag) {
+  // Saving the keyframe status
+  mbKeyFrameStatus = keyframe_flag;
+  // Indicating a new status is available
+  mbKeyFrameStatusAvailable = true;
+}
+
+bool DenseMappingInterface::isKeyFrameStatusAvailable() {
+  return mbKeyFrameStatusAvailable;
+}
+bool DenseMappingInterface::getKeyFrameStatus() {
+  unique_lock<mutex> lock(mMutexKeyFrameStatus);
+  mbKeyFrameStatusAvailable = false;
+  return mbKeyFrameStatus;
+}
+
 bool DenseMappingInterface::compareKeyframes(KeyFrame* keyframe_1, KeyFrame* keyframe_2) {
     return (keyframe_1->mnId) < (keyframe_2->mnId);
 }
 
-
 }  // namespace ORB_SLAM

src/KeyFrame.cc

@@ -463,6 +463,9 @@ void KeyFrame::SetBadFlag()
         }
     }
 
+    // DEBUG OUTPUT
+    std::cout << "Keyframe being erased with ID: " << mnId << std::endl;
+
     for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
         mit->first->EraseConnection(this);
 

src/System.cc

@@ -90,7 +90,7 @@ System::System(const string &strVocFile, const string &strSettingsFile,
 
   // Initialize the Tracking thread
   //(it will live in the main thread of execution, the one that called this
-  //constructor)
+  // constructor)
   mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                            mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
 
@@ -123,7 +123,8 @@ System::System(const string &strVocFile, const string &strSettingsFile,
 }
 
 cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight,
-                            const double &timestamp) {
+                            const double &timestamp, bool *keyframe_flag,
+                            bool *big_change_flag) {
   if (mSensor != STEREO) {
     cerr << "ERROR: you called TrackStereo but input sensor was not set to "
             "STEREO."
@@ -161,7 +162,8 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight,
     }
   }
 
-  cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft, imRight, timestamp);
+  cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft, imRight, timestamp,
+                                           keyframe_flag, big_change_flag);
 
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;
@@ -171,7 +173,7 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight,
 }
 
 cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap,
-                          const double &timestamp) {
+                          const double &timestamp, bool* keyframe_flag) {
   if (mSensor != RGBD) {
     cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD."
          << endl;
@@ -208,7 +210,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap,
     }
   }
 
-  cv::Mat Tcw = mpTracker->GrabImageRGBD(im, depthmap, timestamp);
+  cv::Mat Tcw = mpTracker->GrabImageRGBD(im, depthmap, timestamp, keyframe_flag);
 
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;
@@ -217,7 +219,7 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap,
   return Tcw;
 }
 
-cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {
+cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, bool* keyframe_flag) {
   if (mSensor != MONOCULAR) {
     cerr << "ERROR: you called TrackMonocular but input sensor was not set to "
             "Monocular."
@@ -255,7 +257,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {
     }
   }
 
-  cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);
+  cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp, keyframe_flag);
 
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;
@@ -481,8 +483,20 @@ bool System::isUpdatedTrajectoryAvailable() {
   return mpDenseMappingInterface->isUpdatedTrajectoryAvailable();
 }
 
-std::vector<PoseStamped> System::GetUpdatedTrajectory() {
+std::vector<PoseWithID> System::GetUpdatedTrajectory() {
   return mpDenseMappingInterface->getUpdatedTrajectory();
 }
 
+bool System::isKeyFrameStatusAvailable() {
+  return mpDenseMappingInterface->isKeyFrameStatusAvailable();
+}
+
+bool System::getKeyFrameStatus() {
+  return mpDenseMappingInterface->getKeyFrameStatus();
+}
+
+long unsigned int System::getLastKeyFrameID() {
+  return mpTracker->getLastKeyFrameID();
+}
+
 }  // namespace ORB_SLAM

src/Tracking.cc

@@ -164,7 +164,7 @@ void Tracking::SetViewer(Viewer *pViewer)
 }
 
 
-cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp)
+cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp, bool* keyframe_indicator, bool* big_change_indicator)
 {
     mImGray = imRectLeft;
     cv::Mat imGrayRight = imRectRight;
@@ -198,13 +198,25 @@ cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRe
 
     mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
 
-    Track();
+    Track(keyframe_indicator);
+
+    // TODO(alex.millane): hacky place to do this but whatever
+    static int nBigChanges = 0;
+    if (nBigChanges < mpMap->GetLastBigChangeIdx()) {
+        nBigChanges = mpMap->GetLastBigChangeIdx();
+        *big_change_indicator = true;
+        std::cout << "Big change detected" << std::endl;
+    }
+    else {
+        *big_change_indicator = false;
+    }
+
 
     return mCurrentFrame.mTcw.clone();
 }
 
 
-cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp)
+cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp, bool* keyframe_indicator)
 {
     mImGray = imRGB;
     cv::Mat imDepth = imD;
@@ -229,13 +241,13 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d
 
     mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
 
-    Track();
+    Track(keyframe_indicator);
 
     return mCurrentFrame.mTcw.clone();
 }
 
 
-cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
+cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, bool* keyframe_indicator)
 {
     mImGray = im;
 
@@ -259,12 +271,12 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
     else
         mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
 
-    Track();
+    Track(keyframe_indicator);
 
     return mCurrentFrame.mTcw.clone();
 }
 
-void Tracking::Track()
+void Tracking::Track(bool* keyframe_indicator)
 {
     if(mState==NO_IMAGES_YET)
     {
@@ -454,8 +466,12 @@ void Tracking::Track()
             mlpTemporalPoints.clear();
 
             // Check if we need to insert a new keyframe
-            if(NeedNewKeyFrame())
+            if(NeedNewKeyFrame()) {
                 CreateNewKeyFrame();
+                *keyframe_indicator = true;
+            } else {
+                *keyframe_indicator = false;
+            }
 
             // We allow points with high innovation (considererd outliers by the Huber Function)
             // pass to the new keyframe, so that bundle adjustment will finally decide
@@ -1587,6 +1603,8 @@ void Tracking::InformOnlyTracking(const bool &flag)
     mbOnlyTracking = flag;
 }
 
-
+long unsigned int Tracking::getLastKeyFrameID() {
+  return mpLastKeyFrame->mnId;
+}
 
 } //namespace ORB_SLAM