save and load with a mini map

CMakeLists.txt

@@ -38,6 +38,7 @@ endif()
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
+find_library(BOOST_SERIALIZATION boost_serialization)
 
 include_directories(
 ${PROJECT_SOURCE_DIR}
@@ -74,6 +75,7 @@ target_link_libraries(${PROJECT_NAME}
 ${OpenCV_LIBS}
 ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
+${BOOST_SERIALIZATION}
 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
 ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
 )

Thirdparty/DBoW2/DBoW2/BowVector.h

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

Thirdparty/DBoW2/DBoW2/FeatureVector.h

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

include/Frame.h

@@ -57,6 +57,12 @@ class Frame
     // Constructor for Monocular cameras.
     Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
 
+    // for Load Map
+    Frame(const cv::Size& imgSize, const cv::Mat& Tcw, const std::vector<cv::KeyPoint>& keypoint,
+          const std::vector<cv::KeyPoint>& keypoint_un, const cv::Mat& descriptor, const double timeStamp,
+          ORBextractor* const extractor, ORBVocabulary* const voc, const cv::Mat& K, const float bf,
+          const float thDepth, const unsigned long frame_id);
+
     // Extract ORB on the image. 0 for left image and 1 for right image.
     void ExtractORB(int flag, const cv::Mat &im);
 

include/KeyFrame.h

@@ -28,6 +28,7 @@
 #include "ORBextractor.h"
 #include "Frame.h"
 #include "KeyFrameDatabase.h"
+#include "Serialize.h"
 
 #include <mutex>
 
@@ -44,6 +45,8 @@ class KeyFrame
 {
 public:
     KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB);
+    // for serizalize
+    KeyFrame();
 
     // Pose functions
     void SetPose(const cv::Mat &Tcw);
@@ -116,7 +119,10 @@ class KeyFrame
         return pKF1->mnId<pKF2->mnId;
     }
 
-
+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:
 

include/MapPoint.h

@@ -24,6 +24,7 @@
 #include"KeyFrame.h"
 #include"Frame.h"
 #include"Map.h"
+#include "Serialize.h"
 
 #include<opencv2/core/core.hpp>
 #include<mutex>
@@ -41,6 +42,8 @@ class MapPoint
 public:
     MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);
     MapPoint(const cv::Mat &Pos,  Map* pMap, Frame* pFrame, const int &idxF);
+    // for serizalize
+    MapPoint();
 
     void SetWorldPos(const cv::Mat &Pos);
     cv::Mat GetWorldPos();
@@ -81,6 +84,19 @@ class MapPoint
     int PredictScale(const float &currentDist, KeyFrame*pKF);
     int PredictScale(const float &currentDist, Frame* pF);
 
+    void SetRefKF(ORB_SLAM2::KeyFrame *pKF){
+        mpRefKF = pKF;
+    };
+
+    void SetMap(ORB_SLAM2::Map *pMap){
+        mpMap = pMap;
+    };
+
+private:
+    friend class boost::serialization::access;
+    template<class Archive>
+    void serialize(Archive &ar, const unsigned int version);
+
 public:
     long unsigned int mnId;
     static long unsigned int nNextId;
@@ -109,6 +125,7 @@ class MapPoint
     cv::Mat mPosGBA;
     long unsigned int mnBAGlobalForKF;
 
+    KeyFrame* mpRefKF = nullptr;
 
     static std::mutex mGlobalMutex;
 
@@ -127,7 +144,6 @@ class MapPoint
      cv::Mat mDescriptor;
 
      // Reference KeyFrame
-     KeyFrame* mpRefKF;
 
      // Tracking counters
      int mnVisible;

include/Serialize.h

@@ -0,0 +1,85 @@
+/*
+This file contains the necessary serialize functions for complex datatypes
+used by ORBSLAM2 and which are not available in boost serialization library
+*/
+
+#ifndef SERIALIZE_H
+#define SERIALIZE_H
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/serialization/list.hpp>
+#include <boost/serialization/vector.hpp>
+// KeyFrame::mConnectedKeyFrameWeights
+#include <boost/serialization/map.hpp>
+// #include <boost/serialization/split_free.hpp>
+// DBoW2::BowVector and DBoW2::FeatureVector
+#include <boost/serialization/base_object.hpp>
+// KeyFrame::mspChildrens
+#include <boost/serialization/set.hpp>
+#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 {
+
+template <class Archive>
+void serialize(Archive &ar, DBoW2::BowVector &BowVec,
+               const unsigned int file_version) {
+  // Derived classes should include serializations of their base classes.
+  ar &boost::serialization::base_object<DBoW2::BowVector::super>(BowVec);
+}
+
+template <class Archive>
+void serialize(Archive &ar, DBoW2::FeatureVector &FeatVec,
+               const unsigned int file_version) {
+  ar &boost::serialization::base_object<DBoW2::FeatureVector::super>(FeatVec);
+}
+
+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);
+}
+
+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.pt.x;
+  ar &kf.pt.y;
+}
+}
+}
+#endif  // SERIALIZE_H

include/System.h

@@ -122,6 +122,16 @@ class System
     std::vector<MapPoint*> GetTrackedMapPoints();
     std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
 
+    bool SaveMap(const string& file_name);
+    bool LoadMap(const string& file_name);
+
+private:
+    bool SaveKeyFrames(const std::vector<KeyFrame *> keyframes, const string& file_name);
+
+    void LoadKeyFrames(const std::vector<KeyFrame*>& vpKFs, Map* const pMap);
+    void LoadMapPoints(Map* const pMap);
+    void AddKeyFrame(ORB_SLAM2::KeyFrame *keyframe, ORB_SLAM2::Map *pMap);
+
 private:
 
     // Input sensor

include/Tracking.h

@@ -144,6 +144,7 @@ class Tracking
     bool NeedNewKeyFrame();
     void CreateNewKeyFrame();
 
+public:
     // In case of performing only localization, this flag is true when there are no matches to
     // points in the map. Still tracking will continue if there are enough matches with temporal points.
     // In that case we are doing visual odometry. The system will try to do relocalization to recover

src/Frame.cc

@@ -227,6 +227,69 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
     AssignFeaturesToGrid();
 }
 
+Frame::Frame(const cv::Size& imgSize, const cv::Mat& Tcw, const std::vector<cv::KeyPoint>& keypoint,
+             const std::vector<cv::KeyPoint>& keypoint_un, const cv::Mat& descriptor, const double timeStamp,
+             ORBextractor* const extractor, ORBVocabulary* const voc, const cv::Mat& K, const float bf, 
+             const float thDepth, const unsigned long frame_id)
+        :mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
+         mTimeStamp(timeStamp), mK(K.clone()), mbf(bf), mThDepth(thDepth)
+{
+    // Frame ID
+    // mnId=nNextId++;
+    mnId = frame_id;
+
+    // Scale Level Info
+    mnScaleLevels = mpORBextractorLeft->GetLevels();
+    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
+    mfLogScaleFactor = log(mfScaleFactor);
+    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
+    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
+    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
+    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
+
+    // ORB extraction
+    mvKeys = keypoint;
+    mvKeysUn = keypoint_un;
+    mDescriptors = descriptor;
+
+    N = mvKeysUn.size();
+    SetPose(Tcw);
+    /*if(mvKeys.empty())
+        return;*/
+
+    //UndistortKeyPoints();
+
+    // Set no stereo information
+    mvuRight = vector<float>(N,-1);
+    mvDepth = vector<float>(N,-1);
+
+    mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
+    mvbOutlier = vector<bool>(N,false);
+
+    // This is done only for the first Frame (or after a change in the calibration)
+    {
+        mnMinX = 0.0f;
+        mnMaxX = imgSize.width;
+        mnMinY = 0.0f;
+        mnMaxY = imgSize.height;
+
+        mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
+        mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
+
+        fx = K.at<float>(0,0);
+        fy = K.at<float>(1,1);
+        cx = K.at<float>(0,2);
+        cy = K.at<float>(1,2);
+        invfx = 1.0f/fx;
+        invfy = 1.0f/fy;
+
+    }
+
+    mb = mbf/fx;
+    if(mvKeysUn.size()>0)
+        AssignFeaturesToGrid();
+}
+
 void Frame::AssignFeaturesToGrid()
 {
     int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);

src/KeyFrame.cc

@@ -662,4 +662,50 @@ float KeyFrame::ComputeSceneMedianDepth(const int q)
     return vDepths[(vDepths.size()-1)/q];
 }
 
+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) {}
+
+// For boost serialize
+template<class Archive>
+void KeyFrame::serialize(Archive &ar, const unsigned int version)
+{
+    ar & mnId;
+    ar & const_cast<std::vector<cv::KeyPoint> &>(mvKeysUn);
+    ar & const_cast<cv::Mat &>(mDescriptors);
+    ar & Tcw;
+    ar & mvpMapPoints;
+}
+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

src/MapPoint.cc

@@ -416,6 +416,31 @@ int MapPoint::PredictScale(const float &currentDist, Frame* pF)
     return nScale;
 }
 
+MapPoint::MapPoint()
+    : mnFirstKFid(-1),
+      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<MapPoint*>(NULL)),
+      mfMinDistance(0),
+      mfMaxDistance(0)
+      {}
+
+template <class Archive>
+void MapPoint::serialize(Archive& ar, const unsigned int version) {
+    ar& mWorldPos;
+}
+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