Merge pull request #1068 from borglab/fix/inference

borglab · Jan 27, 2022 · 7530623 · 7530623
2 parents 4e5a1b2 + 2fda2a1
commit 7530623
Show file tree

Hide file tree

Showing 7 changed files with 192 additions and 155 deletions.
diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i
@@ -228,19 +228,6 @@ class DiscreteLookupDAG {
   gtsam::DiscreteValues argmax(gtsam::DiscreteValues given) const;
 };
 
-#include <gtsam/inference/DotWriter.h>
-class DotWriter {
-  DotWriter(double figureWidthInches = 5, double figureHeightInches = 5,
-            bool plotFactorPoints = true, bool connectKeysToFactor = true,
-            bool binaryEdges = true);
-
-  double figureWidthInches;
-  double figureHeightInches;
-  bool plotFactorPoints;
-  bool connectKeysToFactor;
-  bool binaryEdges;
-};
-
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 class DiscreteFactorGraph {
   DiscreteFactorGraph();

diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
@@ -0,0 +1,163 @@
+//*************************************************************************
+// inference
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/inference/Key.h>
+
+// Default keyformatter
+void PrintKeyList(
+    const gtsam::KeyList& keys, const string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void PrintKeyVector(
+    const gtsam::KeyVector& keys, const string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+void PrintKeySet(
+    const gtsam::KeySet& keys, const string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+
+#include <gtsam/inference/Symbol.h>
+class Symbol {
+  Symbol();
+  Symbol(char c, uint64_t j);
+  Symbol(size_t key);
+
+  size_t key() const;
+  void print(const string& s = "") const;
+  bool equals(const gtsam::Symbol& expected, double tol) const;
+
+  char chr() const;
+  uint64_t index() const;
+  string string() const;
+};
+
+size_t symbol(char chr, size_t index);
+char symbolChr(size_t key);
+size_t symbolIndex(size_t key);
+
+namespace symbol_shorthand {
+size_t A(size_t j);
+size_t B(size_t j);
+size_t C(size_t j);
+size_t D(size_t j);
+size_t E(size_t j);
+size_t F(size_t j);
+size_t G(size_t j);
+size_t H(size_t j);
+size_t I(size_t j);
+size_t J(size_t j);
+size_t K(size_t j);
+size_t L(size_t j);
+size_t M(size_t j);
+size_t N(size_t j);
+size_t O(size_t j);
+size_t P(size_t j);
+size_t Q(size_t j);
+size_t R(size_t j);
+size_t S(size_t j);
+size_t T(size_t j);
+size_t U(size_t j);
+size_t V(size_t j);
+size_t W(size_t j);
+size_t X(size_t j);
+size_t Y(size_t j);
+size_t Z(size_t j);
+}  // namespace symbol_shorthand
+
+#include <gtsam/inference/LabeledSymbol.h>
+class LabeledSymbol {
+  LabeledSymbol(size_t full_key);
+  LabeledSymbol(const gtsam::LabeledSymbol& key);
+  LabeledSymbol(unsigned char valType, unsigned char label, size_t j);
+
+  size_t key() const;
+  unsigned char label() const;
+  unsigned char chr() const;
+  size_t index() const;
+
+  gtsam::LabeledSymbol upper() const;
+  gtsam::LabeledSymbol lower() const;
+  gtsam::LabeledSymbol newChr(unsigned char c) const;
+  gtsam::LabeledSymbol newLabel(unsigned char label) const;
+
+  void print(string s = "") const;
+};
+
+size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
+unsigned char mrsymbolChr(size_t key);
+unsigned char mrsymbolLabel(size_t key);
+size_t mrsymbolIndex(size_t key);
+
+#include <gtsam/inference/Ordering.h>
+class Ordering {
+  /// Type of ordering to use
+  enum OrderingType { COLAMD, METIS, NATURAL, CUSTOM };
+
+  // Standard Constructors and Named Constructors
+  Ordering();
+  Ordering(const gtsam::Ordering& other);
+
+  template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
+                            gtsam::GaussianFactorGraph}>
+  static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
+
+  // Testable
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::Ordering& ord, double tol) const;
+
+  // Standard interface
+  size_t size() const;
+  size_t at(size_t key) const;
+  void push_back(size_t key);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/inference/DotWriter.h>
+class DotWriter {
+  DotWriter(double figureWidthInches = 5, double figureHeightInches = 5,
+            bool plotFactorPoints = true, bool connectKeysToFactor = true,
+            bool binaryEdges = true);
+
+  double figureWidthInches;
+  double figureHeightInches;
+  bool plotFactorPoints;
+  bool connectKeysToFactor;
+  bool binaryEdges;
+};
+
+#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();
+  // TODO: Templetize constructor when wrap supports it
+  // template<T = {gtsam::FactorGraph}>
+  // VariableIndex(const T& factorGraph, size_t nVariables);
+  // VariableIndex(const T& factorGraph);
+  VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
+  VariableIndex(const gtsam::GaussianFactorGraph& gfg);
+  VariableIndex(const gtsam::NonlinearFactorGraph& fg);
+  VariableIndex(const gtsam::VariableIndex& other);
+
+  // Testable
+  bool equals(const gtsam::VariableIndex& other, double tol) const;
+  void print(string s = "VariableIndex: ",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+
+  // Standard interface
+  size_t size() const;
+  size_t nFactors() const;
+  size_t nEntries() const;
+};
+
+}  // namespace gtsam
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -23,121 +23,9 @@ namespace gtsam {
 #include <gtsam/geometry/SOn.h>
 #include <gtsam/geometry/StereoPoint2.h>
 #include <gtsam/geometry/Unit3.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/NavState.h>
 
-class Symbol {
-  Symbol();
-  Symbol(char c, uint64_t j);
-  Symbol(size_t key);
-
-  size_t key() const;
-  void print(const string& s = "") const;
-  bool equals(const gtsam::Symbol& expected, double tol) const;
-
-  char chr() const;
-  uint64_t index() const;
-  string string() const;
-};
-
-size_t symbol(char chr, size_t index);
-char symbolChr(size_t key);
-size_t symbolIndex(size_t key);
-
-namespace symbol_shorthand {
-size_t A(size_t j);
-size_t B(size_t j);
-size_t C(size_t j);
-size_t D(size_t j);
-size_t E(size_t j);
-size_t F(size_t j);
-size_t G(size_t j);
-size_t H(size_t j);
-size_t I(size_t j);
-size_t J(size_t j);
-size_t K(size_t j);
-size_t L(size_t j);
-size_t M(size_t j);
-size_t N(size_t j);
-size_t O(size_t j);
-size_t P(size_t j);
-size_t Q(size_t j);
-size_t R(size_t j);
-size_t S(size_t j);
-size_t T(size_t j);
-size_t U(size_t j);
-size_t V(size_t j);
-size_t W(size_t j);
-size_t X(size_t j);
-size_t Y(size_t j);
-size_t Z(size_t j);
-}  // namespace symbol_shorthand
-
-// Default keyformatter
-void PrintKeyList(
-    const gtsam::KeyList& keys, const string& s = "",
-    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
-void PrintKeyVector(
-    const gtsam::KeyVector& keys, const string& s = "",
-    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
-void PrintKeySet(
-    const gtsam::KeySet& keys, const string& s = "",
-    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
-
-#include <gtsam/inference/LabeledSymbol.h>
-class LabeledSymbol {
-  LabeledSymbol(size_t full_key);
-  LabeledSymbol(const gtsam::LabeledSymbol& key);
-  LabeledSymbol(unsigned char valType, unsigned char label, size_t j);
-
-  size_t key() const;
-  unsigned char label() const;
-  unsigned char chr() const;
-  size_t index() const;
-
-  gtsam::LabeledSymbol upper() const;
-  gtsam::LabeledSymbol lower() const;
-  gtsam::LabeledSymbol newChr(unsigned char c) const;
-  gtsam::LabeledSymbol newLabel(unsigned char label) const;
-
-  void print(string s = "") const;
-};
-
-size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
-unsigned char mrsymbolChr(size_t key);
-unsigned char mrsymbolLabel(size_t key);
-size_t mrsymbolIndex(size_t key);
-
-#include <gtsam/inference/Ordering.h>
-class Ordering {
-  /// Type of ordering to use
-  enum OrderingType {
-    COLAMD, METIS, NATURAL, CUSTOM
-  };
-
-  // Standard Constructors and Named Constructors
-  Ordering();
-  Ordering(const gtsam::Ordering& other);
-
-  template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
-                            gtsam::GaussianFactorGraph}>
-  static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
-
-  // Testable
-  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                gtsam::DefaultKeyFormatter) const;
-  bool equals(const gtsam::Ordering& ord, double tol) const;
-
-  // Standard interface
-  size_t size() const;
-  size_t at(size_t key) const;
-  void push_back(size_t key);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
 #include <gtsam/nonlinear/GraphvizFormatting.h>
 class GraphvizFormatting : gtsam::DotWriter {
   GraphvizFormatting();

diff --git a/gtsam/symbolic/symbolic.i b/gtsam/symbolic/symbolic.i
@@ -3,11 +3,6 @@
 //*************************************************************************
 namespace gtsam {
 
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-
-// ###################
-
 #include <gtsam/symbolic/SymbolicFactor.h>
 virtual class SymbolicFactor {
   // Standard Constructors and Named Constructors
@@ -173,29 +168,4 @@ class SymbolicBayesTreeClique {
   void deleteCachedShortcuts();
 };
 
-#include <gtsam/inference/VariableIndex.h>
-class VariableIndex {
-  // Standard Constructors and Named Constructors
-  VariableIndex();
-  // TODO: Templetize constructor when wrap supports it
-  // template<T = {gtsam::FactorGraph}>
-  // VariableIndex(const T& factorGraph, size_t nVariables);
-  // VariableIndex(const T& factorGraph);
-  VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
-  VariableIndex(const gtsam::GaussianFactorGraph& gfg);
-  VariableIndex(const gtsam::NonlinearFactorGraph& fg);
-  VariableIndex(const gtsam::VariableIndex& other);
-
-  // Testable
-  bool equals(const gtsam::VariableIndex& other, double tol) const;
-  void print(string s = "VariableIndex: ",
-             const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
-
-  // Standard interface
-  size_t size() const;
-  size_t nFactors() const;
-  size_t nEntries() const;
-};
-
 }  // namespace gtsam
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
@@ -53,6 +53,7 @@ set(ignore
 set(interface_headers
     ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i
     ${PROJECT_SOURCE_DIR}/gtsam/base/base.i
+    ${PROJECT_SOURCE_DIR}/gtsam/inference/inference.i
     ${PROJECT_SOURCE_DIR}/gtsam/discrete/discrete.i
     ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i

diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h
@@ -0,0 +1,15 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+
+#include <pybind11/stl.h>
+
diff --git a/python/gtsam/specializations/inference.h b/python/gtsam/specializations/inference.h
@@ -0,0 +1,13 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like
+ * interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
+ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
+ * and saves one copy operation.
+ */
+