12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
release/4.3a0
Gerry Chen 2019-10-20 01:15:20 -04:00
parent 5a358489dc
commit 1733f3ac98
15 changed files with 406 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/BayesTree-inst.h
View File

@ -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);
}
/* ************************************************************************* */

2
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -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);
}
}

315
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -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);
}
}
}

95
gtsam/inference/EliminateableFactorGraph.h
View File

@ -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;

9
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -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();
}

9
gtsam/linear/GaussianFactorGraph.h
View File

@ -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;
/**

25
gtsam/linear/IterativeSolver.cpp
View File

@ -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) {

16
gtsam/linear/IterativeSolver.h
View File

@ -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
gtsam/linear/Scatter.cpp
View File

@ -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));

14
gtsam/linear/Scatter.h
View File

@ -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);
};

64
gtsam/nonlinear/Marginals.cpp
View File

@ -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

41
gtsam/nonlinear/Marginals.h
View File

@ -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);
};

9
gtsam_unstable/discrete/CSP.cpp
View File

@ -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;

5
gtsam_unstable/discrete/CSP.h
View File

@ -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

4
tests/testGaussianFactorGraphB.cpp
View File

@ -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();