Skip to content

Commit

Permalink
remove conditional compilation of map save/load functionality
Browse files Browse the repository at this point in the history
  • Loading branch information
Alkaid-Benetnash authored and v-gezuo committed Aug 28, 2017
1 parent a7a91d1 commit e443ba5
Show file tree
Hide file tree
Showing 16 changed files with 33 additions and 109 deletions.
17 changes: 6 additions & 11 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -77,18 +77,13 @@ ${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
)
if (NOT DEFINED USE_MAP_SAVE_LOAD)
set(USE_MAP_SAVE_LOAD OFF)
endif()
if(USE_MAP_SAVE_LOAD)
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})
target_compile_definitions(${PROJECT_NAME} PUBLIC FUNC_MAP_SAVE_LOAD=1)

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

Expand Down
6 changes: 1 addition & 5 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -256,11 +256,7 @@ PS: binary format is loaded faster and text format is more human-readable.

#### Enable:

You can enable this feature by defining a new variable `USE_MAP_SAVE_LOAD` when running cmake.

For example, you can change `cmake .. -DCMAKE_BUILD_TYPE=Release` to `cmake .. -DCMAKE_BUILD_TYPE=Release -DUSE_MAP_SAVE_LOAD=1` after `echo "Configuring and building ORB_SLAM2 ..."` in `build.sh`.

But `CMakeCache.txt` should be deleted if you want to undefine this variable.
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:

Expand Down
6 changes: 2 additions & 4 deletions include/KeyFrame.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,8 @@
#include "KeyFrameDatabase.h"

#include <mutex>
#ifdef FUNC_MAP_SAVE_LOAD
#include "BoostArchiver.h"
#endif

namespace ORB_SLAM2
{

Expand Down Expand Up @@ -117,7 +116,7 @@ class KeyFrame
static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){
return pKF1->mnId<pKF2->mnId;
}
#ifdef FUNC_MAP_SAVE_LOAD

public:
// for serialization
KeyFrame(); // Default constructor for serialization, need to deal with const member
Expand All @@ -127,7 +126,6 @@ class KeyFrame
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive &ar, const unsigned int version);
#endif

// The following variables are accesed from only 1 thread or never change (no mutex needed).
public:
Expand Down
6 changes: 2 additions & 4 deletions include/KeyFrameDatabase.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,7 @@
#include "ORBVocabulary.h"

#include<mutex>
#ifdef FUNC_MAP_SAVE_LOAD
#include "BoostArchiver.h"
#endif

namespace ORB_SLAM2
{
Expand All @@ -58,7 +56,7 @@ class KeyFrameDatabase

// Relocalization
std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F);
#ifdef FUNC_MAP_SAVE_LOAD

public:
// for serialization
KeyFrameDatabase() {}
Expand All @@ -68,7 +66,7 @@ class KeyFrameDatabase
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive &ar, const unsigned int version);
#endif

protected:

// Associated vocabulary
Expand Down
7 changes: 3 additions & 4 deletions include/Map.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,8 @@

#include <mutex>

#ifdef FUNC_MAP_SAVE_LOAD
#include "BoostArchiver.h"
#endif


namespace ORB_SLAM2
{
Expand Down Expand Up @@ -68,13 +67,13 @@ class Map
// This avoid that two points are created simultaneously in separate threads (id conflict)
std::mutex mMutexPointCreation;

#ifdef FUNC_MAP_SAVE_LOAD

private:
// serialize is recommended to be private
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive &ar, const unsigned int version);
#endif

protected:
std::set<MapPoint*> mspMapPoints;
std::set<KeyFrame*> mspKeyFrames;
Expand Down
4 changes: 0 additions & 4 deletions include/MapPoint.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,7 @@

#include<opencv2/core/core.hpp>
#include<mutex>
#ifdef FUNC_MAP_SAVE_LOAD
#include "BoostArchiver.h"
#endif

namespace ORB_SLAM2
{
Expand Down Expand Up @@ -84,7 +82,6 @@ class MapPoint
int PredictScale(const float &currentDist, KeyFrame*pKF);
int PredictScale(const float &currentDist, Frame* pF);

#ifdef FUNC_MAP_SAVE_LOAD
public:
// for serialization
MapPoint();
Expand All @@ -93,7 +90,6 @@ class MapPoint
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive &ar, const unsigned int version);
#endif

public:
long unsigned int mnId;
Expand Down
14 changes: 4 additions & 10 deletions include/System.h
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,10 @@
#include "ORBVocabulary.h"
#include "Viewer.h"

#ifdef FUNC_MAP_SAVE_LOAD
#include "BoostArchiver.h"
// for map file io
#include <fstream>
#endif

namespace ORB_SLAM2
{

Expand All @@ -64,11 +63,7 @@ class System
public:

// Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
#ifdef FUNC_MAP_SAVE_LOAD
System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, bool is_save_map_=false);
#else
System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true);
#endif

// 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.
Expand Down Expand Up @@ -125,12 +120,11 @@ class System
int GetTrackingState();
std::vector<MapPoint*> GetTrackedMapPoints();
std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
#ifdef FUNC_MAP_SAVE_LOAD

private:
// Save/Load functions
void SaveMap(const string &filename);
bool LoadMap(const string &filename);
#endif

private:

Expand All @@ -145,10 +139,10 @@ class System

// Map structure that stores the pointers to all KeyFrames and MapPoints.
Map* mpMap;
#ifdef FUNC_MAP_SAVE_LOAD

string mapfile;
bool is_save_map;
#endif

// 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.
Expand Down
6 changes: 1 addition & 5 deletions include/Tracking.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,13 +54,9 @@ class Tracking
{

public:
#ifdef FUNC_MAP_SAVE_LOAD

Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, bool bReuseMap=false);
#else
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
#endif

// 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 &timestamp);
Expand Down
9 changes: 3 additions & 6 deletions include/Viewer.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,9 @@ class System;
class Viewer
{
public:
#ifdef FUNC_MAP_SAVE_LOAD

Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath, bool mbReuseMap);
#else
Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath);
#endif


// 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.
Expand Down Expand Up @@ -83,9 +81,8 @@ class Viewer

bool mbStopped;
bool mbStopRequested;
#ifdef FUNC_MAP_SAVE_LOAD
bool mbReuseMap;
#endif

std::mutex mMutexStop;

};
Expand Down
4 changes: 2 additions & 2 deletions src/KeyFrame.cc
Original file line number Diff line number Diff line change
Expand Up @@ -661,7 +661,7 @@ float KeyFrame::ComputeSceneMedianDepth(const int q)

return vDepths[(vDepths.size()-1)/q];
}
#ifdef FUNC_MAP_SAVE_LOAD

// Default serializing Constructor
KeyFrame::KeyFrame():
mnFrameId(0), mTimeStamp(0.0), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
Expand Down Expand Up @@ -745,5 +745,5 @@ void KeyFrame::serialize(Archive &ar, const unsigned int version)
}
template void KeyFrame::serialize(boost::archive::binary_iarchive&, const unsigned int);
template void KeyFrame::serialize(boost::archive::binary_oarchive&, const unsigned int);
#endif

} //namespace ORB_SLAM
3 changes: 1 addition & 2 deletions src/KeyFrameDatabase.cc
Original file line number Diff line number Diff line change
Expand Up @@ -308,7 +308,6 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
return vpRelocCandidates;
}

#ifdef FUNC_MAP_SAVE_LOAD
template<class Archive>
void KeyFrameDatabase::serialize(Archive &ar, const unsigned int version)
{
Expand All @@ -319,5 +318,5 @@ void KeyFrameDatabase::serialize(Archive &ar, const unsigned int version)
}
template void KeyFrameDatabase::serialize(boost::archive::binary_iarchive&, const unsigned int);
template void KeyFrameDatabase::serialize(boost::archive::binary_oarchive&, const unsigned int);
#endif

} //namespace ORB_SLAM
4 changes: 2 additions & 2 deletions src/Map.cc
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,7 @@ void Map::clear()
mvpReferenceMapPoints.clear();
mvpKeyFrameOrigins.clear();
}
#ifdef FUNC_MAP_SAVE_LOAD

template<class Archive>
void Map::serialize(Archive &ar, const unsigned int version)
{
Expand All @@ -142,5 +142,5 @@ void Map::serialize(Archive &ar, const unsigned int version)
}
template void Map::serialize(boost::archive::binary_iarchive&, const unsigned int);
template void Map::serialize(boost::archive::binary_oarchive&, const unsigned int);
#endif

} //namespace ORB_SLAM
3 changes: 1 addition & 2 deletions src/MapPoint.cc
Original file line number Diff line number Diff line change
Expand Up @@ -416,7 +416,6 @@ int MapPoint::PredictScale(const float &currentDist, Frame* pF)
return nScale;
}

#ifdef FUNC_MAP_SAVE_LOAD
MapPoint::MapPoint():
nObs(0), mnTrackReferenceForFrame(0),
mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
Expand Down Expand Up @@ -454,6 +453,6 @@ void MapPoint::serialize(Archive &ar, const unsigned int version)
}
template void MapPoint::serialize(boost::archive::binary_iarchive&, const unsigned int);
template void MapPoint::serialize(boost::archive::binary_oarchive&, const unsigned int);
#endif


} //namespace ORB_SLAM
Loading

0 comments on commit e443ba5

Please sign in to comment.