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

@@ -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})

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

Examples/Monocular/mono_tum.cc

@@ -35,9 +35,9 @@ void LoadImages(const string &strFile, vector<string> &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<float> vTimesTrack;

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,49 +178,49 @@ 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`.
 
   ```
   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.

Thirdparty/DBoW2/DBoW2/BowVector.h

@@ -57,7 +57,7 @@ class BowVector:
 	public std::map<WordId, WordValue>
 {
 public:
-
+    typedef std::map<WordId, WordValue> super;
 	/** 
 	 * Constructor
 	 */

Thirdparty/DBoW2/DBoW2/FeatureVector.h

@@ -22,7 +22,7 @@ class FeatureVector:
   public std::map<NodeId, std::vector<unsigned int> >
 {
 public:
-
+  typedef std::map<NodeId, std::vector<unsigned int> > super;
   /**
    * Constructor
    */

Thirdparty/g2o/g2o/core/base_binary_edge.h

@@ -56,8 +56,8 @@ namespace g2o {
       typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
       typedef typename BaseEdge<D,E>::InformationType InformationType;
 
-      typedef Eigen::Map<Matrix<double, Di, Dj>, Matrix<double, Di, Dj>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
-      typedef Eigen::Map<Matrix<double, Dj, Di>, Matrix<double, Dj, Di>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType;
+      typedef Eigen::Map<Matrix<double, Di, Dj>, Matrix<double, Di, Dj>::Flags & PacketAccessBit ? Aligned : Unaligned > HessianBlockType;
+      typedef Eigen::Map<Matrix<double, Dj, Di>, Matrix<double, Dj, Di>::Flags & PacketAccessBit ? Aligned : Unaligned > HessianBlockTransposedType;
 
       BaseBinaryEdge() : BaseEdge<D,E>(),
       _hessianRowMajor(false),

Thirdparty/g2o/g2o/core/base_multi_edge.h

@@ -66,7 +66,7 @@ namespace g2o {
       typedef MatrixXd::MapType JacobianType;
       typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
       typedef typename BaseEdge<D,E>::InformationType InformationType;
-      typedef Eigen::Map<MatrixXd, MatrixXd::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
+      typedef Eigen::Map<MatrixXd, MatrixXd::Flags & PacketAccessBit ? Aligned : Unaligned > HessianBlockType;
 
       BaseMultiEdge() : BaseEdge<D,E>()
       {

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<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned >  HessianBlockType;
+    typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & PacketAccessBit ? Aligned : Unaligned >  HessianBlockType;
 
   public:
     BaseVertex();

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 <boost/serialization/list.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/serialization/set.hpp>
+// set serialization needed by KeyFrame::mspChildrens ...
+#include <boost/serialization/map.hpp>
+// map serialization needed by KeyFrame::mConnectedKeyFrameWeights ...
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/base_object.hpp>
+// base object needed by DBoW2::BowVector and DBoW2::FeatureVector
+#include <opencv2/core/core.hpp>
+
+#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<class Archive>
+    void serialize(Archive &ar, DBoW2::BowVector &BowVec, const unsigned int file_version)
+    {
+        ar & boost::serialization::base_object<DBoW2::BowVector::super>(BowVec);
+    }
+    /* serialization for DBoW2 FeatureVector */
+    template<class Archive>
+    void serialize(Archive &ar, DBoW2::FeatureVector &FeatVec, const unsigned int file_version)
+    {
+        ar & boost::serialization::base_object<DBoW2::FeatureVector::super>(FeatVec);
+    }
+
+    /* serialization for CV KeyPoint */
+    template<class Archive>
+    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<class Archive>
+    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<class Archive>
+    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

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);

include/KeyFrame.h

@@ -30,7 +30,7 @@
 #include "KeyFrameDatabase.h"
 
 #include <mutex>
-
+#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->mnId<pKF2->mnId;
     }
 
+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<class Archive>
+    void serialize(Archive &ar, const unsigned int version);
 
     // The following variables are accesed from only 1 thread or never change (no mutex needed).
 public: