Merge pull request VIS4ROB-lab#41 from VIS4ROB-lab/devel

Update master branch to v1.1

.gitignore

@@ -5,3 +5,5 @@ cslam/thirdparty/g2o/build/
 cslam/thirdparty/g2o/lib/
 *~
 *.user
+cslam/output/map_data
+cslam/output/saved_maps

cslam/CMakeLists.txt

@@ -63,9 +63,17 @@ if(NOT ${ROS_VERSION_INDIGO})
     find_package(OpenCV REQUIRED)
 endif()
 
-find_package(catkin REQUIRED COMPONENTS roscpp cv_bridge tf ccmslam_msgs pcl_ros tf_conversions image_transport)
+find_package(catkin REQUIRED COMPONENTS roscpp cv_bridge tf ccmslam_msgs pcl_ros tf_conversions image_transport std_msgs message_generation)
 find_package(cmake_modules REQUIRED)
 
+add_service_files(
+  FILES
+  ServiceSaveMap.srv
+)
+
+# Generate added messages and services
+generate_messages(DEPENDENCIES std_msgs)
+
 if(NOT ${ROS_VERSION_INDIGO})
 find_package(Eigen3 REQUIRED 3.0)
 else()
@@ -96,6 +104,7 @@ catkin_package(
 include_directories(
   ${PROJECT_SOURCE_DIR}
   ${PROJECT_SOURCE_DIR}/include
+  ${PROJECT_SOURCE_DIR}/thirdparty
   ${catkin_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${Eigen_INCLUDE_DIRS}

cslam/conf/config.yaml

@@ -6,6 +6,11 @@
 
 Stats.WriteKFsToFile: 1
 
+# Format for the trajectory output file
+Stats.trajectory_format: 1 # {0=EUROC|1=TUM}
+# 0 = EUROC: EuRoC format - stamp[ns], tx, ty, tz, qw, qx, qy, qz
+# 1 = TUM: TUM format - stamp[s] tx ty tz qx qy qz qw
+
 #--------------------------------------------------------------------------------------------
 # Timing Parameters - After every iteration, the respective model will sleept for x micro-secs
 #--------------------------------------------------------------------------------------------
@@ -88,7 +93,7 @@ Comm.Client.PubMaxKFs: 40
 Comm.Client.PubMaxMPs: 2500
 
 Comm.Server.PubFreq: 1.0
-Comm.Server.KfsToClient: 5
+Comm.Server.KfsToClient: 0
 
 # Maximum Number of KFs that can be processed per iteration of the communication module
 Comm.Server.KfItBound: 400
@@ -141,7 +146,7 @@ Viewer.CovGraphMinFeats: 100
 Viewer.ScaleFactor: 20.0
 
 # Line Diameters
-Viewer.TrajMarkerSize: 0.5
+Viewer.TrajMarkerSize: 0.25
 Viewer.CovGraphMarkerSize: 0.02
 Viewer.LoopMarkerSize: 0.2
 Viewer.MarkerSphereDiameter: 0.05

cslam/include/cslam/ClientHandler.h

@@ -83,7 +83,7 @@ class ClientHandler : public boost::enable_shared_from_this<ClientHandler>
     typedef boost::shared_ptr<KeyFrameDatabase> dbptr;
     typedef boost::shared_ptr<KeyFrame> kfptr;
 public:
-    ClientHandler(ros::NodeHandle Nh, ros::NodeHandle NhPrivate, vocptr pVoc, dbptr pDB, mapptr pMap, size_t ClientId, uidptr pUID, eSystemState SysState, const string &strCamFile, viewptr pViewer);
+    ClientHandler(ros::NodeHandle Nh, ros::NodeHandle NhPrivate, vocptr pVoc, dbptr pDB, mapptr pMap, size_t ClientId, uidptr pUID, eSystemState SysState, const string &strCamFile, viewptr pViewer, bool bLoadMap = false);
     #ifdef LOGGING
     void InitializeThreads(boost::shared_ptr<estd::mylog> pLogger = nullptr);
     #else
@@ -109,6 +109,11 @@ class ClientHandler : public boost::enable_shared_from_this<ClientHandler>
     void CamImgCb(sensor_msgs::ImageConstPtr pMsg);
     void Reset();
 
+    //---Map Save/Load---
+    void LoadMap(const string &path_name);
+    void SaveMap(const string &path_name);
+    bool mbLoadedMap = false; //indicates that map for this client was loaded from a file (only works for client 0)
+
 //    #ifdef LOGGING
 //    void SetLogger(boost::shared_ptr<estd::mylog> pLogger);
 //    #endif
@@ -120,7 +125,7 @@ class ClientHandler : public boost::enable_shared_from_this<ClientHandler>
     void InitializeCC();
     #endif
     void InitializeClient();
-    void InitializeServer();
+    void InitializeServer(bool bLoadMap = false);
 
     //infrastructure
     ccptr mpCC;

cslam/include/cslam/Communicator.h

@@ -107,7 +107,7 @@ class Communicator : public boost::enable_shared_from_this<Communicator>
 
 public:
     //---constructor---
-    Communicator(ccptr pCC, vocptr pVoc, mapptr pMap, dbptr pKFDB);
+    Communicator(ccptr pCC, vocptr pVoc, mapptr pMap, dbptr pKFDB, bool bLoadedMap = false);
 
     //---main---
     void RunClient();
@@ -142,6 +142,7 @@ class Communicator : public boost::enable_shared_from_this<Communicator>
     matchptr mpMapMatcher;
     size_t mClientId;
     const double mdPeriodicTime;
+    bool mbLoadedMap = false; //indicates that map for this client was loaded from a file (only works for client 0)
 
     kfptr mpNearestKF; //client: store last nearest KF -- server: store current nearest KF
     idpair mNearestKfId;

cslam/include/cslam/KeyFrame.h

@@ -53,6 +53,18 @@
 #include <ccmslam_msgs/KFred.h>
 #include <ccmslam_msgs/Map.h>
 
+//SERIALIZATION
+#include "../../thirdparty/cereal/cereal.hpp"
+#include "../../thirdparty/cereal/types/memory.hpp"
+#include "../../thirdparty/cereal/types/utility.hpp"
+#include "../../thirdparty/cereal/types/vector.hpp"
+#include "../../thirdparty/cereal/types/polymorphic.hpp"
+#include "../../thirdparty/cereal/types/concepts/pair_associative_container.hpp"
+#include "../../thirdparty/cereal/types/base_class.hpp"
+#include "../../thirdparty/cereal/archives/binary.hpp"
+#include "../../thirdparty/cereal/archives/binary.hpp"
+#include "../../thirdparty/cereal/access.hpp"
+
 using namespace std;
 using namespace estd;
 
@@ -78,9 +90,9 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
 public:
     //---constructor---
     KeyFrame(Frame &F, mapptr pMap, dbptr pKFDB, commptr pComm, eSystemState SysState, size_t UniqueId);
-
     KeyFrame(ccmslam_msgs::KF* pMsg, vocptr pVoc, mapptr pMap, dbptr pKFDB, commptr pComm, eSystemState SysState,
                        size_t UniqueId = defid, g2o::Sim3 mg2oS_wcurmap_wclientmap = g2o::Sim3()); //constructor for message input
+    KeyFrame(vocptr pVoc, mapptr pMap, dbptr pKFDB, commptr pComm, eSystemState SysState, size_t UniqueId); // constructor for save/load
 
     void EstablishInitialConnectionsServer(); //this is necessary, because we cannot use shared_from_this() in constructor
     void EstablishInitialConnectionsClient(); //this is necessary, because we cannot use shared_from_this() in constructor
@@ -194,6 +206,69 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
       return  pKF1->mTimeStamp > pKF2->mTimeStamp;
     }
 
+    //---save/load
+
+    friend class cereal::access;                                                                                                // Serialization
+
+    template<class Archive>
+    void save(Archive &archive) const {
+        // pre-process data
+        this->SaveData();
+        // save
+        archive(mdServerTimestamp, mTimeStamp, mdInsertStamp,
+                mFrameId, mId,
+//                mUniqueId,
+                mVisId,
+                mnGridCols, mnGridRows, mfGridElementWidthInv, mfGridElementHeightInv,
+                fx, fy, cx, cy, invfx, invfy,
+                N,
+//                mvKeys, mvKeysUn,
+//                mKeysAsCvMat,
+                mKeysUnAsCvMat,
+                mDescriptors,
+                mTcp,
+                mnScaleLevels, mfScaleFactor, mfLogScaleFactor,
+                mvScaleFactors, mvLevelSigma2, mvInvLevelSigma2,
+                mnMinX, mnMinY, mnMaxX, mnMaxY, mK,
+                mT_SC,
+                mbSentOnce,
+                Tcw, Twc, Ow, mdPoseTime, Cw,
+                mmMapPoints_minimal,
+//                mParentId,
+                mHalfBaseline
+                );
+    }
+
+    template<class Archive>
+    void load(Archive &archive) {
+        // pre-process data
+        mmMapPoints_minimal.clear();
+        mvLoopEdges_minimal.clear();
+        // load
+        archive(mdServerTimestamp, mTimeStamp, mdInsertStamp,
+                mFrameId, mId,
+//                mUniqueId,
+                mVisId,
+                mnGridCols, mnGridRows, mfGridElementWidthInv, mfGridElementHeightInv,
+                fx, fy, cx, cy, invfx, invfy,
+                N,
+//                mvKeys, mvKeysUn,
+//                mKeysAsCvMat,
+                mKeysUnAsCvMat,
+                mDescriptors,
+                mTcp,
+                mnScaleLevels, mfScaleFactor, mfLogScaleFactor,
+                mvScaleFactors, mvLevelSigma2, mvInvLevelSigma2,
+                mnMinX, mnMinY, mnMaxX, mnMaxY, mK,
+                mT_SC,
+                mbSentOnce,
+                Tcw, Twc, Ow, mdPoseTime, Cw,
+                mmMapPoints_minimal,
+//                mParentId,
+                mHalfBaseline
+                );
+    }
+
     // The following variables are accesed from only 1 thread or never change (no mutex needed).
 public:
     //---environment---
@@ -276,6 +351,15 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
     // Transformation to body frame (for KF write-out
     Eigen::Matrix4d mT_SC;
 
+    //---save/load
+    mutable std::map<int, idpair> mmMapPoints_minimal;
+//    mutable idpair mParentId = defpair;
+    mutable vector<idpair> mvLoopEdges_minimal;
+//    mutable cv::Mat mKeysAsCvMat; // distorted Keys not used on server side
+    mutable cv::Mat mKeysUnAsCvMat;
+    void SaveData() const;
+    void ProcessAfterLoad(map<idpair, idpair> saved_kf_ids_to_sys_ids);
+
     // The following variables need to be accessed trough a mutex to be thread safe.
 protected:
     //---communication---
@@ -339,4 +423,86 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
 
 } //end namespace
 
+namespace cereal {
+
+//save and load function for Eigen::Matrix type
+
+    template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+    inline
+    typename std::enable_if<traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value, void>::type
+    save(Archive& ar, const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix) {
+        const std::int32_t rows = static_cast<std::int32_t>(matrix.rows());
+        const std::int32_t cols = static_cast<std::int32_t>(matrix.cols());
+        ar(rows);
+        ar(cols);
+        ar(binary_data(matrix.data(), rows * cols * sizeof(_Scalar)));
+    }
+
+    template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+    inline
+    typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value, void>::type
+    load(Archive& ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix) {
+        std::int32_t rows;
+        std::int32_t cols;
+        ar(rows);
+        ar(cols);
+
+        matrix.resize(rows, cols);
+
+        ar(binary_data(matrix.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
+    }
+
+//save and load function for cv::Mat type
+    template<class Archive>
+    inline
+    void save(Archive& ar, const cv::Mat& mat) {
+        int rows, cols, type;
+        bool continuous;
+
+        rows = mat.rows;
+        cols = mat.cols;
+        type = mat.type();
+        continuous = mat.isContinuous();
+
+        ar & rows & cols & type & continuous;
+
+        if (continuous) {
+            const int data_size = rows * cols * static_cast<int>(mat.elemSize());
+            auto mat_data = cereal::binary_data(mat.ptr(), data_size);
+            ar & mat_data;
+        }
+        else {
+            const int row_size = cols * static_cast<int>(mat.elemSize());
+            for (int i = 0; i < rows; i++) {
+                auto row_data = cereal::binary_data(mat.ptr(i), row_size);
+                ar & row_data;
+            }
+        }
+    }
+
+    template<class Archive>
+    void load(Archive& ar, cv::Mat& mat) {
+        int rows, cols, type;
+        bool continuous;
+
+        ar & rows & cols & type & continuous;
+
+        if (continuous) {
+            mat.create(rows, cols, type);
+            const int data_size = rows * cols * static_cast<int>(mat.elemSize());
+            auto mat_data = cereal::binary_data(mat.ptr(), data_size);
+            ar & mat_data;
+        }
+        else {
+            mat.create(rows, cols, type);
+            const int row_size = cols * static_cast<int>(mat.elemSize());
+            for (int i = 0; i < rows; i++) {
+                auto row_data = cereal::binary_data(mat.ptr(i), row_size);
+                ar & row_data;
+            }
+        }
+    }
+
+} /* namespace cereal */
+
 #endif

cslam/include/cslam/Map.h

@@ -42,6 +42,7 @@
 #include <cslam/MapPoint.h>
 #include <cslam/KeyFrame.h>
 #include <cslam/Communicator.h>
+#include <cslam/ORBVocabulary.h>
 
 #ifdef DENSEMAP2
 #include <dense_mapping_backend/Interfaces.h>
@@ -60,6 +61,7 @@ class CentralControl;
 class Communicator;
 class KeyFrame;
 class MapPoint;
+class KeyFrameDatabase;
 //------------
 
 class Map : public boost::enable_shared_from_this<Map>
@@ -70,6 +72,7 @@ class Map : public boost::enable_shared_from_this<Map>
     typedef boost::shared_ptr<MapPoint> mpptr;
     typedef boost::shared_ptr<CentralControl> ccptr;
     typedef boost::shared_ptr<Communicator> commptr;
+    typedef boost::shared_ptr<KeyFrameDatabase> dbptr;
 
     struct kftimecmp{
         bool operator() (const kfptr pA, const kfptr pB) const;
@@ -258,6 +261,10 @@ class Map : public boost::enable_shared_from_this<Map>
     void WriteStateToCsv(const std::string& filename,
                          const size_t clientId);
 
+    //---Map Save/Load---
+    void LoadMap(const string &path_name, vocptr voc, commptr comm, dbptr kfdb, uidptr uid);
+    void SaveMap(const string &path_name);
+
     #ifdef LOGGING
     bool IsInUse(){return !mbOutdated;}
     #endif