12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Test for not increasing graph size when adding marginal factors

release/4.3a0
Grady Williams 2022-04-16 13:53:39 -07:00
parent 0f951643bd
commit 9e1046f40e
1 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
tests/testGaussianISAM2.cpp
View File

@ -945,6 +945,37 @@ TEST(ISAM2, MarginalizeRoot)
EXPECT(estimate.empty());
}
/* ************************************************************************* */
TEST(ISAM2, marginalizationSize)
{
const boost::shared_ptr<noiseModel::Isotropic> nm = noiseModel::Isotropic::Sigma(6, 1.0);
NonlinearFactorGraph factors;
Values values;
ISAM2Params params;
params.findUnusedFactorSlots = true;
ISAM2 isam{params};
// Create a pose variable
Key aKey(0);
values.insert(aKey, Pose3::identity());
factors.addPrior(aKey, Pose3::identity(), nm);
// Create another pose variable linked to the first
Pose3 b = Pose3::identity();
Key bKey(1);
values.insert(bKey, Pose3::identity());
factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::identity(), nm);
// Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam));
// Now remove a variable -> we should not see the number of factors increase
gtsam::KeySet to_remove;
to_remove.insert(aKey);
const auto numFactorsBefore = isam.getFactorsUnsafe().size();
updateAndMarginalize({}, {}, to_remove, isam);
EXPECT(numFactorsBefore == isam.getFactorsUnsafe().size());
}
/* ************************************************************************* */
TEST(ISAM2, marginalCovariance)
{