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

Removed variant as it did not play well with the python wrapper.

release/4.3a0
Frank Dellaert 2023-02-05 14:25:12 -08:00
parent a77b5bc1d7
commit b9ccdf9b11
3 changed files with 227 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -235,7 +235,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
OrderingKeyVectorVariant variables,
const Ordering& variables,
const Eliminate& function, OptionalVariableIndex variableIndex) const
{
if(!variableIndex) {
@ -245,16 +245,12 @@ namespace gtsam {
} else {
// No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD.
bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != nullptr);
const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
? get_pointer(std::get<const OrderingConstRef>(variables))
: get_pointer(std::get<const KeyVectorConstRef>(variables));
constexpr bool forceOrder = true;
Ordering totalOrdering =
Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
const size_t nVars = variables.size();
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
@ -267,7 +263,35 @@ namespace gtsam {
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
OrderingKeyVectorVariant variables,
const KeyVector& variables,
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, function, std::cref(index));
} else {
// No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD.
const constexpr bool forceOrder = false;
Ordering totalOrdering =
Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
// Split up ordering
const size_t nVars = variables.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(std::cref(marginalVarsOrdering), marginalizationOrdering, function, variableIndex);
}
}
/* ************************************************************************* */
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
const Ordering& variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function, OptionalVariableIndex variableIndex) const
{
@ -282,18 +306,33 @@ namespace gtsam {
const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(std::get_if<const OrderingConstRef>(&variables))
{
const Ordering* varsAsOrdering = get_pointer(std::get<const OrderingConstRef>(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(Ordering::COLAMD, function);
}
// An ordering was also provided for the unmarginalized variables, so we can also
// eliminate them in the order requested.
return factorGraph->eliminateSequential(variables, function);
}
}
/* ************************************************************************* */
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
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.
const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
return factorGraph->eliminateSequential(Ordering::COLAMD, function);
}
}
@ -301,7 +340,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
OrderingKeyVectorVariant variables,
const Ordering& variables,
const Eliminate& function, OptionalVariableIndex variableIndex) const
{
if(!variableIndex) {
@ -311,16 +350,12 @@ namespace gtsam {
} else {
// No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD.
bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != 0);
const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
? get_pointer(std::get<const OrderingConstRef>(variables))
: get_pointer(std::get<const KeyVectorConstRef>(variables));
constexpr bool forceOrder = true;
Ordering totalOrdering =
Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
const size_t nVars = variables.size();
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
@ -333,7 +368,35 @@ namespace gtsam {
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
OrderingKeyVectorVariant variables,
const KeyVector& variables,
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, function, std::cref(computedVariableIndex));
} else {
// No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD.
constexpr bool forceOrder = false;
Ordering totalOrdering =
Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
// Split up ordering
const size_t nVars = variables.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(std::cref(marginalVarsOrdering), marginalizationOrdering, function, variableIndex);
}
}
/* ************************************************************************* */
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
const Ordering& variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function, OptionalVariableIndex variableIndex) const
{
@ -348,18 +411,33 @@ namespace gtsam {
const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
if(std::get_if<const OrderingConstRef>(&variables))
{
const Ordering* varsAsOrdering = get_pointer(std::get<const OrderingConstRef>(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(Ordering::COLAMD, function);
}
// An ordering was also provided for the unmarginalized variables, so we can also
// eliminate them in the order requested.
return factorGraph->eliminateMultifrontal(variables, function);
}
}
/* ************************************************************************* */
template<class FACTORGRAPH>
std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
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, std::cref(computedVariableIndex));
} else {
gttic(marginalMultifrontalBayesTree);
// An ordering was provided for the marginalized variables, so we can first eliminate them
// in the order requested.
const auto [bayesTree, factorGraph] =
eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
}
}

105
gtsam/inference/EliminateableFactorGraph.h
View File

@ -22,20 +22,11 @@
#include <cstddef>
#include <functional>
#include <optional>
#include <variant>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
namespace gtsam {
// Creating an alias for the variant type since it is verbose
template <typename T>
using ref_wrap = std::reference_wrapper<T>;
using OrderingConstRef = std::reference_wrapper<const Ordering>;
using KeyVectorConstRef = std::reference_wrapper<const KeyVector>;
using OrderingKeyVectorVariant =
std::variant<const OrderingConstRef, const KeyVectorConstRef>;
/// Traits class for eliminateable factor graphs, specifies the types that result from
/// elimination, etc. This must be defined for each factor graph that inherits from
/// EliminateableFactorGraph.
@ -224,52 +215,89 @@ namespace gtsam {
/** 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 function Optional dense elimination function, if not provided the default will be
* used.
* @param variables Determines the *ordered* variables whose marginal to compute,
* will be ordered in the returned BayesNet.
* @param function Optional dense elimination function.
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */
* provided one will be computed.
*/
std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
OrderingKeyVectorVariant variables,
const Ordering& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const;
/** 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,
* will be ordered using constrained COLAMD.
* @param function Optional dense elimination function.
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed.
*/
std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) 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 variables Determines the *ordered* variables whose marginal to compute,
* will be ordered in the returned BayesNet.
* @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 function Optional dense elimination function.
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */
* provided one will be computed.
*/
std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
OrderingKeyVectorVariant variables,
const Ordering& variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) 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,
* 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.
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed.
*/
std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
const KeyVector& variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) 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 variables Determines the *ordered* variables whose marginal to compute,
* will be ordered in the returned BayesNet.
* @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. */
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
OrderingKeyVectorVariant variables,
const Ordering& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) 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,
* 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. */
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal of the requested variables and return the result as a Bayes tree.
* @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 variables Determines the *ordered* variables whose marginal to compute,
* will be ordered in the returned BayesNet.
* @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
@ -277,7 +305,22 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
OrderingKeyVectorVariant variables,
const Ordering& variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const;
/** Compute the marginal of the requested variables and return the result as a Bayes tree.
* @param variables Determines the variables whose marginal to compute,
* 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. */
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
const KeyVector& variables,
const Ordering& marginalizedVariableOrdering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = {}) const;

41
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -124,15 +124,40 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
}
/* ************************************************************************* */
TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
auto expectedBayesNet =
SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional(
1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3));
TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrdering) {
auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({1, 2, 3})({2, 3})({3});
auto ordering = Ordering{0,1,2,3};
SymbolicBayesNet actual1 =
*simpleTestGraph2.marginalMultifrontalBayesNet(std::cref(ordering));
EXPECT(assert_equal(expectedBayesNet, actual1));
SymbolicBayesNet actual =
*simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3});
EXPECT(assert_equal(expectedBayesNet, actual));
}
TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVector) {
auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({2, 1, 3})({1, 3})({3});
SymbolicBayesNet actual =
*simpleTestGraph2.marginalMultifrontalBayesNet(KeyVector{0, 1, 2, 3});
EXPECT(assert_equal(expectedBayesNet, actual));
}
TEST_UNSAFE(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrderingPlus) {
auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional{0, 3})({3});
const Ordering orderedVariables{0, 3},
marginalizedVariableOrdering{1, 2, 4, 5};
SymbolicBayesNet actual = *simpleTestGraph2.marginalMultifrontalBayesNet(
orderedVariables, marginalizedVariableOrdering);
EXPECT(assert_equal(expectedBayesNet, actual));
}
TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVectorPlus) {
auto expectedBayesNet = SymbolicBayesNet({0, 1, 3})({3, 1})({1});
const KeyVector variables{0, 1, 3};
const Ordering marginalizedVariableOrdering{2, 4, 5};
SymbolicBayesNet actual = *simpleTestGraph2.marginalMultifrontalBayesNet(
variables, marginalizedVariableOrdering);
EXPECT(assert_equal(expectedBayesNet, actual));
}
/* ************************************************************************* */