CCM-SLAM Update 1.1: Testing, cleaning, readme update

cslam/conf/config.yaml

@@ -88,7 +88,7 @@ Comm.Client.PubMaxKFs: 40
 Comm.Client.PubMaxMPs: 2500
 
 Comm.Server.PubFreq: 1.0
-Comm.Server.KfsToClient: 2
+Comm.Server.KfsToClient: 0
 
 # Maximum Number of KFs that can be processed per iteration of the communication module
 Comm.Server.KfItBound: 400

cslam/include/cslam/KeyFrame.h

@@ -214,15 +214,6 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
     void save(Archive &archive) const {
         // pre-process data
         this->SaveData();
-//        mmMapPoints_minimal.clear();
-//        mvLoopEdges_minimal.clear();
-//        for (size_t indx = 0; indx < mvpMapPoints.size(); indx++) {
-//            if(mvpMapPoints[indx] != nullptr)
-//                mmMapPoints_minimal.insert(std::make_pair(indx, mvpMapPoints[indx]->mId));
-//        }
-//        if(mpParent) mParentId = mpParent->mId;
-//        for(auto kf : mspLoopEdges)
-//            mvLoopEdges_minimal.push_back(kf->mId);
         // save
         archive(mdServerTimestamp, mTimeStamp, mdInsertStamp,
                 mFrameId, mId,
@@ -232,7 +223,8 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
                 fx, fy, cx, cy, invfx, invfy,
                 N,
 //                mvKeys, mvKeysUn,
-                mKeysAsCvMat, mKeysUnAsCvMat,
+//                mKeysAsCvMat,
+                mKeysUnAsCvMat,
                 mDescriptors,
                 mTcp,
                 mnScaleLevels, mfScaleFactor, mfLogScaleFactor,
@@ -261,7 +253,8 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
                 fx, fy, cx, cy, invfx, invfy,
                 N,
 //                mvKeys, mvKeysUn,
-                mKeysAsCvMat, mKeysUnAsCvMat,
+//                mKeysAsCvMat,
+                mKeysUnAsCvMat,
                 mDescriptors,
                 mTcp,
                 mnScaleLevels, mfScaleFactor, mfLogScaleFactor,
@@ -362,7 +355,7 @@ class KeyFrame : public boost::enable_shared_from_this<KeyFrame>
     mutable std::map<int, idpair> mmMapPoints_minimal;
 //    mutable idpair mParentId = defpair;
     mutable vector<idpair> mvLoopEdges_minimal;
-    mutable cv::Mat mKeysAsCvMat;
+//    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);

cslam/launch/Server.launch

@@ -23,7 +23,7 @@
 
   <param name="NumOfClients" type="int" value="4" />
 
-  <param name="LoadMap" type="bool" value="true" />
+  <param name="LoadMap" type="bool" value="false" />
 
   <!-- Client 0 -->
 

cslam/src/ClientHandler.cpp

@@ -314,17 +314,6 @@ void ClientHandler::LoadMap(const std::string &path_name) {
 //    cout << "Trigger GBA" << endl;
 //    mpMap->RequestBA(mpCC->mClientId);
 
-//    std::cout << "--> Show map" << std::endl;
-//    if(params::vis::mbActive)
-//        mpViewer->DrawMap(mpMap);
-//    std::cout << "----> Done" << std::endl;
-
-//    std::cout << "... push key to continue" << std::endl;
-//    std::cin.get();
-
-//    cout << "Trigger GBA" << endl;
-//    mpMap->RequestBA(mpCC->mClientId);
-
 //    std::cout << "--> Show map" << std::endl;
 //    if(params::vis::mbActive)
 //        mpViewer->DrawMap(mpMap);

cslam/src/Communicator.cpp

@@ -48,7 +48,6 @@ Communicator::Communicator(ccptr pCC, vocptr pVoc, mapptr pMap, dbptr pKFDB, boo
     mnWeakAckKF = KFRANGE;
     mnWeakAckMP = MPRANGE;
 
-//    std::cout << "mbLoadedMap: " << (int)mbLoadedMap << std::endl;
     if(mbLoadedMap) return; //do not register communication infrastructure (publisher/subscriber) when map is loaded
 
     //Topics

cslam/src/KeyFrame.cpp

@@ -1246,28 +1246,12 @@ void KeyFrame::SaveData() const {
         if(mvpMapPoints[indx] != nullptr)
             mmMapPoints_minimal.insert(std::make_pair(indx, mvpMapPoints[indx]->mId));
     }
-//    if(mpParent) mParentId = mpParent->mId;
     for(auto kf : mspLoopEdges)
         mvLoopEdges_minimal.push_back(kf->mId);
 
-//    if(mvKeys.size() != mvKeysUn.size()) {
-//        std::cout << COUTERROR << "mvKeys.size() != mvKeysUn.size()" << std::endl;
-//        std::cout << "mvKeys.size(): " << mvKeys.size() << std::endl;
-//        std::cout << "mvKeysUn.size(): " << mvKeysUn.size() << std::endl;
-//        exit(-1);
-//    }
-
-//    mKeysAsCvMat = cv::Mat(6,mvKeys.size(),5);
     mKeysUnAsCvMat = cv::Mat(6,mvKeysUn.size(),5);
 
     for(int i=0;i<mvKeysUn.size();++i) {
-//        mKeysAsCvMat.at<float>(0,i) = mvKeys[i].pt.x;
-//        mKeysAsCvMat.at<float>(1,i) = mvKeys[i].pt.y;
-//        mKeysAsCvMat.at<float>(2,i) = mvKeys[i].angle;
-//        mKeysAsCvMat.at<float>(3,i) = mvKeys[i].octave;
-//        mKeysAsCvMat.at<float>(4,i) = mvKeys[i].response;
-//        mKeysAsCvMat.at<float>(5,i) = mvKeys[i].size;
-
         mKeysUnAsCvMat.at<float>(0,i) = mvKeysUn[i].pt.x;
         mKeysUnAsCvMat.at<float>(1,i) = mvKeysUn[i].pt.y;
         mKeysUnAsCvMat.at<float>(2,i) = mvKeysUn[i].angle;

cslam/src/LoopFinder.cpp

@@ -862,16 +862,6 @@ void LoopFinder::RunGBA(idpair nLoopKF, set<idpair> sChangedKFs)
 {
     cout << "-> Starting Global Bundle Adjustment" << endl;
 
-//    if(params::stats::mbWriteKFsToFile)
-//    {
-//        for(int it=0;it<4;++it)
-//        {
-//            std::stringstream ss;
-//            ss << params::stats::msOutputDir << "KF_bef_GBA_" << it << ".csv";
-//            mpMap->WriteStateToCsv(ss.str(),it);
-//        }
-//    }
-
 //    struct timeval tStart,tNow;
 //    double dEl;
 //    gettimeofday(&tStart,NULL);

cslam/src/Map.cpp

@@ -428,7 +428,6 @@ void Map::LoadMap(const string &path_name, vocptr voc, commptr comm, dbptr kfdb,
     size_t next_kf_id0 = 0;
     size_t next_mp_id0 = 0;
 
-    // quickfix char*/string compatibility [TODO] cleaner solution
     std::string kf_tmp = "/keyframes/";
     std::string mp_tmp = "/mappoints/";
     char cstr0[path_name.size()+mp_tmp.size()+1];
@@ -595,8 +594,6 @@ void Map::LoadMap(const string &path_name, vocptr voc, commptr comm, dbptr kfdb,
 
     std::cout << "----> Keyframes" << std::endl;
     for(auto kf : keyframes) {
-//        kf->SetPredecessor(this->GetKeyframe(kf->msg_.id_predecessor));
-//        kf->SetSuccessor(this->GetKeyframe(kf->msg_.id_successor));
         for(auto mit = kf->mmMapPoints_minimal.begin(); mit!=kf->mmMapPoints_minimal.end();++mit){
             size_t feat_id = mit->first;
             idpair lm_id = mit->second;
@@ -615,9 +612,9 @@ void Map::LoadMap(const string &path_name, vocptr voc, commptr comm, dbptr kfdb,
         }
 //        kf->ProcessAfterLoad();
         kf->UpdateConnections();
-        if(!kf->GetParent()) {
-            std::cout << COUTWARN << "KF " << kf->mId.first << "|" << kf->mId.second << " has no parent" << std::endl;
-        }
+//        if(!kf->GetParent()) {
+//            std::cout << COUTWARN << "KF " << kf->mId.first << "|" << kf->mId.second << " has no parent" << std::endl;
+//        }
     }
 
     std::cout << "+++ DONE +++" << std::endl;

cslam/src/MapMerger.cpp

@@ -523,11 +523,11 @@ MapMerger::mapptr MapMerger::MergeMaps(mapptr pMapCurr, mapptr pMapMatch, vector
     cout << "--- Launch GBA thread" << endl;
     pFusedMap->mpThreadGBA = new thread(&MapMerger::RunGBA,this,nLoopKf,pFusedMap);
 
-    std::cout << "MapMerger: Wait for GBA to finish" << std::endl;
-    while(pFusedMap->isRunningGBA()) {
-        usleep(10000);
-    }
-    std::cout << "MapMerger: GBA finished - continue" << std::endl;
+//    std::cout << "MapMerger: Wait for GBA to finish" << std::endl;
+//    while(pFusedMap->isRunningGBA()) {
+//        usleep(10000);
+//    }
+//    std::cout << "MapMerger: GBA finished - continue" << std::endl;
 
     cout << "\033[1;32;41m!!! MAPS MERGED !!!\033[0m" << endl;
     this->SetIdle();

cslam/src/Optimizer.cpp

@@ -792,16 +792,12 @@ void Optimizer::MapFusionGBA(mapptr pMap, size_t ClientId, int nIterations, bool
     optimizer.initMultiThreading();
     std::cout << "------> max Iterations: " << nIterations << std::endl;
     std::cout << "------> vertices|edges: " << vertices << "|" << edges << std::endl;
-//    struct timeval tStart,tNow;
-//    double dEl;
-//    gettimeofday(&tStart,NULL);
+
     auto start = std::chrono::system_clock::now();
     optimizer.optimize(nIterations);
     auto end = std::chrono::system_clock::now();
 
     std::chrono::duration<double> elapsed_seconds = end-start;
-//    gettimeofday(&tNow,NULL);
-//    dEl = tNow.tv_sec - tStart.tv_sec + (tNow.tv_usec - tStart.tv_usec)/1000000.0;
     std::cout << "------> Optimization Time: " << elapsed_seconds.count() << " [s]" << std::endl;
 
     // Recover optimized data

cslam/src/server/ServerSystem.cpp

@@ -95,7 +95,6 @@ void ServerSystem::InitializeClients()
     //Client 0
     bool bLoadMapFromFile;
     mNhPrivate.param("LoadMap",bLoadMapFromFile,false);
-//    cout << "bLoadMapFromFile: " << (int)bLoadMapFromFile << endl;
 
     mpClient0.reset(new ClientHandler(mNh,mNhPrivate,mpVoc,mpKFDB,mpMap0,0,mpUID,eSystemState::SERVER,string(),mpViewer,bLoadMapFromFile));
 

readme.md

@@ -1,5 +1,7 @@
 # CCM-SLAM -- **C**entralized **C**ollaborative **M**onocular SLAM
 
+*Version 1.1*
+
 # 1 Related Publications
 
 [1] Patrik Schmuck and Margarita Chli. **Multi-UAV Collaborative Monocular SLAM**. *IEEE International Conference on Robotics and Automation (ICRA)*, 2017. **[PDF](https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/272499/eth-50606-01.pdf?sequence=1&isAllowed=y)**.
@@ -25,7 +27,7 @@ Compared to the implementation described in [2], some modules of this framework
 
 CCM-SLAM is released under a [GPLv3 license](https://github.com/VIS4ROB-lab/ccm_slam/blob/master/licencse_gpl.txt). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/VIS4ROB-lab/ccm_slam/blob/master/cslam/thirdparty/thirdparty_code.md).
 
-For a closed-source version of CCM-SLAM for commercial purposes, please contact the authors:  
+For a closed-source version of CCM-SLAM for commercial purposes, please contact the author(s):  
 pschmuck (at) ethz (dot) ch.
 
 If you use CCM-SLAM in an academic work, please cite:
@@ -156,13 +158,19 @@ We provide two launch files for the KITTI odometry [dataset](http://www.cvlibs.n
 * ```Client0_kitti.launch``` loads the camera parameters from ```kitti_mono.yaml``` and uses the images from the dataset *as is*
 * ```Client0_kitti_half_res.launch``` loads the camera parameters from ```kitti_mono_half_res.yaml``` for images *downsampled by factor 0.5*. In our tests, this alleviated the initialization problems.
 
-## Running CCM-SLAM on multiple PCs
+## 4.3 Running CCM-SLAM on multiple PCs
 
 * All PCs need to be in the same network!
 * Find the IP address of the PC intented to run the server using ```ifconfig```. Make sure to pick the IP from the wireless interface.
 * Start a ```roscore``` on the Server PC.
 * On **all** participating PC, in **every** terminal used for CCM-SLAM (no matter whether it is for running camera drivers, bagfiles, a CCM-SLAM launch file or RVIZ), execute: ```export ROS_MASTER_URI=http://IP_OF_SERVER:11311```
 
+## 4.4
+
+* CCM-SLAM offers functionalities to save and load maps. The folder for map data is ```~/ccmslam_ws/src/ccm_slam/cslam/output/map_data```.
+* Maps can be saved using the ROS service ```ccmslam_savemap```: ```rosservice call ccmslam/ccmslam_savemap X``` where ```X``` is the ID if the map to be saved (usually 0). The folder ```map_data``` needs to be emtpy to save a map.
+* To load maps, set the parameter ```LoadMap``` in ```Server.launch``` to true. No matter whether the map contains data from 1 ore more agents, all data is re-mapped to agent 0 (i.e. agent ID is 0). Since the loaded map is associated to agent 0, the communication for this agent is deactivated (i.e. after loading a map, ```Client0_euroc.launch``` cannot be used)
+
 # 5. Using your own Data
 
 For using you own datasets or camera, you need to create according calibration and launch files:
@@ -203,3 +211,11 @@ System parameters are loaded from ```conf/config.yaml```. We explain the functio
 
 **Other**
 * ```Stats.WriteKFsToFile```: Write KFs to cslam/output. **Attention**: Before being written to the csv-file, KFs are transformed to the body frame of the robot using the transformation given in the camera calibration file by ```T_imu_cam0```.
+
+# 7. Update Notes
+
+### Update 1.1:
+* Added functionalities to save and load maps
+* Tracking: All MapPoints in the local map on the agent are now projected into the current frame to improve robustness
+* Fixed problems with GBA interruption with multiple agents
+* Final GBA: If GBA is interrupted during the active part of the SLAM mission, a GBA will be trigerred after no new messages have arrived at the server for 30s.