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

Merge pull request #1574 from borglab/feature/improved_wrapper

release/4.3a0
Frank Dellaert 2023-07-16 21:43:31 +02:00 committed by GitHub
parent eca51af19d 2453c37b2b
commit 13c7dafba3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
23 changed files with 704 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -28,9 +28,9 @@
namespace gtsam { namespace gtsam {
/** /**
* Algebraic Decision Trees fix the range to double * An algebraic decision tree fixes the range of a DecisionTree to double.
* Just has some nice constructors and some syntactic sugar * Just has some nice constructors and some syntactic sugar.
* TODO: consider eliminating this class altogether? * TODO(dellaert): consider eliminating this class altogether?
* *
* @ingroup discrete * @ingroup discrete
*/ */
@ -80,20 +80,62 @@ namespace gtsam {
AlgebraicDecisionTree(const L& label, double y1, double y2) AlgebraicDecisionTree(const L& label, double y1, double y2)
: Base(label, y1, y2) {} : Base(label, y1, y2) {}
/** Create a new leaf function splitting on a variable */ /**
* @brief Create a new leaf function splitting on a variable
*
* @param labelC: The label with cardinality 2
* @param y1: The value for the first key
* @param y2: The value for the second key
*
* Example:
* @code{.cpp}
* std::pair<string, size_t> A {"a", 2};
* AlgebraicDecisionTree<string> a(A, 0.6, 0.4);
* @endcode
*/
AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1, AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1,
double y2) double y2)
: Base(labelC, y1, y2) {} : Base(labelC, y1, y2) {}
/** Create from keys and vector table */ /**
* @brief Create from keys with cardinalities and a vector table
*
* @param labelCs: The keys, with cardinalities, given as pairs
* @param ys: The vector table
*
* Example with three keys, A, B, and C, with cardinalities 2, 3, and 2,
* respectively, and a vector table of size 12:
* @code{.cpp}
* DiscreteKey A(0, 2), B(1, 3), C(2, 2);
* const vector<double> cpt{
* 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, //
* 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10};
* AlgebraicDecisionTree<Key> expected(A & B & C, cpt);
* @endcode
* The table is given in the following order:
* A=0, B=0, C=0
* A=0, B=0, C=1
* ...
* A=1, B=1, C=1
* Hence, the first line in the table is for A==0, and the second for A==1.
* In each line, the first two entries are for B==0, the next two for B==1,
* and the last two for B==2. Each pair is for a C value of 0 and 1.
*/
AlgebraicDecisionTree // AlgebraicDecisionTree //
(const std::vector<typename Base::LabelC>& labelCs, (const std::vector<typename Base::LabelC>& labelCs,
const std::vector<double>& ys) { const std::vector<double>& ys) {
this->root_ = this->root_ =
Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end());
} }
/** Create from keys and string table */ /**
* @brief Create from keys and string table
*
* @param labelCs: The keys, with cardinalities, given as pairs
* @param table: The string table, given as a string of doubles.
*
* @note Table needs to be in same order as the vector table in the other constructor.
*/
AlgebraicDecisionTree // AlgebraicDecisionTree //
(const std::vector<typename Base::LabelC>& labelCs, (const std::vector<typename Base::LabelC>& labelCs,
const std::string& table) { const std::string& table) {
@ -108,7 +150,13 @@ namespace gtsam {
Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end());
} }
/** Create a new function splitting on a variable */ /**
* @brief Create a range of decision trees, splitting on a single variable.
*
* @param begin: Iterator to beginning of a range of decision trees
* @param end: Iterator to end of a range of decision trees
* @param label: The label to split on
*/
template <typename Iterator> template <typename Iterator>
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label)
: Base(nullptr) { : Base(nullptr) {

2
gtsam/discrete/DecisionTree-inl.h
View File

@ -622,7 +622,7 @@ namespace gtsam {
// B=1 // B=1
// A=0: 3 // A=0: 3
// A=1: 4 // A=1: 4
// Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce // Note, through the magic of "compose", create([A B],[1 3 2 4]) will produce
// exactly the same tree as above: the highest label is always the root. // exactly the same tree as above: the highest label is always the root.
// However, it will be *way* faster if labels are given highest to lowest. // However, it will be *way* faster if labels are given highest to lowest.
template<typename L, typename Y> template<typename L, typename Y>

31
gtsam/discrete/DecisionTree.h
View File

@ -37,9 +37,23 @@
namespace gtsam { namespace gtsam {
/** /**
* Decision Tree * @brief a decision tree is a function from assignments to values.
* L = label for variables * @tparam L label for variables
* Y = function range (any algebra), e.g., bool, int, double * @tparam Y function range (any algebra), e.g., bool, int, double
*
* After creating a decision tree on some variables, the tree can be evaluated
* on an assignment to those variables. Example:
*
* @code{.cpp}
* // Create a decision stump one one variable 'a' with values 10 and 20.
* DecisionTree<char, int> tree('a', 10, 20);
*
* // Evaluate the tree on an assignment to the variable.
* int value0 = tree({{'a', 0}}); // value0 = 10
* int value1 = tree({{'a', 1}}); // value1 = 20
* @endcode
*
* More examples can be found in testDecisionTree.cpp
* *
* @ingroup discrete * @ingroup discrete
*/ */
@ -132,7 +146,8 @@ namespace gtsam {
NodePtr root_; NodePtr root_;
protected: protected:
/** Internal recursive function to create from keys, cardinalities, /**
* Internal recursive function to create from keys, cardinalities,
* and Y values * and Y values
*/ */
template<typename It, typename ValueIt> template<typename It, typename ValueIt>
@ -163,7 +178,13 @@ namespace gtsam {
/** Create a constant */ /** Create a constant */
explicit DecisionTree(const Y& y); explicit DecisionTree(const Y& y);
/// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label` /**
* @brief Create tree with 2 assignments `y1`, `y2`, splitting on variable `label`
*
* @param label The variable to split on.
* @param y1 The value for the first assignment.
* @param y2 The value for the second assignment.
*/
DecisionTree(const L& label, const Y& y1, const Y& y2); DecisionTree(const L& label, const Y& y1, const Y& y2);
/** Allow Label+Cardinality for convenience */ /** Allow Label+Cardinality for convenience */

41
gtsam/discrete/DecisionTreeFactor.h
View File

@ -63,11 +63,46 @@ namespace gtsam {
/** Constructor from DiscreteKeys and AlgebraicDecisionTree */ /** Constructor from DiscreteKeys and AlgebraicDecisionTree */
DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials); DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
/** Constructor from doubles */ /**
* @brief Constructor from doubles
*
* @param keys The discrete keys.
* @param table The table of values.
*
* @throw std::invalid_argument if the size of `table` does not match the
* number of assignments.
*
* Example:
* @code{.cpp}
* DiscreteKey X(0,2), Y(1,3);
* const std::vector<double> table {2, 5, 3, 6, 4, 7};
* DecisionTreeFactor f1({X, Y}, table);
* @endcode
*
* The values in the table should be laid out so that the first key varies
* the slowest, and the last key the fastest.
*/
DecisionTreeFactor(const DiscreteKeys& keys, DecisionTreeFactor(const DiscreteKeys& keys,
const std::vector<double>& table); const std::vector<double>& table);
/** Constructor from string */ /**
* @brief Constructor from string
*
* @param keys The discrete keys.
* @param table The table of values.
*
* @throw std::invalid_argument if the size of `table` does not match the
* number of assignments.
*
* Example:
* @code{.cpp}
* DiscreteKey X(0,2), Y(1,3);
* DecisionTreeFactor factor({X, Y}, "2 5 3 6 4 7");
* @endcode
*
* The values in the table should be laid out so that the first key varies
* the slowest, and the last key the fastest.
*/
DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table); DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
/// Single-key specialization /// Single-key specialization

5
gtsam/discrete/DiscreteBayesTree.h
View File

@ -59,6 +59,11 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
//** evaluate conditional probability of subtree for given DiscreteValues */ //** evaluate conditional probability of subtree for given DiscreteValues */
double evaluate(const DiscreteValues& values) const; double evaluate(const DiscreteValues& values) const;
//** (Preferred) sugar for the above for given DiscreteValues */
double operator()(const DiscreteValues& values) const {
return evaluate(values);
}
}; };
/* ************************************************************************* */ /* ************************************************************************* */

39
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -42,16 +42,30 @@ class DiscreteJunctionTree;
/** /**
* @brief Main elimination function for DiscreteFactorGraph. * @brief Main elimination function for DiscreteFactorGraph.
* *
* @param factors * @param factors The factor graph to eliminate.
* @param keys * @param frontalKeys An ordering for which variables to eliminate.
* @return GTSAM_EXPORT * @return A pair of the resulting conditional and the separator factor.
* @ingroup discrete * @ingroup discrete
*/ */
GTSAM_EXPORT std::pair<boost::shared_ptr<DiscreteConditional>, DecisionTreeFactor::shared_ptr> GTSAM_EXPORT
EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys); std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/**
* @brief Alternate elimination function for that creates non-normalized lookup tables.
*
* @param factors The factor graph to eliminate.
* @param frontalKeys An ordering for which variables to eliminate.
* @return A pair of the resulting lookup table and the separator factor.
* @ingroup discrete
*/
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/* ************************************************************************* */
template<> struct EliminationTraits<DiscreteFactorGraph> template<> struct EliminationTraits<DiscreteFactorGraph>
{ {
typedef DiscreteFactor FactorType; ///< Type of factors in factor graph typedef DiscreteFactor FactorType; ///< Type of factors in factor graph
@ -61,12 +75,14 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
typedef DiscreteEliminationTree EliminationTreeType; ///< Type of elimination tree typedef DiscreteEliminationTree EliminationTreeType; ///< Type of elimination tree
typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree
typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function /// The default dense elimination function
static std::pair<boost::shared_ptr<ConditionalType>, static std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> > boost::shared_ptr<FactorType> >
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminateDiscrete(factors, keys); return EliminateDiscrete(factors, keys);
} }
/// The default ordering generation function /// The default ordering generation function
static Ordering DefaultOrderingFunc( static Ordering DefaultOrderingFunc(
const FactorGraphType& graph, const FactorGraphType& graph,
@ -75,7 +91,6 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
} }
}; };
/* ************************************************************************* */
/** /**
* A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. * A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
* Factor == DiscreteFactor * Factor == DiscreteFactor
@ -109,8 +124,8 @@ class GTSAM_EXPORT DiscreteFactorGraph
/** Implicit copy/downcast constructor to override explicit template container /** Implicit copy/downcast constructor to override explicit template container
* constructor */ * constructor */
template <class DERIVEDFACTOR> template <class DERIVED_FACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} DiscreteFactorGraph(const FactorGraph<DERIVED_FACTOR>& graph) : Base(graph) {}
/// Destructor /// Destructor
virtual ~DiscreteFactorGraph() {} virtual ~DiscreteFactorGraph() {}
@ -231,10 +246,6 @@ class GTSAM_EXPORT DiscreteFactorGraph
/// @} /// @}
}; // \ DiscreteFactorGraph }; // \ DiscreteFactorGraph
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
/// traits /// traits
template <> template <>
struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {}; struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};

2
gtsam/discrete/DiscreteJunctionTree.h
View File

@ -66,4 +66,6 @@ namespace gtsam {
DiscreteJunctionTree(const DiscreteEliminationTree& eliminationTree); DiscreteJunctionTree(const DiscreteEliminationTree& eliminationTree);
}; };
/// typedef for wrapper:
using DiscreteCluster = DiscreteJunctionTree::Cluster;
} }

5
gtsam/discrete/DiscreteValues.h
View File

@ -120,6 +120,11 @@ class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
/// @} /// @}
}; };
/// Free version of CartesianProduct.
inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
return DiscreteValues::CartesianProduct(keys);
}
/// Free version of markdown. /// Free version of markdown.
std::string markdown(const DiscreteValues& values, std::string markdown(const DiscreteValues& values,
const KeyFormatter& keyFormatter = DefaultKeyFormatter, const KeyFormatter& keyFormatter = DefaultKeyFormatter,

124
gtsam/discrete/discrete.i
View File

@ -17,6 +17,8 @@ class DiscreteKeys {
}; };
// DiscreteValues is added in specializations/discrete.h as a std::map // DiscreteValues is added in specializations/discrete.h as a std::map
std::vector<gtsam::DiscreteValues> cartesianProduct(
const gtsam::DiscreteKeys& keys);
string markdown( string markdown(
const gtsam::DiscreteValues& values, const gtsam::DiscreteValues& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
@ -31,27 +33,30 @@ string html(const gtsam::DiscreteValues& values,
std::map<gtsam::Key, std::vector<std::string>> names); std::map<gtsam::Key, std::vector<std::string>> names);
#include <gtsam/discrete/DiscreteFactor.h> #include <gtsam/discrete/DiscreteFactor.h>
class DiscreteFactor { virtual class DiscreteFactor : gtsam::Factor {
void print(string s = "DiscreteFactor\n", void print(string s = "DiscreteFactor\n",
const gtsam::KeyFormatter& keyFormatter = const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const; bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const;
bool empty() const;
size_t size() const;
double operator()(const gtsam::DiscreteValues& values) const; double operator()(const gtsam::DiscreteValues& values) const;
}; };
#include <gtsam/discrete/DecisionTreeFactor.h> #include <gtsam/discrete/DecisionTreeFactor.h>
virtual class DecisionTreeFactor : gtsam::DiscreteFactor { virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
DecisionTreeFactor(); DecisionTreeFactor();
DecisionTreeFactor(const gtsam::DiscreteKey& key, DecisionTreeFactor(const gtsam::DiscreteKey& key,
const std::vector<double>& spec); const std::vector<double>& spec);
DecisionTreeFactor(const gtsam::DiscreteKey& key, string table); DecisionTreeFactor(const gtsam::DiscreteKey& key, string table);
DecisionTreeFactor(const gtsam::DiscreteKeys& keys,
const std::vector<double>& table);
DecisionTreeFactor(const gtsam::DiscreteKeys& keys, string table); DecisionTreeFactor(const gtsam::DiscreteKeys& keys, string table);
DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys,
const std::vector<double>& table);
DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table); DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& keys, string table);
DecisionTreeFactor(const gtsam::DiscreteConditional& c); DecisionTreeFactor(const gtsam::DiscreteConditional& c);
void print(string s = "DecisionTreeFactor\n", void print(string s = "DecisionTreeFactor\n",
@ -59,6 +64,8 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const; bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const;
size_t cardinality(gtsam::Key j) const;
double operator()(const gtsam::DiscreteValues& values) const; double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
size_t cardinality(gtsam::Key j) const; size_t cardinality(gtsam::Key j) const;
@ -66,6 +73,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
gtsam::DecisionTreeFactor* sum(size_t nrFrontals) const; gtsam::DecisionTreeFactor* sum(size_t nrFrontals) const;
gtsam::DecisionTreeFactor* sum(const gtsam::Ordering& keys) const; gtsam::DecisionTreeFactor* sum(const gtsam::Ordering& keys) const;
gtsam::DecisionTreeFactor* max(size_t nrFrontals) const; gtsam::DecisionTreeFactor* max(size_t nrFrontals) const;
gtsam::DecisionTreeFactor* max(const gtsam::Ordering& keys) const;
string dot( string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@ -203,10 +211,16 @@ class DiscreteBayesTreeClique {
DiscreteBayesTreeClique(const gtsam::DiscreteConditional* conditional); DiscreteBayesTreeClique(const gtsam::DiscreteConditional* conditional);
const gtsam::DiscreteConditional* conditional() const; const gtsam::DiscreteConditional* conditional() const;
bool isRoot() const; bool isRoot() const;
size_t nrChildren() const;
const gtsam::DiscreteBayesTreeClique* operator[](size_t i) const;
void print(string s = "DiscreteBayesTreeClique",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printSignature( void printSignature(
const string& s = "Clique: ", const string& s = "Clique: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const; const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
double evaluate(const gtsam::DiscreteValues& values) const; double evaluate(const gtsam::DiscreteValues& values) const;
double operator()(const gtsam::DiscreteValues& values) const;
}; };
class DiscreteBayesTree { class DiscreteBayesTree {
@ -220,6 +234,9 @@ class DiscreteBayesTree {
bool empty() const; bool empty() const;
const DiscreteBayesTreeClique* operator[](size_t j) const; const DiscreteBayesTreeClique* operator[](size_t j) const;
double evaluate(const gtsam::DiscreteValues& values) const;
double operator()(const gtsam::DiscreteValues& values) const;
string dot(const gtsam::KeyFormatter& keyFormatter = string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
void saveGraph(string s, void saveGraph(string s,
@ -242,9 +259,9 @@ class DiscreteBayesTree {
class DiscreteLookupTable : gtsam::DiscreteConditional{ class DiscreteLookupTable : gtsam::DiscreteConditional{
DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys, DiscreteLookupTable(size_t nFrontals, const gtsam::DiscreteKeys& keys,
const gtsam::DecisionTreeFactor::ADT& potentials); const gtsam::DecisionTreeFactor::ADT& potentials);
void print( void print(string s = "Discrete Lookup Table: ",
const std::string& s = "Discrete Lookup Table: ", const gtsam::KeyFormatter& keyFormatter =
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
size_t argmax(const gtsam::DiscreteValues& parentsValues) const; size_t argmax(const gtsam::DiscreteValues& parentsValues) const;
}; };
@ -263,6 +280,14 @@ class DiscreteLookupDAG {
}; };
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors,
const gtsam::Ordering& frontalKeys);
std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
EliminateForMPE(const gtsam::DiscreteFactorGraph& factors,
const gtsam::Ordering& frontalKeys);
class DiscreteFactorGraph { class DiscreteFactorGraph {
DiscreteFactorGraph(); DiscreteFactorGraph();
DiscreteFactorGraph(const gtsam::DiscreteBayesNet& bayesNet); DiscreteFactorGraph(const gtsam::DiscreteBayesNet& bayesNet);
@ -277,6 +302,7 @@ class DiscreteFactorGraph {
void add(const gtsam::DiscreteKey& j, const std::vector<double>& spec); void add(const gtsam::DiscreteKey& j, const std::vector<double>& spec);
void add(const gtsam::DiscreteKeys& keys, string spec); void add(const gtsam::DiscreteKeys& keys, string spec);
void add(const std::vector<gtsam::DiscreteKey>& keys, string spec); void add(const std::vector<gtsam::DiscreteKey>& keys, string spec);
void add(const std::vector<gtsam::DiscreteKey>& keys, const std::vector<double>& spec);
bool empty() const; bool empty() const;
size_t size() const; size_t size() const;
@ -290,25 +316,46 @@ class DiscreteFactorGraph {
double operator()(const gtsam::DiscreteValues& values) const; double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DiscreteValues optimize() const; gtsam::DiscreteValues optimize() const;
gtsam::DiscreteBayesNet sumProduct(); gtsam::DiscreteBayesNet sumProduct(
gtsam::DiscreteBayesNet sumProduct(gtsam::Ordering::OrderingType type); gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteBayesNet sumProduct(const gtsam::Ordering& ordering); gtsam::DiscreteBayesNet sumProduct(const gtsam::Ordering& ordering);
gtsam::DiscreteLookupDAG maxProduct(); gtsam::DiscreteLookupDAG maxProduct(
gtsam::DiscreteLookupDAG maxProduct(gtsam::Ordering::OrderingType type); gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering); gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesNet* eliminateSequential(); gtsam::DiscreteBayesNet* eliminateSequential(
gtsam::DiscreteBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type); gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteBayesNet* eliminateSequential(
gtsam::Ordering::OrderingType type,
const gtsam::DiscreteFactorGraph::Eliminate& function);
gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering); gtsam::DiscreteBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesNet* eliminateSequential(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*> pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
eliminatePartialSequential(const gtsam::Ordering& ordering); eliminatePartialSequential(const gtsam::Ordering& ordering);
pair<gtsam::DiscreteBayesNet*, gtsam::DiscreteFactorGraph*>
eliminatePartialSequential(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
gtsam::DiscreteBayesTree* eliminateMultifrontal(); gtsam::DiscreteBayesTree* eliminateMultifrontal(
gtsam::DiscreteBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type); gtsam::Ordering::OrderingType type = gtsam::Ordering::COLAMD);
gtsam::DiscreteBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); gtsam::DiscreteBayesTree* eliminateMultifrontal(
gtsam::Ordering::OrderingType type,
const gtsam::DiscreteFactorGraph::Eliminate& function);
gtsam::DiscreteBayesTree* eliminateMultifrontal(
const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree* eliminateMultifrontal(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*> pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering); eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::DiscreteBayesTree*, gtsam::DiscreteFactorGraph*>
eliminatePartialMultifrontal(
const gtsam::Ordering& ordering,
const gtsam::DiscreteFactorGraph::Eliminate& function);
string dot( string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
@ -328,4 +375,41 @@ class DiscreteFactorGraph {
std::map<gtsam::Key, std::vector<std::string>> names) const; std::map<gtsam::Key, std::vector<std::string>> names) const;
}; };
#include <gtsam/discrete/DiscreteEliminationTree.h>
class DiscreteEliminationTree {
DiscreteEliminationTree(const gtsam::DiscreteFactorGraph& factorGraph,
const gtsam::VariableIndex& structure,
const gtsam::Ordering& order);
DiscreteEliminationTree(const gtsam::DiscreteFactorGraph& factorGraph,
const gtsam::Ordering& order);
void print(
string name = "EliminationTree: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteEliminationTree& other,
double tol = 1e-9) const;
};
#include <gtsam/discrete/DiscreteJunctionTree.h>
class DiscreteCluster {
gtsam::Ordering orderedFrontalKeys;
gtsam::DiscreteFactorGraph factors;
const gtsam::DiscreteCluster& operator[](size_t i) const;
size_t nrChildren() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
};
class DiscreteJunctionTree {
DiscreteJunctionTree(const gtsam::DiscreteEliminationTree& eliminationTree);
void print(
string name = "JunctionTree: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
size_t nrRoots() const;
const gtsam::DiscreteCluster& operator[](size_t i) const;
};
} // namespace gtsam } // namespace gtsam

68
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -71,6 +71,19 @@ struct traits<CrazyDecisionTree> : public Testable<CrazyDecisionTree> {};
GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree) GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree)
/* ************************************************************************** */
// Test char labels and int range
/* ************************************************************************** */
// Create a decision stump one one variable 'a' with values 10 and 20.
TEST(DecisionTree, Constructor) {
DecisionTree<char, int> tree('a', 10, 20);
// Evaluate the tree on an assignment to the variable.
EXPECT_LONGS_EQUAL(10, tree({{'a', 0}}));
EXPECT_LONGS_EQUAL(20, tree({{'a', 1}}));
}
/* ************************************************************************** */ /* ************************************************************************** */
// Test string labels and int range // Test string labels and int range
/* ************************************************************************** */ /* ************************************************************************** */
@ -114,18 +127,47 @@ struct Ring {
static inline int mul(const int& a, const int& b) { return a * b; } static inline int mul(const int& a, const int& b) { return a * b; }
}; };
/* ************************************************************************** */
// Check that creating decision trees respects key order.
TEST(DecisionTree, ConstructorOrder) {
// Create labels
string A("A"), B("B");
const std::vector<int> ys1 = {1, 2, 3, 4};
DT tree1({{B, 2}, {A, 2}}, ys1); // faster version, as B is "higher" than A!
const std::vector<int> ys2 = {1, 3, 2, 4};
DT tree2({{A, 2}, {B, 2}}, ys2); // slower version !
// Both trees will be the same, tree is order from high to low labels.
// Choice(B)
// 0 Choice(A)
// 0 0 Leaf 1
// 0 1 Leaf 2
// 1 Choice(A)
// 1 0 Leaf 3
// 1 1 Leaf 4
EXPECT(tree2.equals(tree1));
// Check the values are as expected by calling the () operator:
EXPECT_LONGS_EQUAL(1, tree1({{A, 0}, {B, 0}}));
EXPECT_LONGS_EQUAL(3, tree1({{A, 0}, {B, 1}}));
EXPECT_LONGS_EQUAL(2, tree1({{A, 1}, {B, 0}}));
EXPECT_LONGS_EQUAL(4, tree1({{A, 1}, {B, 1}}));
}
/* ************************************************************************** */ /* ************************************************************************** */
// test DT // test DT
TEST(DecisionTree, example) { TEST(DecisionTree, Example) {
// Create labels // Create labels
string A("A"), B("B"), C("C"); string A("A"), B("B"), C("C");
// create a value // Create assignments using brace initialization:
Assignment<string> x00, x01, x10, x11; Assignment<string> x00{{A, 0}, {B, 0}};
x00[A] = 0, x00[B] = 0; Assignment<string> x01{{A, 0}, {B, 1}};
x01[A] = 0, x01[B] = 1; Assignment<string> x10{{A, 1}, {B, 0}};
x10[A] = 1, x10[B] = 0; Assignment<string> x11{{A, 1}, {B, 1}};
x11[A] = 1, x11[B] = 1;
// empty // empty
DT empty; DT empty;
@ -237,8 +279,7 @@ TEST(DecisionTree, ConvertValuesOnly) {
StringBoolTree f2(f1, bool_of_int); StringBoolTree f2(f1, bool_of_int);
// Check a value // Check a value
Assignment<string> x00; Assignment<string> x00 {{A, 0}, {B, 0}};
x00["A"] = 0, x00["B"] = 0;
EXPECT(!f2(x00)); EXPECT(!f2(x00));
} }
@ -262,10 +303,11 @@ TEST(DecisionTree, ConvertBoth) {
// Check some values // Check some values
Assignment<Label> x00, x01, x10, x11; Assignment<Label> x00, x01, x10, x11;
x00[X] = 0, x00[Y] = 0; x00 = {{X, 0}, {Y, 0}};
x01[X] = 0, x01[Y] = 1; x01 = {{X, 0}, {Y, 1}};
x10[X] = 1, x10[Y] = 0; x10 = {{X, 1}, {Y, 0}};
x11[X] = 1, x11[Y] = 1; x11 = {{X, 1}, {Y, 1}};
EXPECT(!f2(x00)); EXPECT(!f2(x00));
EXPECT(!f2(x01)); EXPECT(!f2(x01));
EXPECT(f2(x10)); EXPECT(f2(x10));

25
gtsam/discrete/tests/testDecisionTreeFactor.cpp
View File

@ -27,6 +27,18 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */
TEST(DecisionTreeFactor, ConstructorsMatch) {
// Declare two keys
DiscreteKey X(0, 2), Y(1, 3);
// Create with vector and with string
const std::vector<double> table {2, 5, 3, 6, 4, 7};
DecisionTreeFactor f1({X, Y}, table);
DecisionTreeFactor f2({X, Y}, "2 5 3 6 4 7");
EXPECT(assert_equal(f1, f2));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( DecisionTreeFactor, constructors) TEST( DecisionTreeFactor, constructors)
{ {
@ -41,16 +53,13 @@ TEST( DecisionTreeFactor, constructors)
EXPECT_LONGS_EQUAL(2,f2.size()); EXPECT_LONGS_EQUAL(2,f2.size());
EXPECT_LONGS_EQUAL(3,f3.size()); EXPECT_LONGS_EQUAL(3,f3.size());
DiscreteValues values; DiscreteValues x121{{0, 1}, {1, 2}, {2, 1}};
values[0] = 1; // x EXPECT_DOUBLES_EQUAL(8, f1(x121), 1e-9);
values[1] = 2; // y EXPECT_DOUBLES_EQUAL(7, f2(x121), 1e-9);
values[2] = 1; // z EXPECT_DOUBLES_EQUAL(75, f3(x121), 1e-9);
EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9);
EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9);
EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9);
// Assert that error = -log(value) // Assert that error = -log(value)
EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(-log(f1(x121)), f1.error(x121), 1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */

233
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -16,23 +16,24 @@
*/ */
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/discrete/DiscreteBayesNet.h> #include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteBayesTree.h> #include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream> #include <iostream>
#include <vector> #include <vector>
using namespace std;
using namespace gtsam; using namespace gtsam;
static constexpr bool debug = false; static constexpr bool debug = false;
/* ************************************************************************* */ /* ************************************************************************* */
struct TestFixture { struct TestFixture {
vector<DiscreteKey> keys; DiscreteKeys keys;
std::vector<DiscreteValues> assignments;
DiscreteBayesNet bayesNet; DiscreteBayesNet bayesNet;
boost::shared_ptr<DiscreteBayesTree> bayesTree; boost::shared_ptr<DiscreteBayesTree> bayesTree;
@ -47,6 +48,9 @@ struct TestFixture {
keys.push_back(key_i); keys.push_back(key_i);
} }
// Enumerate all assignments.
assignments = DiscreteValues::CartesianProduct(keys);
// Create thin-tree Bayesnet. // Create thin-tree Bayesnet.
bayesNet.add(keys[14] % "1/3"); bayesNet.add(keys[14] % "1/3");
@ -74,9 +78,9 @@ struct TestFixture {
}; };
/* ************************************************************************* */ /* ************************************************************************* */
// Check that BN and BT give the same answer on all configurations
TEST(DiscreteBayesTree, ThinTree) { TEST(DiscreteBayesTree, ThinTree) {
const TestFixture self; TestFixture self;
const auto& keys = self.keys;
if (debug) { if (debug) {
GTSAM_PRINT(self.bayesNet); GTSAM_PRINT(self.bayesNet);
@ -95,47 +99,56 @@ TEST(DiscreteBayesTree, ThinTree) {
EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals())); EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals()));
} }
auto R = self.bayesTree->roots().front(); for (const auto& x : self.assignments) {
// Check whether BN and BT give the same answer on all configurations
auto allPosbValues = DiscreteValues::CartesianProduct(
keys[0] & keys[1] & keys[2] & keys[3] & keys[4] & keys[5] & keys[6] &
keys[7] & keys[8] & keys[9] & keys[10] & keys[11] & keys[12] & keys[13] &
keys[14]);
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteValues x = allPosbValues[i];
double expected = self.bayesNet.evaluate(x); double expected = self.bayesNet.evaluate(x);
double actual = self.bayesTree->evaluate(x); double actual = self.bayesTree->evaluate(x);
DOUBLES_EQUAL(expected, actual, 1e-9); DOUBLES_EQUAL(expected, actual, 1e-9);
} }
}
// Calculate all some marginals for DiscreteValues==all1 /* ************************************************************************* */
Vector marginals = Vector::Zero(15); // Check calculation of separator marginals
double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0, TEST(DiscreteBayesTree, SeparatorMarginals) {
joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, TestFixture self;
joint_4_11 = 0, joint_11_13 = 0, joint_11_13_14 = 0,
joint_11_12_13_14 = 0, joint_9_11_12_13 = 0, joint_8_11_12_13 = 0; // Calculate some marginals for DiscreteValues==all1
for (size_t i = 0; i < allPosbValues.size(); ++i) { double marginal_14 = 0, joint_8_12 = 0;
DiscreteValues x = allPosbValues[i]; for (auto& x : self.assignments) {
double px = self.bayesTree->evaluate(x); double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
if (x[12] && x[14]) {
joint_12_14 += px;
if (x[9]) joint_9_12_14 += px;
if (x[8]) joint_8_12_14 += px;
}
if (x[8] && x[12]) joint_8_12 += px; if (x[8] && x[12]) joint_8_12 += px;
if (x[2]) { if (x[14]) marginal_14 += px;
if (x[8]) joint82 += px; }
if (x[1]) joint12 += px; DiscreteValues all1 = self.assignments.back();
}
if (x[4]) { // check separator marginal P(S0)
if (x[2]) joint24 += px; auto clique = (*self.bayesTree)[0];
if (x[5]) joint45 += px; DiscreteFactorGraph separatorMarginal0 =
if (x[6]) joint46 += px; clique->separatorMarginal(EliminateDiscrete);
if (x[11]) joint_4_11 += px; DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
}
// check separator marginal P(S9), should be P(14)
clique = (*self.bayesTree)[9];
DiscreteFactorGraph separatorMarginal9 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(marginal_14, separatorMarginal9(all1), 1e-9);
// check separator marginal of root, should be empty
clique = (*self.bayesTree)[11];
DiscreteFactorGraph separatorMarginal11 =
clique->separatorMarginal(EliminateDiscrete);
LONGS_EQUAL(0, separatorMarginal11.size());
}
/* ************************************************************************* */
// Check shortcuts in the tree
TEST(DiscreteBayesTree, Shortcuts) {
TestFixture self;
// Calculate some marginals for DiscreteValues==all1
double joint_11_13 = 0, joint_11_13_14 = 0, joint_11_12_13_14 = 0,
joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
for (auto& x : self.assignments) {
double px = self.bayesTree->evaluate(x);
if (x[11] && x[13]) { if (x[11] && x[13]) {
joint_11_13 += px; joint_11_13 += px;
if (x[8] && x[12]) joint_8_11_12_13 += px; if (x[8] && x[12]) joint_8_11_12_13 += px;
@ -148,32 +161,12 @@ TEST(DiscreteBayesTree, ThinTree) {
} }
} }
} }
DiscreteValues all1 = allPosbValues.back(); DiscreteValues all1 = self.assignments.back();
// check separator marginal P(S0) auto R = self.bayesTree->roots().front();
auto clique = (*self.bayesTree)[0];
DiscreteFactorGraph separatorMarginal0 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
// check separator marginal P(S9), should be P(14)
clique = (*self.bayesTree)[9];
DiscreteFactorGraph separatorMarginal9 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
// check separator marginal of root, should be empty
clique = (*self.bayesTree)[11];
DiscreteFactorGraph separatorMarginal11 =
clique->separatorMarginal(EliminateDiscrete);
LONGS_EQUAL(0, separatorMarginal11.size());
// check shortcut P(S9||R) to root // check shortcut P(S9||R) to root
clique = (*self.bayesTree)[9]; auto clique = (*self.bayesTree)[9];
DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete); DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete);
LONGS_EQUAL(1, shortcut.size()); LONGS_EQUAL(1, shortcut.size());
DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9); DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
@ -202,15 +195,67 @@ TEST(DiscreteBayesTree, ThinTree) {
shortcut.print("shortcut:"); shortcut.print("shortcut:");
} }
} }
}
/* ************************************************************************* */
// Check all marginals
TEST(DiscreteBayesTree, MarginalFactors) {
TestFixture self;
Vector marginals = Vector::Zero(15);
for (size_t i = 0; i < self.assignments.size(); ++i) {
DiscreteValues& x = self.assignments[i];
double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
}
// Check all marginals // Check all marginals
DiscreteFactor::shared_ptr marginalFactor; DiscreteValues all1 = self.assignments.back();
for (size_t i = 0; i < 15; i++) { for (size_t i = 0; i < 15; i++) {
marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete); auto marginalFactor = self.bayesTree->marginalFactor(i, EliminateDiscrete);
double actual = (*marginalFactor)(all1); double actual = (*marginalFactor)(all1);
DOUBLES_EQUAL(marginals[i], actual, 1e-9); DOUBLES_EQUAL(marginals[i], actual, 1e-9);
} }
}
/* ************************************************************************* */
// Check a number of joint marginals.
TEST(DiscreteBayesTree, Joints) {
TestFixture self;
// Calculate some marginals for DiscreteValues==all1
Vector marginals = Vector::Zero(15);
double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint82 = 0,
joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0, joint_4_11 = 0;
for (size_t i = 0; i < self.assignments.size(); ++i) {
DiscreteValues& x = self.assignments[i];
double px = self.bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
if (x[12] && x[14]) {
joint_12_14 += px;
if (x[9]) joint_9_12_14 += px;
if (x[8]) joint_8_12_14 += px;
}
if (x[2]) {
if (x[8]) joint82 += px;
if (x[1]) joint12 += px;
}
if (x[4]) {
if (x[2]) joint24 += px;
if (x[5]) joint45 += px;
if (x[6]) joint46 += px;
if (x[11]) joint_4_11 += px;
}
}
// regression tests:
DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
DiscreteValues all1 = self.assignments.back();
DiscreteBayesNet::shared_ptr actualJoint; DiscreteBayesNet::shared_ptr actualJoint;
// Check joint P(8, 2) // Check joint P(8, 2)
@ -240,8 +285,8 @@ TEST(DiscreteBayesTree, ThinTree) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(DiscreteBayesTree, Dot) { TEST(DiscreteBayesTree, Dot) {
const TestFixture self; TestFixture self;
string actual = self.bayesTree->dot(); std::string actual = self.bayesTree->dot();
EXPECT(actual == EXPECT(actual ==
"digraph G{\n" "digraph G{\n"
"0[label=\"13, 11, 6, 7\"];\n" "0[label=\"13, 11, 6, 7\"];\n"
@ -268,6 +313,62 @@ TEST(DiscreteBayesTree, Dot) {
"}"); "}");
} }
/* ************************************************************************* */
// Check that we can have a multi-frontal lookup table
TEST(DiscreteBayesTree, Lookup) {
using gtsam::symbol_shorthand::A;
using gtsam::symbol_shorthand::X;
// Make a small planning-like graph: 3 states, 2 actions
DiscreteFactorGraph graph;
const DiscreteKey x1{X(1), 3}, x2{X(2), 3}, x3{X(3), 3};
const DiscreteKey a1{A(1), 2}, a2{A(2), 2};
// Constraint on start and goal
graph.add(DiscreteKeys{x1}, std::vector<double>{1, 0, 0});
graph.add(DiscreteKeys{x3}, std::vector<double>{0, 0, 1});
// Should I stay or should I go?
// "Reward" (exp(-cost)) for an action is 10, and rewards multiply:
const double r = 10;
std::vector<double> table{
r, 0, 0, 0, r, 0, // x1 = 0
0, r, 0, 0, 0, r, // x1 = 1
0, 0, r, 0, 0, r // x1 = 2
};
graph.add(DiscreteKeys{x1, a1, x2}, table);
graph.add(DiscreteKeys{x2, a2, x3}, table);
// eliminate for MPE (maximum probable explanation).
Ordering ordering{A(2), X(3), X(1), A(1), X(2)};
auto lookup = graph.eliminateMultifrontal(ordering, EliminateForMPE);
// Check that the lookup table is correct
EXPECT_LONGS_EQUAL(2, lookup->size());
auto lookup_x1_a1_x2 = (*lookup)[X(1)]->conditional();
EXPECT_LONGS_EQUAL(3, lookup_x1_a1_x2->frontals().size());
// check that sum is 100
DiscreteValues empty;
EXPECT_DOUBLES_EQUAL(100, (*lookup_x1_a1_x2->sum(3))(empty), 1e-9);
// And that only non-zero reward is for x1 a1 x2 == 0 1 1
EXPECT_DOUBLES_EQUAL(100, (*lookup_x1_a1_x2)({{X(1),0},{A(1),1},{X(2),1}}), 1e-9);
auto lookup_a2_x3 = (*lookup)[X(3)]->conditional();
// check that the sum depends on x2 and is non-zero only for x2 \in {1,2}
auto sum_x2 = lookup_a2_x3->sum(2);
EXPECT_DOUBLES_EQUAL(0, (*sum_x2)({{X(2),0}}), 1e-9);
EXPECT_DOUBLES_EQUAL(10, (*sum_x2)({{X(2),1}}), 1e-9);
EXPECT_DOUBLES_EQUAL(20, (*sum_x2)({{X(2),2}}), 1e-9);
EXPECT_LONGS_EQUAL(2, lookup_a2_x3->frontals().size());
// And that the non-zero rewards are for
// x2 a2 x3 == 1 1 2
EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),1},{A(2),1},{X(3),2}}), 1e-9);
// x2 a2 x3 == 2 0 2
EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),2},{A(2),0},{X(3),2}}), 1e-9);
// x2 a2 x3 == 2 1 2
EXPECT_DOUBLES_EQUAL(10, (*lookup_a2_x3)({{X(2),2},{A(2),1},{X(3),2}}), 1e-9);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

5
gtsam/hybrid/hybrid.i
View File

@ -35,14 +35,11 @@ class HybridValues {
}; };
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
virtual class HybridFactor { virtual class HybridFactor : gtsam::Factor {
void print(string s = "HybridFactor\n", void print(string s = "HybridFactor\n",
const gtsam::KeyFormatter& keyFormatter = const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const; bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const;
bool empty() const;
size_t size() const;
gtsam::KeyVector keys() const;
// Standard interface: // Standard interface:
double error(const gtsam::HybridValues &values) const; double error(const gtsam::HybridValues &values) const;

8
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -140,9 +140,15 @@ namespace gtsam {
/** Access the conditional */ /** Access the conditional */
const sharedConditional& conditional() const { return conditional_; } const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */ /// Return true if this clique is the root of a Bayes tree.
inline bool isRoot() const { return parent_.expired(); } inline bool isRoot() const { return parent_.expired(); }
/// Return the number of children.
size_t nrChildren() const { return children.size(); }
/// Return the child at index i.
const derived_ptr operator[](size_t i) const { return children[i]; }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */ /** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const; size_t treeSize() const;

4
gtsam/inference/ClusterTree.h
View File

@ -49,7 +49,7 @@ class ClusterTree {
virtual ~Cluster() {} virtual ~Cluster() {}
const Cluster& operator[](size_t i) const { const Cluster& operator[](size_t i) const {
return *(children[i]); return *(children.at(i));
} }
/// Construct from factors associated with a single key /// Construct from factors associated with a single key
@ -161,7 +161,7 @@ class ClusterTree {
} }
const Cluster& operator[](size_t i) const { const Cluster& operator[](size_t i) const {
return *(roots_[i]); return *(roots_.at(i));
} }
/// @} /// @}

10
gtsam/inference/EliminateableFactorGraph.h
View File

@ -52,12 +52,12 @@ namespace gtsam {
* algorithms. Any factor graph holding eliminateable factors can derive from this class to * algorithms. Any factor graph holding eliminateable factors can derive from this class to
* expose functions for computing marginals, conditional marginals, doing multifrontal and * expose functions for computing marginals, conditional marginals, doing multifrontal and
* sequential elimination, etc. */ * sequential elimination, etc. */
template<class FACTORGRAPH> template<class FACTOR_GRAPH>
class EliminateableFactorGraph class EliminateableFactorGraph
{ {
private: private:
typedef EliminateableFactorGraph<FACTORGRAPH> This; ///< Typedef to this class. typedef EliminateableFactorGraph<FACTOR_GRAPH> This; ///< Typedef to this class.
typedef FACTORGRAPH FactorGraphType; ///< Typedef to factor graph type typedef FACTOR_GRAPH FactorGraphType; ///< Typedef to factor graph type
// Base factor type stored in this graph (private because derived classes will get this from // Base factor type stored in this graph (private because derived classes will get this from
// their FactorGraph base class) // their FactorGraph base class)
typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType; typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
@ -139,7 +139,7 @@ namespace gtsam {
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not /** 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 * provided, the ordering will be computed using either COLAMD or METIS, depending on
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS) * the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
* *
* <b> Example - Full Cholesky elimination in COLAMD order: </b> * <b> Example - Full Cholesky elimination in COLAMD order: </b>
@ -160,7 +160,7 @@ namespace gtsam {
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not /** 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 * provided, the ordering will be computed using either COLAMD or METIS, depending on
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS) * the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
* *
* <b> Example - Full QR elimination in specified order: * <b> Example - Full QR elimination in specified order:

14
gtsam/inference/inference.i
View File

@ -104,6 +104,7 @@ class Ordering {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
Ordering(); Ordering();
Ordering(const gtsam::Ordering& other); Ordering(const gtsam::Ordering& other);
Ordering(const std::vector<size_t>& keys);
template < template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
@ -147,7 +148,7 @@ class Ordering {
// Standard interface // Standard interface
size_t size() const; size_t size() const;
size_t at(size_t key) const; size_t at(size_t i) const;
void push_back(size_t key); void push_back(size_t key);
// enabling serialization functionality // enabling serialization functionality
@ -197,4 +198,15 @@ class VariableIndex {
size_t nEntries() const; size_t nEntries() const;
}; };
#include <gtsam/inference/Factor.h>
virtual class Factor {
void print(string s = "Factor\n", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printKeys(string s = "") const;
bool equals(const gtsam::Factor& other, double tol = 1e-9) const;
bool empty() const;
size_t size() const;
gtsam::KeyVector keys() const;
};
} // namespace gtsam } // namespace gtsam

10
gtsam/linear/linear.i
View File

@ -261,8 +261,7 @@ class VectorValues {
}; };
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor { virtual class GaussianFactor : gtsam::Factor {
gtsam::KeyVector keys() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
@ -273,8 +272,6 @@ virtual class GaussianFactor {
Matrix information() const; Matrix information() const;
Matrix augmentedJacobian() const; Matrix augmentedJacobian() const;
pair<Matrix, Vector> jacobian() const; pair<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
}; };
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
@ -301,10 +298,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
//Testable //Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const;
gtsam::KeyVector& keys() const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
size_t size() const;
Vector unweighted_error(const gtsam::VectorValues& c) const; Vector unweighted_error(const gtsam::VectorValues& c) const;
Vector error_vector(const gtsam::VectorValues& c) const; Vector error_vector(const gtsam::VectorValues& c) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
@ -346,10 +340,8 @@ virtual class HessianFactor : gtsam::GaussianFactor {
HessianFactor(const gtsam::GaussianFactorGraph& factors); HessianFactor(const gtsam::GaussianFactorGraph& factors);
//Testable //Testable
size_t size() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;

5
gtsam/nonlinear/nonlinear.i
View File

@ -110,13 +110,10 @@ class NonlinearFactorGraph {
}; };
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
virtual class NonlinearFactor { virtual class NonlinearFactor : gtsam::Factor {
// Factor base class // Factor base class
size_t size() const;
gtsam::KeyVector keys() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const;
// NonlinearFactor // NonlinearFactor
bool equals(const gtsam::NonlinearFactor& other, double tol) const; bool equals(const gtsam::NonlinearFactor& other, double tol) const;
double error(const gtsam::Values& c) const; double error(const gtsam::Values& c) const;

2
gtsam/symbolic/SymbolicJunctionTree.h
View File

@ -65,4 +65,6 @@ namespace gtsam {
SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree); SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree);
}; };
/// typedef for wrapper:
using SymbolicCluster = SymbolicJunctionTree::Cluster;
} }

91
gtsam/symbolic/symbolic.i
View File

@ -4,7 +4,7 @@
namespace gtsam { namespace gtsam {
#include <gtsam/symbolic/SymbolicFactor.h> #include <gtsam/symbolic/SymbolicFactor.h>
virtual class SymbolicFactor { virtual class SymbolicFactor : gtsam::Factor {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
SymbolicFactor(const gtsam::SymbolicFactor& f); SymbolicFactor(const gtsam::SymbolicFactor& f);
SymbolicFactor(); SymbolicFactor();
@ -18,12 +18,10 @@ virtual class SymbolicFactor {
static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js);
// From Factor // From Factor
size_t size() const;
void print(string s = "SymbolicFactor", void print(string s = "SymbolicFactor",
const gtsam::KeyFormatter& keyFormatter = const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::SymbolicFactor& other, double tol) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const;
gtsam::KeyVector keys();
}; };
#include <gtsam/symbolic/SymbolicFactorGraph.h> #include <gtsam/symbolic/SymbolicFactorGraph.h>
@ -139,7 +137,60 @@ class SymbolicBayesNet {
const gtsam::DotWriter& writer = gtsam::DotWriter()) const; const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
}; };
#include <gtsam/symbolic/SymbolicEliminationTree.h>
class SymbolicEliminationTree {
SymbolicEliminationTree(const gtsam::SymbolicFactorGraph& factorGraph,
const gtsam::VariableIndex& structure,
const gtsam::Ordering& order);
SymbolicEliminationTree(const gtsam::SymbolicFactorGraph& factorGraph,
const gtsam::Ordering& order);
void print(
string name = "EliminationTree: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::SymbolicEliminationTree& other,
double tol = 1e-9) const;
};
#include <gtsam/symbolic/SymbolicJunctionTree.h>
class SymbolicCluster {
gtsam::Ordering orderedFrontalKeys;
gtsam::SymbolicFactorGraph factors;
const gtsam::SymbolicCluster& operator[](size_t i) const;
size_t nrChildren() const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
};
class SymbolicJunctionTree {
SymbolicJunctionTree(const gtsam::SymbolicEliminationTree& eliminationTree);
void print(
string name = "JunctionTree: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
size_t nrRoots() const;
const gtsam::SymbolicCluster& operator[](size_t i) const;
};
#include <gtsam/symbolic/SymbolicBayesTree.h> #include <gtsam/symbolic/SymbolicBayesTree.h>
class SymbolicBayesTreeClique {
SymbolicBayesTreeClique();
SymbolicBayesTreeClique(const gtsam::SymbolicConditional* conditional);
bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter);
const gtsam::SymbolicConditional* conditional() const;
bool isRoot() const;
gtsam::SymbolicBayesTreeClique* parent() const;
size_t treeSize() const;
size_t numCachedSeparatorMarginals() const;
void deleteCachedShortcuts();
};
class SymbolicBayesTree { class SymbolicBayesTree {
// Constructors // Constructors
SymbolicBayesTree(); SymbolicBayesTree();
@ -151,9 +202,14 @@ class SymbolicBayesTree {
bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
// Standard Interface // Standard Interface
// size_t findParentClique(const gtsam::IndexVector& parents) const; bool empty() const;
size_t size(); size_t size() const;
void saveGraph(string s) const;
const gtsam::SymbolicBayesTreeClique* operator[](size_t j) const;
void saveGraph(string s,
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void clear(); void clear();
void deleteCachedShortcuts(); void deleteCachedShortcuts();
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
@ -161,28 +217,9 @@ class SymbolicBayesTree {
gtsam::SymbolicConditional* marginalFactor(size_t key) const; gtsam::SymbolicConditional* marginalFactor(size_t key) const;
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
};
class SymbolicBayesTreeClique { string dot(const gtsam::KeyFormatter& keyFormatter =
SymbolicBayesTreeClique(); gtsam::DefaultKeyFormatter) const;
// SymbolicBayesTreeClique(gtsam::sharedConditional* conditional);
bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
size_t numCachedSeparatorMarginals() const;
// gtsam::sharedConditional* conditional() const;
bool isRoot() const;
size_t treeSize() const;
gtsam::SymbolicBayesTreeClique* parent() const;
// // TODO: need wrapped versions graphs, BayesNet
// BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function)
// const; FactorGraph<FactorType> marginal(derived_ptr root, Eliminate
// function) const; FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr
// root, Eliminate function) const;
//
void deleteCachedShortcuts();
}; };
} // namespace gtsam } // namespace gtsam

6
python/gtsam/tests/test_DecisionTreeFactor.py
View File

@ -25,7 +25,13 @@ class TestDecisionTreeFactor(GtsamTestCase):
self.B = (5, 2) self.B = (5, 2)
self.factor = DecisionTreeFactor([self.A, self.B], "1 2 3 4 5 6") self.factor = DecisionTreeFactor([self.A, self.B], "1 2 3 4 5 6")
def test_from_floats(self):
"""Test whether we can construct a factor from floats."""
actual = DecisionTreeFactor([self.A, self.B], [1., 2., 3., 4., 5., 6.])
self.gtsamAssertEquals(actual, self.factor)
def test_enumerate(self): def test_enumerate(self):
"""Test whether we can enumerate the factor."""
actual = self.factor.enumerate() actual = self.factor.enumerate()
_, values = zip(*actual) _, values = zip(*actual)
self.assertEqual(list(values), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]) self.assertEqual(list(values), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0])

107
python/gtsam/tests/test_DiscreteBayesTree.py
View File

@ -13,10 +13,15 @@ Author: Frank Dellaert
import unittest import unittest
from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique, import numpy as np
DiscreteConditional, DiscreteFactorGraph, Ordering) from gtsam.symbol_shorthand import A, X
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique,
DiscreteConditional, DiscreteFactorGraph,
DiscreteValues, Ordering)
class TestDiscreteBayesNet(GtsamTestCase): class TestDiscreteBayesNet(GtsamTestCase):
"""Tests for Discrete Bayes Nets.""" """Tests for Discrete Bayes Nets."""
@ -27,7 +32,7 @@ class TestDiscreteBayesNet(GtsamTestCase):
# Define DiscreteKey pairs. # Define DiscreteKey pairs.
keys = [(j, 2) for j in range(15)] keys = [(j, 2) for j in range(15)]
# Create thin-tree Bayesnet. # Create thin-tree Bayes net.
bayesNet = DiscreteBayesNet() bayesNet = DiscreteBayesNet()
bayesNet.add(keys[0], [keys[8], keys[12]], "2/3 1/4 3/2 4/1") bayesNet.add(keys[0], [keys[8], keys[12]], "2/3 1/4 3/2 4/1")
@ -65,15 +70,105 @@ class TestDiscreteBayesNet(GtsamTestCase):
# bayesTree[key].printSignature() # bayesTree[key].printSignature()
# bayesTree.saveGraph("test_DiscreteBayesTree.dot") # bayesTree.saveGraph("test_DiscreteBayesTree.dot")
self.assertFalse(bayesTree.empty())
self.assertEqual(12, bayesTree.size())
# The root is P( 8 12 14), we can retrieve it by key: # The root is P( 8 12 14), we can retrieve it by key:
root = bayesTree[8] root = bayesTree[8]
self.assertIsInstance(root, DiscreteBayesTreeClique) self.assertIsInstance(root, DiscreteBayesTreeClique)
self.assertTrue(root.isRoot()) self.assertTrue(root.isRoot())
self.assertIsInstance(root.conditional(), DiscreteConditional) self.assertIsInstance(root.conditional(), DiscreteConditional)
# Test all methods in DiscreteBayesTree
self.gtsamAssertEquals(bayesTree, bayesTree)
# Check value at 0
zero_values = DiscreteValues()
for j in range(15):
zero_values[j] = 0
value_at_zeros = bayesTree.evaluate(zero_values)
self.assertAlmostEqual(value_at_zeros, 0.0)
# Check value at max
values_star = factorGraph.optimize()
max_value = bayesTree.evaluate(values_star)
self.assertAlmostEqual(max_value, 0.002548)
# Check operator sugar
max_value = bayesTree(values_star)
self.assertAlmostEqual(max_value, 0.002548)
self.assertFalse(bayesTree.empty())
self.assertEqual(12, bayesTree.size())
@unittest.skip("TODO: segfaults on gcc 7 and gcc 9")
def test_discrete_bayes_tree_lookup(self):
"""Check that we can have a multi-frontal lookup table."""
# Make a small planning-like graph: 3 states, 2 actions
graph = DiscreteFactorGraph()
x1, x2, x3 = (X(1), 3), (X(2), 3), (X(3), 3)
a1, a2 = (A(1), 2), (A(2), 2)
# Constraint on start and goal
graph.add([x1], np.array([1, 0, 0]))
graph.add([x3], np.array([0, 0, 1]))
# Should I stay or should I go?
# "Reward" (exp(-cost)) for an action is 10, and rewards multiply:
r = 10
table = np.array([
r, 0, 0, 0, r, 0, # x1 = 0
0, r, 0, 0, 0, r, # x1 = 1
0, 0, r, 0, 0, r # x1 = 2
])
graph.add([x1, a1, x2], table)
graph.add([x2, a2, x3], table)
# print(graph) will give:
# size: 4
# factor 0: f[ (x1,3), ] ...
# factor 1: f[ (x3,3), ] ...
# factor 2: f[ (x1,3), (a1,2), (x2,3), ] ...
# factor 3: f[ (x2,3), (a2,2), (x3,3), ] ...
# Eliminate for MPE (maximum probable explanation).
ordering = Ordering(keys=[A(2), X(3), X(1), A(1), X(2)])
lookup = graph.eliminateMultifrontal(ordering, gtsam.EliminateForMPE)
# print(lookup) will give:
# DiscreteBayesTree
# : cliques: 2, variables: 5
# - g( x1 a1 x2 ): ...
# | - g( a2 x3 ; x2 ): ...
# Check that the lookup table is correct
assert lookup.size() == 2
lookup_x1_a1_x2 = lookup[X(1)].conditional()
assert lookup_x1_a1_x2.nrFrontals() == 3
# Check that sum is 100
empty = gtsam.DiscreteValues()
self.assertAlmostEqual(lookup_x1_a1_x2.sum(3)(empty), 100)
# And that only non-zero reward is for x1 a1 x2 == 0 1 1
values = DiscreteValues()
values[X(1)] = 0
values[A(1)] = 1
values[X(2)] = 1
self.assertAlmostEqual(lookup_x1_a1_x2(values), 100)
lookup_a2_x3 = lookup[X(3)].conditional()
# Check that the sum depends on x2 and is non-zero only for x2 in {1, 2}
sum_x2 = lookup_a2_x3.sum(2)
values = DiscreteValues()
values[X(2)] = 0
self.assertAlmostEqual(sum_x2(values), 0)
values[X(2)] = 1
self.assertAlmostEqual(sum_x2(values), 10)
values[X(2)] = 2
self.assertAlmostEqual(sum_x2(values), 20)
assert lookup_a2_x3.nrFrontals() == 2
# And that the non-zero rewards are for x2 a2 x3 == 1 1 2
values = DiscreteValues()
values[X(2)] = 1
values[A(2)] = 1
values[X(3)] = 2
self.assertAlmostEqual(lookup_a2_x3(values), 10)
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()