openspacelabs · amadoantonini · Jul 19, 2022 · Jul 19, 2022 · Jul 19, 2022
diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h
@@ -19,8 +19,6 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
-using namespace gtsam;
-
 namespace gtsam {
 
 using JacobianVector = std::vector<Matrix>;

diff --git a/gtsam/nonlinear/custom.i b/gtsam/nonlinear/custom.i
@@ -0,0 +1,38 @@
+//*************************************************************************
+// Custom Factor wrapping
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor : gtsam::NoiseModelFactor {
+  /*
+   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
+   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
+   * ignore list in `matlab/CMakeLists.txt`.
+   */
+  CustomFactor();
+  /*
+   * Example:
+   * ```
+   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+   *    <calculated error>
+   *    if not H is None:
+   *        <calculate the Jacobian>
+   *        H[0] = J1 # 2-d numpy array for a Jacobian block
+   *        H[1] = J2
+   *        ...
+   *    return error # 1-d numpy array
+   *
+   * cf = CustomFactor(noise_model, keys, error_func)
+   * ```
+   */
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
+               const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "",
+             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
+}
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -99,11 +99,11 @@ class NonlinearFactorGraph {
   string dot(
       const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& writer = GraphvizFormatting());
+      const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting());
   void saveGraph(
       const string& s, const gtsam::Values& values,
       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
-      const GraphvizFormatting& writer = GraphvizFormatting()) const;
+      const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor {
   Vector whitenedError(const gtsam::Values& x) const;
 };
 
-#include <gtsam/nonlinear/CustomFactor.h>
-virtual class CustomFactor : gtsam::NoiseModelFactor {
-  /*
-   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting
-   * machinery there. This is achieved by adding `gtsam::CustomFactor` to the
-   * ignore list in `matlab/CMakeLists.txt`.
-   */
-  CustomFactor();
-  /*
-   * Example:
-   * ```
-   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
-   *    <calculated error>
-   *    if not H is None:
-   *        <calculate the Jacobian>
-   *        H[0] = J1 # 2-d numpy array for a Jacobian block
-   *        H[1] = J2
-   *        ...
-   *    return error # 1-d numpy array
-   *
-   * cf = CustomFactor(noise_model, keys, error_func)
-   * ```
-   */
-  CustomFactor(const gtsam::SharedNoiseModel& noiseModel,
-               const gtsam::KeyVector& keys,
-               const gtsam::CustomErrorFunction& errorFunction);
-
-  void print(string s = "",
-             gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
-};
-
 #include <gtsam/nonlinear/Values.h>
 class Values {
   Values();
@@ -554,7 +523,7 @@ virtual class GncParams {
   GncParams(const PARAMS& baseOptimizerParams);
   GncParams();
   BaseOptimizerParameters baseOptimizerParams;
-  GncLossType lossType;
+  gtsam::GncLossType lossType;
   size_t maxIterations;
   double muStep;
   double relativeCostTol;
@@ -563,12 +532,12 @@ virtual class GncParams {
   std::vector<size_t> knownInliers;
   std::vector<size_t> knownOutliers;
 
-  void setLossType(const GncLossType type);
+  void setLossType(const gtsam::GncLossType type);
   void setMaxIterations(const size_t maxIter);
   void setMuStep(const double step);
   void setRelativeCostTol(double value);
   void setWeightsTol(double value);
-  void setVerbosityGNC(const This::Verbosity value);
+  void setVerbosityGNC(const gtsam::This::Verbosity value);
   void setKnownInliers(const std::vector<size_t>& knownIn);
   void setKnownOutliers(const std::vector<size_t>& knownOut);
   void print(const string& str = "GncParams: ") const;
@@ -900,7 +869,7 @@ template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::PinholeCamera<gtsam::Cal3Unified>,
                gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
-  NonlinearEquality2(Key key1, Key key2, double mu = 1e4);
+  NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4);
   gtsam::Vector evaluateError(const T& x1, const T& x2);
 };
 

diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt
@@ -73,6 +73,7 @@ set(interface_files
     ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
     ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i
     ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
     ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
     ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
@@ -61,6 +61,7 @@ set(interface_headers
     ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i
     ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i
     ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i
     ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i
     ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i
     ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i

diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h
@@ -0,0 +1,12 @@
+/* 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++.
+ */
diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h
@@ -10,3 +10,4 @@
  * 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/custom.h b/python/gtsam/specializations/custom.h
@@ -0,0 +1,12 @@
+/* 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.
+ */