Fixed Python factor for TBB

gtsam/nonlinear/CustomFactor.cpp

@@ -24,6 +24,7 @@ namespace gtsam {
  */
 Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
   if(this->active(x)) {
+
     if(H) {
       /*
        * In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
@@ -45,7 +46,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vecto
       return this->error_function_(*this, x, H.get_ptr());
     } else {
       /*
-       * In this case, we pass the a `nullptr` to pybind, and it will translated to `None` in Python.
+       * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.
        * Users can check for `None` in their callback to determine if the Jacobian is requested.
        */
       return this->error_function_(*this, x, nullptr);

gtsam/nonlinear/CustomFactor.h

@@ -35,7 +35,7 @@ class CustomFactor;
  * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
  * Thus the pointer will never be invalidated.
  */
-using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;
+using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
 
 /**
  * @brief Custom factor that takes a std::function as the error
@@ -46,7 +46,7 @@ using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Valu
  */
 class CustomFactor: public NoiseModelFactor {
 protected:
-   CustomErrorFunction error_function_;
+  CustomErrorFunction error_function_;
 
 protected:
 
@@ -66,7 +66,7 @@ public:
    * @param keys keys of the variables
    * @param errorFunction the error functional
    */
-  CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :
+  CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
       Base(noiseModel, keys) {
     this->error_function_ = errorFunction;
   }
@@ -80,16 +80,22 @@ public:
   Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
 
   /** print */
-  void print(const std::string& s,
-             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
+  void print(const std::string &s,
+             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
 
+  /**
+   * Mark not sendable
+   */
+  bool sendable() const override {
+    return false;
+  }
 
 private:
 
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
     ar & boost::serialization::make_nvp("CustomFactor",
                                         boost::serialization::base_object<Base>(*this));
   }

gtsam/nonlinear/NonlinearFactor.h

@@ -95,7 +95,7 @@ public:
 
   /**
    * Checks whether a factor should be used based on a set of values.
-   * This is primarily used to implment inequality constraints that
+   * This is primarily used to implement inequality constraints that
    * require a variable active set. For all others, the default implementation
    * returning true solves this problem.
    *
@@ -134,6 +134,14 @@ public:
    */
   shared_ptr rekey(const KeyVector& new_keys) const;
 
+  /**
+   * Should the factor be evaluated in the same thread as the caller
+   * This is to enable factors that has shared states (like the Python GIL lock)
+   */
+   virtual bool sendable() const {
+    return true;
+  }
+
 }; // \class NonlinearFactor
 
 /// traits

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -325,7 +325,7 @@ public:
   // Operator that linearizes a given range of the factors
   void operator()(const tbb::blocked_range<size_t>& blocked_range) const {
     for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) {
-      if (nonlinearGraph_[i])
+      if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable())
         result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_);
       else
         result_[i] = GaussianFactor::shared_ptr();
@@ -348,9 +348,19 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
 
   linearFG->resize(size());
   TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
+
+  // First linearize all sendable factors
   tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
     _LinearizeOneFactor(*this, linearizationPoint, *linearFG));
 
+  // Linearize all non-sendable factors
+  for(int i = 0; i < size(); i++) {
+    auto& factor = (*this)[i];
+    if(factor && !(factor->sendable())) {
+      (*linearFG)[i] = factor->linearize(linearizationPoint);
+    }
+  }
+
 #else
 
   linearFG->reserve(size());

python/gtsam/examples/CustomFactorExample.py

@@ -0,0 +1,75 @@
+import gtsam
+import numpy as np
+
+from typing import List, Optional
+from functools import partial
+
+# Simulate a car for one second
+x0 = 0
+dt = 0.25  # 4 Hz, typical GPS
+v = 144 * 1000 / 3600  # 144 km/hour = 90mph, pretty fast
+x = [x0 + v * dt * i for i in range(5)]
+print(x)
+
+# %%
+add_noise = True  # set this to False to run with "perfect" measurements
+
+# GPS measurements
+sigma_gps = 3.0  # assume GPS is +/- 3m
+g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
+     for k in range(5)]
+
+# Odometry measurements
+sigma_odo = 0.1  # assume Odometry is 10cm accurate at 4Hz
+o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
+     for k in range(4)]
+
+# Landmark measurements:
+sigma_lm = 1  # assume landmark measurement is accurate up to 1m
+
+# Assume first landmark is at x=5, we measure it at time k=0
+lm_0 = 5.0
+z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
+
+# Assume other landmark is at x=28, we measure it at time k=3
+lm_3 = 28.0
+z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
+
+unknown = [gtsam.symbol('x', k) for k in range(5)]
+
+print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
+
+# We now can use nonlinear factor graphs
+factor_graph = gtsam.NonlinearFactorGraph()
+
+# Add factors for GPS measurements
+I = np.eye(1)
+gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
+
+
+def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray):
+    key = this.keys()[0]
+    estimate = values.atVector(key)
+    error = measurement - estimate
+    if jacobians is not None:
+        jacobians[0] = -I
+
+    return error
+
+
+for k in range(5):
+    factor_graph.add(gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, measurement=np.array([g[k]]))))
+
+v = gtsam.Values()
+
+for i in range(5):
+    v.insert(unknown[i], np.array([0.0]))
+
+linearized: gtsam.GaussianFactorGraph = factor_graph.linearize(v)
+print(linearized.at(1))
+
+params = gtsam.GaussNewtonParams()
+optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
+
+result = optimizer.optimize()
+print(result)