|
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)); |
|
} |