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