diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 7d9457b9e..3ccdf9b72 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -80,8 +80,6 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ - /** ---------------------------------------------------**/ - double t_COLAMD_ordering, t_METIS_ordering; //, t_NATURAL_ordering; LevenbergMarquardtParams params_using_COLAMD, params_using_METIS, params_using_NATURAL; try { diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 923b0b9de..0f61a4220 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 5e261e200..b4fc3b3a6 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -55,9 +55,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -93,9 +93,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); + 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/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 7cb3d9817..aebd3325a 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 0882b87d1..9f4a81d05 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -38,14 +38,14 @@ FastMap Ordering::invert() const { } /* ************************************************************************* */ -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(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, std::vector& cmember) { gttic(Ordering_COLAMDConstrained); @@ -115,7 +115,7 @@ Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, } /* ************************************************************************* */ -Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) { gttic(Ordering_COLAMDConstrainedLast); @@ -137,11 +137,11 @@ Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex, ++group; } - return Ordering::colamdConstrained(variableIndex, cmember); + return Ordering::ColamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ -Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex, +Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) { gttic(Ordering_COLAMDConstrainedFirst); @@ -170,11 +170,11 @@ Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex, 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); size_t n = variableIndex.size(); @@ -193,11 +193,11 @@ Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, 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); vector xadj = met.xadj(); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 109fa1784..0ec5774e5 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -38,7 +38,7 @@ public: /// Type of ordering to use enum OrderingType { - COLAMD, METIS, CUSTOM, NATURAL + COLAMD, METIS, NATURAL, CUSTOM }; typedef Ordering This; ///< Typedef to this class @@ -78,12 +78,12 @@ public: /// 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 @@ -94,9 +94,9 @@ public: /// 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, + return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } @@ -106,7 +106,7 @@ public: /// 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( + static GTSAM_EXPORT Ordering ColamdConstrainedLast( const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder = false); @@ -119,9 +119,9 @@ public: /// 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, + return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } @@ -132,7 +132,7 @@ public: /// 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( + static GTSAM_EXPORT Ordering ColamdConstrainedFirst( const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); @@ -146,9 +146,9 @@ public: /// 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 @@ -158,12 +158,12 @@ public: /// 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( + static GTSAM_EXPORT Ordering ColamdConstrained( const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers template - static Ordering natural(const FactorGraph &fg) { + static Ordering Natural(const FactorGraph &fg) { FastSet src = fg.keys(); std::vector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); @@ -176,11 +176,11 @@ public: 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) { - return metis(MetisIndex(graph)); + static Ordering Metis(const FactorGraph& graph) { + return Metis(MetisIndex(graph)); } /// @} @@ -193,11 +193,11 @@ public: switch (orderingType) { case COLAMD: - return colamd(graph); + return Colamd(graph); case METIS: - return metis(graph); + return Metis(graph); case NATURAL: - return natural(graph); + return Natural(graph); case CUSTOM: throw std::runtime_error( "Ordering::Create error: called with CUSTOM ordering type."); @@ -222,7 +222,7 @@ public: private: /// Internal COLAMD function - static GTSAM_EXPORT Ordering colamdConstrained( + static GTSAM_EXPORT Ordering ColamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); /** Serialization function */ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 8f2c417d3..d789da9b0 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -46,17 +46,17 @@ TEST(Ordering, constrained_ordering) { SymbolicFactorGraph sfg = example::symbolicChain(); // 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, + 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)); @@ -76,7 +76,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)); } @@ -195,7 +195,7 @@ TEST(Ordering, csr_format_4) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - Ordering metOrder = Ordering::metis(sfg); + Ordering metOrder = Ordering::Metis(sfg); // Test different symbol types sfg.push_factor(Symbol('l', 1)); @@ -204,7 +204,7 @@ TEST(Ordering, csr_format_4) { sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); - Ordering metOrder2 = Ordering::metis(sfg); + Ordering metOrder2 = Ordering::Metis(sfg); } /* ************************************************************************* */ @@ -226,7 +226,7 @@ TEST(Ordering, metis) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::metis(sfg); + Ordering metis = Ordering::Metis(sfg); } /* ************************************************************************* */ diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 89486962b..ab3ccde13 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -86,7 +86,7 @@ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::natural(fg)) { + : ordering_(Ordering::Natural(fg)) { initialize(fg); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index af4e9dd4f..f5ee4ddfc 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -366,7 +366,7 @@ std::vector SubgraphBuilder::sample(const std::vector &weights, Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::natural(gfg); + const Ordering inverse_ordering = Ordering::Natural(gfg); const FastMap forward_ordering = inverse_ordering.invert(); const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 3b3d76285..97735b5d5 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/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 238d3bc56..99a58a989 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_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 05b0bb49e..952b134b5 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 fcaae9626..40d1746ac 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 0f6515056..c410ad528 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 f398a33fe..8e6b033e9 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)); }