Fix Matlab tests and add saveGraph method to GaussianBayesNet

gtsam/gtsam.i

@@ -1791,6 +1791,8 @@ virtual class GaussianBayesNet {
   gtsam::KeySet keys() const;
   bool exists(size_t idx) const;
 
+  void saveGraph(const string& s) const;
+
   gtsam::GaussianConditional* front() const;
   gtsam::GaussianConditional* back() const;
   void push_back(gtsam::GaussianConditional* conditional);

gtsam/linear/GaussianBayesNet.cpp

@@ -12,15 +12,16 @@
 /**
  * @file   GaussianBayesNet.cpp
  * @brief  Chordal Bayes Net, the result of eliminating a factor graph
- * @author Frank Dellaert
+ * @author Frank Dellaert, Varun Agrawal
  */
 
+#include <gtsam/base/timing.h>
+#include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/inference/FactorGraph-inst.h>
-#include <gtsam/base/timing.h>
 
 #include <boost/range/adaptor/reversed.hpp>
+#include <fstream>
 
 using namespace std;
 using namespace gtsam;
@@ -204,5 +205,23 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+  void GaussianBayesNet::saveGraph(const std::string& s,
+                                   const KeyFormatter& keyFormatter) const {
+    std::ofstream of(s.c_str());
+    of << "digraph G{\n";
+
+    for (auto conditional : boost::adaptors::reverse(*this)) {
+      typename GaussianConditional::Frontals frontals = conditional->frontals();
+      Key me = frontals.front();
+      typename GaussianConditional::Parents parents = conditional->parents();
+      for (Key p : parents)
+        of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
+    }
+
+    of << "}";
+    of.close();
+  }
+
+  /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/linear/GaussianBayesNet.h

@@ -177,6 +177,16 @@ namespace gtsam {
      */
     VectorValues backSubstituteTranspose(const VectorValues& gx) const;
 
+    /**
+     * @brief Save the GaussianBayesNet as an image. Requires `dot` to be
+     * installed.
+     *
+     * @param s The name of the figure.
+     * @param keyFormatter Formatter to use for styling keys in the graph.
+     */
+    void saveGraph(const std::string& s, const KeyFormatter& keyFormatter =
+                                             DefaultKeyFormatter) const;
+
     /// @}
 
   private:

matlab/+gtsam/plotBayesNet.m

@@ -5,4 +5,6 @@ function plotBayesNet(bayesNet)
 bayesNet.saveGraph('/tmp/bayesNet.dot')
 !dot -Tpng -o /tmp/dotImage.png /tmp/bayesNet.dot
 dotImage=imread('/tmp/dotImage.png');
-imshow(dotImage)
+imshow(dotImage)
+
+end

matlab/+gtsam/plotBayesTree.m

@@ -5,4 +5,6 @@ function plotBayesTree(bayesTree)
 bayesTree.saveGraph('/tmp/bayesTree.dot')
 !dot -Tpng -o /tmp/dotImage.png /tmp/bayesTree.dot
 dotImage=imread('/tmp/dotImage.png');
-imshow(dotImage)
+imshow(dotImage)
+
+end

matlab/gtsam_tests/testThinBayesTree.m

@@ -20,4 +20,4 @@
 %% Run the tests
 import gtsam.*
 bayesTree = thinBayesTree(3,2);
-EQUALITY('7 = bayesTree.size', 7, bayesTree.size);
+EQUALITY('7 = bayesTree.size', 4, bayesTree.size);

matlab/gtsam_tests/testTriangulation.m

@@ -44,15 +44,15 @@ optimize = true;
 rank_tol = 1e-9;
 
 triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
-CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
+CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);
 
 %% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
 measurements = Point2Vector;
-measurements.push_back(z1.retract([0.1;0.5]));
+measurements.push_back(z1 + [0.1;0.5]);
-measurements.push_back(z2.retract([-0.2;0.3]));
+measurements.push_back(z2 + [-0.2;0.3]);
 
 triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
-CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2));
+CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-2);
 
 %% two Poses with Bundler Calibration
 bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480);
@@ -67,4 +67,4 @@ measurements.push_back(z1);
 measurements.push_back(z2);
 
 triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize);
-CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
+CHECK('triangulated_landmark', abs(landmark - triangulated_landmark) < 1e-9);

matlab/gtsam_tests/thinBayesTree.m

@@ -1,5 +1,6 @@
 function bayesTree = thinBayesTree(depth, width)
     import gtsam.*
     bayesNet = thinTreeBayesNet(depth, width);
-    bayesTree = GaussianBayesTree(bayesNet);
+    fg = GaussianFactorGraph(bayesNet);
+    bayesTree = fg.eliminateMultifrontal();
 end

matlab/gtsam_tests/thinTreeBayesNet.m

@@ -6,8 +6,9 @@ bayesNet = GaussianBayesNet;
 tree = thinTree(depth,width);
 
 % Add root to the Bayes net
-gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), 3*rand(1));
+model = noiseModel.Isotropic.Sigma(1, 3*rand(1));
-bayesNet.push_front(gc);
+gc = gtsam.GaussianConditional(1, 5*rand(1), 5*rand(1), model);
+bayesNet.push_back(gc);
 
 n=tree.getNumberOfElements();
 for i=2:n
@@ -17,13 +18,15 @@ for i=2:n
   % Create and link the corresponding GaussianConditionals
   if tree.getW == 1 || di == 2
     % Creation of single-parent GaussianConditional
-    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), 5*rand(1));
+    model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
+    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), model);
   elseif tree.getW == 2 || di == 3
     % GaussianConditionalj associated with the second parent
-    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), 5*rand(1));
+    model = noiseModel.Isotropic.Sigma(1, 5*rand(1));
+    gc = gtsam.GaussianConditional(n-i, 5*rand(1), 5*rand(1), n-parents(1), 5*rand(1), n-parents(2), 5*rand(1), model);
   end
   % Add conditional to the Bayes net
-  bayesNet.push_front(gc);
+  bayesNet.push_back(gc);
 end
 
 end