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 breakingrelease/4.3a0
							parent
							
								
									5a358489dc
								
							
						
					
					
						commit
						1733f3ac98
					
				|  | @ -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); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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); | ||||
|       } | ||||
|     } | ||||
| 
 | ||||
|  |  | |||
|  | @ -28,33 +28,19 @@ namespace gtsam { | |||
|   template<class FACTORGRAPH> | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::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<BayesNetType> bayesNet; | ||||
|       boost::shared_ptr<FactorGraphType> 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<class FACTORGRAPH> | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal( | ||||
|     OptionalOrdering ordering, const Eliminate& function, | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::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<BayesNetType> bayesNet; | ||||
|       boost::shared_ptr<FactorGraphType> 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<class FACTORGRAPH> | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::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<class FACTORGRAPH> | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::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<BayesTreeType> bayesTree; | ||||
|       boost::shared_ptr<FactorGraphType> 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<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet( | ||||
|     boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> bayesTree; | ||||
|         boost::shared_ptr<FactorGraphType> factorGraph; | ||||
|         boost::tie(bayesTree,factorGraph) = | ||||
|           eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); | ||||
| 
 | ||||
|         if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&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<const Ordering&>(&variables) != 0); | ||||
|         const KeyVector* variablesOrOrdering = | ||||
|           unmarginalizedAreOrdered ? | ||||
|           boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&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<const Ordering&>(&variables) != 0); | ||||
|       const KeyVector* variablesOrOrdering = | ||||
|         unmarginalizedAreOrdered ? | ||||
|         boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&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<class FACTORGRAPH> | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet( | ||||
|     boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> bayesTree; | ||||
|       boost::shared_ptr<FactorGraphType> factorGraph; | ||||
|       boost::tie(bayesTree,factorGraph) = | ||||
|         eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); | ||||
| 
 | ||||
|       if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&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<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree( | ||||
|     boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> bayesTree; | ||||
|         boost::shared_ptr<FactorGraphType> factorGraph; | ||||
|         boost::tie(bayesTree,factorGraph) = | ||||
|           eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); | ||||
| 
 | ||||
|         if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&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<const Ordering&>(&variables) != 0); | ||||
|         const KeyVector* variablesOrOrdering = | ||||
|           unmarginalizedAreOrdered ? | ||||
|           boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&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<const Ordering&>(&variables) != 0); | ||||
|       const KeyVector* variablesOrOrdering = | ||||
|         unmarginalizedAreOrdered ? | ||||
|         boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&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<class FACTORGRAPH> | ||||
|   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> | ||||
|     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree( | ||||
|     boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> bayesTree; | ||||
|       boost::shared_ptr<FactorGraphType> factorGraph; | ||||
|       boost::tie(bayesTree,factorGraph) = | ||||
|         eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); | ||||
| 
 | ||||
|       if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&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); | ||||
|       } | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -88,9 +88,6 @@ namespace gtsam { | |||
|     /// The function type that does a single dense elimination step on a subgraph.
 | ||||
|     typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate; | ||||
| 
 | ||||
|     /// Typedef for an optional ordering as an argument to elimination functions
 | ||||
|     typedef const boost::optional<Ordering>& OptionalOrdering; | ||||
| 
 | ||||
|     /// Typedef for an optional variable index as an argument to elimination functions
 | ||||
|     typedef const boost::optional<VariableIndex>& OptionalVariableIndex; | ||||
| 
 | ||||
|  | @ -108,25 +105,40 @@ namespace gtsam { | |||
|      *  <b> Example - METIS ordering for elimination | ||||
|      *  \code | ||||
|      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS); | ||||
|      * | ||||
|      *  <b> Example - Full QR elimination in specified order: | ||||
|      *  \code | ||||
|      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, myOrdering); | ||||
|      *  \endcode | ||||
|      * | ||||
|      *  <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b> | ||||
|      *  \code | ||||
|      *  VariableIndex varIndex(graph); // Build variable index
 | ||||
|      *  Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
 | ||||
|      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); | ||||
|      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, varIndex, boost::none); | ||||
|      *  \endcode | ||||
|      *  */ | ||||
|     boost::shared_ptr<BayesNetType> 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.
 | ||||
|      * | ||||
|      *  <b> Example - Full QR elimination in specified order: | ||||
|      *  \code | ||||
|      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR); | ||||
|      *  \endcode | ||||
|      * | ||||
|      *  <b> Example - Reusing an existing VariableIndex to improve performance: </b> | ||||
|      *  \code | ||||
|      *  VariableIndex varIndex(graph); // Build variable index
 | ||||
|      *  Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
 | ||||
|      *  boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none); | ||||
|      *  \endcode | ||||
|      *  */ | ||||
|     boost::shared_ptr<BayesNetType> 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<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateCholesky); | ||||
|      *  \endcode | ||||
|      * | ||||
|      *  <b> Example - Full QR elimination in specified order: | ||||
|      *  \code | ||||
|      *  boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering); | ||||
|      *  \endcode | ||||
|      * | ||||
|      *  <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b> | ||||
|      *  \code | ||||
|      *  VariableIndex varIndex(graph); // Build variable index
 | ||||
|  | @ -149,11 +156,25 @@ namespace gtsam { | |||
|      *  \endcode | ||||
|      *  */ | ||||
|     boost::shared_ptr<BayesTreeType> 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) | ||||
|      * | ||||
|      *  <b> Example - Full QR elimination in specified order: | ||||
|      *  \code | ||||
|      *  boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering); | ||||
|      *  \endcode | ||||
|      *  */ | ||||
|     boost::shared_ptr<BayesTreeType> 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<BayesNetType> marginalMultifrontalBayesNet( | ||||
|       boost::variant<const Ordering&, const KeyVector&> 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<BayesNetType> marginalMultifrontalBayesNet( | ||||
|       boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> marginalMultifrontalBayesTree( | ||||
|       boost::variant<const Ordering&, const KeyVector&> 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<BayesTreeType> marginalMultifrontalBayesTree( | ||||
|       boost::variant<const Ordering&, const KeyVector&> 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; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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(); | ||||
|   } | ||||
|  |  | |||
|  | @ -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; | ||||
| 
 | ||||
|     /**
 | ||||
|  |  | |||
|  | @ -82,29 +82,20 @@ string IterativeOptimizationParameters::verbosityTranslator( | |||
|     return "UNKNOWN"; | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     boost::optional<const KeyInfo&> keyInfo, | ||||
|     boost::optional<const std::map<Key, Vector>&> lambda) { | ||||
|   return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), | ||||
|       lambda ? *lambda : std::map<Key, Vector>()); | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) { | ||||
|   return optimize(gfg, keyInfo, lambda, keyInfo.x0()); | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo& keyInfo) { | ||||
|   return optimize(gfg, keyInfo, std::map<Key, Vector>()); | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     const std::map<Key, Vector>& lambda) { | ||||
|   return optimize(gfg, KeyInfo(gfg), lambda); | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) { | ||||
|   return optimize(gfg, KeyInfo(gfg), std::map<Key, Vector>()); | ||||
| } | ||||
| 
 | ||||
| /****************************************************************************/ | ||||
| KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : | ||||
|     ordering_(ordering) { | ||||
|  |  | |||
|  | @ -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<const KeyInfo&> = boost::none, | ||||
|       boost::optional<const std::map<Key, Vector>&> lambda = boost::none); | ||||
| 
 | ||||
|   /* interface to the nonlinear optimizer, without initial estimate */ | ||||
|   GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, | ||||
|       const std::map<Key, Vector> &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<Key, Vector>& 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<Key, Vector> &lambda, | ||||
|  |  | |||
|  | @ -34,17 +34,7 @@ string SlotEntry::toString() const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Scatter::Scatter(const GaussianFactorGraph& gfg, | ||||
|     boost::optional<const Ordering&> 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)); | ||||
|  |  | |||
|  | @ -23,8 +23,6 @@ | |||
| #include <gtsam/base/FastMap.h> | ||||
| #include <gtsam/dllexport.h> | ||||
| 
 | ||||
| #include <boost/optional.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class GaussianFactorGraph; | ||||
|  | @ -53,15 +51,21 @@ class Scatter : public FastVector<SlotEntry> { | |||
|   /// Default Constructor
 | ||||
|   Scatter() {} | ||||
| 
 | ||||
|   /// Construct from gaussian factor graph, with optional (partial or complete) ordering
 | ||||
|   Scatter(const GaussianFactorGraph& gfg, | ||||
|           boost::optional<const Ordering&> 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); | ||||
| }; | ||||
|  |  | |||
|  | @ -26,8 +26,16 @@ using namespace std; | |||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, | ||||
|                      EliminateableFactorGraph<GaussianFactorGraph>::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<GaussianFactorGraph>::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<GaussianFactorGraph>::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<GaussianFactorGraph>::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
 | ||||
|  |  | |||
|  | @ -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<GaussianFactorGraph>::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<GaussianFactorGraph>::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<GaussianFactorGraph>::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<GaussianFactorGraph>::OptionalOrdering ordering); | ||||
|   void computeBayesTree(); | ||||
| 
 | ||||
|   /** Compute the Bayes Tree as a helper function to the constructor */ | ||||
|   void computeBayesTree(const Ordering& ordering); | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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(); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue