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

c++17 style eliminatePartialSequential calls

release/4.3a0
Frank Dellaert 2023-02-04 08:41:46 -08:00
parent 616b366d8a
commit eeda8a7ff2
16 changed files with 44 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -131,9 +131,7 @@ TEST(HybridBayesNet, Choose) {
const Ordering ordering(s.linearizationPoint.keys());
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteValues assignment;

8
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -99,9 +99,7 @@ TEST(HybridBayesTree, OptimizeAssignment) {
Ordering ordering;
for (size_t k = 0; k < s.K; k++) ordering += X(k);
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
GaussianBayesNet gbn = hybridBayesNet->choose(assignment);
@ -143,9 +141,7 @@ TEST(HybridBayesTree, Optimize) {
Ordering ordering;
for (size_t k = 0; k < s.K; k++) ordering += X(k);
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteFactorGraph dfg;

12
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -239,10 +239,8 @@ std::vector<size_t> getDiscreteSequence(size_t x) {
*/
AlgebraicDecisionTree<Key> getProbPrimeTree(
const HybridGaussianFactorGraph& graph) {
HybridBayesNet::shared_ptr bayesNet;
HybridGaussianFactorGraph::shared_ptr remainingGraph;
Ordering continuous(graph.continuousKeySet());
std::tie(bayesNet, remainingGraph) =
const auto [bayesNet, remainingGraph] =
graph.eliminatePartialSequential(continuous);
auto last_conditional = bayesNet->at(bayesNet->size() - 1);
@ -297,9 +295,7 @@ TEST(HybridEstimation, Probability) {
// Continuous elimination
Ordering continuous_ordering(graph.continuousKeySet());
HybridBayesNet::shared_ptr bayesNet;
HybridGaussianFactorGraph::shared_ptr discreteGraph;
std::tie(bayesNet, discreteGraph) =
auto [bayesNet, discreteGraph] =
graph.eliminatePartialSequential(continuous_ordering);
// Discrete elimination
@ -347,9 +343,7 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
// Eliminate continuous
Ordering continuous_ordering(graph.continuousKeySet());
HybridBayesTree::shared_ptr bayesTree;
HybridGaussianFactorGraph::shared_ptr discreteGraph;
std::tie(bayesTree, discreteGraph) =
const auto [bayesTree, discreteGraph] =
graph.eliminatePartialMultifrontal(continuous_ordering);
// Get the last continuous conditional which will have all the discrete keys

19
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -270,9 +270,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
auto ordering_full =
Ordering::ColamdConstrainedLast(hfg, {M(0), M(1), M(2), M(3)});
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
const auto [hbt, remaining] = hfg.eliminatePartialMultifrontal(ordering_full);
// 9 cliques in the bayes tree and 0 remaining variables to eliminate.
EXPECT_LONGS_EQUAL(9, hbt->size());
@ -353,9 +351,7 @@ TEST(HybridGaussianFactorGraph, Switching) {
// GTSAM_PRINT(*hfg);
// GTSAM_PRINT(ordering_full);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering_full);
// 12 cliques in the bayes tree and 0 remaining variables to eliminate.
EXPECT_LONGS_EQUAL(12, hbt->size());
@ -411,9 +407,7 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
}
auto ordering_full = Ordering(ordering);
HybridBayesTree::shared_ptr hbt;
HybridGaussianFactorGraph::shared_ptr remaining;
std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
const auto [hbt, remaining] = hfg->eliminatePartialMultifrontal(ordering_full);
auto new_fg = makeSwitchingChain(12);
auto isam = HybridGaussianISAM(*hbt);
@ -847,10 +841,9 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
EXPECT(ratioTest(bn, measurements, fg));
// Do elimination of X(2) only:
auto result = fg.eliminatePartialSequential(Ordering{X(2)});
auto fg1 = *result.second;
fg1.push_back(*result.first);
EXPECT(ratioTest(bn, measurements, fg1));
auto [bn1, fg1] = fg.eliminatePartialSequential(Ordering{X(2)});
fg1->push_back(*bn1);
EXPECT(ratioTest(bn, measurements, *fg1));
// Create ordering that eliminates in time order, then discrete modes:
Ordering ordering {X(2), X(1), X(0), N(0), N(1), N(2), M(1), M(2)};

8
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -132,9 +132,7 @@ TEST(HybridGaussianElimination, IncrementalInference) {
ordering += X(2);
// Now we calculate the expected factors using full elimination
HybridBayesTree::shared_ptr expectedHybridBayesTree;
HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph;
std::tie(expectedHybridBayesTree, expectedRemainingGraph) =
const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
// The densities on X(0) should be the same
@ -231,9 +229,7 @@ TEST(HybridGaussianElimination, Approx_inference) {
}
// Now we calculate the actual factors using full elimination
HybridBayesTree::shared_ptr unprunedHybridBayesTree;
HybridGaussianFactorGraph::shared_ptr unprunedRemainingGraph;
std::tie(unprunedHybridBayesTree, unprunedRemainingGraph) =
const auto [unprunedHybridBayesTree, unprunedRemainingGraph] =
switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering);
size_t maxNrLeaves = 5;

16
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -385,9 +385,7 @@ TEST(HybridFactorGraph, Partial_Elimination) {
for (size_t k = 0; k < self.K; k++) ordering += X(k);
// Eliminate partially i.e. only continuous part.
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
const auto [hybridBayesNet, remainingFactorGraph] =
linearizedFactorGraph.eliminatePartialSequential(ordering);
CHECK(hybridBayesNet);
@ -415,8 +413,6 @@ TEST(HybridFactorGraph, Full_Elimination) {
auto linearizedFactorGraph = self.linearizedFactorGraph;
// We first do a partial elimination
HybridBayesNet::shared_ptr hybridBayesNet_partial;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph_partial;
DiscreteBayesNet discreteBayesNet;
{
@ -425,7 +421,7 @@ TEST(HybridFactorGraph, Full_Elimination) {
for (size_t k = 0; k < self.K; k++) ordering += X(k);
// Eliminate partially.
std::tie(hybridBayesNet_partial, remainingFactorGraph_partial) =
const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
linearizedFactorGraph.eliminatePartialSequential(ordering);
DiscreteFactorGraph discrete_fg;
@ -489,9 +485,7 @@ TEST(HybridFactorGraph, Printing) {
for (size_t k = 0; k < self.K; k++) ordering += X(k);
// Eliminate partially.
HybridBayesNet::shared_ptr hybridBayesNet;
HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
std::tie(hybridBayesNet, remainingFactorGraph) =
const auto [hybridBayesNet, remainingFactorGraph] =
linearizedFactorGraph.eliminatePartialSequential(ordering);
string expected_hybridFactorGraph = R"(
@ -721,11 +715,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
ordering += X(1);
HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate);
gtsam::HybridBayesNet::shared_ptr hybridBayesNet;
gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph;
// This should NOT fail
std::tie(hybridBayesNet, remainingFactorGraph) =
const auto [hybridBayesNet, remainingFactorGraph] =
linearized.eliminatePartialSequential(ordering);
EXPECT_LONGS_EQUAL(4, hybridBayesNet->size());
EXPECT_LONGS_EQUAL(1, remainingFactorGraph->size());

8
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -149,9 +149,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
ordering += X(2);
// Now we calculate the actual factors using full elimination
HybridBayesTree::shared_ptr expectedHybridBayesTree;
HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph;
std::tie(expectedHybridBayesTree, expectedRemainingGraph) =
const auto [expectedHybridBayesTree, expectedRemainingGraph] =
switching.linearizedFactorGraph
.BaseEliminateable::eliminatePartialMultifrontal(ordering);
@ -250,9 +248,7 @@ TEST(HybridNonlinearISAM, Approx_inference) {
}
// Now we calculate the actual factors using full elimination
HybridBayesTree::shared_ptr unprunedHybridBayesTree;
HybridGaussianFactorGraph::shared_ptr unprunedRemainingGraph;
std::tie(unprunedHybridBayesTree, unprunedRemainingGraph) =
const auto [unprunedHybridBayesTree, unprunedRemainingGraph] =
switching.linearizedFactorGraph
.BaseEliminateable::eliminatePartialMultifrontal(ordering);

16
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -72,9 +72,7 @@ namespace gtsam {
gttic(eliminateSequential);
// Do elimination
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
std::shared_ptr<BayesNetType> bayesNet;
std::shared_ptr<FactorGraphType> factorGraph;
std::tie(bayesNet,factorGraph) = etree.eliminate(function);
const auto [bayesNet, factorGraph] = etree.eliminate(function);
// If any factors are remaining, the ordering was incomplete
if(!factorGraph->empty())
throw InconsistentEliminationRequested();
@ -136,9 +134,7 @@ namespace gtsam {
// Do elimination with given ordering
EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
JunctionTreeType junctionTree(etree);
std::shared_ptr<BayesTreeType> bayesTree;
std::shared_ptr<FactorGraphType> factorGraph;
std::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
const auto [bayesTree, factorGraph] = junctionTree.eliminate(function);
// If any factors are remaining, the ordering was incomplete
if(!factorGraph->empty())
throw InconsistentEliminationRequested();
@ -274,9 +270,7 @@ namespace gtsam {
gttic(marginalMultifrontalBayesNet);
// An ordering was provided for the marginalized variables, so we can first eliminate them
// in the order requested.
std::shared_ptr<BayesTreeType> bayesTree;
std::shared_ptr<FactorGraphType> factorGraph;
std::tie(bayesTree,factorGraph) =
const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
@ -341,9 +335,7 @@ namespace gtsam {
gttic(marginalMultifrontalBayesTree);
// An ordering was provided for the marginalized variables, so we can first eliminate them
// in the order requested.
std::shared_ptr<BayesTreeType> bayesTree;
std::shared_ptr<FactorGraphType> factorGraph;
std::tie(bayesTree,factorGraph) =
const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))

2
gtsam/inference/EliminationTree-inst.h
View File

@ -198,7 +198,7 @@ namespace gtsam {
allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
// Return result
return std::make_pair(result, allRemainingFactors);
return {result, allRemainingFactors};
}
/* ************************************************************************* */

4
gtsam/linear/KalmanFilter.cpp
View File

@ -38,12 +38,12 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
// Eliminate the graph using the provided Eliminate function
Ordering ordering(factorGraph.keys());
GaussianBayesNet::shared_ptr bayesNet = //
const auto bayesNet = //
factorGraph.eliminateSequential(ordering, function_);
// As this is a filter, all we need is the posterior P(x_t).
// This is the last GaussianConditional in the resulting BayesNet
GaussianConditional::shared_ptr posterior = *(--bayesNet->end());
GaussianConditional::shared_ptr posterior = bayesNet->back();
return std::make_shared<GaussianDensity>(*posterior);
}

4
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -245,9 +245,7 @@ TEST(GaussianFactorGraph, eliminate_empty) {
// eliminate an empty factor
GaussianFactorGraph gfg;
gfg.add(JacobianFactor());
GaussianBayesNet::shared_ptr actualBN;
GaussianFactorGraph::shared_ptr remainingGFG;
std::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());
const auto [actualBN, remainingGFG] = gfg.eliminatePartialSequential(Ordering());
// expected Bayes net is empty
GaussianBayesNet expectedBN;

26
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -65,17 +65,13 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) {
const auto expectedSfg = SymbolicFactorGraph(SymbolicFactor(2, 3))(
SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
SymbolicBayesNet::shared_ptr actualBayesNet;
SymbolicFactorGraph::shared_ptr actualSfg;
std::tie(actualBayesNet, actualSfg) =
const auto [actualBayesNet, actualSfg] =
simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
EXPECT(assert_equal(expectedSfg, *actualSfg));
EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
SymbolicBayesNet::shared_ptr actualBayesNet2;
SymbolicFactorGraph::shared_ptr actualSfg2;
std::tie(actualBayesNet2, actualSfg2) =
const auto [actualBayesNet2, actualSfg2] =
simpleTestGraph2.eliminatePartialSequential(Ordering{0, 1});
EXPECT(assert_equal(expectedSfg, *actualSfg2));
@ -106,9 +102,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
SymbolicFactorGraph(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(
SymbolicFactor(1, 3))(SymbolicFactor(2, 3))(SymbolicFactor(1));
SymbolicBayesTree::shared_ptr actualBayesTree;
SymbolicFactorGraph::shared_ptr actualFactorGraph;
std::tie(actualBayesTree, actualFactorGraph) =
const auto [actualBayesTree, actualFactorGraph] =
simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5});
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
@ -122,9 +116,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
std::make_shared<SymbolicConditional>(5, 4)));
expectedBayesTree2.insertRoot(root2);
SymbolicBayesTree::shared_ptr actualBayesTree2;
SymbolicFactorGraph::shared_ptr actualFactorGraph2;
std::tie(actualBayesTree2, actualFactorGraph2) =
const auto [actualBayesTree2, actualFactorGraph2] =
simpleTestGraph2.eliminatePartialMultifrontal(KeyVector{4, 5});
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
@ -152,11 +144,11 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
// create expected Chordal bayes Net
SymbolicBayesNet expected;
expected.push_back(std::make_shared<SymbolicConditional>(0, 1, 2));
expected.push_back(std::make_shared<SymbolicConditional>(1, 2));
expected.push_back(std::make_shared<SymbolicConditional>(2));
expected.push_back(std::make_shared<SymbolicConditional>(3, 4));
expected.push_back(std::make_shared<SymbolicConditional>(4));
expected.emplace_shared<SymbolicConditional>(0, 1, 2);
expected.emplace_shared<SymbolicConditional>(1, 2);
expected.emplace_shared<SymbolicConditional>(2);
expected.emplace_shared<SymbolicConditional>(3, 4);
expected.emplace_shared<SymbolicConditional>(4);
Ordering order;
order += 0, 1, 2, 3, 4;

4
gtsam_unstable/discrete/examples/schedulingQuals12.cpp
View File

@ -178,9 +178,6 @@ void solveStaged(size_t addMutex = 2) {
gttoc_(eliminate);
// find root node
// chordal->back()->print("back: ");
// chordal->front()->print("front: ");
// exit(0);
DiscreteConditional::shared_ptr root = chordal->back();
if (debug)
root->print(""/*scheduler.studentName(s)*/);
@ -211,7 +208,6 @@ DiscreteBayesNet::shared_ptr createSampler(size_t i,
SETDEBUG("Scheduler::buildGraph", false);
scheduler.addStudentSpecificConstraints(0, slot);
DiscreteBayesNet::shared_ptr chordal = scheduler.eliminate();
// chordal->print(scheduler[i].studentKey(0).name()); // large !
schedulers.push_back(scheduler);
return chordal;
}

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -584,10 +584,10 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
allkeys.erase(key);
}
KeyVector variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
const auto [bn, fg] = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0);
for(const GaussianFactor::shared_ptr& factor: *result.second) {
for(const GaussianFactor::shared_ptr& factor: *fg) {
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
}

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -584,10 +584,10 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
KeySet allkeys = LinFactorGraph->keys();
for (const auto key : filterSeparatorValues.keys()) allkeys.erase(key);
KeyVector variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
const auto [bn, fg] = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0);
for(const GaussianFactor::shared_ptr& factor: *result.second) {
for(const GaussianFactor::shared_ptr& factor: *fg) {
expectedSmootherSummarization.push_back(LinearContainerFactor(factor, allValues));
}

6
gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp
View File

@ -86,8 +86,8 @@ TEST(NonlinearClusterTree, Clusters) {
// Calculate expected result of only evaluating the marginalCluster
Ordering ordering;
ordering.push_back(x1);
auto expected = gfg->eliminatePartialSequential(ordering);
auto expectedFactor = std::dynamic_pointer_cast<HessianFactor>(expected.second->at(0));
const auto [bn, fg] = gfg->eliminatePartialSequential(ordering);
auto expectedFactor = std::dynamic_pointer_cast<HessianFactor>(fg->at(0));
if (!expectedFactor)
throw std::runtime_error("Expected HessianFactor");
@ -95,7 +95,7 @@ TEST(NonlinearClusterTree, Clusters) {
auto actual = marginalCluster->linearizeAndEliminate(initial);
const GaussianBayesNet& bayesNet = actual.first;
const HessianFactor& factor = *actual.second;
EXPECT(assert_equal(*expected.first->at(0), *bayesNet.at(0), 1e-6));
EXPECT(assert_equal(*bn->at(0), *bayesNet.at(0), 1e-6));
EXPECT(assert_equal(*expectedFactor, factor, 1e-6));
}