diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..dd3e3a4eb6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,26 @@ +CMakeLists.txt.user +Examples/Monocular/mono_euroc +Examples/Monocular/mono_kitti +Examples/Monocular/mono_tum +Examples/RGB-D/rgbd_tum +Examples/ROS/ORB_SLAM2/CMakeLists.txt.user +Examples/ROS/ORB_SLAM2/Mono +Examples/ROS/ORB_SLAM2/MonoAR +Examples/ROS/ORB_SLAM2/RGBD +Examples/ROS/ORB_SLAM2/Stereo +Examples/ROS/ORB_SLAM2/build/ +Examples/ROS/ORB_SLAM2/src/AR/ +Examples/Stereo/stereo_euroc +Examples/Stereo/stereo_kitti +KeyFrameTrajectory.txt +Thirdparty/DBoW2/build/ +Thirdparty/DBoW2/lib/ +Thirdparty/g2o/build/ +Thirdparty/g2o/config.h +Thirdparty/g2o/lib/ +Vocabulary/ORBvoc.txt +build/ +build_ros.sh +*~ +lib/ + diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt index 3c032fe8ab..9c02407db4 100644 --- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt +++ b/Examples/ROS/ORB_SLAM2/CMakeLists.txt @@ -59,6 +59,17 @@ target_link_libraries(Mono ${LIBS} ) +# Node for monocular camera (Augmented Reality Demo) +rosbuild_add_executable(MonoAR +src/AR/ros_mono_ar.cc +src/AR/ViewerAR.h +src/AR/ViewerAR.cc +) + +target_link_libraries(MonoAR +${LIBS} +) + # Node for stereo camera rosbuild_add_executable(Stereo src/ros_stereo.cc diff --git a/include/Map.h b/include/Map.h index 49ffab7374..a75339feea 100644 --- a/include/Map.h +++ b/include/Map.h @@ -45,6 +45,8 @@ class Map void EraseMapPoint(MapPoint* pMP); void EraseKeyFrame(KeyFrame* pKF); void SetReferenceMapPoints(const std::vector &vpMPs); + void InformNewBigChange(); + int GetLastBigChangeIdx(); std::vector GetAllKeyFrames(); std::vector GetAllMapPoints(); @@ -72,6 +74,9 @@ class Map long unsigned int mnMaxKFid; + // Index related to a big change in the map (loop closure, global BA) + int mnBigChangeIdx; + std::mutex mMutexMap; }; diff --git a/include/System.h b/include/System.h index 59f4491e90..b377b453d1 100644 --- a/include/System.h +++ b/include/System.h @@ -82,6 +82,10 @@ class System // This resumes local mapping thread and performs SLAM again. void DeactivateLocalizationMode(); + // Returns true if there have been a big map change (loop closure, global BA) + // since last call to this function + bool MapChanged(); + // Reset the system (clear map) void Reset(); @@ -112,6 +116,12 @@ class System // SaveMap(const string &filename); // LoadMap(const string &filename); + // Information from most recent processed frame + // You can call this right after TrackMonocular (or stereo or RGBD) + int GetTrackingState(); + std::vector GetTrackedMapPoints(); + std::vector GetTrackedKeyPointsUn(); + private: // Input sensor @@ -158,6 +168,12 @@ class System std::mutex mMutexMode; bool mbActivateLocalizationMode; bool mbDeactivateLocalizationMode; + + // Tracking state + int mTrackingState; + std::vector mTrackedMapPoints; + std::vector mTrackedKeyPointsUn; + std::mutex mMutexState; }; }// namespace ORB_SLAM diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index ef028a9464..5e317dd420 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -566,6 +566,8 @@ void LoopClosing::CorrectLoop() // Optimize graph Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale); + mpMap->InformNewBigChange(); + // Add loop edge mpMatchedKF->AddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpMatchedKF); @@ -579,8 +581,6 @@ void LoopClosing::CorrectLoop() // Loop closed. Release Local Mapping. mpLocalMapper->Release(); - cout << "Loop Closed!" << endl; - mLastLoopKFid = mpCurrentKF->mnId; } @@ -734,7 +734,9 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) pMP->SetWorldPos(Rwc*Xc+twc); } - } + } + + mpMap->InformNewBigChange(); mpLocalMapper->Release(); diff --git a/src/Map.cc b/src/Map.cc index f285161644..15fcd86914 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -25,7 +25,7 @@ namespace ORB_SLAM2 { -Map::Map():mnMaxKFid(0) +Map::Map():mnMaxKFid(0),mnBigChangeIdx(0) { } @@ -67,6 +67,18 @@ void Map::SetReferenceMapPoints(const vector &vpMPs) mvpReferenceMapPoints = vpMPs; } +void Map::InformNewBigChange() +{ + unique_lock lock(mMutexMap); + mnBigChangeIdx++; +} + +int Map::GetLastBigChangeIdx() +{ + unique_lock lock(mMutexMap); + return mnBigChangeIdx; +} + vector Map::GetAllKeyFrames() { unique_lock lock(mMutexMap); diff --git a/src/System.cc b/src/System.cc index fce8b65b80..48586e406d 100644 --- a/src/System.cc +++ b/src/System.cc @@ -30,7 +30,7 @@ namespace ORB_SLAM2 { System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, - const bool bUseViewer):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(false), + const bool bUseViewer):mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) { // Output welcome message @@ -95,11 +95,12 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser); //Initialize the Viewer thread and launch - mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile); if(bUseViewer) + { + mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile); mptViewer = new thread(&Viewer::Run, mpViewer); - - mpTracker->SetViewer(mpViewer); + mpTracker->SetViewer(mpViewer); + } //Set pointers between threads mpTracker->SetLocalMapper(mpLocalMapper); @@ -154,7 +155,13 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const } } - return mpTracker->GrabImageStereo(imLeft,imRight,timestamp); + cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp); + + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + return Tcw; } cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) @@ -199,7 +206,13 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub } } - return mpTracker->GrabImageRGBD(im,depthmap,timestamp); + cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp); + + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + return Tcw; } cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) @@ -244,7 +257,14 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) } } - return mpTracker->GrabImageMonocular(im,timestamp); + cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp); + + unique_lock lock2(mMutexState); + mTrackingState = mpTracker->mState; + mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; + mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + + return Tcw; } void System::ActivateLocalizationMode() @@ -259,6 +279,19 @@ void System::DeactivateLocalizationMode() mbDeactivateLocalizationMode = true; } +bool System::MapChanged() +{ + static int n=0; + int curn = mpMap->GetLastBigChangeIdx(); + if(n lock(mMutexReset); @@ -269,7 +302,8 @@ void System::Shutdown() { mpLocalMapper->RequestFinish(); mpLoopCloser->RequestFinish(); - mpViewer->RequestFinish(); + if(mpViewer) + mpViewer->RequestFinish(); // Wait until all thread have effectively stopped while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || @@ -433,4 +467,22 @@ void System::SaveTrajectoryKITTI(const string &filename) cout << endl << "trajectory saved!" << endl; } +int System::GetTrackingState() +{ + unique_lock lock(mMutexState); + return mTrackingState; +} + +vector System::GetTrackedMapPoints() +{ + unique_lock lock(mMutexState); + return mTrackedMapPoints; +} + +vector System::GetTrackedKeyPointsUn() +{ + unique_lock lock(mMutexState); + return mTrackedKeyPointsUn; +} + } //namespace ORB_SLAM diff --git a/src/Tracking.cc b/src/Tracking.cc index 1c3130a4fd..2273b2ce48 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -45,7 +45,7 @@ namespace ORB_SLAM2 Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor): mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), - mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), + mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) { // Load camera parameters from settings file @@ -1503,11 +1503,14 @@ bool Tracking::Relocalization() void Tracking::Reset() { - mpViewer->RequestStop(); cout << "System Reseting" << endl; - while(!mpViewer->isStopped()) - usleep(3000); + if(mpViewer) + { + mpViewer->RequestStop(); + while(!mpViewer->isStopped()) + usleep(3000); + } // Reset Local Mapping cout << "Reseting Local Mapper..."; @@ -1542,7 +1545,8 @@ void Tracking::Reset() mlFrameTimes.clear(); mlbLost.clear(); - mpViewer->Release(); + if(mpViewer) + mpViewer->Release(); } void Tracking::ChangeCalibration(const string &strSettingPath) diff --git a/src/Viewer.cc b/src/Viewer.cc index 92fa8468cc..dec3204f53 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -28,7 +28,7 @@ namespace ORB_SLAM2 Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath): mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking), - mbFinishRequested(false), mbFinished(true), mbStopped(false), mbStopRequested(false) + mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false) { cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); @@ -54,6 +54,7 @@ Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer void Viewer::Run() { mbFinished = false; + mbStopped = false; pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768);