Merged in feature/matrix_tests (pull request #404)

Some facilities to better test linear inference

Approved-by: Mandy Xie <manxie@gatech.edu>

gtsam/linear/GaussianBayesNet.cpp

@@ -138,23 +138,34 @@ namespace gtsam {
   //}
 
   /* ************************************************************************* */
-  pair<Matrix, Vector> GaussianBayesNet::matrix() const {
+  Ordering GaussianBayesNet::ordering() const {
     GaussianFactorGraph factorGraph(*this);
-    KeySet keys = factorGraph.keys();
+    auto keys = factorGraph.keys();
     // add frontal keys in order
     Ordering ordering;
-    for (const sharedConditional& cg: *this)
+    for (const sharedConditional& cg : *this)
       if (cg) {
-        for (Key key: cg->frontals()) {
+        for (Key key : cg->frontals()) {
           ordering.push_back(key);
           keys.erase(key);
         }
       }
     // add remaining keys in case Bayes net is incomplete
-    for (Key key: keys)
-      ordering.push_back(key);
-    // return matrix and RHS
+    for (Key key : keys) ordering.push_back(key);
+    return ordering;
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianBayesNet::matrix(boost::optional<const Ordering&> ordering) const {
+    if (ordering) {
+      // Convert to a GaussianFactorGraph and use its machinery
+      GaussianFactorGraph factorGraph(*this);
       return factorGraph.jacobian(ordering);
+    } else {
+      // recursively call with default ordering
+      const auto defaultOrdering = this->ordering();
+      return matrix(defaultOrdering);
+    }
   }
 
   ///* ************************************************************************* */

gtsam/linear/GaussianBayesNet.h

@@ -74,6 +74,14 @@ namespace gtsam {
     /// Version of optimize for incomplete BayesNet, needs solution for missing variables
     VectorValues optimize(const VectorValues& solutionForMissing) const;
 
+    /**
+     * Return ordering corresponding to a topological sort.
+     * There are many topological sorts of a Bayes net. This one
+     * corresponds to the one that makes 'matrix' below upper-triangular.
+     * In case Bayes net is incomplete any non-frontal are added to the end.
+     */
+    Ordering ordering() const;
+
     ///@}
 
     ///@name Linear Algebra
@@ -81,8 +89,10 @@ namespace gtsam {
     
     /**
      * Return (dense) upper-triangular matrix representation
+     * Will return upper-triangular matrix only when using 'ordering' above.
+     * In case Bayes net is incomplete zero columns are added to the end.
      */
-    std::pair<Matrix, Vector> matrix() const;
+    std::pair<Matrix, Vector> matrix(boost::optional<const Ordering&> ordering = boost::none) const;
 
     /**
      * Optimize along the gradient direction, with a closed-form computation to perform the line

gtsam/linear/VectorValues.cpp

@@ -142,17 +142,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Vector VectorValues::vector() const
-  {
+  Vector VectorValues::vector() const {
     // Count dimensions
     DenseIndex totalDim = 0;
-    for(const Vector& v: *this | map_values)
-      totalDim += v.size();
+    for (const Vector& v : *this | map_values) totalDim += v.size();
 
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
-    for(const Vector& v: *this | map_values) {
+    for (const Vector& v : *this | map_values) {
       result.segment(pos, v.size()) = v;
       pos += v.size();
     }

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -136,6 +136,15 @@ TEST( GaussianBayesNet, optimize3 )
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(GaussianBayesNet, ordering) 
+{
+  Ordering expected;
+  expected += 0, 1;
+  const auto actual = noisyBayesNet.ordering();
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST( GaussianBayesNet, backSubstituteTranspose )
 {
@@ -152,6 +161,34 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
 
   VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
   EXPECT(assert_equal(expected, actual));
+  
+  const auto ordering = noisyBayesNet.ordering();
+  const Matrix R = smallBayesNet.matrix(ordering).first;
+  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
+  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
+}
+
+/* ************************************************************************* */
+TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
+{
+  // x=R'*y, expected=inv(R')*x
+  // 2 = 1    2
+  // 5   1 1  3
+  VectorValues
+    x = map_list_of<Key, Vector>
+      (_x_, Vector1::Constant(2))
+      (_y_, Vector1::Constant(5)),
+    expected = map_list_of<Key, Vector>
+      (_x_, Vector1::Constant(4))
+      (_y_, Vector1::Constant(9));
+
+  VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
+  EXPECT(assert_equal(expected, actual));
+  
+  const auto ordering = noisyBayesNet.ordering();
+  const Matrix R = noisyBayesNet.matrix(ordering).first;
+  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
+  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
 }
 
 /* ************************************************************************* */