Run DistributedMapper tests and support for gtsam 4.0+ (#4)

* Compile with gtsam (latest develop branch) * Unclean changes to fix the tests * Edit scripts * Add optimized files * Consolidate changes for PR Minor changes * Fixes to the keyAnchor and use constants from GTSAM Minor changes Minor Minor * Update README.md and remove erroneously added optimizedTUM files Remove more optimizedTUM.txt files Update gtsam commit id on Dockerfile
CogRob · Apr 2, 2021 · a3bbfa9 · a3bbfa9
1 parent 603e346
commit a3bbfa9
Show file tree

Hide file tree

Showing 12 changed files with 92 additions and 97 deletions.
diff --git a/README.md b/README.md
@@ -1,25 +1,26 @@
 Distributed-Mapper
 ===================================================
-This library is an implementation of the algorithm described in Distributed Trajectory Estimation with Privacy and Communication Constraints: 
+This library is an implementation of the algorithm described in Distributed Trajectory Estimation with Privacy and Communication Constraints:
 a Two-Stage Distributed Gauss-Seidel Approach. The core library is developed in C++
 language.
 
-Distributed-Mapper is developed by [Siddharth Choudhary](http://www.cc.gatech.edu/~choudhar/), 
-[Luca Carlone](http://www.lucacarlone.com/), [Carlos Nieto](https://scholar.google.com/citations?user=p3S78jsAAAAJ&hl=en) and [John Rogers](https://scholar.google.com/citations?user=uH_LDocAAAAJ&hl=en) as part of the collaboration between Georgia Tech, MIT and Army Research Lab. 
+Distributed-Mapper is developed by [Siddharth Choudhary](http://www.cc.gatech.edu/~choudhar/),
+[Luca Carlone](http://www.lucacarlone.com/), [Carlos Nieto](https://scholar.google.com/citations?user=p3S78jsAAAAJ&hl=en) and [John Rogers](https://scholar.google.com/citations?user=uH_LDocAAAAJ&hl=en) as part of the collaboration between Georgia Tech, MIT and Army Research Lab.
 
 Prerequisites
 ------
 
 - CMake (Ubuntu: `sudo apt-get install cmake`), compilation configuration tool.
 - [Boost](http://www.boost.org/)  (Ubuntu: `sudo apt-get install libboost-all-dev`), portable C++ source libraries.
-- [GTSAM](https://bitbucket.org/gtborg/gtsam) develop branch, a C++ library that implement smoothing and mapping (SAM) framework in robotics and vision. Here we use factor graph implementations and inference/optimization tools provided by GTSAM. To install a particular commit of GTSAM follow the following instructions: 
+- [GTSAM](https://github.com/borglab/gtsam) develop branch, a C++ library that implement smoothing and mapping (SAM) framework in robotics and vision. Here we use factor graph implementations and inference/optimization tools provided by GTSAM. This repository has been tested on the latest develop branch of GTSAM 4.0 (Commit ID: `d304358deeaa4625cf24a8e0d94145bb3435d5bc`).
+- To install a particular commit of GTSAM follow the following instructions:
 
 ```
-$ git clone https://bitbucket.org/gtborg/gtsam
-$ git checkout b7c695fa71efd43b40972eec154df265617fc07d -b dist-mapper
-$ mkdir build
+$ git clone https://github.com/borglab/gtsam.git && cd gtsam
+$ git checkout d304358deeaa4625cf24a8e0d94145bb3435d5bc -b dist-mapper
+$ mkdir build && cd build
 $ cmake ..
-$ make -j8
+$ make -j$(nproc)
 $ sudo make install
 ```
 
@@ -33,7 +34,7 @@ $ mkdir build
 $ cd build
 $ cmake ..
 $ make -j3
-$ make check  # optonal, run unit tests
+$ make check  # optional, run unit tests
 $ make install
 ```
 
@@ -71,8 +72,8 @@ Data Format
 Each robot's graph is written in [g2o](https://github.com/RainerKuemmerle/g2o/wiki/File-Format) format and is indexed from 0. For example, for a 4 robot scenario, the directory will contain ```0.g2o```, ```1.g2o```, ```2.g2o``` and ```3.g2o```. An example dataset for 4 robots is given in ```data/example_4robots```. Each robot is specified using a character prefix symbol like 'a', 'b', 'c', 'd' for 4 robot case.
 
 ### Vertices ###
-All the vertices corresponding to the first robot will be prefixed using 'a' using ```gtsam.Symbol``` like  ```gtsam.Symbol('a', 1)```, ```gtsam.Symbol('a',2)``` etc. Similarly the second robot will be prefixed using 'b' like ```gtsam.Symbol('b', 1)```, ```gtsam.Symbol('b',2)``` etc. 
-For example, the vertices for the first robot (in ```0.g2o```) in 4 robot scenario is written as,  
+All the vertices corresponding to the first robot will be prefixed using 'a' using ```gtsam.Symbol``` like  ```gtsam.Symbol('a', 1)```, ```gtsam.Symbol('a',2)``` etc. Similarly the second robot will be prefixed using 'b' like ```gtsam.Symbol('b', 1)```, ```gtsam.Symbol('b',2)``` etc.
+For example, the vertices for the first robot (in ```0.g2o```) in 4 robot scenario is written as,
 ```
 VERTEX_SE3:QUAT 6989586621679009792 0.324676 0.212487 0.042821 0.00270783 0.0121983 0.00760222 0.999893
 VERTEX_SE3:QUAT 6989586621679009793 0.0716917 2.00724 -0.0729262 -0.00363348 0.00166876 0.00765756 0.999963
@@ -92,8 +93,8 @@ VERTEX_SE3:QUAT 6989586621679009844 1.8204 -0.431461 6.01878 0.00230292 -0.00198
 VERTEX_SE3:QUAT 6989586621679009845 1.92566 2.23857 6.19073 0.0152246 0.0101054 -0.00845407 0.999797
 ```
 ### Intra-Robot Edges ###
- All the non-communication edges corresponding to the first robot is written in g2o format in ```0.g2o``` and likewise for the other robots in ```1.g2o```, ```2.g2o```, ```3.g2o``` respectively. For example, the intra-robot edges for the first robot (in ```0.g2o```) in 4 robot scenario is written as,  
- 
+ All the non-communication edges corresponding to the first robot is written in g2o format in ```0.g2o``` and likewise for the other robots in ```1.g2o```, ```2.g2o```, ```3.g2o``` respectively. For example, the intra-robot edges for the first robot (in ```0.g2o```) in 4 robot scenario is written as,
+
 ```
 EDGE_SE3:QUAT 6989586621679009792 6989586621679009793 -0.390787 2.11308 0.0155589 0.00042254 -0.00328797 -0.010647 0.999938 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
 EDGE_SE3:QUAT 6989586621679009792 6989586621679009796 2.30676 0.546708 0.0334878 -0.00546279 -0.00389958 0.00693602 0.999953 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
@@ -126,7 +127,7 @@ EDGE_SE3:QUAT 6989586621679009829 6989586621679009845 -0.00151322 0.142061 1.907
 ```
 
 ### Inter-Robot Communication Edges ###
-Communication edges between the two robots are written in the g2o files corresponding to both the robots.  For example, the communication edges between the first and second robot (in ```0.g2o``` and ```1.g2o```) in 4 robot scenario is written as,  
+Communication edges between the two robots are written in the g2o files corresponding to both the robots.  For example, the communication edges between the first and second robot (in ```0.g2o``` and ```1.g2o```) in 4 robot scenario is written as,
 
 ```
 EDGE_SE3:QUAT 6989586621679009793 7061644215716937730 0.160437 1.90624 -0.00931847 0.00910267 0.0069412 0.000641041 0.999934 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
@@ -163,7 +164,7 @@ If you use this work, please cite any of the following publications:
 	       John Rogers and
                Henrik I. Christensen and
                Frank Dellaert},
-  title     = {Distributed Trajectory Estimation with Privacy and Communication Constraints: 
+  title     = {Distributed Trajectory Estimation with Privacy and Communication Constraints:
 a Two-Stage Distributed Gauss-Seidel Approach},
   booktitle = {IEEE International Conference on Robotics and Automation 2016},
   year      = {2016}

diff --git a/distributed_mapper_core/cpp/CMakeLists.txt b/distributed_mapper_core/cpp/CMakeLists.txt
@@ -12,6 +12,7 @@ message("GTSAM Libraries: ${GTSAM_LIBRARIES}")
 
 # for unittest scripts
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools")
+include(GtsamBuildTypes)
 include(GtsamTesting)
 
 ###########################################################################

diff --git a/distributed_mapper_core/cpp/scripts/runDistributedMapper.cpp b/distributed_mapper_core/cpp/scripts/runDistributedMapper.cpp
@@ -262,8 +262,6 @@ int main(int argc, char* argv[])
   // Distributed Estimate
 
   if(!disconnectedGraph){
-      try{
-        // try optimizing
         vector< Values > estimates =  distributedOptimizer(distMappers, maxIter, updateType, gamma,
                                                            rotationEstimateChangeThreshold, poseEstimateChangeThreshold, useFlaggedInit, useLandmarks,
                                                            debug, rotationTrace, poseTrace, subgraphRotationTrace, subgraphPoseTrace, rotationVectorValuesTrace);
@@ -337,12 +335,6 @@ int main(int argc, char* argv[])
         ////////////////////////////////////////////////////////////////////////////////
         std::cout << "Distributed Error: " << chordalGraph.error(distributed) << std::endl;
 
-      }
-      catch(...){
-        // Optimization failed (maybe due to disconnected graph)
-        // Copy initial to optimized g2o files in that case
-        copyInitial(nrRobots, dataDir);
-      }
     }
   else{
       // Graph is disconnected

diff --git a/distributed_mapper_core/cpp/src/BetweenChordalFactor.h b/distributed_mapper_core/cpp/src/BetweenChordalFactor.h
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -69,14 +69,14 @@ namespace gtsam {
     virtual ~BetweenChordalFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /** implement functions needed for Testable */
 
     /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "BetweenChordalFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
@@ -85,7 +85,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
     }
@@ -97,22 +97,22 @@ namespace gtsam {
 
     /** vector of errors */
   Vector evaluateError(const T& pi, const T& pj, boost::optional<Matrix&> Hi =
-      boost::none, boost::optional<Matrix&> Hj = boost::none) const {
+      boost::none, boost::optional<Matrix&> Hj = boost::none) const override {
 
     Matrix3 S1 = skewSymmetric(-1,0,0); //(0 0 0, 0 0 1, 0 -1 0);
     Matrix3 S2 = skewSymmetric(0,-1,0); //sparse([0 0 -1; 0 0 0; 1  0 0]);
     Matrix3 S3 = skewSymmetric(0,0,-1); //sparse([0 1  0;-1 0 0; 0  0 0]);
 
     Matrix3 Rij = measured_.rotation().matrix();
     Matrix3 Rijt = Rij.transpose();
-    Vector3 tij = measured_.translation().vector();
+    Vector3 tij = measured_.translation();
 
 
     Matrix3 Ri = pi.rotation().matrix();
-    Vector3 ti = pi.translation().vector();
+    Vector3 ti = pi.translation();
 
     Matrix3 Rj = pj.rotation().matrix();
-    Vector3 tj = pj.translation().vector();
+    Vector3 tj = pj.translation();
 
     Matrix3 Ri_Rij = Ri*Rij;
     Matrix3 error_R = Ri_Rij - Rj;
@@ -131,14 +131,14 @@ namespace gtsam {
       Hi->block(3,0,3,3) = Ri_Rij*S2*Rijt;
       Hi->block(6,0,3,3) = Ri_Rij*S3*Rijt;
       Hi->block(9,0,3,3) = Ri*skewSymmetric(tij);
-      Hi->block(9,3,3,3) = -eye(3); // TODO: define once outside
+      Hi->block(9,3,3,3) = -Matrix::Identity(3, 3); // TODO: define once outside
 
       // fill in Jacobian wrt pj
       *Hj = Matrix::Zero(12,6);
       Hj->block(0,0,3,3) = - Rj*S1;
       Hj->block(3,0,3,3) = - Rj*S2;
       Hj->block(6,0,3,3) = - Rj*S3;
-      Hj->block(9,3,3,3) = eye(3);  // TODO: define once outside
+      Hj->block(9,3,3,3) = Matrix::Identity(3, 3);  // TODO: define once outside
 
     }
 

diff --git a/distributed_mapper_core/cpp/src/DistributedMapper.cpp b/distributed_mapper_core/cpp/src/DistributedMapper.cpp
@@ -69,7 +69,7 @@ DistributedMapper::createSubgraphInnerAndSepEdges(const NonlinearFactorGraph& su
 void
 DistributedMapper::loadSubgraphAndCreateSubgraphEdge(GraphAndValues graphAndValues){
   graph_ = *(graphAndValues.first);
-  initial_ = *(graphAndValues.second);    
+  initial_ = *(graphAndValues.second);
 
   // Convert initial values into row major vector values
   linearizedRotation_ = multirobot_util::rowMajorVectorValues(initial_);
@@ -112,7 +112,7 @@ DistributedMapper::estimateRotation(){
 
     Pose3 relativePose = pose3Between->measured();
     Matrix3 R01t = relativePose.rotation().transpose().matrix();
-    Matrix M9 = Matrix::Zero(9,9);
+    Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = R01t;
     M9.block(3,3,3,3) = R01t;
     M9.block(6,6,3,3) = R01t;
@@ -131,7 +131,7 @@ DistributedMapper::estimateRotation(){
     // if using between noise, use the factor noise model converted to a conservative diagonal estimate
     SharedDiagonal model = rotationNoiseModel_;
     if(useBetweenNoise_){
-        model = multirobot_util::convertToDiagonalNoise(pose3Between->get_noiseModel());
+        model = multirobot_util::convertToDiagonalNoise(pose3Between->noiseModel());
       }
 
     if(robot0 == robotName_){ // robot i owns the first key
@@ -144,7 +144,7 @@ DistributedMapper::estimateRotation(){
       if(!useFlaggedInit_ || neighboringRobotsInitialized_[robot0]){ // if use flagged initialization and robot sharing the edge is already optimized
         Vector r0 = neighborsLinearizedRotations_.at(key0);
         Vector M9_r0 = M9*r0;
-        rotSubgraph.add(key1, I9, M9_r0, model);
+        rotSubgraph.add(key1, I_9x9, M9_r0, model);
       }
     }
     else{
@@ -166,7 +166,7 @@ DistributedMapper::estimateRotation(){
 
     double error = rotSubgraph.error(newLinearizedRotation_);
     rotationErrorTrace_.push_back(error);
-  }  
+  }
 }
 
 //*****************************************************************************
@@ -182,7 +182,7 @@ DistributedMapper::chordalFactorGraph(){
           Pose3 measured = factor->measured();
           if(useBetweenNoise_){
               // Convert noise model to chordal factor noise
-              SharedNoiseModel chordalNoise = multirobot_util::convertToChordalNoise(factor->get_noiseModel());
+              SharedNoiseModel chordalNoise = multirobot_util::convertToChordalNoise(factor->noiseModel());
               //chordalNoise->print("Chordal Noise: \n");
               chordalGraph_.add(BetweenChordalFactor<Pose3>(key1, key2, measured, chordalNoise));
             }
@@ -243,21 +243,21 @@ DistributedMapper::estimatePoses(){
             Vector b = -(M1 * neighborsLinearizedPoses_.at(key1) + error);
             if(useBetweenNoise_){
                 Rot3 rotation = initial_.at<Pose3>(key0).rotation();
-                SharedNoiseModel chordalNoise = multirobot_util::convertToChordalNoise(pose3Between->get_noiseModel(), rotation.matrix());
+                SharedNoiseModel chordalNoise = multirobot_util::convertToChordalNoise(pose3Between->noiseModel(), rotation.matrix());
                 chordalNoise->WhitenSystem(A, b);
               }
             distGFG.add(key0, A, b, poseNoiseModel_);
           }
       }
     else if(robot1 == robotName_){ // robot i owns the second key
-        if(!useFlaggedInit_ || neighboringRobotsInitialized_[robot0]){ // if use flagged initialization and robot sharing the edge is already optimized            
+        if(!useFlaggedInit_ || neighboringRobotsInitialized_[robot0]){ // if use flagged initialization and robot sharing the edge is already optimized
             Vector error = betweenChordalFactor.evaluateError(neighbors_.at<Pose3>(key0), initial_.at<Pose3>(key1), M0, M1);
             // Robot i owns the second key_i, on which we put a prior
             Matrix A = M1;
             Vector b = -(M0 * neighborsLinearizedPoses_.at(key0) + error);
             if(useBetweenNoise_){
                 Rot3 rotation = neighbors_.at<Pose3>(key0).rotation();
-                SharedNoiseModel chordalNoise = multirobot_util::convertToChordalNoise(pose3Between->get_noiseModel(), rotation.matrix());
+                SharedNoiseModel chordalNoise = multirobot_util::convertToChordalNoise(pose3Between->noiseModel(), rotation.matrix());
                 chordalNoise->WhitenSystem(A, b);
               }
             distGFG.add(key1, A, b, poseNoiseModel_);

diff --git a/distributed_mapper_core/cpp/src/DistributedMapper.h b/distributed_mapper_core/cpp/src/DistributedMapper.h
@@ -23,10 +23,8 @@ namespace distributed_mapper{
 
 
 // Static Consts
-static const gtsam::Matrix I9 = gtsam::eye(9);
-static const gtsam::Vector zero9 = gtsam::Vector::Zero(9);
 static const size_t maxIter_ = 1000;
-static const gtsam::Key keyAnchor = gtsam::symbol('Z', 9999999);
+static const gtsam::Key keyAnchor = 99999999;
 
 /**
  * @brief The DistributedMapper class runs distributed mapping algorithm
@@ -219,7 +217,7 @@ class DistributedMapper{
       }
       else{
         neighbors_.insert(key, pose);
-        neighborsLinearizedPoses_.insert(key, gtsam::zero(6));
+        neighborsLinearizedPoses_.insert(key, gtsam::Matrix::Zero(6, 6));
         gtsam::Matrix3 R = pose.rotation().matrix();
         gtsam::Vector r = multirobot_util::rowMajorVector(R);
         neighborsLinearizedRotations_.insert(key, r);
@@ -458,7 +456,7 @@ class DistributedMapper{
 
     char robotName_;// Key for each robot
     gtsam::NonlinearFactorGraph graph_; // subgraph corresponding to each robot
-    gtsam::Values initial_; // subinitials corresponding to each robot        
+    gtsam::Values initial_; // subinitials corresponding to each robot
     gtsam::NonlinearFactorGraph innerEdges_; // edges involving keys from a single robot (exclude separator edges)
     std::vector<size_t>  separatorEdgeIds_; // for each robot stores the position of the factors corresponding to separator edges
     gtsam::Values neighbors_; // contains keys of all the neighboring robots