diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 1fae4c0b9..452ba523e 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -55,10 +55,10 @@ int main(int argc, char** argv) { LevenbergMarquardtParams params; // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" // By default this parameter is set to OrderingType::COLAMD - params.orderingType = OrderingType::METIS; + params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); result.print("Final Result:\n"); return 0; -} \ No newline at end of file +} diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 26abfff85..3dd978ee3 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -589,7 +589,7 @@ void runStats() { cout << "Gathering statistics..." << endl; GaussianFactorGraph linear = *datasetMeasurements.linearize(initial); - GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear))); + GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear))); treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt); ofstream file; diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index c0c95ce88..5e261e200 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -54,10 +54,10 @@ namespace gtsam { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == OrderingType::METIS) - return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) + return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); else - return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -92,10 +92,10 @@ namespace gtsam { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == OrderingType::METIS) - return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) + return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); else - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -125,7 +125,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -163,7 +163,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -216,7 +216,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -275,7 +275,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -301,7 +301,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index f0baf55a0..5fb5fbdb1 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -95,7 +95,7 @@ namespace gtsam { typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + typedef boost::optional OptionalOrderingType; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 645499b2c..7cb3d9817 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -46,7 +46,7 @@ namespace gtsam { const VariableIndex varIndex(factors); const FastSet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = - Ordering::COLAMDConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); + Ordering::colamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index e54ad3fde..dcc2bd00a 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -39,15 +39,15 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering Ordering::COLAMD(const VariableIndex& variableIndex) + Ordering Ordering::colamd(const VariableIndex& variableIndex) { // Call constrained version with all groups set to zero vector dummy_groups(variableIndex.size(), 0); - return Ordering::COLAMDConstrained(variableIndex, dummy_groups); + return Ordering::colamdConstrained(variableIndex, dummy_groups); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrained( + Ordering Ordering::colamdConstrained( const VariableIndex& variableIndex, std::vector& cmember) { gttic(Ordering_COLAMDConstrained); @@ -114,7 +114,7 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrainedLast( + Ordering Ordering::colamdConstrainedLast( const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) { gttic(Ordering_COLAMDConstrainedLast); @@ -137,11 +137,11 @@ namespace gtsam { ++ group; } - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrainedFirst( + Ordering Ordering::colamdConstrainedFirst( const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) { gttic(Ordering_COLAMDConstrainedFirst); @@ -171,11 +171,11 @@ namespace gtsam { if(c == none) c = group; - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex, + Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, const FastMap& groups) { gttic(Ordering_COLAMDConstrained); @@ -195,12 +195,12 @@ namespace gtsam { cmember[keyIndices.at(p.first)] = p.second; } - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::METIS(const MetisIndex& met) + Ordering Ordering::metis(const MetisIndex& met) { gttic(Ordering_METIS); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 3c5b41535..24c811841 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,15 +30,17 @@ namespace gtsam { - enum OrderingType { - COLAMD, METIS, CUSTOM - }; - class Ordering : public std::vector { protected: typedef std::vector Base; public: + + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, CUSTOM + }; + typedef Ordering This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class @@ -69,11 +71,11 @@ namespace gtsam { /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, /// it is faster to use COLAMD(const VariableIndex&) template - static Ordering COLAMD(const FactorGraph& graph) { - return COLAMD(VariableIndex(graph)); } + static Ordering colamd(const FactorGraph& graph) { + return colamd(VariableIndex(graph)); } /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex); + static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// for note on performance). This internally builds a VariableIndex so if you already have a @@ -84,9 +86,9 @@ namespace gtsam { /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering COLAMDConstrainedLast(const FactorGraph& graph, + static Ordering colamdConstrainedLast(const FactorGraph& graph, const std::vector& constrainLast, bool forceOrder = false) { - return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } + return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// function constrains the variables in \c constrainLast to the end of the ordering, and orders @@ -94,7 +96,7 @@ namespace gtsam { /// variables in \c constrainLast will be ordered in the same order specified in the vector /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder = false); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details @@ -106,9 +108,9 @@ namespace gtsam { /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering COLAMDConstrainedFirst(const FactorGraph& graph, + static Ordering colamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { - return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } + return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// function constrains the variables in \c constrainFirst to the front of the ordering, and @@ -117,7 +119,7 @@ namespace gtsam { /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. - static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details @@ -130,9 +132,9 @@ namespace gtsam { /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the /// CCOLAMD documentation for more information. template - static Ordering COLAMDConstrained(const FactorGraph& graph, + static Ordering colamdConstrained(const FactorGraph& graph, const FastMap& groups) { - return COLAMDConstrained(VariableIndex(graph), groups); } + return colamdConstrained(VariableIndex(graph), groups); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this /// function, a group for each variable should be specified in \c groups, and each group of @@ -141,7 +143,7 @@ namespace gtsam { /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers @@ -158,12 +160,12 @@ namespace gtsam { static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering METIS(const MetisIndex& met); + static GTSAM_EXPORT Ordering metis(const MetisIndex& met); template - static Ordering METIS(const FactorGraph& graph) + static Ordering metis(const FactorGraph& graph) { - return METIS(MetisIndex(graph)); + return metis(MetisIndex(graph)); } /// @} @@ -178,7 +180,7 @@ namespace gtsam { private: /// Internal COLAMD function - static GTSAM_EXPORT Ordering COLAMDConstrained( + static GTSAM_EXPORT Ordering colamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 7e1ccb838..57de00646 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include using namespace std; using namespace gtsam; @@ -40,17 +40,17 @@ TEST(Ordering, constrained_ordering) { sfg.push_factor(4,5); // unconstrained version - Ordering actUnconstrained = Ordering::COLAMD(sfg); + Ordering actUnconstrained = Ordering::colamd(sfg); Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expUnconstrained, actUnconstrained)); // constrained version - push one set to the end - Ordering actConstrained = Ordering::COLAMDConstrainedLast(sfg, list_of(2)(4)); + Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4)); Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::COLAMDConstrainedFirst(sfg, list_of(2)(4)); + Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); EXPECT(assert_equal(expConstrained2, actConstrained2)); } @@ -74,7 +74,7 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[4] = 1; constraints[5] = 2; - Ordering actConstrained = Ordering::COLAMDConstrained(sfg, constraints); + Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints); Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); EXPECT(assert_equal(expConstrained, actConstrained)); } @@ -109,17 +109,18 @@ TEST(Ordering, csr_format) { MetisIndex mi(sfg); - vector xadjExpected{ 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44}; - vector adjExpected{ 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, + vector xadjExpected, adjExpected; + xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; + adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, - 13, 8, 12, 14, 9, 13 }; + 13, 8, 12, 14, 9, 13 ; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); } -/* ************************************************************************* */ +/* ************************************************************************* */ TEST(Ordering, csr_format_2) { SymbolicFactorGraph sfg; @@ -132,16 +133,17 @@ TEST(Ordering, csr_format_2) { MetisIndex mi(sfg); - vector xadjExpected { 0, 1, 4, 6, 8, 10 }; - vector adjExpected { 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 4, 6, 8, 10; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); } -/* ************************************************************************* */ +/* ************************************************************************* */ TEST(Ordering, csr_format_3) { SymbolicFactorGraph sfg; @@ -154,40 +156,43 @@ TEST(Ordering, csr_format_3) { MetisIndex mi(sfg); - vector xadjExpected{ 0, 1, 4, 6, 8, 10 }; - vector adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; - size_t minKey = mi.minKey(); + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 4, 6, 8, 10; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + size_t minKey = mi.minKey(); - vector adjAcutal = mi.adj(); + vector adjAcutal = mi.adj(); - // Normalize, subtract the smallest key - std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), std::bind2nd(std::minus(), minKey)); + // Normalize, subtract the smallest key + std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), + std::bind2nd(std::minus(), minKey)); EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT(adjExpected == adjAcutal); + EXPECT(adjExpected == adjAcutal); } /* ************************************************************************* */ TEST(Ordering, metis) { - + SymbolicFactorGraph sfg; sfg.push_factor(0); sfg.push_factor(0, 1); - sfg.push_factor(1, 2); + sfg.push_factor(1, 2); MetisIndex mi(sfg); - vector xadjExpected{ 0, 1, 3, 4 }; - vector adjExpected { 1, 0, 2, 1 }; + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 3, 4; + adjExpected += 1, 0, 2, 1; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::METIS(sfg); + Ordering metis = Ordering::metis(sfg); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 082cc66c8..e148bd65d 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -96,7 +96,7 @@ void DoglegOptimizer::iterate(void) { /* ************************************************************************* */ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); return params; } diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 823f3a6ac..7a115e1c4 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -49,7 +49,7 @@ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( GaussNewtonParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); return params; } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6d6785b14..3b3d76285 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -341,7 +341,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe Ordering order; if(constrainKeys) { - order = Ordering::COLAMDConstrained(variableIndex_, *constrainKeys); + order = Ordering::colamdConstrained(variableIndex_, *constrainKeys); } else { @@ -351,11 +351,11 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe FastMap constraintGroups; BOOST_FOREACH(Key var, observedKeys) constraintGroups[var] = 1; - order = Ordering::COLAMDConstrained(variableIndex_, constraintGroups); + order = Ordering::colamdConstrained(variableIndex_, constraintGroups); } else { - order = Ordering::COLAMD(variableIndex_); + order = Ordering::colamd(variableIndex_); } } gttoc(ordering); @@ -481,7 +481,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::COLAMDConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 59acad3c3..473caa35e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -341,10 +341,10 @@ void LevenbergMarquardtOptimizer::iterate() { LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { if (!params.ordering){ - if (params.orderingType = OrderingType::METIS) - params.ordering = Ordering::METIS(graph); + if (params.orderingType == Ordering::METIS) + params.ordering = Ordering::metis(graph); else - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); } return params; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 63a1a2218..fd14750ac 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -282,13 +282,13 @@ FastSet NonlinearFactorGraph::keys() const { /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { - return Ordering::COLAMD(*this); + return Ordering::colamd(*this); } /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - return Ordering::COLAMDConstrained(*this, constraints); + return Ordering::colamdConstrained(*this, constraints); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 79bc64414..5a163ffb9 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -109,10 +109,10 @@ void NonlinearOptimizerParams::print(const std::string& str) const { } switch (orderingType){ - case OrderingType::COLAMD: + case Ordering::COLAMD: std::cout << " ordering: COLAMD\n"; break; - case OrderingType::METIS: + case Ordering::METIS: std::cout << " ordering: METIS\n"; break; default: @@ -165,11 +165,11 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type) const{ +std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{ switch (type) { - case OrderingType::METIS: + case Ordering::METIS: return "METIS"; - case OrderingType::COLAMD: + case Ordering::COLAMD: return "COLAMD"; default: if (ordering) @@ -181,11 +181,11 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type) } /* ************************************************************************* */ -OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ +Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ if (type == "METIS") - return OrderingType::METIS; + return Ordering::METIS; if (type == "COLAMD") - return OrderingType::COLAMD; + return Ordering::COLAMD; throw std::invalid_argument( "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 2cb055465..10de6994f 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -43,12 +43,12 @@ public: double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) + Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(COLAMD) { - } + 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), + linearSolverType(MULTIFRONTAL_CHOLESKY) {} virtual ~NonlinearOptimizerParams() { } @@ -154,7 +154,7 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = OrderingType::CUSTOM; + this->orderingType = Ordering::CUSTOM; } std::string getOrderingType() const { @@ -171,9 +171,9 @@ private: LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(OrderingType type) const; + std::string orderingTypeTranslator(Ordering::OrderingType type) const; - OrderingType orderingTypeTranslator(const std::string& type) const; + Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index ed4b66616..05b0bb49e 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -191,7 +191,7 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); if(debug) { ordering_.print("New Ordering: "); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 3bb9c7e44..fcaae9626 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 if(keysToMove && keysToMove->size() > 0) { - ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); }else{ - ordering_ = Ordering::COLAMD(factors_); + ordering_ = Ordering::colamd(factors_); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 8606538bf..0f6515056 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() { variableIndex_ = VariableIndex(factors_); FastList separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::COLAMDConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9c2eddcc3..f398a33fe 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING) { Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); - Ordering actual = Ordering::COLAMD(nlfg); + Ordering actual = Ordering::colamd(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); FastMap constraints; constraints[X(2)] = 1; - Ordering actualConstrained = Ordering::COLAMDConstrained(nlfg, constraints); + Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints); EXPECT(assert_equal(expectedConstrained, actualConstrained)); }