Load more than 1 .g2o files

[smalltheater]

Hello author,
thank your work for implementation of PCM. I am confused about loading pose graphs. In your testLoadGraph.cpp:

Kimera-RPGO/tests/testLoadGraph.cpp

Lines 164 to 221 in 19436dc

 TEST(RobustSolver, Add2) { 
   // load graph for robot a (same as above) 
   gtsam::NonlinearFactorGraph::shared_ptr nfg; 
   gtsam::Values::shared_ptr values; 
   boost::tie(nfg, values) = 
       gtsam::load3D(std::string(DATASET_PATH) + "/robot_a.g2o"); 
  
   // set up KimeraRPGO solver 
   RobustSolverParams params; 
   params.setPcm3DParams(100.0, 100.0, Verbosity::QUIET); 
  
   std::unique_ptr<RobustSolver> pgo = 
       KimeraRPGO::make_unique<RobustSolver>(params); 
  
   static const gtsam::SharedNoiseModel& noise = 
       gtsam::noiseModel::Isotropic::Variance(6, 0.01); 
  
   gtsam::Key init_key = gtsam::Symbol('a', 0); 
   gtsam::PriorFactor<gtsam::Pose3> init( 
       init_key, values->at<gtsam::Pose3>(init_key), noise); 
   nfg->add(init); 
   pgo->update(*nfg, *values);  // first load 
  
   // add graph 
   // read g2o file for robot b 
   gtsam::NonlinearFactorGraph::shared_ptr nfg_b; 
   gtsam::Values::shared_ptr values_b; 
   boost::tie(nfg_b, values_b) = 
       gtsam::load3D(std::string(DATASET_PATH) + "/robot_b.g2o"); 
  
   // add robot b 
   pgo->update(*nfg_b, *values_b); 
  
   gtsam::NonlinearFactorGraph nfg_out = pgo->getFactorsUnsafe(); 
   gtsam::Values values_out = pgo->calculateEstimate(); 
  
   // Since thresholds are high, should have all the edges 
   EXPECT(nfg_out.size() == size_t(96)); 
   EXPECT(values_out.size() == size_t(92)); 
  
   // Try add another loop closuer 
   // create the between factor for connection 
   gtsam::Key key_b1 = gtsam::Symbol('b', 1); 
   gtsam::Key key_a1 = gtsam::Symbol('a', 1); 
   gtsam::Pose3 a1b1 = gtsam::Pose3(); 
   gtsam::BetweenFactor<gtsam::Pose3> a1tob1(key_a1, key_b1, a1b1, noise); 
  
   gtsam::NonlinearFactorGraph newfactors; 
   newfactors.add(a1tob1); 
   gtsam::Values newvalues; 
   pgo->update(newfactors, newvalues); 
  
   nfg_out = pgo->getFactorsUnsafe(); 
   values_out = pgo->calculateEstimate(); 
  
   EXPECT(nfg_out.size() == size_t(97)); 
   EXPECT(values_out.size() == size_t(92)); 
 } 

you load 2 graphs at the same time. The Key used for loop closure are a1,b1. However, the keys in robot_a.g2o and robot_b.g2o are numbers:

Kimera-RPGO/tests/data/robot_a.g2o

Lines 1 to 6 in 19436dc

 VERTEX_SE3:QUAT 6989586621679009792 0 0 0 0 0 0 1 
 VERTEX_SE3:QUAT 6989586621679009793 0.92635 0.0182106 0.397159 0.0555375 -0.0738074 -0.00793144 0.995693 
 VERTEX_SE3:QUAT 6989586621679009794 1.93601 0.0173125 0.506064 0.0303068 -0.0540801 0.00257062 0.998073 
 VERTEX_SE3:QUAT 6989586621679009795 2.94811 0.046416 0.618074 0.0431276 -0.091336 0.00782434 0.994855 
 VERTEX_SE3:QUAT 6989586621679009796 3.96537 0.0537431 0.70129 0.0513305 -0.074802 0.000208149 0.995876 
 VERTEX_SE3:QUAT 6989586621679009797 5.01936 0.0874456 0.737571 0.0415649 -0.0911669 -0.0024415 0.994965 

The connections between a1 and b1 do not make sense. I think the loop closure should be built with that number keys(6989586621679009794).

[Yun Chang]

Hi,
This should be correct, the confusion might come from how GTSAM symbol works. https://gtsam.org/doxygen/4.0.0/a03039.html
a is the chr and 1 is the index.