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

Merge pull request #1037 from borglab/feature/discrete_operators

release/4.3a0
Frank Dellaert 2022-01-16 16:43:04 -05:00 committed by GitHub
parent 1000825b03 0b11b12760
commit f3d9486253
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 420 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/discrete/DecisionTreeFactor.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
/** Default constructor for I/O */
DecisionTreeFactor();
/** Constructor from Indices, Ordering, and AlgebraicDecisionDiagram */
/** Constructor from DiscreteKeys and AlgebraicDecisionTree */
DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
/** Constructor from doubles */
@ -139,14 +139,14 @@ namespace gtsam {
/**
* Apply binary operator (*this) "op" f
* @param f the second argument for op
* @param op a binary operator that operates on AlgebraicDecisionDiagram potentials
* @param op a binary operator that operates on AlgebraicDecisionTree
*/
DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const;
/**
* Combine frontal variables using binary operator "op"
* @param nrFrontals nr. of frontal to combine variables in this factor
* @param op a binary operator that operates on AlgebraicDecisionDiagram potentials
* @param op a binary operator that operates on AlgebraicDecisionTree
* @return shared pointer to newly created DecisionTreeFactor
*/
shared_ptr combine(size_t nrFrontals, ADT::Binary op) const;
@ -154,7 +154,7 @@ namespace gtsam {
/**
* Combine frontal variables in an Ordering using binary operator "op"
* @param nrFrontals nr. of frontal to combine variables in this factor
* @param op a binary operator that operates on AlgebraicDecisionDiagram potentials
* @param op a binary operator that operates on AlgebraicDecisionTree
* @return shared pointer to newly created DecisionTreeFactor
*/
shared_ptr combine(const Ordering& keys, ADT::Binary op) const;

100
gtsam/discrete/DiscreteConditional.cpp
View File

@ -30,6 +30,7 @@
#include <string>
#include <vector>
#include <utility>
#include <set>
using namespace std;
using std::stringstream;
@ -38,38 +39,97 @@ using std::pair;
namespace gtsam {
// Instantiate base class
template class GTSAM_EXPORT Conditional<DecisionTreeFactor, DiscreteConditional> ;
template class GTSAM_EXPORT
Conditional<DecisionTreeFactor, DiscreteConditional>;
/* ******************************************************************************** */
/* ************************************************************************** */
DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
const DecisionTreeFactor& f) :
BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {
}
const DecisionTreeFactor& f)
: BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {}
/* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal) :
BaseFactor(
ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional(
joint.size()-marginal.size()) {
if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
cout << (firstFrontalKey()) << endl; //TODO Print all keys
}
/* ************************************************************************** */
DiscreteConditional::DiscreteConditional(size_t nrFrontals,
const DiscreteKeys& keys,
const ADT& potentials)
: BaseFactor(keys, potentials), BaseConditional(nrFrontals) {}
/* ******************************************************************************** */
/* ************************************************************************** */
DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal, const Ordering& orderedKeys) :
DiscreteConditional(joint, marginal) {
const DecisionTreeFactor& marginal)
: BaseFactor(joint / marginal),
BaseConditional(joint.size() - marginal.size()) {}
/* ************************************************************************** */
DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal,
const Ordering& orderedKeys)
: DiscreteConditional(joint, marginal) {
keys_.clear();
keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
}
/* ******************************************************************************** */
/* ************************************************************************** */
DiscreteConditional::DiscreteConditional(const Signature& signature)
: BaseFactor(signature.discreteKeys(), signature.cpt()),
BaseConditional(1) {}
/* ******************************************************************************** */
/* ************************************************************************** */
DiscreteConditional DiscreteConditional::operator*(
const DiscreteConditional& other) const {
// Take union of frontal keys
std::set<Key> newFrontals;
for (auto&& key : this->frontals()) newFrontals.insert(key);
for (auto&& key : other.frontals()) newFrontals.insert(key);
// Check if frontals overlapped
if (nrFrontals() + other.nrFrontals() > newFrontals.size())
throw std::invalid_argument(
"DiscreteConditional::operator* called with overlapping frontal keys.");
// Now, add cardinalities.
DiscreteKeys discreteKeys;
for (auto&& key : frontals())
discreteKeys.emplace_back(key, cardinality(key));
for (auto&& key : other.frontals())
discreteKeys.emplace_back(key, other.cardinality(key));
// Sort
std::sort(discreteKeys.begin(), discreteKeys.end());
// Add parents to set, to make them unique
std::set<DiscreteKey> parents;
for (auto&& key : this->parents())
if (!newFrontals.count(key)) parents.emplace(key, cardinality(key));
for (auto&& key : other.parents())
if (!newFrontals.count(key)) parents.emplace(key, other.cardinality(key));
// Finally, add parents to keys, in order
for (auto&& dk : parents) discreteKeys.push_back(dk);
ADT product = ADT::apply(other, ADT::Ring::mul);
return DiscreteConditional(newFrontals.size(), discreteKeys, product);
}
/* ************************************************************************** */
DiscreteConditional DiscreteConditional::marginal(Key key) const {
if (nrParents() > 0)
throw std::invalid_argument(
"DiscreteConditional::marginal: single argument version only valid for "
"fully specified joint distributions (i.e., no parents).");
// Calculate the keys as the frontal keys without the given key.
DiscreteKeys discreteKeys{{key, cardinality(key)}};
// Calculate sum
ADT adt(*this);
for (auto&& k : frontals())
if (k != key) adt = adt.sum(k, cardinality(k));
// Return new factor
return DiscreteConditional(1, discreteKeys, adt);
}
/* ************************************************************************** */
void DiscreteConditional::print(const string& s,
const KeyFormatter& formatter) const {
cout << s << " P( ";
@ -82,7 +142,7 @@ void DiscreteConditional::print(const string& s,
cout << formatter(*it) << " ";
}
}
cout << ")";
cout << "):\n";
ADT::print("");
cout << endl;
}

77
gtsam/discrete/DiscreteConditional.h
View File

@ -49,14 +49,21 @@ class GTSAM_EXPORT DiscreteConditional
/// @name Standard Constructors
/// @{
/** default constructor needed for serialization */
/// Default constructor needed for serialization.
DiscreteConditional() {}
/** constructor from factor */
/// Construct from factor, taking the first `nFrontals` keys as frontals.
DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
/**
* Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first
* `nFrontals` keys as frontals, in the order given.
*/
DiscreteConditional(size_t nFrontals, const DiscreteKeys& keys,
const ADT& potentials);
/** Construct from signature */
DiscreteConditional(const Signature& signature);
explicit DiscreteConditional(const Signature& signature);
/**
* Construct from key, parents, and a Signature::Table specifying the
@ -86,27 +93,41 @@ class GTSAM_EXPORT DiscreteConditional
DiscreteConditional(const DiscreteKey& key, const std::string& spec)
: DiscreteConditional(Signature(key, {}, spec)) {}
/** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
/**
* @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
* Assumes but *does not check* that f(Y)=sum_X f(X,Y).
*/
DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal);
/** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
/**
* @brief construct P(X|Y) = f(X,Y)/f(Y) from f(X,Y) and f(Y)
* Assumes but *does not check* that f(Y)=sum_X f(X,Y).
* Makes sure the keys are ordered as given. Does not check orderedKeys.
*/
DiscreteConditional(const DecisionTreeFactor& joint,
const DecisionTreeFactor& marginal,
const Ordering& orderedKeys);
/**
* Combine several conditional into a single one.
* The conditionals must be given in increasing order, meaning that the
* parents of any conditional may not include a conditional coming before it.
* @param firstConditional Iterator to the first conditional to combine, must
* dereference to a shared_ptr<DiscreteConditional>.
* @param lastConditional Iterator to after the last conditional to combine,
* must dereference to a shared_ptr<DiscreteConditional>.
* */
template <typename ITERATOR>
static shared_ptr Combine(ITERATOR firstConditional,
ITERATOR lastConditional);
* @brief Combine two conditionals, yielding a new conditional with the union
* of the frontal keys, ordered by gtsam::Key.
*
* The two conditionals must make a valid Bayes net fragment, i.e.,
* the frontal variables cannot overlap, and must be acyclic:
* Example of correct use:
* P(A,B) = P(A|B) * P(B)
* P(A,B|C) = P(A|B) * P(B|C)
* P(A,B,C) = P(A,B|C) * P(C)
* Example of incorrect use:
* P(A|B) * P(A|C) = ?
* P(A|B) * P(B|A) = ?
* We check for overlapping frontals, but do *not* check for cyclic.
*/
DiscreteConditional operator*(const DiscreteConditional& other) const;
/** Calculate marginal on given key, no parent case. */
DiscreteConditional marginal(Key key) const;
/// @}
/// @name Testable
@ -136,11 +157,6 @@ class GTSAM_EXPORT DiscreteConditional
return ADT::operator()(values);
}
/** Convert to a factor */
DecisionTreeFactor::shared_ptr toFactor() const {
return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this));
}
/** Restrict to given parent values, returns DecisionTreeFactor */
DecisionTreeFactor::shared_ptr choose(
const DiscreteValues& parentsValues) const;
@ -208,23 +224,4 @@ class GTSAM_EXPORT DiscreteConditional
template <>
struct traits<DiscreteConditional> : public Testable<DiscreteConditional> {};
/* ************************************************************************* */
template <typename ITERATOR>
DiscreteConditional::shared_ptr DiscreteConditional::Combine(
ITERATOR firstConditional, ITERATOR lastConditional) {
// TODO: check for being a clique
// multiply all the potentials of the given conditionals
size_t nrFrontals = 0;
DecisionTreeFactor product;
for (ITERATOR it = firstConditional; it != lastConditional;
++it, ++nrFrontals) {
DiscreteConditional::shared_ptr c = *it;
DecisionTreeFactor::shared_ptr factor = c->toFactor();
product = (*factor) * product;
}
// and then create a new multi-frontal conditional
return boost::make_shared<DiscreteConditional>(nrFrontals, product);
}
} // namespace gtsam

14
gtsam/discrete/DiscretePrior.h
View File

@ -48,17 +48,17 @@ class GTSAM_EXPORT DiscretePrior : public DiscreteConditional {
DiscretePrior(const Signature& s) : Base(s) {}
/**
* Construct from key and a Signature::Table specifying the
* conditional probability table (CPT).
* Construct from key and a vector of floats specifying the probability mass
* function (PMF).
*
* Example: DiscretePrior P(D, table);
* Example: DiscretePrior P(D, {0.4, 0.6});
*/
DiscretePrior(const DiscreteKey& key, const Signature::Table& table)
: Base(Signature(key, {}, table)) {}
DiscretePrior(const DiscreteKey& key, const std::vector<double>& spec)
: DiscretePrior(Signature(key, {}, Signature::Table{spec})) {}
/**
* Construct from key and a string specifying the conditional
* probability table (CPT).
* Construct from key and a string specifying the probability mass function
* (PMF).
*
* Example: DiscretePrior P(D, "9/1 2/8 3/7 1/9");
*/

16
gtsam/discrete/discrete.i
View File

@ -58,6 +58,15 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const;
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const;
size_t cardinality(gtsam::Key j) const;
gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const;
gtsam::DecisionTreeFactor* sum(size_t nrFrontals) const;
gtsam::DecisionTreeFactor* sum(const gtsam::Ordering& keys) const;
gtsam::DecisionTreeFactor* max(size_t nrFrontals) const;
string dot(
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
bool showZero = true) const;
@ -86,14 +95,18 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
DiscreteConditional(const gtsam::DecisionTreeFactor& joint,
const gtsam::DecisionTreeFactor& marginal,
const gtsam::Ordering& orderedKeys);
gtsam::DiscreteConditional operator*(
const gtsam::DiscreteConditional& other) const;
DiscreteConditional marginal(gtsam::Key key) const;
void print(string s = "Discrete Conditional\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteConditional& other, double tol = 1e-9) const;
size_t nrFrontals() const;
size_t nrParents() const;
void printSignature(
string s = "Discrete Conditional: ",
const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const;
gtsam::DecisionTreeFactor* toFactor() const;
gtsam::DecisionTreeFactor* choose(
const gtsam::DiscreteValues& parentsValues) const;
gtsam::DecisionTreeFactor* likelihood(
@ -120,6 +133,7 @@ virtual class DiscretePrior : gtsam::DiscreteConditional {
DiscretePrior();
DiscretePrior(const gtsam::DecisionTreeFactor& f);
DiscretePrior(const gtsam::DiscreteKey& key, string spec);
DiscretePrior(const gtsam::DiscreteKey& key, std::vector<double> spec);
void print(string s = "Discrete Prior\n",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;

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

@ -17,10 +17,12 @@
* @author Duy-Nguyen Ta
*/
#include <gtsam/discrete/Signature.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscretePrior.h>
#include <gtsam/discrete/Signature.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
@ -51,17 +53,21 @@ TEST( DecisionTreeFactor, constructors)
}
/* ************************************************************************* */
TEST_UNSAFE( DecisionTreeFactor, multiplication)
{
DiscreteKey v0(0,2), v1(1,2), v2(2,2);
TEST(DecisionTreeFactor, multiplication) {
DiscreteKey v0(0, 2), v1(1, 2), v2(2, 2);
// Multiply with a DiscretePrior, i.e., Bayes Law!
DiscretePrior prior(v1 % "1/3");
DecisionTreeFactor f1(v0 & v1, "1 2 3 4");
DecisionTreeFactor expected(v0 & v1, "0.25 1.5 0.75 3");
CHECK(assert_equal(expected, static_cast<DecisionTreeFactor>(prior) * f1));
CHECK(assert_equal(expected, f1 * prior));
// Multiply two factors
DecisionTreeFactor f2(v1 & v2, "5 6 7 8");
DecisionTreeFactor expected(v0 & v1 & v2, "5 6 14 16 15 18 28 32");
DecisionTreeFactor actual = f1 * f2;
CHECK(assert_equal(expected, actual));
DecisionTreeFactor expected2(v0 & v1 & v2, "5 6 14 16 15 18 28 32");
CHECK(assert_equal(expected2, actual));
}
/* ************************************************************************* */

158
gtsam/discrete/tests/testDiscreteConditional.cpp
View File

@ -34,20 +34,21 @@ using namespace gtsam;
TEST(DiscreteConditional, constructors) {
DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !
DiscreteConditional expected(X | Y = "1/1 2/3 1/4");
EXPECT_LONGS_EQUAL(0, *(expected.beginFrontals()));
EXPECT_LONGS_EQUAL(2, *(expected.beginParents()));
EXPECT(expected.endParents() == expected.end());
EXPECT(expected.endFrontals() == expected.beginParents());
DiscreteConditional actual(X | Y = "1/1 2/3 1/4");
EXPECT_LONGS_EQUAL(0, *(actual.beginFrontals()));
EXPECT_LONGS_EQUAL(2, *(actual.beginParents()));
EXPECT(actual.endParents() == actual.end());
EXPECT(actual.endFrontals() == actual.beginParents());
DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
DiscreteConditional actual1(1, f1);
EXPECT(assert_equal(expected, actual1, 1e-9));
DiscreteConditional expected1(1, f1);
EXPECT(assert_equal(expected1, actual, 1e-9));
DecisionTreeFactor f2(
X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DiscreteConditional actual2(1, f2);
EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
DecisionTreeFactor expected2 = f2 / *f2.sum(1);
EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
}
/* ************************************************************************* */
@ -61,6 +62,7 @@ TEST(DiscreteConditional, constructors_alt_interface) {
r3 += 1.0, 4.0;
table += r1, r2, r3;
DiscreteConditional actual1(X, {Y}, table);
DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
DiscreteConditional expected1(1, f1);
EXPECT(assert_equal(expected1, actual1, 1e-9));
@ -68,41 +70,141 @@ TEST(DiscreteConditional, constructors_alt_interface) {
DecisionTreeFactor f2(
X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DiscreteConditional actual2(1, f2);
EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
DecisionTreeFactor expected2 = f2 / *f2.sum(1);
EXPECT(assert_equal(expected2, static_cast<DecisionTreeFactor>(actual2)));
}
/* ************************************************************************* */
TEST(DiscreteConditional, constructors2) {
// Declare keys and ordering
DiscreteKey C(0, 2), B(1, 2);
DecisionTreeFactor actual(C & B, "0.8 0.75 0.2 0.25");
Signature signature((C | B) = "4/1 3/1");
DiscreteConditional expected(signature);
DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
EXPECT(assert_equal(*expectedFactor, actual));
DiscreteConditional actual(signature);
DecisionTreeFactor expected(C & B, "0.8 0.75 0.2 0.25");
EXPECT(assert_equal(expected, static_cast<DecisionTreeFactor>(actual)));
}
/* ************************************************************************* */
TEST(DiscreteConditional, constructors3) {
// Declare keys and ordering
DiscreteKey C(0, 2), B(1, 2), A(2, 2);
DecisionTreeFactor actual(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
Signature signature((C | B, A) = "4/1 1/1 1/1 1/4");
DiscreteConditional expected(signature);
DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
EXPECT(assert_equal(*expectedFactor, actual));
DiscreteConditional actual(signature);
DecisionTreeFactor expected(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
EXPECT(assert_equal(expected, static_cast<DecisionTreeFactor>(actual)));
}
/* ************************************************************************* */
TEST(DiscreteConditional, Combine) {
DiscreteKey A(0, 2), B(1, 2);
vector<DiscreteConditional::shared_ptr> c;
c.push_back(boost::make_shared<DiscreteConditional>(A | B = "1/2 2/1"));
c.push_back(boost::make_shared<DiscreteConditional>(B % "1/2"));
DecisionTreeFactor factor(A & B, "0.111111 0.444444 0.222222 0.222222");
DiscreteConditional expected(2, factor);
auto actual = DiscreteConditional::Combine(c.begin(), c.end());
EXPECT(assert_equal(expected, *actual, 1e-5));
// Check calculation of joint P(A,B)
TEST(DiscreteConditional, Multiply) {
DiscreteKey A(1, 2), B(0, 2);
DiscreteConditional conditional(A | B = "1/2 2/1");
DiscreteConditional prior(B % "1/2");
// The expected factor
DecisionTreeFactor f(A & B, "1 4 2 2");
DiscreteConditional expected(2, f);
// P(A,B) = P(A|B) * P(B) = P(B) * P(A|B)
for (auto&& actual : {prior * conditional, conditional * prior}) {
EXPECT_LONGS_EQUAL(2, actual.nrFrontals());
KeyVector frontals(actual.beginFrontals(), actual.endFrontals());
EXPECT((frontals == KeyVector{0, 1}));
for (auto&& it : actual.enumerate()) {
const DiscreteValues& v = it.first;
EXPECT_DOUBLES_EQUAL(actual(v), conditional(v) * prior(v), 1e-9);
}
// And for good measure:
EXPECT(assert_equal(expected, actual));
}
}
/* ************************************************************************* */
// Check calculation of conditional joint P(A,B|C)
TEST(DiscreteConditional, Multiply2) {
DiscreteKey A(0, 2), B(1, 2), C(2, 2);
DiscreteConditional A_given_B(A | B = "1/3 3/1");
DiscreteConditional B_given_C(B | C = "1/3 3/1");
// P(A,B|C) = P(A|B)P(B|C) = P(B|C)P(A|B)
for (auto&& actual : {A_given_B * B_given_C, B_given_C * A_given_B}) {
EXPECT_LONGS_EQUAL(2, actual.nrFrontals());
EXPECT_LONGS_EQUAL(1, actual.nrParents());
KeyVector frontals(actual.beginFrontals(), actual.endFrontals());
EXPECT((frontals == KeyVector{0, 1}));
for (auto&& it : actual.enumerate()) {
const DiscreteValues& v = it.first;
EXPECT_DOUBLES_EQUAL(actual(v), A_given_B(v) * B_given_C(v), 1e-9);
}
}
}
/* ************************************************************************* */
// Check calculation of conditional joint P(A,B|C), double check keys
TEST(DiscreteConditional, Multiply3) {
DiscreteKey A(1, 2), B(2, 2), C(0, 2); // different keys!!!
DiscreteConditional A_given_B(A | B = "1/3 3/1");
DiscreteConditional B_given_C(B | C = "1/3 3/1");
// P(A,B|C) = P(A|B)P(B|C) = P(B|C)P(A|B)
for (auto&& actual : {A_given_B * B_given_C, B_given_C * A_given_B}) {
EXPECT_LONGS_EQUAL(2, actual.nrFrontals());
EXPECT_LONGS_EQUAL(1, actual.nrParents());
KeyVector frontals(actual.beginFrontals(), actual.endFrontals());
EXPECT((frontals == KeyVector{1, 2}));
for (auto&& it : actual.enumerate()) {
const DiscreteValues& v = it.first;
EXPECT_DOUBLES_EQUAL(actual(v), A_given_B(v) * B_given_C(v), 1e-9);
}
}
}
/* ************************************************************************* */
// Check calculation of conditional joint P(A,B,C|D,E) = P(A,B|D) P(C|D,E)
TEST(DiscreteConditional, Multiply4) {
DiscreteKey A(0, 2), B(1, 2), C(2, 2), D(4, 2), E(3, 2);
DiscreteConditional A_given_B(A | B = "1/3 3/1");
DiscreteConditional B_given_D(B | D = "1/3 3/1");
DiscreteConditional AB_given_D = A_given_B * B_given_D;
DiscreteConditional C_given_DE((C | D, E) = "4/1 1/1 1/1 1/4");
// P(A,B,C|D,E) = P(A,B|D) P(C|D,E) = P(C|D,E) P(A,B|D)
for (auto&& actual : {AB_given_D * C_given_DE, C_given_DE * AB_given_D}) {
EXPECT_LONGS_EQUAL(3, actual.nrFrontals());
EXPECT_LONGS_EQUAL(2, actual.nrParents());
KeyVector frontals(actual.beginFrontals(), actual.endFrontals());
EXPECT((frontals == KeyVector{0, 1, 2}));
KeyVector parents(actual.beginParents(), actual.endParents());
EXPECT((parents == KeyVector{3, 4}));
for (auto&& it : actual.enumerate()) {
const DiscreteValues& v = it.first;
EXPECT_DOUBLES_EQUAL(actual(v), AB_given_D(v) * C_given_DE(v), 1e-9);
}
}
}
/* ************************************************************************* */
// Check calculation of marginals for joint P(A,B)
TEST(DiscreteConditional, marginals) {
DiscreteKey A(1, 2), B(0, 2);
DiscreteConditional conditional(A | B = "1/2 2/1");
DiscreteConditional prior(B % "1/2");
DiscreteConditional pAB = prior * conditional;
DiscreteConditional actualA = pAB.marginal(A.first);
DiscreteConditional pA(A % "5/4");
EXPECT(assert_equal(pA, actualA));
EXPECT_LONGS_EQUAL(1, actualA.nrFrontals());
EXPECT_LONGS_EQUAL(0, actualA.nrParents());
KeyVector frontalsA(actualA.beginFrontals(), actualA.endFrontals());
EXPECT((frontalsA == KeyVector{1}));
DiscreteConditional actualB = pAB.marginal(B.first);
EXPECT(assert_equal(prior, actualB));
EXPECT_LONGS_EQUAL(1, actualB.nrFrontals());
EXPECT_LONGS_EQUAL(0, actualB.nrParents());
KeyVector frontalsB(actualB.beginFrontals(), actualB.endFrontals());
EXPECT((frontalsB == KeyVector{0}));
}
/* ************************************************************************* */

24
gtsam/discrete/tests/testDiscretePrior.cpp
View File

@ -27,12 +27,32 @@ static const DiscreteKey X(0, 2);
/* ************************************************************************* */
TEST(DiscretePrior, constructors) {
DecisionTreeFactor f(X, "0.4 0.6");
DiscretePrior expected(f);
DiscretePrior actual(X % "2/3");
EXPECT_LONGS_EQUAL(1, actual.nrFrontals());
EXPECT_LONGS_EQUAL(0, actual.nrParents());
DecisionTreeFactor f(X, "0.4 0.6");
DiscretePrior expected(f);
EXPECT(assert_equal(expected, actual, 1e-9));
const vector<double> pmf{0.4, 0.6};
DiscretePrior actual2(X, pmf);
EXPECT_LONGS_EQUAL(1, actual2.nrFrontals());
EXPECT_LONGS_EQUAL(0, actual2.nrParents());
EXPECT(assert_equal(expected, actual2, 1e-9));
}
/* ************************************************************************* */
TEST(DiscretePrior, Multiply) {
DiscreteKey A(0, 2), B(1, 2);
DiscreteConditional conditional(A | B = "1/2 2/1");
DiscretePrior prior(B, "1/2");
DiscreteConditional actual = prior * conditional; // P(A|B) * P(B)
EXPECT_LONGS_EQUAL(2, actual.nrFrontals()); // = P(A,B)
DecisionTreeFactor factor(A & B, "1 4 2 2");
DiscreteConditional expected(2, factor);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */

2
gtsam/slam/slam.i
View File

@ -11,7 +11,7 @@ namespace gtsam {
// ######
#include <gtsam/slam/BetweenFactor.h>
template <T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
template <T = {double, Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3,
gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {

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

@ -13,7 +13,7 @@ Author: Frank Dellaert
import unittest
from gtsam import DecisionTreeFactor, DecisionTreeFactor, DiscreteKeys
from gtsam import DecisionTreeFactor, DiscreteValues, DiscretePrior, Ordering
from gtsam.utils.test_case import GtsamTestCase
@ -21,15 +21,59 @@ class TestDecisionTreeFactor(GtsamTestCase):
"""Tests for DecisionTreeFactors."""
def setUp(self):
A = (12, 3)
B = (5, 2)
self.factor = DecisionTreeFactor([A, B], "1 2 3 4 5 6")
self.A = (12, 3)
self.B = (5, 2)
self.factor = DecisionTreeFactor([self.A, self.B], "1 2 3 4 5 6")
def test_enumerate(self):
actual = self.factor.enumerate()
_, values = zip(*actual)
self.assertEqual(list(values), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0])
def test_multiplication(self):
"""Test whether multiplication works with overloading."""
v0 = (0, 2)
v1 = (1, 2)
v2 = (2, 2)
# Multiply with a DiscretePrior, i.e., Bayes Law!
prior = DiscretePrior(v1, [1, 3])
f1 = DecisionTreeFactor([v0, v1], "1 2 3 4")
expected = DecisionTreeFactor([v0, v1], "0.25 1.5 0.75 3")
self.gtsamAssertEquals(DecisionTreeFactor(prior) * f1, expected)
self.gtsamAssertEquals(f1 * prior, expected)
# Multiply two factors
f2 = DecisionTreeFactor([v1, v2], "5 6 7 8")
actual = f1 * f2
expected2 = DecisionTreeFactor([v0, v1, v2], "5 6 14 16 15 18 28 32")
self.gtsamAssertEquals(actual, expected2)
def test_methods(self):
"""Test whether we can call methods in python."""
# double operator()(const DiscreteValues& values) const;
values = DiscreteValues()
values[self.A[0]] = 0
values[self.B[0]] = 0
self.assertIsInstance(self.factor(values), float)
# size_t cardinality(Key j) const;
self.assertIsInstance(self.factor.cardinality(self.A[0]), int)
# DecisionTreeFactor operator/(const DecisionTreeFactor& f) const;
self.assertIsInstance(self.factor / self.factor, DecisionTreeFactor)
# DecisionTreeFactor* sum(size_t nrFrontals) const;
self.assertIsInstance(self.factor.sum(1), DecisionTreeFactor)
# DecisionTreeFactor* sum(const Ordering& keys) const;
ordering = Ordering()
ordering.push_back(self.A[0])
self.assertIsInstance(self.factor.sum(ordering), DecisionTreeFactor)
# DecisionTreeFactor* max(size_t nrFrontals) const;
self.assertIsInstance(self.factor.max(1), DecisionTreeFactor)
def test_markdown(self):
"""Test whether the _repr_markdown_ method."""

57
python/gtsam/tests/test_DiscreteConditional.py
View File

@ -16,6 +16,13 @@ import unittest
from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys
from gtsam.utils.test_case import GtsamTestCase
# Some DiscreteKeys for binary variables:
A = 0, 2
B = 1, 2
C = 2, 2
D = 4, 2
E = 3, 2
class TestDiscreteConditional(GtsamTestCase):
"""Tests for Discrete Conditionals."""
@ -36,6 +43,53 @@ class TestDiscreteConditional(GtsamTestCase):
actual = conditional.sample(2)
self.assertIsInstance(actual, int)
def test_multiply(self):
"""Check calculation of joint P(A,B)"""
conditional = DiscreteConditional(A, [B], "1/2 2/1")
prior = DiscreteConditional(B, "1/2")
# P(A,B) = P(A|B) * P(B) = P(B) * P(A|B)
for actual in [prior * conditional, conditional * prior]:
self.assertEqual(2, actual.nrFrontals())
for v, value in actual.enumerate():
self.assertAlmostEqual(actual(v), conditional(v) * prior(v))
def test_multiply2(self):
"""Check calculation of conditional joint P(A,B|C)"""
A_given_B = DiscreteConditional(A, [B], "1/3 3/1")
B_given_C = DiscreteConditional(B, [C], "1/3 3/1")
# P(A,B|C) = P(A|B)P(B|C) = P(B|C)P(A|B)
for actual in [A_given_B * B_given_C, B_given_C * A_given_B]:
self.assertEqual(2, actual.nrFrontals())
self.assertEqual(1, actual.nrParents())
for v, value in actual.enumerate():
self.assertAlmostEqual(actual(v), A_given_B(v) * B_given_C(v))
def test_multiply4(self):
"""Check calculation of joint P(A,B,C|D,E) = P(A,B|D) P(C|D,E)"""
A_given_B = DiscreteConditional(A, [B], "1/3 3/1")
B_given_D = DiscreteConditional(B, [D], "1/3 3/1")
AB_given_D = A_given_B * B_given_D
C_given_DE = DiscreteConditional(C, [D, E], "4/1 1/1 1/1 1/4")
# P(A,B,C|D,E) = P(A,B|D) P(C|D,E) = P(C|D,E) P(A,B|D)
for actual in [AB_given_D * C_given_DE, C_given_DE * AB_given_D]:
self.assertEqual(3, actual.nrFrontals())
self.assertEqual(2, actual.nrParents())
for v, value in actual.enumerate():
self.assertAlmostEqual(
actual(v), AB_given_D(v) * C_given_DE(v))
def test_marginals(self):
conditional = DiscreteConditional(A, [B], "1/2 2/1")
prior = DiscreteConditional(B, "1/2")
pAB = prior * conditional
self.gtsamAssertEquals(prior, pAB.marginal(B[0]))
pA = DiscreteConditional(A, "5/4")
self.gtsamAssertEquals(pA, pAB.marginal(A[0]))
def test_markdown(self):
"""Test whether the _repr_markdown_ method."""
@ -48,8 +102,7 @@ class TestDiscreteConditional(GtsamTestCase):
conditional = DiscreteConditional(A, parents,
"0/1 1/3 1/1 3/1 0/1 1/0")
expected = \
" *P(A|B,C):*\n\n" \
expected = " *P(A|B,C):*\n\n" \
"|*B*|*C*|0|1|\n" \
"|:-:|:-:|:-:|:-:|\n" \
"|0|0|0|1|\n" \

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

@ -25,12 +25,16 @@ class TestDiscretePrior(GtsamTestCase):
def test_constructor(self):
"""Test various constructors."""
actual = DiscretePrior(X, "2/3")
keys = DiscreteKeys()
keys.push_back(X)
f = DecisionTreeFactor(keys, "0.4 0.6")
expected = DiscretePrior(f)
actual = DiscretePrior(X, "2/3")
self.gtsamAssertEquals(actual, expected)
actual2 = DiscretePrior(X, [0.4, 0.6])
self.gtsamAssertEquals(actual2, expected)
def test_operator(self):
prior = DiscretePrior(X, "2/3")