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

Remove the boost references 

Signed-off-by: Fan Jiang <i@fanjiang.me>
release/4.3a0
Fan Jiang 2023-08-07 23:31:58 -07:00
parent 4d68f0e55a
commit 44d948e98e
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
tests/testGaussianISAM2.cpp
View File

@ -659,10 +659,10 @@ namespace {
return ok;
}
boost::optional<FastMap<Key, int>> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys)
std::optional<FastMap<Key, int>> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys)
{
if (marginalizableKeys.empty()) {
return boost::none;
return {};
} else {
FastMap<Key, int> constrainedKeys = FastMap<Key, int>();
// Generate ordering constraints so that the marginalizable variables will be eliminated first
@ -706,7 +706,7 @@ namespace {
bool updateAndMarginalize(const NonlinearFactorGraph& newFactors, const Values& newValues, const KeySet& marginalizableKeys, ISAM2& isam)
{
// Force ISAM2 to put marginalizable variables at the beginning
const boost::optional<FastMap<Key, int>> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys);
auto orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys);
// Mark additional keys between the marginalized keys and the leaves
KeyList markedKeys;
@ -719,7 +719,7 @@ namespace {
}
// Update
isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, markedKeys);
isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, {}, markedKeys);
if (!marginalizableKeys.empty()) {
FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
@ -865,7 +865,7 @@ TEST(ISAM2, marginalizeLeaves5)
/* ************************************************************************* */
TEST(ISAM2, marginalizeLeaves6)
{
const boost::shared_ptr<noiseModel::Isotropic> nm = noiseModel::Isotropic::Sigma(6, 1.0);
auto nm = noiseModel::Isotropic::Sigma(6, 1.0);
int gridDim = 10;
@ -878,16 +878,16 @@ TEST(ISAM2, marginalizeLeaves6)
// Create a grid of pose variables
for (int i = 0; i < gridDim; ++i) {
for (int j = 0; j < gridDim; ++j) {
Pose3 pose = Pose3(Rot3::identity(), Point3(i, j, 0));
Pose3 pose = Pose3(Rot3::Identity(), Point3(i, j, 0));
Key key = idxToKey(i, j);
// Place a prior on the first pose
factors.addPrior(key, Pose3(Rot3::identity(), Point3(i, j, 0)), nm);
factors.addPrior(key, Pose3(Rot3::Identity(), Point3(i, j, 0)), nm);
values.insert(key, pose);
if (i > 0) {
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i - 1, j), key, Pose3(Rot3::identity(), Point3(1, 0, 0)),nm);
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i - 1, j), key, Pose3(Rot3::Identity(), Point3(1, 0, 0)),nm);
}
if (j > 0) {
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i, j - 1), key, Pose3(Rot3::identity(), Point3(0, 1, 0)),nm);
factors.emplace_shared<BetweenFactor<Pose3>>(idxToKey(i, j - 1), key, Pose3(Rot3::Identity(), Point3(0, 1, 0)),nm);
}
}
}
@ -914,17 +914,17 @@ TEST(ISAM2, marginalizeLeaves6)
/* ************************************************************************* */
TEST(ISAM2, MarginalizeRoot)
{
const boost::shared_ptr<noiseModel::Isotropic> nm = noiseModel::Isotropic::Sigma(6, 1.0);
auto nm = noiseModel::Isotropic::Sigma(6, 1.0);
NonlinearFactorGraph factors;
Values values;
ISAM2 isam;
// Create a factor graph with one variable and a prior
Pose3 root = Pose3::identity();
Pose3 root = Pose3::Identity();
Key rootKey(0);
values.insert(rootKey, root);
factors.addPrior(rootKey, Pose3::identity(), nm);
factors.addPrior(rootKey, Pose3::Identity(), nm);
// Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam));
@ -944,7 +944,7 @@ TEST(ISAM2, MarginalizeRoot)
/* ************************************************************************* */
TEST(ISAM2, marginalizationSize)
{
const boost::shared_ptr<noiseModel::Isotropic> nm = noiseModel::Isotropic::Sigma(6, 1.0);
auto nm = noiseModel::Isotropic::Sigma(6, 1.0);
NonlinearFactorGraph factors;
Values values;
@ -954,13 +954,13 @@ TEST(ISAM2, marginalizationSize)
// Create a pose variable
Key aKey(0);
values.insert(aKey, Pose3::identity());
factors.addPrior(aKey, Pose3::identity(), nm);
values.insert(aKey, Pose3::Identity());
factors.addPrior(aKey, Pose3::Identity(), nm);
// Create another pose variable linked to the first
Pose3 b = Pose3::identity();
Pose3 b = Pose3::Identity();
Key bKey(1);
values.insert(bKey, Pose3::identity());
factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::identity(), nm);
values.insert(bKey, Pose3::Identity());
factors.emplace_shared<BetweenFactor<Pose3>>(aKey, bKey, Pose3::Identity(), nm);
// Optimize the graph
EXPECT(updateAndMarginalize(factors, values, {}, isam));