From 1c2646000b0c295b67415b144c962bb7866890aa Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 12:03:33 -0400
Subject: [PATCH] Added tests for matrix/vector conversion

---
 gtsam/linear/tests/testGaussianBayesNet.cpp | 34 +++++++++++++++++----
 1 file changed, 28 insertions(+), 6 deletions(-)

diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 74f07b92e..184933732 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <CppUnitLite/TestHarness.h>
+#include <boost/tuple/tuple.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
@@ -28,13 +30,11 @@ using namespace boost::assign;
 // STL/C++
 #include <iostream>
 #include <sstream>
-#include <CppUnitLite/TestHarness.h>
-#include <boost/tuple/tuple.hpp>
 
 using namespace std;
 using namespace gtsam;
 
-static const Key _x_=0, _y_=1;
+static const Key _x_ = 11, _y_ = 22, _z_ = 33;
 
 static GaussianBayesNet smallBayesNet =
     list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))(
@@ -42,9 +42,9 @@ static GaussianBayesNet smallBayesNet =
 
 static GaussianBayesNet noisyBayesNet =
     list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1,
-                                noiseModel::Diagonal::Sigmas(Vector1::Constant(2))))(
+                                noiseModel::Isotropic::Sigma(1, 2.0)))(
         GaussianConditional(_y_, Vector1::Constant(5), I_1x1,
-                            noiseModel::Diagonal::Sigmas(Vector1::Constant(3))));
+                            noiseModel::Isotropic::Sigma(1, 3.0)));
 
 /* ************************************************************************* */
 TEST( GaussianBayesNet, Matrix )
@@ -140,11 +140,33 @@ TEST( GaussianBayesNet, optimize3 )
 TEST(GaussianBayesNet, ordering) 
 {
   Ordering expected;
-  expected += 0, 1;
+  expected += _x_, _y_;
   const auto actual = noisyBayesNet.ordering();
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST( GaussianBayesNet, MatrixStress )
+{
+  GaussianBayesNet bn;
+  using GC = GaussianConditional;
+  bn.emplace_shared<GC>(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2);
+  bn.emplace_shared<GC>(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2);
+  bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
+
+  const VectorValues expected = bn.optimize();
+  for (const auto keys :
+       {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
+        KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
+        KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
+    const Ordering ordering(keys);
+    Matrix R;
+    Vector d;
+    boost::tie(R, d) = bn.matrix(ordering);
+    EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
+  }
+}
+
 /* ************************************************************************* */
 TEST( GaussianBayesNet, backSubstituteTranspose )
 {