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

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
+  // Calls the base class assertInvariants, which checks for unique keys
-   * conditioned variable is numbered lower than the parents.
+  void assertInvariants() const { Factor<KEY>::assertInvariants(); }
-   */
-  void assertInvariants() const;
 
 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();
 }
 
 }

gtsam/inference/Factor-inl.h

@@ -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());
+  // Check that keys are all unique
-  assert(uniqueSorted.size() == keys_.size());
+  std::multiset<Key> nonunique(keys_.begin(), keys_.end());
-  assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
+  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();
 }
 
 }

gtsam/inference/Factor.h

@@ -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;
 

gtsam/inference/IndexConditional.cpp

@@ -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 {
 
+  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
+  }
+
 }

gtsam/inference/IndexConditional.h

@@ -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)); }

gtsam/inference/IndexFactor.cpp

@@ -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;
+}
 
 }

gtsam/inference/IndexFactor.h

@@ -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.

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<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;