diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index e14ba329b..153b828d9 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -65,8 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateMultifrontal); @@ -86,16 +86,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); + return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // 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 == Ordering::Type::METIS_) - return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); - else - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::Type::METIS_) + return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + else + return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 820bb2fd3..c048de94b 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -94,8 +94,8 @@ namespace gtsam { /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; - /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + /// Typedef for an optional ordering type + 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. @@ -104,10 +104,10 @@ namespace gtsam { * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); * \endcode - * - * Example - METIS ordering for elimination - * \code - * boost::shared_ptr result = graph.eliminateSequential(Ordering::Type::METIS_); + * + * Example - METIS ordering for elimination + * \code + * boost::shared_ptr result = graph.eliminateSequential(Ordering::Type::METIS_); * * Example - Full QR elimination in specified order: * \code @@ -125,7 +125,7 @@ namespace gtsam { OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -150,8 +150,8 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 57d097876..b058b5564 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -9,7 +9,6 @@ * -------------------------------------------------------------------------- */ - /** * @file MetisIndex.h * @author Andrew Melim @@ -28,55 +27,55 @@ #include namespace gtsam { +/** + * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in + * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built + * fromt a factor graph prior to elimination, and stores the list of factors + * that involve each variable. + * \nosubgrouping + */ +class GTSAM_EXPORT MetisIndex +{ +public: + typedef boost::shared_ptr shared_ptr; + +private: + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; // + size_t minKey_; + +public: + /// @name Standard Constructors + /// @{ + + /** Default constructor, creates empty MetisIndex */ + MetisIndex() : nFactors_(0), nKeys_(0) {} + + template + MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { + augment(factorGraph); } + + ~MetisIndex(){} + /// @} + /// @name Advanced Interface + /// @{ + /** - * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in - * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built - * fromt a factor graph prior to elimination, and stores the list of factors - * that involve each variable. - * \nosubgrouping - */ - class GTSAM_EXPORT MetisIndex - { - public: - typedef boost::shared_ptr shared_ptr; + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FactorGraph& factors); - private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - size_t nFactors_; // Number of factors in the original factor graph - size_t nKeys_; // - size_t minKey_; + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } + size_t minKey() const { return minKey_; } - public: - /// @name Standard Constructors - /// @{ - - /** Default constructor, creates empty MetisIndex */ - MetisIndex() : nFactors_(0), nKeys_(0) {} - - template - MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { - augment(factorGraph); } - - ~MetisIndex(){} - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Augment the variable index with new factors. This can be used when - * solving problems incrementally. - */ - template - void augment(const FactorGraph& factors); - - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } - size_t minKey() const { return minKey_; } - - /// @} - }; + /// @} +}; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 5fca680a2..a8dab8266 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -340,11 +340,12 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering) + if (!params.ordering){ if (params.orderingType = Ordering::Type::METIS_) params.ordering = Ordering::METIS(graph); else params.ordering = Ordering::COLAMD(graph); + } return params; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 1e964a481..626758bcb 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -165,8 +165,7 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type) const -{ +std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type) const{ switch (type) { case Ordering::Type::METIS_: return "METIS"; @@ -182,8 +181,7 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type } /* ************************************************************************* */ -Ordering::Type NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const -{ +Ordering::Type NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ if (type == "METIS") return Ordering::Type::METIS_; if (type == "COLAMD")