Bug fix and unit test HessianFactor::CombineAndEliminate when keys are not sorted

gtsam/inference/BayesTree-inl.h

@@ -43,13 +43,29 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CONDITIONAL>
-	BayesTree<CONDITIONAL>::Clique::Clique() {}
+  void BayesTree<CONDITIONAL>::Clique::assertInvariants() const {
+#ifndef NDEBUG
+    // We rely on the keys being sorted
+    typename BayesNet<CONDITIONAL>::const_iterator cond = this->begin();
+    if(cond != this->end()) {
+      Index lastKey = (*cond)->key();
+      ++cond;
+      for(; cond != this->end(); ++cond)
+        assert(lastKey < (*cond)->key());
+    }
+#endif
+  }
+
+	/* ************************************************************************* */
+	template<class CONDITIONAL>
+	BayesTree<CONDITIONAL>::Clique::Clique() { assertInvariants(); }
 
 	/* ************************************************************************* */
 	template<class CONDITIONAL>
 	BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) {
 		separator_.assign(conditional->parents().begin(), conditional->parents().end());
 		this->push_back(conditional);
+		assertInvariants();
 	}
 
 	/* ************************************************************************* */
@@ -84,6 +100,7 @@ namespace gtsam {
 //	    separator_.assign((*bayesNet.rbegin())->parents().begin(), (*bayesNet.rbegin())->parents().end());
 	    separator_.assign((*bayesNet.rbegin())->beginParents(), (*bayesNet.rbegin())->endParents());
 	  }
+    assertInvariants();
 	}
 
 	/* ************************************************************************* */
@@ -137,6 +154,7 @@ namespace gtsam {
     BOOST_FOREACH(const shared_ptr& child, children_) {
       child->permuteWithInverse(inversePermutation);
     }
+    assertInvariants();
   }
 
   /* ************************************************************************* */
@@ -158,6 +176,7 @@ namespace gtsam {
         (void)child->permuteSeparatorWithInverse(inversePermutation);
       }
     }
+    assertInvariants();
     return changed;
   }
 
@@ -350,6 +369,7 @@ namespace gtsam {
 		p_S_R.permuteWithInverse(toBack);
 
 		// return the parent shortcut P(Sp|R)
+    assertInvariants();
 		return p_S_R;
 	}
 
@@ -372,6 +392,7 @@ namespace gtsam {
 		// Find marginal on the keys we are interested in
 		FactorGraph<typename CONDITIONAL::Factor> p_FSRfg(p_FSR);
 
+    assertInvariants();
 		return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).jointFactorGraph(keys());
 	}
 
@@ -400,6 +421,7 @@ namespace gtsam {
 		// Calculate the marginal
 		vector<Index> keys12vector; keys12vector.reserve(keys12.size());
 		keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
+    assertInvariants();
 		return *GenericSequentialSolver<typename CONDITIONAL::Factor>(joint).jointFactorGraph(keys12vector);
 	}
 
@@ -429,6 +451,7 @@ namespace gtsam {
 			new_clique->parent_ = parent_clique;
 			parent_clique->children_.push_back(new_clique);
 		}
+    new_clique->assertInvariants();
 		return new_clique;
 	}
 
@@ -443,6 +466,7 @@ namespace gtsam {
 		new_clique->children_ = child_cliques;
 		BOOST_FOREACH(sharedClique& child, child_cliques)
 			child->parent_ = new_clique;
+		new_clique->assertInvariants();
 		return new_clique;
 	}
 
@@ -477,6 +501,7 @@ namespace gtsam {
 	  nodes_[key] = clique;
 	  clique->push_front(conditional);
 	  if(debug) clique->print("Expanded clique is ");
+	  clique->assertInvariants();
 	}
 
 	/* ************************************************************************* */

gtsam/inference/BayesTree.h

@@ -53,6 +53,10 @@ namespace gtsam {
 		 */
 		struct Clique: public BayesNet<CONDITIONAL> {
 
+		protected:
+		  void assertInvariants() const;
+
+		public:
 			typedef typename boost::shared_ptr<Clique> shared_ptr;
 			typedef typename boost::weak_ptr<Clique> weak_ptr;
 			weak_ptr parent_;

gtsam/linear/HessianFactor.cpp

@@ -61,8 +61,8 @@ namespace gtsam {
   /* ************************************************************************* */
   HessianFactor::HessianFactor(const Vector& b_in) : info_(matrix_) {
     JacobianFactor jf(b_in);
-    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
     info_.copyStructureFrom(jf.Ab_);
+    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
   }
 
   /* ************************************************************************* */
@@ -70,8 +70,8 @@ namespace gtsam {
       const Vector& b, const SharedDiagonal& model) :
       GaussianFactor(i1), info_(matrix_) {
     JacobianFactor jf(i1, A1, b, model);
-    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
     info_.copyStructureFrom(jf.Ab_);
+    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
   }
 
   /* ************************************************************************* */
@@ -79,8 +79,8 @@ namespace gtsam {
       const Vector& b, const SharedDiagonal& model) :
       GaussianFactor(i1,i2), info_(matrix_) {
     JacobianFactor jf(i1, A1, i2, A2, b, model);
-    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
     info_.copyStructureFrom(jf.Ab_);
+    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
   }
 
   /* ************************************************************************* */
@@ -88,8 +88,8 @@ namespace gtsam {
       Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
       GaussianFactor(i1,i2,i3), info_(matrix_) {
     JacobianFactor jf(i1, A1, i2, A2, i3, A3, b, model);
-    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
     info_.copyStructureFrom(jf.Ab_);
+    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
   }
 
   /* ************************************************************************* */
@@ -97,8 +97,8 @@ namespace gtsam {
       const Vector &b, const SharedDiagonal& model) : info_(matrix_) {
     JacobianFactor jf(terms, b, model);
     keys_ = jf.keys_;
-    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
     info_.copyStructureFrom(jf.Ab_);
+    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
   }
 
   /* ************************************************************************* */
@@ -106,16 +106,16 @@ namespace gtsam {
       const Vector &b, const SharedDiagonal& model) : info_(matrix_) {
     JacobianFactor jf(terms, b, model);
     keys_ = jf.keys_;
-    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
     info_.copyStructureFrom(jf.Ab_);
+    ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
   }
 
   /* ************************************************************************* */
   HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
     JacobianFactor jf(cg);
+    info_.copyStructureFrom(jf.Ab_);
     ublas::noalias(ublas::symmetric_adaptor<MatrixColMajor,ublas::upper>(matrix_)) =
         ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
-    info_.copyStructureFrom(jf.Ab_);
   }
 
   /* ************************************************************************* */
@@ -126,8 +126,8 @@ namespace gtsam {
     if(dynamic_cast<const JacobianFactor*>(&gf)) {
       const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
       JacobianFactor whitened(jf.whiten());
-      matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_);
       info_.copyStructureFrom(whitened.Ab_);
+      matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_);
     } else if(dynamic_cast<const HessianFactor*>(&gf)) {
       const HessianFactor& hf(static_cast<const HessianFactor&>(gf));
       info_.assignNoalias(hf.info_);
@@ -289,9 +289,22 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
     size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
     for(size_t j1=0; j1<=j2; ++j1) {
       size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
+      if(slot2 > slot1) {
         if(debug)
           cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
         ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2);
+      } else if(slot1 > slot2) {
+        if(debug)
+          cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
+        ublas::noalias(this->info_(slot2, slot1)) += ublas::trans(update.info_(j1,j2));
+      } else {
+        if(debug)
+          cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
+        Block thisBlock(this->info_(slot2, slot1));
+        constBlock updateBlock(update.info_(j1,j2));
+        ublas::noalias(ublas::symmetric_adaptor<Block,ublas::upper>(thisBlock)) +=
+            ublas::symmetric_adaptor<constBlock,ublas::upper>(updateBlock);
+      }
     }
   }
   toc(2, "update");

gtsam/linear/tests/testHessianFactor.cpp

@@ -205,6 +205,85 @@ TEST(GaussianFactor, eliminate2 )
   EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3));
 }
 
+/* ************************************************************************* */
+TEST_UNSAFE(GaussianFactor, eliminateUnsorted) {
+  JacobianFactor::shared_ptr factor1(
+      new JacobianFactor(0,
+                         Matrix_(3,3,
+                                 44.7214,     0.0,       0.0,
+                                 0.0,     44.7214,       0.0,
+                                 0.0,         0.0,   44.7214),
+                         1,
+                         Matrix_(3,3,
+                                 -0.179168,    -44.721,  0.717294,
+                                 44.721, -0.179168,  -44.9138,
+                                 0.0,         0.0,  -44.7214),
+                         Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
+                         noiseModel::Unit::Create(3)));
+  HessianFactor::shared_ptr unsorted_factor2(
+      new HessianFactor(0,
+                        Matrix_(6,3,
+                                25.8367,    0.1211,    0.0593,
+                                    0.0,   23.4099,   30.8733,
+                                    0.0,       0.0,   25.8729,
+                                    0.0,       0.0,       0.0,
+                                    0.0,       0.0,       0.0,
+                                    0.0,       0.0,       0.0),
+                        1,
+                        Matrix_(6,3,
+                                25.7429,   -1.6897,    0.4587,
+                                 1.6400,   23.3095,   -8.4816,
+                                 0.0034,    0.0509,  -25.7855,
+                                 0.9997,   -0.0002,    0.0824,
+                                    0.0,    0.9973,    0.9517,
+                                    0.0,       0.0,    0.9973),
+                        Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+                        noiseModel::Unit::Create(6)));
+  Permutation permutation(2);
+  permutation[0] = 1;
+  permutation[1] = 0;
+  unsorted_factor2->permuteWithInverse(permutation);
+
+  HessianFactor::shared_ptr sorted_factor2(
+      new HessianFactor(0,
+                        Matrix_(6,3,
+                                25.7429,   -1.6897,    0.4587,
+                                 1.6400,   23.3095,   -8.4816,
+                                 0.0034,    0.0509,  -25.7855,
+                                 0.9997,   -0.0002,    0.0824,
+                                    0.0,    0.9973,    0.9517,
+                                    0.0,       0.0,    0.9973),
+                        1,
+                        Matrix_(6,3,
+                                25.8367,    0.1211,    0.0593,
+                                    0.0,   23.4099,   30.8733,
+                                    0.0,       0.0,   25.8729,
+                                    0.0,       0.0,       0.0,
+                                    0.0,       0.0,       0.0,
+                                    0.0,       0.0,       0.0),
+                        Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+                        noiseModel::Unit::Create(6)));
+
+  GaussianFactorGraph sortedGraph;
+//  sortedGraph.push_back(factor1);
+  sortedGraph.push_back(sorted_factor2);
+
+  GaussianFactorGraph unsortedGraph;
+//  unsortedGraph.push_back(factor1);
+  unsortedGraph.push_back(unsorted_factor2);
+
+  GaussianBayesNet::shared_ptr expected_bn;
+  GaussianFactor::shared_ptr expected_factor;
+  boost::tie(expected_bn, expected_factor) = GaussianFactor::CombineAndEliminate(sortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY);
+
+  GaussianBayesNet::shared_ptr actual_bn;
+  GaussianFactor::shared_ptr actual_factor;
+  boost::tie(actual_bn, actual_factor) = GaussianFactor::CombineAndEliminate(unsortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY);
+
+  CHECK(assert_equal(*expected_bn, *actual_bn, 1e-10));
+  CHECK(assert_equal(*expected_factor, *actual_factor, 1e-10));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */