Merge pull request #1077 from borglab/feature/more_wrapping

gtsam/discrete/discrete.i

@@ -270,11 +270,14 @@ class DiscreteFactorGraph {
   gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
 
   gtsam::DiscreteBayesNet eliminateSequential();
+  gtsam::DiscreteBayesNet eliminateSequential(gtsam::Ordering::OrderingType type);
   gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
   std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
       eliminatePartialSequential(const gtsam::Ordering& ordering);
+
   gtsam::DiscreteBayesTree eliminateMultifrontal();
-  gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
+  gtsam::DiscreteBayesTree eliminateMultifrontal(gtsam::Ordering::OrderingType type);  
+  gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);  
   std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
       eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
 

gtsam/discrete/tests/testDiscreteBayesTree.cpp

@@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) {
   string actual = self.bayesTree->dot();
   EXPECT(actual ==
          "digraph G{\n"
-         "0[label=\"13,11,6,7\"];\n"
+         "0[label=\"13, 11, 6, 7\"];\n"
          "0->1\n"
-         "1[label=\"14 : 11,13\"];\n"
+         "1[label=\"14 : 11, 13\"];\n"
          "1->2\n"
-         "2[label=\"9,12 : 14\"];\n"
+         "2[label=\"9, 12 : 14\"];\n"
          "2->3\n"
-         "3[label=\"3 : 9,12\"];\n"
+         "3[label=\"3 : 9, 12\"];\n"
          "2->4\n"
-         "4[label=\"2 : 9,12\"];\n"
+         "4[label=\"2 : 9, 12\"];\n"
          "2->5\n"
-         "5[label=\"8 : 12,14\"];\n"
+         "5[label=\"8 : 12, 14\"];\n"
          "5->6\n"
-         "6[label=\"1 : 8,12\"];\n"
+         "6[label=\"1 : 8, 12\"];\n"
          "5->7\n"
-         "7[label=\"0 : 8,12\"];\n"
+         "7[label=\"0 : 8, 12\"];\n"
          "1->8\n"
-         "8[label=\"10 : 13,14\"];\n"
+         "8[label=\"10 : 13, 14\"];\n"
          "8->9\n"
-         "9[label=\"5 : 10,13\"];\n"
+         "9[label=\"5 : 10, 13\"];\n"
          "8->10\n"
-         "10[label=\"4 : 10,13\"];\n"
+         "10[label=\"4 : 10, 13\"];\n"
          "}");
 }
 

gtsam/inference/BayesTree-inst.h

@@ -95,7 +95,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template <class CLIQUE>
   void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
-                              const KeyFormatter& indexFormatter,
+                              const KeyFormatter& keyFormatter,
                               int parentnum) const {
     static int num = 0;
     bool first = true;
@@ -104,10 +104,10 @@ namespace gtsam {
     std::string parent = out.str();
     parent += "[label=\"";
 
-    for (Key index : clique->conditional_->frontals()) {
-      if (!first) parent += ",";
+    for (Key key : clique->conditional_->frontals()) {
+      if (!first) parent += ", ";
       first = false;
-      parent += indexFormatter(index);
+      parent += keyFormatter(key);
     }
 
     if (clique->parent()) {
@@ -116,18 +116,18 @@ namespace gtsam {
     }
 
     first = true;
-    for (Key sep : clique->conditional_->parents()) {
-      if (!first) parent += ",";
+    for (Key parentKey : clique->conditional_->parents()) {
+      if (!first) parent += ", ";
       first = false;
-      parent += indexFormatter(sep);
+      parent += keyFormatter(parentKey);
     }
     parent += "\"];\n";
     s << parent;
     parentnum = num;
 
     for (sharedClique c : clique->children) {
       num++;
-      dot(s, c, indexFormatter, parentnum);
+      dot(s, c, keyFormatter, parentnum);
     }
   }
 

gtsam/inference/inference.i

@@ -4,6 +4,12 @@
 
 namespace gtsam {
 
+// Headers for overloaded methods below, break hierarchy :-/
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+
 #include <gtsam/inference/Key.h>
 
 // Default keyformatter
@@ -98,10 +104,41 @@ class Ordering {
   Ordering();
   Ordering(const gtsam::Ordering& other);
 
-  template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
-                            gtsam::GaussianFactorGraph}>
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
 
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering ColamdConstrainedLast(
+      const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
+      bool forceOrder = false);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering ColamdConstrainedFirst(
+      const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
+      bool forceOrder = false);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
+
+  template <
+      FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
+                                const FACTOR_GRAPH& graph);
+
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
                                 gtsam::DefaultKeyFormatter) const;
@@ -135,12 +172,6 @@ class DotWriter {
 };
 
 #include <gtsam/inference/VariableIndex.h>
-
-// Headers for overloaded methods below, break hierarchy :-/
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/symbolic/SymbolicFactorGraph.h>
-
 class VariableIndex {
   // Standard Constructors and Named Constructors
   VariableIndex();

gtsam/linear/linear.i

@@ -407,8 +407,10 @@ class GaussianFactorGraph {
 
   // Elimination and marginals
   gtsam::GaussianBayesNet* eliminateSequential();
+  gtsam::GaussianBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
   gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
   gtsam::GaussianBayesTree* eliminateMultifrontal();
+  gtsam::GaussianBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);
   gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
   pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
     const gtsam::Ordering& ordering);
@@ -527,6 +529,7 @@ virtual class GaussianBayesNet {
 
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
+  std::pair<Matrix, Vector> matrix() const; 
   gtsam::VectorValues optimizeGradientSearch() const;
   gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
   gtsam::VectorValues gradientAtZero() const;
@@ -556,7 +559,12 @@ virtual class GaussianBayesTree {
   size_t size() const;
   bool empty() const;
   size_t numCachedSeparatorMarginals() const;
-  void saveGraph(string s) const;
+
+  string dot(const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  void saveGraph(string s,
+                const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
 
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimizeGradientSearch() const;

gtsam/nonlinear/nonlinear.i

@@ -98,11 +98,11 @@ class NonlinearFactorGraph {
   string dot(
       const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& formatting = GraphvizFormatting());
+      const GraphvizFormatting& writer = GraphvizFormatting());
   void saveGraph(
       const string& s, const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& formatting = GraphvizFormatting()) const;
+      const GraphvizFormatting& writer = GraphvizFormatting()) const;
 
   // enabling serialization functionality
   void serialize() const;

gtsam/sam/sam.i

@@ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+// between points:
+typedef gtsam::RangeFactor<gtsam::Point2, gtsam::Point2> RangeFactor2;
+typedef gtsam::RangeFactor<gtsam::Point3, gtsam::Point3> RangeFactor3;
+
+// between pose and point:
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
+
+// between poses:
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
+
+// more specialized types:
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3>
     RangeFactorCalibratedCameraPoint;
 typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3>

gtsam/slam/dataset.cpp

@@ -208,15 +208,15 @@ std::map<size_t, Point2> parseVariables<Point2>(const std::string &filename,
 
 /* ************************************************************************* */
 // Interpret noise parameters according to flags
-static SharedNoiseModel
-createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat,
-                 KernelFunctionType kernelFunctionType) {
+static SharedNoiseModel createNoiseModel(
+    const Vector6 &v, bool smart, NoiseFormat noiseFormat,
+    KernelFunctionType kernelFunctionType) {
   if (noiseFormat == NoiseFormatAUTO) {
     // Try to guess covariance matrix layout
-    if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
+    if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 &&  //
         v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) {
       noiseFormat = NoiseFormatGRAPH;
-    } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
+    } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 &&  //
                v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) {
       noiseFormat = NoiseFormatCOV;
     } else {

gtsam/slam/dataset.h

@@ -167,10 +167,11 @@ GTSAM_EXPORT GraphAndValues load2D(
  * @param kernelFunctionType whether to wrap the noise model in a robust kernel
  * @return graph and initial values
  */
-GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
-    SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
-        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
+GTSAM_EXPORT GraphAndValues
+load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
+       size_t maxIndex = 0, bool addNoise = false, bool smart = true,
+       NoiseFormat noiseFormat = NoiseFormatAUTO,  //
+       KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /** save 2d graph */
 GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@@ -185,8 +186,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
  * @param kernelFunctionType whether to wrap the noise model in a robust kernel
  * @return graph and initial values
  */
-GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
-    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
+GTSAM_EXPORT GraphAndValues
+readG2o(const std::string& g2oFile, const bool is3D = false,
+        KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /**
  * @brief This function writes a g2o file from

gtsam/slam/slam.i

@@ -253,17 +253,27 @@ bool writeBAL(string filename, gtsam::SfmData& data);
 gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
 gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
 
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
-    bool addNoise, bool smart);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
-    bool addNoise);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
-    string filename, gtsam::noiseModel::Diagonal* model);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
+enum NoiseFormat {
+  NoiseFormatG2O,
+  NoiseFormatTORO,
+  NoiseFormatGRAPH,
+  NoiseFormatCOV,
+  NoiseFormatAUTO
+};
+
+enum KernelFunctionType {
+  KernelFunctionTypeNONE,
+  KernelFunctionTypeHUBER,
+  KernelFunctionTypeTUKEY
+};
+
+std::pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
+    string filename, gtsam::noiseModel::Diagonal* model = nullptr,
+    size_t maxIndex = 0, bool addNoise = false, bool smart = true,
+    gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormatAUTO,
+    gtsam::KernelFunctionType kernelFunctionType =
+        gtsam::KernelFunctionTypeNONE);
+
 void save2D(const gtsam::NonlinearFactorGraph& graph,
             const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
             string filename);
@@ -290,9 +300,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename);
 
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
 
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
-                                                           bool is3D);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(
+    string filename, const bool is3D = false,
+    gtsam::KernelFunctionType kernelFunctionType =
+        gtsam::KernelFunctionTypeNONE);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
               const gtsam::Values& estimate, string filename);
 

python/gtsam/tests/test_GaussianBayesNet.py

@@ -0,0 +1,53 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Gaussian Bayes Nets.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam import GaussianBayesNet, GaussianConditional
+from gtsam.utils.test_case import GtsamTestCase
+
+# some keys
+_x_ = 11
+_y_ = 22
+_z_ = 33
+
+
+def smallBayesNet():
+    """Create a small Bayes Net for testing"""
+    bayesNet = GaussianBayesNet()
+    I_1x1 = np.eye(1, dtype=float)
+    bayesNet.push_back(GaussianConditional(
+        _x_, [9.0], I_1x1, _y_, I_1x1))
+    bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1))
+    return bayesNet
+
+
+class TestGaussianBayesNet(GtsamTestCase):
+    """Tests for Gaussian Bayes nets."""
+
+    def test_matrix(self):
+        """Test matrix method"""
+        R, d = smallBayesNet().matrix()  # get matrix and RHS
+        R1 = np.array([
+            [1.0, 1.0],
+            [0.0, 1.0]])
+        d1 = np.array([9.0, 5.0])
+        np.testing.assert_equal(R, R1)
+        np.testing.assert_equal(d, d1)
+
+
+if __name__ == '__main__':
+    unittest.main()