diff --git a/include/orb_slam_2/DenseMappingInterface.h b/include/orb_slam_2/DenseMappingInterface.h index b3748e1ae6..3677868e41 100644 --- a/include/orb_slam_2/DenseMappingInterface.h +++ b/include/orb_slam_2/DenseMappingInterface.h @@ -14,6 +14,13 @@ namespace ORB_SLAM2 { // A type of a stamped pose typedef std::pair 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 getUpdatedTrajectory(); + //std::vector getUpdatedTrajectory(); + std::vector 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 mvPoseTrajectory; + std::vector mvPoseTrajectory; - // The latest loop closed trajectory stored here - std::vector 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 diff --git a/include/orb_slam_2/System.h b/include/orb_slam_2/System.h index ad2ff9432c..0beb6038c3 100644 --- a/include/orb_slam_2/System.h +++ b/include/orb_slam_2/System.h @@ -65,7 +65,8 @@ 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 @@ -73,13 +74,13 @@ class System { // 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 GetUpdatedTrajectory(); + std::vector GetUpdatedTrajectory(); + + // Functions related to trajectory publishing + bool isKeyFrameStatusAvailable(); + bool getKeyFrameStatus(); + + // Returns the last frame + long unsigned int getLastKeyFrameID(); private: // Input sensor diff --git a/include/orb_slam_2/Tracking.h b/include/orb_slam_2/Tracking.h index 0acdc0f103..5c0b89f468 100644 --- a/include/orb_slam_2/Tracking.h +++ b/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 ×tamp); - cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp); - cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp); + cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp, bool* keyframe_indicator, bool* big_change_indicator); + cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, bool* keyframe_indicator); + cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp, 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(); diff --git a/src/DenseMappingInterface.cc b/src/DenseMappingInterface.cc index 2ebc36f0b7..e904232ce9 100644 --- a/src/DenseMappingInterface.cc +++ b/src/DenseMappingInterface.cc @@ -19,23 +19,23 @@ void DenseMappingInterface::notifyFinishedGBA() { unique_lock lock(mMutexTrajectory); // Getting the keyframes in the map vector 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 DenseMappingInterface::getUpdatedTrajectory() { - // Locking the trajectory to prevent modification on copy +std::vector DenseMappingInterface::getUpdatedTrajectory() { mbUpdatedTrajectoryAvailable = false; unique_lock 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 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 \ No newline at end of file diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 16fee38eef..04f7bd3e62 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -463,6 +463,9 @@ void KeyFrame::SetBadFlag() } } + // DEBUG OUTPUT + std::cout << "Keyframe being erased with ID: " << mnId << std::endl; + for(map::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) mit->first->EraseConnection(this); diff --git a/src/System.cc b/src/System.cc index d974a7129e..76e4314584 100644 --- a/src/System.cc +++ b/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 ×tamp) { + const double ×tamp, 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 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 ×tamp) { + const double ×tamp, 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 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 ×tamp) { +cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp, 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 ×tamp) { } } - cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp); + cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp, keyframe_flag); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -481,8 +483,20 @@ bool System::isUpdatedTrajectoryAvailable() { return mpDenseMappingInterface->isUpdatedTrajectoryAvailable(); } -std::vector System::GetUpdatedTrajectory() { +std::vector 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 diff --git a/src/Tracking.cc b/src/Tracking.cc index 13e4bb2fbc..fa21c1439f 100644 --- a/src/Tracking.cc +++ b/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 ×tamp) +cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp, 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 ×tamp) +cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, 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 ×tamp) +cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, bool* keyframe_indicator) { mImGray = im; @@ -259,12 +271,12 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) 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