diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 9acdca629..4083d4105 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -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::assertInvariants(); } public: typedef KEY Key; typedef Conditional This; + typedef Factor Base; /** * Typedef to the factor type that produces this conditional and that this @@ -87,25 +86,26 @@ public: typedef boost::iterator_range 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& 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::permuteSeparatorWithInverse(const Permutation& inversePer parent = newParent; } } + assertInvariants(); return parentChanged; } @@ -227,6 +230,7 @@ void Conditional::permuteWithInverse(const Permutation& inversePermutation) } #endif FactorType::permuteWithInverse(inversePermutation); + assertInvariants(); } } diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index 2c42ad05f..68e45f157 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -31,19 +31,20 @@ namespace gtsam { /* ************************************************************************* */ template -Factor::Factor(const Factor& f) : keys_(f.keys_) {} +Factor::Factor(const Factor& f) : keys_(f.keys_) { assertInvariants(); } /* ************************************************************************* */ template -Factor::Factor(const ConditionalType& c) : keys_(c.keys()) {} +Factor::Factor(const ConditionalType& c) : keys_(c.keys()) { assertInvariants(); } /* ************************************************************************* */ template void Factor::assertInvariants() const { #ifndef NDEBUG - std::set 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 nonunique(keys_.begin(), keys_.end()); + std::set 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::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::shared_ptr Factor::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::shared_ptr Factor::eliminate(size_t nrFront template void Factor::permuteWithInverse(const Permutation& inversePermutation) { BOOST_FOREACH(KEY& key, keys_) { key = inversePermutation[key]; } + assertInvariants(); } } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 3d5c41cba..49d31abc0 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -76,7 +76,7 @@ protected: // The keys involved in this factor std::vector 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; diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index df7526b8a..70012a3f2 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -16,10 +16,29 @@ * @created Oct 17, 2010 */ +#include #include +#include namespace gtsam { -template class Conditional; + using namespace std; + using namespace boost::lambda; + + template class Conditional; + + /* ************************************************************************* */ + void IndexConditional::assertInvariants() const { + // Checks for uniqueness of keys + Base::assertInvariants(); +#ifndef NDEBUG + // Check that separator keys are sorted + FastSet 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 + } } diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 1aa20be9e..83ddb9b5d 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -33,6 +33,11 @@ namespace gtsam { */ class IndexConditional : public Conditional { + 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 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& parents) : Base(j, parents) {} + IndexConditional(Index j, const std::vector& parents) : Base(j, parents) { assertInvariants(); } /** Constructor from a frontal variable and an iterator range of parents */ template static shared_ptr FromRange(Index j, ITERATOR firstParent, ITERATOR lastParent) { - return Base::FromRange(j, firstParent, lastParent); } + shared_ptr result(Base::FromRange(j, firstParent, lastParent)); + result->assertInvariants(); + return result; } /** Named constructor from any number of frontal variables and parents */ template static shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { - return Base::FromRange(firstKey, lastKey, nrFrontals); } + shared_ptr result(Base::FromRange(firstKey, lastKey, nrFrontals)); + result->assertInvariants(); + return result; } /** Convert to a factor */ IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); } diff --git a/gtsam/inference/IndexFactor.cpp b/gtsam/inference/IndexFactor.cpp index 1b483ac9c..5041e87dc 100644 --- a/gtsam/inference/IndexFactor.cpp +++ b/gtsam/inference/IndexFactor.cpp @@ -28,8 +28,21 @@ namespace gtsam { template class Factor; -IndexFactor::IndexFactor(const IndexConditional& c) : Base(c) {} +/* ************************************************************************* */ +void IndexFactor::assertInvariants() const { + Base::assertInvariants(); +#ifndef NDEBUG + std::set 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::shared_ptr, IndexFactor::shared_ptr> IndexFactor::CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals) { @@ -51,14 +64,26 @@ pair::shared_ptr, IndexFactor::shared_ptr> IndexFacto return result; } +/* ************************************************************************* */ IndexFactor::shared_ptr IndexFactor::Combine( const FactorGraph& factors, const FastMap >& variableSlots) { - return Base::Combine(factors, variableSlots); } + IndexFactor::shared_ptr combined(Base::Combine(factors, variableSlots)); + combined->assertInvariants(); + return combined; +} +/* ************************************************************************* */ boost::shared_ptr IndexFactor::eliminateFirst() { - return Base::eliminateFirst(); } + boost::shared_ptr result(Base::eliminateFirst()); + assertInvariants(); + return result; +} +/* ************************************************************************* */ boost::shared_ptr > IndexFactor::eliminate(size_t nrFrontals) { - return Base::eliminate(nrFrontals); } + boost::shared_ptr > result(Base::eliminate(nrFrontals)); + assertInvariants(); + return result; +} } diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 54c9eec4f..2ff0dd940 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -41,6 +41,11 @@ namespace gtsam { */ class IndexFactor : public Factor { + 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 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 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& js) : Base(js) {} + IndexFactor(const std::set& js) : Base(js) { assertInvariants(); } /** * Combine and eliminate several factors. diff --git a/gtsam/inference/tests/testSymbolicFactor.cpp b/gtsam/inference/tests/testSymbolicFactor.cpp index 5f57c9914..39bb1233c 100644 --- a/gtsam/inference/tests/testSymbolicFactor.cpp +++ b/gtsam/inference/tests/testSymbolicFactor.cpp @@ -28,6 +28,22 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +/* ************************************************************************* */ +TEST(SymbolicFactor, constructor) { + + // Frontals sorted, parents not sorted + vector keys1; keys1 += 3, 4, 5, 9, 7, 8; + (void)IndexConditional::FromRange(keys1.begin(), keys1.end(), 3); + +// // Frontals not sorted +// vector keys2; keys2 += 3, 5, 4, 9, 7, 8; +// (void)IndexConditional::FromRange(keys2.begin(), keys2.end(), 3); + +// // Frontals not before parents +// vector keys3; keys3 += 3, 4, 5, 1, 7, 8; +// (void)IndexConditional::FromRange(keys3.begin(), keys3.end(), 3); +} + /* ************************************************************************* */ TEST(SymbolicFactor, eliminate) { vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;