From cb63a7cb2c5f8a1bc4f3efea7c9ee823758600a9 Mon Sep 17 00:00:00 2001 From: Alkaid Date: Fri, 14 Jul 2017 02:13:46 +0800 Subject: [PATCH] add map save/load function update README.md for map save/load and binary vocabulary format just use boost to serialization to handle complicated pointer reference graph. --- CMakeLists.txt | 10 +++ Examples/Monocular/TUM1.yaml | 4 + Examples/Monocular/mono_tum.cc | 6 +- README.md | 59 ++++++++++++-- Thirdparty/DBoW2/DBoW2/BowVector.h | 2 +- Thirdparty/DBoW2/DBoW2/FeatureVector.h | 2 +- Thirdparty/g2o/g2o/core/base_binary_edge.h | 4 +- Thirdparty/g2o/g2o/core/base_multi_edge.h | 2 +- Thirdparty/g2o/g2o/core/base_vertex.h | 2 +- include/BoostArchiver.h | 95 ++++++++++++++++++++++ include/FrameDrawer.h | 2 +- include/KeyFrame.h | 12 ++- include/KeyFrameDatabase.h | 16 +++- include/LoopClosing.h | 2 +- include/Map.h | 8 ++ include/MapPoint.h | 10 +++ include/System.h | 19 +++-- include/Tracking.h | 3 +- include/Viewer.h | 6 +- src/FrameDrawer.cc | 7 +- src/KeyFrame.cc | 84 +++++++++++++++++++ src/KeyFrameDatabase.cc | 17 +++- src/Map.cc | 13 +++ src/MapPoint.cc | 41 +++++++++- src/ORBmatcher.cc | 2 + src/System.cc | 76 +++++++++++++++-- src/Tracking.cc | 8 +- src/Viewer.cc | 6 +- 28 files changed, 464 insertions(+), 54 deletions(-) create mode 100644 include/BoostArchiver.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 08a8af468e..421a33dfc8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,13 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so ) +message(STATUS "Compile With map save/load function") +find_library(BOOST_SERIALIZATION boost_serialization) +if (NOT BOOST_SERIALIZATION) + message(FATAL_ERROR "Can't find libboost_serialization") +endif() +target_link_libraries(${PROJECT_NAME} ${BOOST_SERIALIZATION}) + # Build examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) @@ -111,3 +118,6 @@ add_executable(mono_euroc Examples/Monocular/mono_euroc.cc) target_link_libraries(mono_euroc ${PROJECT_NAME}) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Vocabulary) +add_executable(bin_vocabulary Vocabulary/bin_vocabulary.cpp) +target_link_libraries(bin_vocabulary ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so ${OpenCV_LIBS}) diff --git a/Examples/Monocular/TUM1.yaml b/Examples/Monocular/TUM1.yaml index 145f792f43..49600bb76a 100644 --- a/Examples/Monocular/TUM1.yaml +++ b/Examples/Monocular/TUM1.yaml @@ -56,3 +56,7 @@ Viewer.ViewpointY: -0.7 Viewer.ViewpointZ: -1.8 Viewer.ViewpointF: 500 +#-------------------------------------------------------------------------------------------- +# Map Parapeters +#-------------------------------------------------------------------------------------------- +Map.mapfile: map.bin diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc index 75ccb3ee31..3d14cb011b 100644 --- a/Examples/Monocular/mono_tum.cc +++ b/Examples/Monocular/mono_tum.cc @@ -35,9 +35,9 @@ void LoadImages(const string &strFile, vector &vstrImageFilenames, int main(int argc, char **argv) { - if(argc != 4) + if(argc != 5) { - cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl; + cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence [1|0](save map?)" << endl; return 1; } @@ -50,7 +50,7 @@ int main(int argc, char **argv) int nImages = vstrImageFilenames.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true, (bool)atoi(argv[4])); // Vector for tracking time statistics vector vTimesTrack; diff --git a/README.md b/README.md index 7f8322fa86..fa82614471 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # ORB-SLAM2 **Authors:** [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)) +**14 Jul 2017**: Binary format ORB vocabulary and Map save/load are now supported(See section 10 and 11). + **13 Jan 2017**: OpenCV 3 and Eigen 3.3 are now supported. **22 Dec 2016**: Added AR demo (see section 7). @@ -72,6 +74,12 @@ We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) libra ## ROS (optional) We provide some examples to process the live input of a monocular, stereo or RGB-D camera using [ROS](ros.org). Building these examples is optional. In case you want to use ROS, a version Hydro or newer is needed. +## Boost(optional) + +Map save/load feature needs boost library and more specifically the`libboost_serialization` library. + +See section 11 + # 3. Building ORB-SLAM2 library and examples Clone the repository: @@ -170,21 +178,21 @@ This will create **libORB_SLAM2.so** at *lib* folder and the executables **mono ``` export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS ``` - + 2. Execute `build_ros.sh` script: ``` chmod +x build_ros.sh ./build_ros.sh ``` - + ### Running Monocular Node For a monocular input from topic `/camera/image_raw` run node ORB_SLAM2/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above. ``` rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ``` - + ### Running Monocular Augmented Reality Demo This is a demo of augmented reality where you can use an interface to insert virtual cubes in planar regions of the scene. The node reads images from topic `/camera/image_raw`. @@ -192,27 +200,27 @@ The node reads images from topic `/camera/image_raw`. ``` rosrun ORB_SLAM2 MonoAR PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ``` - + ### Running Stereo Node For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM2/Stereo. You will need to provide the vocabulary file and a settings file. If you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. ``` rosrun ORB_SLAM2 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION ``` - + **Example**: Download a rosbag (e.g. V1_01_easy.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab: ``` roscore ``` - + ``` rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true ``` - + ``` rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw ``` - + Once ORB-SLAM2 has loaded the vocabulary, press space in the rosbag tab. Enjoy!. Note: a powerful computer is required to run the most exigent sequences of this dataset. ### Running RGB_D Node @@ -221,7 +229,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist ``` rosrun ORB_SLAM2 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ``` - + # 8. Processing your own sequences You will need to create a settings file with the calibration of your camera. See the settings file provided for the TUM and KITTI datasets for monocular, stereo and RGB-D cameras. We use the calibration model of OpenCV. See the examples to learn how to create a program that makes use of the ORB-SLAM2 library and how to pass images to the SLAM system. Stereo input must be synchronized and rectified. RGB-D input must be synchronized and depth registered. @@ -234,3 +242,36 @@ This is the default mode. The system runs in parallal three threads: Tracking, L ### Localization Mode This mode can be used when you have a good map of your working area. In this mode the Local Mapping and Loop Closing are deactivated. The system localizes the camera in the map (which is no longer updated), using relocalization if needed. +# 10. Binary Format ORB Vocabulary + +You can load ORB vocabulary in either text or binary format. The format is determined by suffix(.txt for text format and .bin for binary format). + +`build.sh` will generate a text-to-binary convertor `bin_vocabulary` in `Vocabulary/` . You can also find it as a target in `CMakeLists.txt`. + +`bin_vocabulary` will convert `./ORBvoc.txt` to `./ORBvoc.bin` and you can use the new `ORBvoc.bin` as `PATH_TO_VOCABULARY` wherever needed. + +PS: binary format is loaded faster and text format is more human-readable. + +# 11. Map Save/Load + +#### Enable: + +Considering this feature doesn't hurt performance, and it is annonying to deal with conditional compilation flags, so this feature will be enabled unconditionally. + +#### Usage: + +This feature is integrated with `class System`. The path of mapfile can be set by adding `Map.mapfile: map.bin` to ORB_SLAM2's settings file. See the last few line of `Example/Monocular/TUM1.xml`. + +To save a map, you need construct `ORB_SLAM2::System` with the last parameter be `true`. Then the `System` will save map to mapfile specified in setting file when `ShutDown`. + +With a readable mapfile, map will be loaded automatically and `System` will run in localization mode, but you can change it to SLAM mode later. + +If you set a mapfile but it doesn't exist, `System` will create new map. + +mono_tum has been updated as a simple example of this functionality. An extra command line parameter(0 or 1) should be given to indicate whether you want to save map or not. + +#### Implementation related: + +I use boost_serialization library to serialize `Map`, `MapPoint`, `KeyFrame`,`KeyFrameDatabase`, `cv::Mat`, `DBoW2::BowVector`, `DBoW2::FeatureVector`. In brief, only the `ORBVector` isn't serialized. + +This feature is tested with boost 1.64 and it works fine mostly. There is still some occasional segmentfault to dig in. diff --git a/Thirdparty/DBoW2/DBoW2/BowVector.h b/Thirdparty/DBoW2/DBoW2/BowVector.h index f559811deb..16787d4ee5 100644 --- a/Thirdparty/DBoW2/DBoW2/BowVector.h +++ b/Thirdparty/DBoW2/DBoW2/BowVector.h @@ -57,7 +57,7 @@ class BowVector: public std::map { public: - + typedef std::map super; /** * Constructor */ diff --git a/Thirdparty/DBoW2/DBoW2/FeatureVector.h b/Thirdparty/DBoW2/DBoW2/FeatureVector.h index 08a91def7c..74f84143bd 100644 --- a/Thirdparty/DBoW2/DBoW2/FeatureVector.h +++ b/Thirdparty/DBoW2/DBoW2/FeatureVector.h @@ -22,7 +22,7 @@ class FeatureVector: public std::map > { public: - + typedef std::map > super; /** * Constructor */ diff --git a/Thirdparty/g2o/g2o/core/base_binary_edge.h b/Thirdparty/g2o/g2o/core/base_binary_edge.h index 660e83afd6..c18f97fe08 100644 --- a/Thirdparty/g2o/g2o/core/base_binary_edge.h +++ b/Thirdparty/g2o/g2o/core/base_binary_edge.h @@ -56,8 +56,8 @@ namespace g2o { typedef typename BaseEdge::ErrorVector ErrorVector; typedef typename BaseEdge::InformationType InformationType; - typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; - typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType; + typedef Eigen::Map, Matrix::Flags & PacketAccessBit ? Aligned : Unaligned > HessianBlockType; + typedef Eigen::Map, Matrix::Flags & PacketAccessBit ? Aligned : Unaligned > HessianBlockTransposedType; BaseBinaryEdge() : BaseEdge(), _hessianRowMajor(false), diff --git a/Thirdparty/g2o/g2o/core/base_multi_edge.h b/Thirdparty/g2o/g2o/core/base_multi_edge.h index dd2261fd48..cccc7edaf1 100644 --- a/Thirdparty/g2o/g2o/core/base_multi_edge.h +++ b/Thirdparty/g2o/g2o/core/base_multi_edge.h @@ -66,7 +66,7 @@ namespace g2o { typedef MatrixXd::MapType JacobianType; typedef typename BaseEdge::ErrorVector ErrorVector; typedef typename BaseEdge::InformationType InformationType; - typedef Eigen::Map HessianBlockType; + typedef Eigen::Map HessianBlockType; BaseMultiEdge() : BaseEdge() { diff --git a/Thirdparty/g2o/g2o/core/base_vertex.h b/Thirdparty/g2o/g2o/core/base_vertex.h index e375fdeef9..a444ec3300 100644 --- a/Thirdparty/g2o/g2o/core/base_vertex.h +++ b/Thirdparty/g2o/g2o/core/base_vertex.h @@ -59,7 +59,7 @@ namespace g2o { static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space - typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; + typedef Eigen::Map, Matrix::Flags & PacketAccessBit ? Aligned : Unaligned > HessianBlockType; public: BaseVertex(); diff --git a/include/BoostArchiver.h b/include/BoostArchiver.h new file mode 100644 index 0000000000..8b419069bf --- /dev/null +++ b/include/BoostArchiver.h @@ -0,0 +1,95 @@ +/* + * map save/load extension for ORB_SLAM2 + * This header contains boost headers needed by serialization + * + * object to save: + * - KeyFrame + * - KeyFrameDatabase + * - Map + * - MapPoint + */ +#ifndef BOOST_ARCHIVER_H +#define BOOST_ARCHIVER_H +#include +#include +#include +// set serialization needed by KeyFrame::mspChildrens ... +#include +// map serialization needed by KeyFrame::mConnectedKeyFrameWeights ... +#include +#include +#include +#include +// base object needed by DBoW2::BowVector and DBoW2::FeatureVector +#include + +#include "Thirdparty/DBoW2/DBoW2/BowVector.h" +#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" + +BOOST_SERIALIZATION_SPLIT_FREE(::cv::Mat) +namespace boost{ + namespace serialization { + + /* serialization for DBoW2 BowVector */ + template + void serialize(Archive &ar, DBoW2::BowVector &BowVec, const unsigned int file_version) + { + ar & boost::serialization::base_object(BowVec); + } + /* serialization for DBoW2 FeatureVector */ + template + void serialize(Archive &ar, DBoW2::FeatureVector &FeatVec, const unsigned int file_version) + { + ar & boost::serialization::base_object(FeatVec); + } + + /* serialization for CV KeyPoint */ + template + void serialize(Archive &ar, ::cv::KeyPoint &kf, const unsigned int file_version) + { + ar & kf.angle; + ar & kf.class_id; + ar & kf.octave; + ar & kf.response; + ar & kf.response; + ar & kf.pt.x; + ar & kf.pt.y; + } + /* serialization for CV Mat */ + template + void save(Archive &ar, const ::cv::Mat &m, const unsigned int file_version) + { + cv::Mat m_ = m; + if (!m.isContinuous()) + m_ = m.clone(); + size_t elem_size = m_.elemSize(); + size_t elem_type = m_.type(); + ar & m_.cols; + ar & m_.rows; + ar & elem_size; + ar & elem_type; + + const size_t data_size = m_.cols * m_.rows * elem_size; + + ar & boost::serialization::make_array(m_.ptr(), data_size); + } + template + void load(Archive & ar, ::cv::Mat& m, const unsigned int version) + { + int cols, rows; + size_t elem_size, elem_type; + + ar & cols; + ar & rows; + ar & elem_size; + ar & elem_type; + + m.create(rows, cols, elem_type); + size_t data_size = m.cols * m.rows * elem_size; + + ar & boost::serialization::make_array(m.ptr(), data_size); + } + } +} +// TODO: boost::iostream zlib compressed binary format +#endif // BOOST_ARCHIVER_H diff --git a/include/FrameDrawer.h b/include/FrameDrawer.h index 95c1df9d9e..6182d88820 100644 --- a/include/FrameDrawer.h +++ b/include/FrameDrawer.h @@ -40,7 +40,7 @@ class Viewer; class FrameDrawer { public: - FrameDrawer(Map* pMap); + FrameDrawer(Map* pMap, bool bReuseMap=false); // Update info from the last processed frame. void Update(Tracking *pTracker); diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 67f4348273..29cb960db0 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -30,7 +30,7 @@ #include "KeyFrameDatabase.h" #include - +#include "BoostArchiver.h" namespace ORB_SLAM2 { @@ -43,6 +43,7 @@ class KeyFrameDatabase; class KeyFrame { public: + KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); // Pose functions @@ -116,6 +117,15 @@ class KeyFrame return pKF1->mnIdmnId; } +public: + // for serialization + KeyFrame(); // Default constructor for serialization, need to deal with const member + void SetORBvocabulary(ORBVocabulary *porbv) {mpORBvocabulary=porbv;} +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); // The following variables are accesed from only 1 thread or never change (no mutex needed). public: diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h index fa3735762d..f79bbad1d2 100644 --- a/include/KeyFrameDatabase.h +++ b/include/KeyFrameDatabase.h @@ -30,7 +30,7 @@ #include "ORBVocabulary.h" #include - +#include "BoostArchiver.h" namespace ORB_SLAM2 { @@ -43,7 +43,7 @@ class KeyFrameDatabase { public: - KeyFrameDatabase(const ORBVocabulary &voc); + KeyFrameDatabase(ORBVocabulary *voc); void add(KeyFrame* pKF); @@ -57,10 +57,20 @@ class KeyFrameDatabase // Relocalization std::vector DetectRelocalizationCandidates(Frame* F); +public: + // for serialization + KeyFrameDatabase() {} + void SetORBvocabulary(ORBVocabulary *porbv) {mpVoc=porbv;} +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + protected: // Associated vocabulary - const ORBVocabulary* mpVoc; + ORBVocabulary* mpVoc; // Inverted file std::vector > mvInvertedFile; diff --git a/include/LoopClosing.h b/include/LoopClosing.h index 7eb0416b15..af9be9b43a 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -143,7 +143,7 @@ class LoopClosing bool mbFixScale; - bool mnFullBAIdx; + int mnFullBAIdx; }; } //namespace ORB_SLAM diff --git a/include/Map.h b/include/Map.h index a75339feea..f4d9750e4f 100644 --- a/include/Map.h +++ b/include/Map.h @@ -27,6 +27,7 @@ #include +#include "BoostArchiver.h" namespace ORB_SLAM2 @@ -66,6 +67,13 @@ class Map // This avoid that two points are created simultaneously in separate threads (id conflict) std::mutex mMutexPointCreation; + +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + protected: std::set mspMapPoints; std::set mspKeyFrames; diff --git a/include/MapPoint.h b/include/MapPoint.h index f26893d89d..756ab89bf7 100644 --- a/include/MapPoint.h +++ b/include/MapPoint.h @@ -27,6 +27,7 @@ #include #include +#include "BoostArchiver.h" namespace ORB_SLAM2 { @@ -81,6 +82,15 @@ class MapPoint int PredictScale(const float ¤tDist, KeyFrame*pKF); int PredictScale(const float ¤tDist, Frame* pF); +public: + // for serialization + MapPoint(); +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + public: long unsigned int mnId; static long unsigned int nNextId; diff --git a/include/System.h b/include/System.h index b377b453d1..817060b550 100644 --- a/include/System.h +++ b/include/System.h @@ -36,6 +36,10 @@ #include "ORBVocabulary.h" #include "Viewer.h" +#include "BoostArchiver.h" +// for map file io +#include + namespace ORB_SLAM2 { @@ -59,7 +63,7 @@ class System public: // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. - System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true); + System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, bool is_save_map_=false); // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. @@ -111,17 +115,17 @@ class System // Call first Shutdown() // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php void SaveTrajectoryKITTI(const string &filename); - - // TODO: Save/Load functions - // 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: + // Save/Load functions + void SaveMap(const string &filename); + bool LoadMap(const string &filename); + private: // Input sensor @@ -136,6 +140,9 @@ class System // Map structure that stores the pointers to all KeyFrames and MapPoints. Map* mpMap; + string mapfile; + bool is_save_map; + // Tracker. It receives a frame and computes the associated camera pose. // It also decides when to insert a new keyframe, create some new MapPoints and // performs relocalization if tracking fails. diff --git a/include/Tracking.h b/include/Tracking.h index 5aaa93ef26..1fda297eee 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -54,8 +54,9 @@ class Tracking { public: + Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, - KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); + KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, bool bReuseMap=false); // 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); diff --git a/include/Viewer.h b/include/Viewer.h index 251e223c54..8571294dd6 100644 --- a/include/Viewer.h +++ b/include/Viewer.h @@ -40,7 +40,9 @@ class System; class Viewer { public: - Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); + + Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath, bool mbReuseMap); + // Main thread function. Draw points, keyframes, the current camera pose and the last processed // frame. Drawing is refreshed according to the camera fps. We use Pangolin. @@ -79,6 +81,8 @@ class Viewer bool mbStopped; bool mbStopRequested; + bool mbReuseMap; + std::mutex mMutexStop; }; diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc index e23b86c207..5362ba1c96 100644 --- a/src/FrameDrawer.cc +++ b/src/FrameDrawer.cc @@ -29,9 +29,12 @@ namespace ORB_SLAM2 { -FrameDrawer::FrameDrawer(Map* pMap):mpMap(pMap) +FrameDrawer::FrameDrawer(Map* pMap, bool bReuseMap):mpMap(pMap) { - mState=Tracking::SYSTEM_NOT_READY; + if (bReuseMap) + mState=Tracking::LOST; + else + mState=Tracking::SYSTEM_NOT_READY; mIm = cv::Mat(480,640,CV_8UC3, cv::Scalar(0,0,0)); } diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 4ef1e78e0f..7dbed7daaa 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -662,4 +662,88 @@ float KeyFrame::ComputeSceneMedianDepth(const int q) return vDepths[(vDepths.size()-1)/q]; } +// Default serializing Constructor +KeyFrame::KeyFrame(): + mnFrameId(0), mTimeStamp(0.0), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), + mfGridElementWidthInv(0.0), mfGridElementHeightInv(0.0), + mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), + mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), + fx(0.0), fy(0.0), cx(0.0), cy(0.0), invfx(0.0), invfy(0.0), + mbf(0.0), mb(0.0), mThDepth(0.0), N(0), mnScaleLevels(0), mfScaleFactor(0), + mfLogScaleFactor(0.0), + mnMinX(0), mnMinY(0), mnMaxX(0), + mnMaxY(0) +{} +template +void KeyFrame::serialize(Archive &ar, const unsigned int version) +{ + // no mutex needed vars + ar & nNextId; + ar & mnId; + ar & const_cast(mnFrameId); + ar & const_cast(mTimeStamp); + // Grid related vars + ar & const_cast(mnGridCols); + ar & const_cast(mnGridRows); + ar & const_cast(mfGridElementWidthInv); + ar & const_cast(mfGridElementHeightInv); + // Tracking related vars + ar & mnTrackReferenceForFrame & mnFuseTargetForKF; + // LocalMaping related vars + ar & mnBALocalForKF & mnBAFixedForKF; + // KeyFrameDB related vars + ar & mnLoopQuery & mnLoopWords & mLoopScore & mnRelocQuery & mnRelocWords & mRelocScore; + // LoopClosing related vars + ar & mTcwGBA & mTcwBefGBA & mnBAGlobalForKF; + // calibration parameters + ar & const_cast(fx) & const_cast(fy) & const_cast(cx) & const_cast(cy); + ar & const_cast(invfx) & const_cast(invfy) & const_cast(mbf); + ar & const_cast(mb) & const_cast(mThDepth); + // Number of KeyPoints; + ar & const_cast(N); + // KeyPoints, stereo coordinate and descriptors + ar & const_cast &>(mvKeys); + ar & const_cast &>(mvKeysUn); + ar & const_cast &>(mvuRight); + ar & const_cast &>(mvDepth); + ar & const_cast(mDescriptors); + // Bow + ar & mBowVec & mFeatVec; + // Pose relative to parent + ar & mTcp; + // Scale related + ar & const_cast(mnScaleLevels) & const_cast(mfScaleFactor) & const_cast(mfLogScaleFactor); + ar & const_cast &>(mvScaleFactors) & const_cast &>(mvLevelSigma2) & const_cast &>(mvInvLevelSigma2); + // Image bounds and calibration + ar & const_cast(mnMinX) & const_cast(mnMinY) & const_cast(mnMaxX) & const_cast(mnMaxY); + ar & const_cast(mK); + + // mutex needed vars, but don't lock mutex in the save/load procedure + { + unique_lock lock_pose(mMutexPose); + ar & Tcw & Twc & Ow & Cw; + } + { + unique_lock lock_feature(mMutexFeatures); + ar & mvpMapPoints; // hope boost deal with the pointer graph well + } + // BoW + ar & mpKeyFrameDB; + // mpORBvocabulary restore elsewhere(see SetORBvocab) + { + // Grid related + unique_lock lock_connection(mMutexConnections); + ar & mGrid & mConnectedKeyFrameWeights & mvpOrderedConnectedKeyFrames & mvOrderedWeights; + // Spanning Tree and Loop Edges + ar & mbFirstConnection & mpParent & mspChildrens & mspLoopEdges; + // Bad flags + ar & mbNotErase & mbToBeErased & mbBad & mHalfBaseline; + } + // Map Points + ar & mpMap; + // don't save mutex +} +template void KeyFrame::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void KeyFrame::serialize(boost::archive::binary_oarchive&, const unsigned int); + } //namespace ORB_SLAM diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc index 826860cab9..19e1af409f 100644 --- a/src/KeyFrameDatabase.cc +++ b/src/KeyFrameDatabase.cc @@ -30,10 +30,10 @@ using namespace std; namespace ORB_SLAM2 { -KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc): - mpVoc(&voc) +KeyFrameDatabase::KeyFrameDatabase (ORBVocabulary *voc): + mpVoc(voc) { - mvInvertedFile.resize(voc.size()); + mvInvertedFile.resize(voc->size()); } @@ -308,4 +308,15 @@ vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) return vpRelocCandidates; } +template +void KeyFrameDatabase::serialize(Archive &ar, const unsigned int version) +{ + // don't save associated vocabulary, KFDB restore by created explicitly from a new ORBvocabulary instance + // inverted file + ar & mvInvertedFile; + // don't save mutex +} +template void KeyFrameDatabase::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void KeyFrameDatabase::serialize(boost::archive::binary_oarchive&, const unsigned int); + } //namespace ORB_SLAM diff --git a/src/Map.cc b/src/Map.cc index 15fcd86914..4b13bee58c 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -130,4 +130,17 @@ void Map::clear() mvpKeyFrameOrigins.clear(); } +template +void Map::serialize(Archive &ar, const unsigned int version) +{ + // don't save mutex + ar & mspMapPoints; + ar & mvpKeyFrameOrigins; + ar & mspKeyFrames; + ar & mvpReferenceMapPoints; + ar & mnMaxKFid & mnBigChangeIdx; +} +template void Map::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void Map::serialize(boost::archive::binary_oarchive&, const unsigned int); + } //namespace ORB_SLAM diff --git a/src/MapPoint.cc b/src/MapPoint.cc index 3b2921197b..11c53b8761 100644 --- a/src/MapPoint.cc +++ b/src/MapPoint.cc @@ -30,7 +30,7 @@ long unsigned int MapPoint::nNextId=0; mutex MapPoint::mGlobalMutex; MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap): - mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0), + mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0),mbTrackInView(false), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false), mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap) @@ -44,7 +44,7 @@ MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap): } MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF): - mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), + mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0),mbTrackInView(false), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0), mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast(NULL)), mnVisible(1), mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap) @@ -416,6 +416,43 @@ int MapPoint::PredictScale(const float ¤tDist, Frame* pF) return nScale; } +MapPoint::MapPoint(): + nObs(0), mnTrackReferenceForFrame(0), + mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0),mnVisible(1), mnFound(1), mbBad(false), + mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0) +{} +template +void MapPoint::serialize(Archive &ar, const unsigned int version) +{ + ar & mnId & nNextId & mnFirstKFid & mnFirstFrame & nObs; + // Tracking related vars + ar & mTrackProjX; + ar & mTrackProjY; + ar & mTrackProjXR; + ar & mbTrackInView; + ar & mnTrackScaleLevel; + ar & mTrackViewCos; + ar & mnTrackReferenceForFrame; + ar & mnLastFrameSeen; + // Local Mapping related vars + ar & mnBALocalForKF & mnFuseCandidateForKF; + // Loop Closing related vars + ar & mnLoopPointForKF & mnCorrectedByKF & mnCorrectedReference & mPosGBA & mnBAGlobalForKF; + // don't save the mutex + ar & mWorldPos; + ar & mObservations; + ar & mNormalVector; + ar & mDescriptor; + ar & mpRefKF; + ar & mnVisible & mnFound; + ar & mbBad & mpReplaced; + ar & mfMinDistance & mfMaxDistance; + ar & mpMap; + // don't save the mutex +} +template void MapPoint::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void MapPoint::serialize(boost::archive::binary_oarchive&, const unsigned int); } //namespace ORB_SLAM diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 56bf279d0f..c26714a8f0 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -759,6 +759,7 @@ int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F { const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2]; vMatches12[idx1]=bestIdx2; + vbMatched2[bestIdx2] = true; nmatches++; if(mbCheckOrientation) @@ -802,6 +803,7 @@ int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F continue; for(size_t j=0, jend=rotHist[i].size(); j(NULL)), mbReset(false),mbActivateLocalizationMode(false), - mbDeactivateLocalizationMode(false) + const bool bUseViewer, bool is_save_map_):mSensor(sensor), is_save_map(is_save_map_), mpViewer(static_cast(NULL)), mbReset(false), + mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) { // Output welcome message cout << endl << @@ -63,6 +63,12 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS exit(-1); } + cv::FileNode mapfilen = fsSettings["Map.mapfile"]; + bool bReuseMap = false; + if (!mapfilen.empty()) + { + mapfile = (string)mapfilen; + } //Load ORB Vocabulary cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; @@ -83,20 +89,27 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS } cout << "Vocabulary loaded!" << endl << endl; - //Create KeyFrame Database - mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); + //Create KeyFrame Database //Create the Map - mpMap = new Map(); + if (!mapfile.empty() && LoadMap(mapfile)) + { + bReuseMap = true; + } + else + { + mpKeyFrameDatabase = new KeyFrameDatabase(mpVocabulary); + mpMap = new Map(); + } //Create Drawers. These are used by the Viewer - mpFrameDrawer = new FrameDrawer(mpMap); + mpFrameDrawer = new FrameDrawer(mpMap, bReuseMap); mpMapDrawer = new MapDrawer(mpMap, strSettingsFile); //Initialize the Tracking thread //(it will live in the main thread of execution, the one that called this constructor) mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, - mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor); + mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor, bReuseMap); //Initialize the Local Mapping thread and launch mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); @@ -109,7 +122,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS //Initialize the Viewer thread and launch if(bUseViewer) { - mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile); + mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile, bReuseMap); mptViewer = new thread(&Viewer::Run, mpViewer); mpTracker->SetViewer(mpViewer); } @@ -328,9 +341,10 @@ void System::Shutdown() { std::this_thread::sleep_for(std::chrono::microseconds(5000)); } - if(mpViewer) pangolin::BindToContext("ORB-SLAM2: Map Viewer"); + if (is_save_map) + SaveMap(mapfile); } void System::SaveTrajectoryTUM(const string &filename) @@ -503,4 +517,48 @@ vector System::GetTrackedKeyPointsUn() return mTrackedKeyPointsUn; } +void System::SaveMap(const string &filename) +{ + std::ofstream out(filename, std::ios_base::binary); + if (!out) + { + cerr << "Cannot Write to Mapfile: " << mapfile << std::endl; + exit(-1); + } + cout << "Saving Mapfile: " << mapfile << std::flush; + boost::archive::binary_oarchive oa(out, boost::archive::no_header); + oa << mpMap; + oa << mpKeyFrameDatabase; + cout << " ...done" << std::endl; + out.close(); +} +bool System::LoadMap(const string &filename) +{ + std::ifstream in(filename, std::ios_base::binary); + if (!in) + { + cerr << "Cannot Open Mapfile: " << mapfile << " , Create a new one" << std::endl; + return false; + } + cout << "Loading Mapfile: " << mapfile << std::flush; + boost::archive::binary_iarchive ia(in, boost::archive::no_header); + ia >> mpMap; + ia >> mpKeyFrameDatabase; + mpKeyFrameDatabase->SetORBvocabulary(mpVocabulary); + cout << " ...done" << std::endl; + cout << "Map Reconstructing" << flush; + vector vpKFS = mpMap->GetAllKeyFrames(); + unsigned long mnFrameId = 0; + for (auto it:vpKFS) { + it->SetORBvocabulary(mpVocabulary); + it->ComputeBoW(); + if (it->mnFrameId > mnFrameId) + mnFrameId = it->mnFrameId; + } + Frame::nNextId = mnFrameId; + cout << " ...done" << endl; + in.close(); + return true; +} + } //namespace ORB_SLAM diff --git a/src/Tracking.cc b/src/Tracking.cc index 88804aa25d..4bad4841e9 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -43,7 +43,7 @@ using namespace std; namespace ORB_SLAM2 { -Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor): +Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, bool bReuseMap): mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) @@ -145,7 +145,8 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, else mDepthMapFactor = 1.0f/mDepthMapFactor; } - + if (bReuseMap) + mState = LOST; } void Tracking::SetLocalMapper(LocalMapping *pLocalMapper) @@ -497,7 +498,8 @@ void Tracking::Track() else { // This can happen if tracking is lost - mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); + if (!mlRelativeFramePoses.empty()) + mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); mlpReferences.push_back(mlpReferences.back()); mlFrameTimes.push_back(mlFrameTimes.back()); mlbLost.push_back(mState==LOST); diff --git a/src/Viewer.cc b/src/Viewer.cc index c50de6835e..882259fa00 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -26,9 +26,9 @@ namespace ORB_SLAM2 { -Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath): +Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath, bool mbReuseMap_): mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking), - mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false) + mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false), mbReuseMap(mbReuseMap_) { cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); @@ -70,7 +70,7 @@ void Viewer::Run() pangolin::Var menuShowPoints("menu.Show Points",true,true); pangolin::Var menuShowKeyFrames("menu.Show KeyFrames",true,true); pangolin::Var menuShowGraph("menu.Show Graph",true,true); - pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); + pangolin::Var menuLocalizationMode("menu.Localization Mode",mbReuseMap,true); pangolin::Var menuReset("menu.Reset",false,false); // Define Camera Render Object (for view / scene browsing)