From 1733f3ac98323cd24c042e0f2cc774e2da2c45dc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 20 Oct 2019 01:15:20 -0400 Subject: [PATCH] convert all optional Ordering to function overloads compiles and passes tests, but some potentially code-breaking changes in: Marginals.h - order of arguments had to change since `factorization` has a default value EliminatableFactorGraph.h - marginalMultifrontalBayesNet and marginalMultifrontalBayesTree no longer accept `boost::none` as a placeholder to specify later arguments Notes: EliminateableFactorGraph.h - `orderingType` is no longer needed in function overloads that specify ordering, but I left it for the time being to avoid potential code breaking --- gtsam/inference/BayesTree-inst.h | 4 +- gtsam/inference/BayesTreeCliqueBase-inst.h | 2 +- .../inference/EliminateableFactorGraph-inst.h | 315 ++++++++++-------- gtsam/inference/EliminateableFactorGraph.h | 95 ++++-- gtsam/linear/GaussianFactorGraph.cpp | 9 +- gtsam/linear/GaussianFactorGraph.h | 9 +- gtsam/linear/IterativeSolver.cpp | 25 +- gtsam/linear/IterativeSolver.h | 16 +- gtsam/linear/Scatter.cpp | 34 +- gtsam/linear/Scatter.h | 14 +- gtsam/nonlinear/Marginals.cpp | 64 +++- gtsam/nonlinear/Marginals.h | 41 ++- gtsam_unstable/discrete/CSP.cpp | 9 +- gtsam_unstable/discrete/CSP.h | 5 +- tests/testGaussianFactorGraphB.cpp | 4 +- 15 files changed, 406 insertions(+), 240 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 4df234004..639bcbab0 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -262,7 +262,7 @@ namespace gtsam { // Now, marginalize out everything that is not variable j BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( - Ordering(cref_list_of<1,Key>(j)), boost::none, function); + Ordering(cref_list_of<1,Key>(j)), function); // The Bayes net should contain only one conditional for variable j, so return it return marginalBN.front(); @@ -383,7 +383,7 @@ namespace gtsam { } // now, marginalize out everything that is not variable j1 or j2 - return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), boost::none, function); + return p_BC1C2.marginalMultifrontalBayesNet(Ordering(cref_list_of<2,Key>(j1)(j2)), function); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 6bcfb434d..e762786f5 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -171,7 +171,7 @@ namespace gtsam { // The variables we want to keepSet are exactly the ones in S KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); - cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); + cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); } } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index af2a91257..a77d96537 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,33 +28,19 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const - { - if(ordering && variableIndex) { - gttic(eliminateSequential); - // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, *ordering); - boost::shared_ptr bayesNet; - boost::shared_ptr factorGraph; - boost::tie(bayesNet,factorGraph) = etree.eliminate(function); - // If any factors are remaining, the ordering was incomplete - if(!factorGraph->empty()) - throw InconsistentEliminationRequested(); - // Return the Bayes net - return bayesNet; - } - else if(!variableIndex) { + const Eliminate& function, OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + if(!variableIndex) { // 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. + // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex + // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + return eliminateSequential(function, computedVariableIndex, 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. + else { + // Compute an ordering and call this function again. We are guaranteed to have a + // VariableIndex already here because we computed one if needed in the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); return eliminateSequential(computedOrdering, function, variableIndex, orderingType); @@ -67,15 +53,73 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesTreeType> - EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::eliminateSequential( + const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { - if(ordering && variableIndex) { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + } else { + gttic(eliminateSequential); + // Do elimination + EliminationTreeType etree(asDerived(), *variableIndex, ordering); + boost::shared_ptr bayesNet; + boost::shared_ptr factorGraph; + boost::tie(bayesNet,factorGraph) = etree.eliminate(function); + // If any factors are remaining, the ordering was incomplete + if(!factorGraph->empty()) + throw InconsistentEliminationRequested(); + // Return the Bayes net + return bayesNet; + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const Eliminate& function, OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const + { + if(!variableIndex) { + // 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. When removing optional from VariableIndex, create VariableIndex + // before creating ordering. + VariableIndex computedVariableIndex(asDerived()); + return eliminateMultifrontal(function, computedVariableIndex, orderingType); + } + else { + // Compute an ordering and call this function again. We are guaranteed to have a + // VariableIndex already here because we computed one if needed in the previous 'if' block. + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const Ordering& ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + { + if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); + } else { gttic(eliminateMultifrontal); // Do elimination with given ordering - EliminationTreeType etree(asDerived(), *variableIndex, *ordering); + EliminationTreeType etree(asDerived(), *variableIndex, ordering); JunctionTreeType junctionTree(etree); boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; @@ -86,25 +130,6 @@ namespace gtsam { // Return the Bayes tree return bayesTree; } - else if(!variableIndex) { - // 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. - VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex, 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::METIS) { - Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); - } else { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); - } - } } /* ************************************************************************* */ @@ -191,57 +216,65 @@ namespace gtsam { boost::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { - if(variableIndex) - { - if(marginalizedVariableOrdering) - { - gttic(marginalMultifrontalBayesNet); - // An ordering was provided for the marginalized variables, so we can first eliminate them - // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; - boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); - - if(const Ordering* varsAsOrdering = boost::get(&variables)) - { - // An ordering was also provided for the unmarginalized variables, so we can also - // eliminate them in the order requested. - return factorGraph->eliminateSequential(*varsAsOrdering, function); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(boost::none, function); - } - } - else - { - // No ordering was provided for the marginalized variables, so order them using constrained - // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); - - Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); - - // Split up ordering - const size_t nVars = variablesOrOrdering->size(); - Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); - Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); - - // Call this function again with the computed orderings - return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); - } + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex index(asDerived()); + return marginalMultifrontalBayesNet(variables, function, index); } else { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const KeyVector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get(&variables); + + Ordering totalOrdering = + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::marginalMultifrontalBayesNet( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + } else { + gttic(marginalMultifrontalBayesNet); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateSequential(*varsAsOrdering, function); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateSequential(function); + } } } @@ -250,57 +283,65 @@ namespace gtsam { boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { - if(variableIndex) - { - if(marginalizedVariableOrdering) - { - gttic(marginalMultifrontalBayesTree); - // An ordering was provided for the marginalized variables, so we can first eliminate them - // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; - boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); - - if(const Ordering* varsAsOrdering = boost::get(&variables)) - { - // An ordering was also provided for the unmarginalized variables, so we can also - // eliminate them in the order requested. - return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(boost::none, function); - } - } - else - { - // No ordering was provided for the marginalized variables, so order them using constrained - // COLAMD. - bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); - const KeyVector* variablesOrOrdering = - unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get(&variables); - - Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); - - // Split up ordering - const size_t nVars = variablesOrOrdering->size(); - Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); - Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); - - // Call this function again with the computed orderings - return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); - } + if(!variableIndex) { + // If no variable index is provided, compute one and call this function again + VariableIndex computedVariableIndex(asDerived()); + return marginalMultifrontalBayesTree(variables, function, computedVariableIndex); } else { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + const KeyVector* variablesOrOrdering = + unmarginalizedAreOrdered ? + boost::get(&variables) : boost::get(&variables); + + Ordering totalOrdering = + Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = variablesOrOrdering->size(); + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } + + /* ************************************************************************* */ + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::marginalMultifrontalBayesTree( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, + const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); + } else { + gttic(marginalMultifrontalBayesTree); + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + boost::shared_ptr bayesTree; + boost::shared_ptr factorGraph; + boost::tie(bayesTree,factorGraph) = + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + + if(const Ordering* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return factorGraph->eliminateMultifrontal(function); + } } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 249c594b6..8a216918c 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -88,9 +88,6 @@ namespace gtsam { /// The function type that does a single dense elimination step on a subgraph. typedef boost::function Eliminate; - /// Typedef for an optional ordering as an argument to elimination functions - typedef const boost::optional& OptionalOrdering; - /// Typedef for an optional variable index as an argument to elimination functions typedef const boost::optional& OptionalVariableIndex; @@ -108,25 +105,40 @@ namespace gtsam { * Example - METIS ordering for elimination * \code * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); - * - * Example - Full QR elimination in specified order: - * \code - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); * \endcode * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, varIndex, boost::none); * \endcode * */ boost::shared_ptr eliminateSequential( - OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const; + /** Do sequential elimination of all variables to produce a Bayes net. + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none); + * \endcode + * */ + boost::shared_ptr eliminateSequential( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; // orderingType is not necessary anymore, kept for backwards compatibility + /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) @@ -136,11 +148,6 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); * \endcode * - * Example - Full QR elimination in specified order: - * \code - * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); - * \endcode - * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -149,11 +156,25 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( - OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, 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 will be computed using either COLAMD or METIS, dependeing on + * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); + * \endcode + * */ + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; // orderingType no longer needed + /** 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$, * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ @@ -194,20 +215,47 @@ namespace gtsam { const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - /** Compute the marginal of the requested variables and return the result as a Bayes net. + /** Compute the marginal of the requested variables and return the result as a Bayes net. Uses + * COLAMD marginalization ordering by default * @param variables Determines the variables whose marginal to compute, if provided as an * Ordering they will be ordered in the returned BayesNet as specified, and if provided * as a KeyVector they will be ordered using constrained COLAMD. - * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized - * out, i.e. all variables not in \c variables. If this is boost::none, the ordering - * will be computed with COLAMD. * @param function Optional dense elimination function, if not provided the default will be * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering = boost::none, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes net. + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a KeyVector they will be ordered using constrained COLAMD. + * @param marginalizedVariableOrdering Ordering for the variables being marginalized out, + * i.e. all variables not in \c variables. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant variables, + const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + + /** Compute the marginal of the requested variables and return the result as a Bayes tree. Uses + * COLAMD marginalization order by default + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a KeyVector they will be ordered using constrained COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -215,16 +263,15 @@ namespace gtsam { * @param variables Determines the variables whose marginal to compute, if provided as an * Ordering they will be ordered in the returned BayesNet as specified, and if provided * as a KeyVector they will be ordered using constrained COLAMD. - * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized - * out, i.e. all variables not in \c variables. If this is boost::none, the ordering - * will be computed with COLAMD. + * @param marginalizedVariableOrdering Ordering for the variables being marginalized out, + * i.e. all variables not in \c variables. * @param function Optional dense elimination function, if not provided the default will be * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, - OptionalOrdering marginalizedVariableOrdering = boost::none, + const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index fe0a9b2df..faa3f8bd6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -282,8 +282,13 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const - { + VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { + gttic(GaussianFactorGraph_optimize); + return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, const Eliminate& function) const { gttic(GaussianFactorGraph_optimize); return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize(); } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 8bdf4e475..f24fb602d 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -282,7 +282,14 @@ namespace gtsam { * the dense elimination function specified in \c function (default EliminatePreferCholesky), * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent * to calling graph.eliminateMultifrontal()->optimize(). */ - VectorValues optimize(OptionalOrdering ordering = boost::none, + VectorValues optimize( + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using + * the dense elimination function specified in \c function (default EliminatePreferCholesky), + * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent + * to calling graph.eliminateMultifrontal()->optimize(). */ + VectorValues optimize(const Ordering&, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; /** diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 9478f6fbf..c7d4e5405 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -82,29 +82,20 @@ string IterativeOptimizationParameters::verbosityTranslator( return "UNKNOWN"; } +/*****************************************************************************/ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + boost::optional keyInfo, + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); +} + /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo& keyInfo) { - return optimize(gfg, keyInfo, std::map()); -} - -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const std::map& lambda) { - return optimize(gfg, KeyInfo(gfg), lambda); -} - -/*****************************************************************************/ -VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) { - return optimize(gfg, KeyInfo(gfg), std::map()); -} - /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : ordering_(ordering) { diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 84ebe52cb..758d2aec9 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -91,21 +91,15 @@ public: virtual ~IterativeSolver() { } + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + boost::optional = boost::none, + boost::optional&> lambda = boost::none); + /* interface to the nonlinear optimizer, without initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda); - /* interface to the nonlinear optimizer, without damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo& keyInfo); - - /* interface to the nonlinear optimizer, without metadata and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, - const std::map& lambda); - - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg); - /* interface to the nonlinear optimizer that the subclasses have to implement */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 5312da34b..448120cdd 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,17 +34,7 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - - // If we have an ordering, pre-fill the ordered variables first - if (ordering) { - for (Key key : *ordering) { - add(key, 0); - } - } - +void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { // Now, find dimensions of variables and/or extend for (const auto& factor : gfg) { if (!factor) @@ -68,10 +58,30 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // To keep the same behavior as before, sort the keys after the ordering iterator first = begin(); - if (ordering) first += ordering->size(); + first += sortStart; if (first != end()) std::sort(first, end()); } +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg) { + gttic(Scatter_Constructor); + + ScatterHelper(gfg, 0); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + const Ordering& ordering) { + gttic(Scatter_Constructor); + + // pre-fill the ordered variables first + for (Key key : ordering) { + add(key, 0); + } + + ScatterHelper(gfg, ordering.size()); +} + /* ************************************************************************* */ void Scatter::add(Key key, size_t dim) { emplace_back(SlotEntry(key, dim)); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 793961c59..8936f827c 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { class GaussianFactorGraph; @@ -53,15 +51,21 @@ class Scatter : public FastVector { /// Default Constructor Scatter() {} - /// Construct from gaussian factor graph, with optional (partial or complete) ordering - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); + /// Construct from gaussian factor graph, without ordering + explicit Scatter(const GaussianFactorGraph& gfg); + + /// Construct from gaussian factor graph, with (partial or complete) ordering + explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering); /// Add a key/dim pair void add(Key key, size_t dim); private: + /// Helper function for constructors, adds/finds dimensions of variables and + // sorts starting from sortStart + void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart); + /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); }; diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index c24cb6143..c29a79623 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -26,8 +26,16 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) + : values_(solution), factorization_(factorization) { + gttic(MarginalsConstructor); + graph_ = *graph.linearize(solution); + computeBayesTree(); +} + +/* ************************************************************************* */ +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, + Factorization factorization) : values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); graph_ = *graph.linearize(solution); @@ -35,28 +43,52 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, } /* ************************************************************************* */ -Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) - : graph_(graph), factorization_(factorization) { +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization) + : graph_(graph), values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); - Values vals; - for (const auto& keyValue: solution) { - vals.insert(keyValue.first, keyValue.second); - } - values_ = vals; - computeBayesTree(ordering); + computeBayesTree(); } /* ************************************************************************* */ -Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, - EliminateableFactorGraph::OptionalOrdering ordering) +Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, + Factorization factorization) : graph_(graph), values_(solution), factorization_(factorization) { gttic(MarginalsConstructor); computeBayesTree(ordering); } /* ************************************************************************* */ -void Marginals::computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering) { +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization) + : graph_(graph), factorization_(factorization) { + gttic(MarginalsConstructor); + for (const auto& keyValue: solution) { + values_.insert(keyValue.first, keyValue.second); + } + computeBayesTree(); +} + +/* ************************************************************************* */ +Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, + Factorization factorization) + : graph_(graph), factorization_(factorization) { + gttic(MarginalsConstructor); + for (const auto& keyValue: solution) { + values_.insert(keyValue.first, keyValue.second); + } + computeBayesTree(ordering); +} + +/* ************************************************************************* */ +void Marginals::computeBayesTree() { + // Compute BayesTree + if(factorization_ == CHOLESKY) + bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); + else if(factorization_ == QR) + bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); +} + +/* ************************************************************************* */ +void Marginals::computeBayesTree(const Ordering& ordering) { // Compute BayesTree if(factorization_ == CHOLESKY) bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); @@ -128,9 +160,9 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR); } else { if(factorization_ == CHOLESKY) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminatePreferCholesky)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminatePreferCholesky)); else if(factorization_ == QR) - jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, boost::none, EliminateQR)); + jointFG = GaussianFactorGraph(*graph_.marginalMultifrontalBayesTree(variables, EliminateQR)); } // Get information matrix diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 54a290196..abad71ea7 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -55,10 +55,33 @@ public: * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). - * @param ordering An optional variable ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a nonlinear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering The ordering for elimination. + */ + Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + + /** Construct a marginals class from a linear factor graph. + * @param graph The factor graph defining the full joint density on all variables. + * @param solution The solution point to compute Gaussian marginals. + * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering The ordering for elimination. + */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -66,8 +89,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. * @param graph The factor graph defining the full joint density on all variables. @@ -75,8 +97,8 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, - EliminateableFactorGraph::OptionalOrdering ordering = boost::none); + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Factorization factorization = CHOLESKY); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -103,7 +125,10 @@ public: protected: /** Compute the Bayes Tree as a helper function to the constructor */ - void computeBayesTree(EliminateableFactorGraph::OptionalOrdering ordering); + void computeBayesTree(); + + /** Compute the Bayes Tree as a helper function to the constructor */ + void computeBayesTree(const Ordering& ordering); }; diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 0223250b5..525abd098 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -14,7 +14,14 @@ using namespace std; namespace gtsam { /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment(OptionalOrdering ordering) const { + CSP::sharedValues CSP::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); + sharedValues mpe = chordal->optimize(); + return mpe; + } + + /// Find the best total assignment - can be expensive + CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); sharedValues mpe = chordal->optimize(); return mpe; diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index bbdadd3dc..9e843f667 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -60,7 +60,10 @@ namespace gtsam { // } /// Find the best total assignment - can be expensive - sharedValues optimalAssignment(OptionalOrdering ordering = boost::none) const; + sharedValues optimalAssignment() const; + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment(const Ordering& ordering) const; // /* // * Perform loopy belief propagation diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index c4e9d26f5..bf968c8d7 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,7 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = fg.optimize(boost::none, EliminateCholesky); + VectorValues actual = fg.optimize(EliminateCholesky); // verify VectorValues expected = createCorrectDelta(); @@ -220,7 +220,7 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = fg.optimize(boost::none, EliminateQR); + VectorValues actual = fg.optimize(EliminateQR); // verify VectorValues expected = createCorrectDelta();