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

Moved sorted-key invariant out of Factor and into IndexFactor, also out of Conditional and into IndexConditional

release/4.3a0
Richard Roberts 2011-03-01 21:47:08 +00:00
parent 86ac4d5374
commit 07ad9ac957
8 changed files with 118 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/inference/Conditional.h
View File

@ -54,15 +54,14 @@ protected:
/** The first nFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
/** Debugging invariant that the keys should be in order, including that the
* conditioned variable is numbered lower than the parents.
*/
void assertInvariants() const;
// Calls the base class assertInvariants, which checks for unique keys
void assertInvariants() const { Factor<KEY>::assertInvariants(); }
public:
typedef KEY Key;
typedef Conditional<Key> This;
typedef Factor<Key> Base;
/**
* Typedef to the factor type that produces this conditional and that this
@ -87,25 +86,26 @@ public:
typedef boost::iterator_range<const_iterator> Parents;
/** Empty Constructor to make serialization possible */
Conditional() : nrFrontals_(0) {}
Conditional() : nrFrontals_(0) { assertInvariants(); }
/** No parents */
Conditional(Key key) : FactorType(key), nrFrontals_(1) {}
Conditional(Key key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
/** Single parent */
Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) {}
Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
/** Two parents */
Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) {}
Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
/** Three parents */
Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {}
Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
/** Constructor from a frontal variable and a vector of parents */
Conditional(Key key, const std::vector<Key>& parents) : nrFrontals_(1) {
FactorType::keys_.resize(1 + parents.size());
*(beginFrontals()) = key;
std::copy(parents.begin(), parents.end(), beginParents());
assertInvariants();
}
/** Constructor from a frontal variable and an iterator range of parents */
@ -115,6 +115,7 @@ public:
conditional->nrFrontals_ = 1;
conditional->keys_.push_back(key);
std::copy(firstParent, lastParent, back_inserter(conditional->keys_));
conditional->This::assertInvariants();
return conditional;
}
@ -124,6 +125,7 @@ public:
typename DERIVED::shared_ptr conditional(new DERIVED);
conditional->nrFrontals_ = nrFrontals;
std::copy(firstKey, lastKey, back_inserter(conditional->keys_));
conditional->This::assertInvariants();
return conditional;
}
@ -212,6 +214,7 @@ bool Conditional<KEY>::permuteSeparatorWithInverse(const Permutation& inversePer
parent = newParent;
}
}
assertInvariants();
return parentChanged;
}
@ -227,6 +230,7 @@ void Conditional<KEY>::permuteWithInverse(const Permutation& inversePermutation)
}
#endif
FactorType::permuteWithInverse(inversePermutation);
assertInvariants();
}
}

14
gtsam/inference/Factor-inl.h
View File

@ -31,19 +31,20 @@ namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const Factor<KEY>& f) : keys_(f.keys_) {}
Factor<KEY>::Factor(const Factor<KEY>& f) : keys_(f.keys_) { assertInvariants(); }
/* ************************************************************************* */
template<typename KEY>
Factor<KEY>::Factor(const ConditionalType& c) : keys_(c.keys()) {}
Factor<KEY>::Factor(const ConditionalType& c) : keys_(c.keys()) { assertInvariants(); }
/* ************************************************************************* */
template<typename KEY>
void Factor<KEY>::assertInvariants() const {
#ifndef NDEBUG
std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
assert(uniqueSorted.size() == keys_.size());
assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
// Check that keys are all unique
std::multiset<Key> nonunique(keys_.begin(), keys_.end());
std::set<Key> unique(keys_.begin(), keys_.end());
assert(nonunique.size() == unique.size() && std::equal(nonunique.begin(), nonunique.end(), unique.begin()));
#endif
}
@ -84,6 +85,7 @@ typename CONDITIONAL::shared_ptr Factor<KEY>::eliminateFirst() {
assertInvariants();
KEY eliminated = keys_.front();
keys_.erase(keys_.begin());
assertInvariants();
return typename CONDITIONAL::shared_ptr(new CONDITIONAL(eliminated, keys_));
}
@ -100,6 +102,7 @@ typename BayesNet<CONDITIONAL>::shared_ptr Factor<KEY>::eliminate(size_t nrFront
nextFrontal, const_iterator(this->end()), 1));
if(nrFrontals > 0)
keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents());
assertInvariants();
return fragment;
}
@ -107,6 +110,7 @@ typename BayesNet<CONDITIONAL>::shared_ptr Factor<KEY>::eliminate(size_t nrFront
template<typename KEY>
void Factor<KEY>::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(KEY& key, keys_) { key = inversePermutation[key]; }
assertInvariants();
}
}

2
gtsam/inference/Factor.h
View File

@ -76,7 +76,7 @@ protected:
// The keys involved in this factor
std::vector<Key> keys_;
// Internal check to make sure keys are sorted. If NDEBUG is defined, this
// Internal check to make sure keys are unique. If NDEBUG is defined, this
// is empty and optimized out.
void assertInvariants() const;

21
gtsam/inference/IndexConditional.cpp
View File

@ -16,10 +16,29 @@
* @created Oct 17, 2010
*/
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/IndexConditional.h>
#include <boost/lambda/lambda.hpp>
namespace gtsam {
template class Conditional<Index>;
using namespace std;
using namespace boost::lambda;
template class Conditional<Index>;
/* ************************************************************************* */
void IndexConditional::assertInvariants() const {
// Checks for uniqueness of keys
Base::assertInvariants();
#ifndef NDEBUG
// Check that separator keys are sorted
FastSet<Index> uniquesorted(beginFrontals(), endFrontals());
assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals()));
// Check that separator keys are less than parent keys
BOOST_FOREACH(Index j, frontals()) {
assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); }
#endif
}
}

25
gtsam/inference/IndexConditional.h
View File

@ -33,6 +33,11 @@ namespace gtsam {
*/
class IndexConditional : public Conditional<Index> {
protected:
// Checks that frontal indices are sorted and lower than parent indices
void assertInvariants() const;
public:
typedef IndexConditional This;
@ -41,32 +46,36 @@ namespace gtsam {
typedef boost::shared_ptr<IndexConditional> shared_ptr;
/** Empty Constructor to make serialization possible */
IndexConditional() {}
IndexConditional() { assertInvariants(); }
/** No parents */
IndexConditional(Index j) : Base(j) {}
IndexConditional(Index j) : Base(j) { assertInvariants(); }
/** Single parent */
IndexConditional(Index j, Index parent) : Base(j, parent) {}
IndexConditional(Index j, Index parent) : Base(j, parent) { assertInvariants(); }
/** Two parents */
IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) {}
IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); }
/** Three parents */
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) {}
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); }
/** Constructor from a frontal variable and a vector of parents */
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) {}
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) { assertInvariants(); }
/** Constructor from a frontal variable and an iterator range of parents */
template<typename ITERATOR>
static shared_ptr FromRange(Index j, ITERATOR firstParent, ITERATOR lastParent) {
return Base::FromRange<This>(j, firstParent, lastParent); }
shared_ptr result(Base::FromRange<This>(j, firstParent, lastParent));
result->assertInvariants();
return result; }
/** Named constructor from any number of frontal variables and parents */
template<typename ITERATOR>
static shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) {
return Base::FromRange<This>(firstKey, lastKey, nrFrontals); }
shared_ptr result(Base::FromRange<This>(firstKey, lastKey, nrFrontals));
result->assertInvariants();
return result; }
/** Convert to a factor */
IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); }

33
gtsam/inference/IndexFactor.cpp
View File

@ -28,8 +28,21 @@ namespace gtsam {
template class Factor<Index>;
IndexFactor::IndexFactor(const IndexConditional& c) : Base(c) {}
/* ************************************************************************* */
void IndexFactor::assertInvariants() const {
Base::assertInvariants();
#ifndef NDEBUG
std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
assert(uniqueSorted.size() == keys_.size() && std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
#endif
}
/* ************************************************************************* */
IndexFactor::IndexFactor(const IndexConditional& c): Base(c) {
assertInvariants();
}
/* ************************************************************************* */
pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> IndexFactor::CombineAndEliminate(
const FactorGraph<This>& factors, size_t nrFrontals) {
@ -51,14 +64,26 @@ pair<BayesNet<IndexConditional>::shared_ptr, IndexFactor::shared_ptr> IndexFacto
return result;
}
/* ************************************************************************* */
IndexFactor::shared_ptr IndexFactor::Combine(
const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots) {
return Base::Combine<This>(factors, variableSlots); }
IndexFactor::shared_ptr combined(Base::Combine<This>(factors, variableSlots));
combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() {
return Base::eliminateFirst<IndexConditional>(); }
boost::shared_ptr<IndexConditional> result(Base::eliminateFirst<IndexConditional>());
assertInvariants();
return result;
}
/* ************************************************************************* */
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate(size_t nrFrontals) {
return Base::eliminate<IndexConditional>(nrFrontals); }
boost::shared_ptr<BayesNet<IndexConditional> > result(Base::eliminate<IndexConditional>(nrFrontals));
assertInvariants();
return result;
}
}

21
gtsam/inference/IndexFactor.h
View File

@ -41,6 +41,11 @@ namespace gtsam {
*/
class IndexFactor : public Factor<Index> {
protected:
// Internal function for checking class invariants (sorted keys for this factor)
void assertInvariants() const;
public:
typedef IndexFactor This;
@ -53,32 +58,32 @@ namespace gtsam {
typedef boost::shared_ptr<IndexFactor> shared_ptr;
/** Copy constructor */
IndexFactor(const This& f) : Base(f) {}
IndexFactor(const This& f) : Base(f) { assertInvariants(); }
/** Construct from derived type */
IndexFactor(const IndexConditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey, KeyIterator endKey) :
Base(beginKey, endKey) {}
Base(beginKey, endKey) { assertInvariants(); }
/** Default constructor for I/O */
IndexFactor() {}
IndexFactor() { assertInvariants(); }
/** Construct unary factor */
IndexFactor(Index j) : Base(j) {}
IndexFactor(Index j) : Base(j) { assertInvariants(); }
/** Construct binary factor */
IndexFactor(Index j1, Index j2) : Base(j1, j2) {}
IndexFactor(Index j1, Index j2) : Base(j1, j2) { assertInvariants(); }
/** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) {}
IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) { assertInvariants(); }
/** Construct 4-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) {}
IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) { assertInvariants(); }
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) : Base(js) {}
IndexFactor(const std::set<Index>& js) : Base(js) { assertInvariants(); }
/**
* Combine and eliminate several factors.

16
gtsam/inference/tests/testSymbolicFactor.cpp
View File

@ -28,6 +28,22 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST(SymbolicFactor, constructor) {
// Frontals sorted, parents not sorted
vector<Index> keys1; keys1 += 3, 4, 5, 9, 7, 8;
(void)IndexConditional::FromRange(keys1.begin(), keys1.end(), 3);
// // Frontals not sorted
// vector<Index> keys2; keys2 += 3, 5, 4, 9, 7, 8;
// (void)IndexConditional::FromRange(keys2.begin(), keys2.end(), 3);
// // Frontals not before parents
// vector<Index> keys3; keys3 += 3, 4, 5, 1, 7, 8;
// (void)IndexConditional::FromRange(keys3.begin(), keys3.end(), 3);
}
/* ************************************************************************* */
TEST(SymbolicFactor, eliminate) {
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;