diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b64ebe259..0fe65e4c0 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,7 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateSequential); @@ -47,13 +48,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 eliminateSequential(ordering, function, VariableIndex(asDerived())); + return eliminateSequential(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. - return eliminateSequential(Ordering::COLAMD(*variableIndex), function); + if (orderingType == Ordering::Type::METIS_) + return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); } } @@ -61,7 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateMultifrontal); @@ -81,13 +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())); + 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. - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function); + 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 717f49167..820bb2fd3 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -94,6 +94,9 @@ 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; + /** 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. * @@ -101,6 +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 - Full QR elimination in specified order: * \code @@ -117,7 +124,8 @@ namespace gtsam { boost::shared_ptr eliminateSequential( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + 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. @@ -142,7 +150,8 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = 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-inl.h b/gtsam/inference/MetisIndex-inl.h index 356118dbb..6da83b8bc 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { @@ -46,9 +47,9 @@ namespace gtsam { xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - vector temp; + std::vector temp; // Copy from the FastSet into a temporary vector - copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b4498bee4..eadfc5bb5 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -107,7 +107,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), + boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams