From 880d5b57af73e3d6d59eacb89d2284cfc20dd955 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 5 Jun 2021 00:18:45 -0400 Subject: [PATCH] Fixed Python factor for TBB --- gtsam/nonlinear/CustomFactor.cpp | 3 +- gtsam/nonlinear/CustomFactor.h | 18 +++-- gtsam/nonlinear/NonlinearFactor.h | 10 ++- gtsam/nonlinear/NonlinearFactorGraph.cpp | 12 +++- python/gtsam/examples/CustomFactorExample.py | 75 ++++++++++++++++++++ 5 files changed, 109 insertions(+), 9 deletions(-) create mode 100644 python/gtsam/examples/CustomFactorExample.py diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index a6d6f1f7b..e33caed6f 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -24,6 +24,7 @@ namespace gtsam { */ Vector CustomFactor::unwhitenedError(const Values& x, boost::optional&> H) const { if(this->active(x)) { + if(H) { /* * In this case, we pass the raw pointer to the `std::vector` object directly to pybind. @@ -45,7 +46,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optionalerror_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); diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 7ee5f1f77..dbaf31898 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/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; +using CustomErrorFunction = std::function; /** * @brief Custom factor that takes a std::function as the error @@ -46,7 +46,7 @@ using CustomErrorFunction = std::functionerror_function_ = errorFunction; } @@ -80,16 +80,22 @@ public: Vector unwhitenedError(const Values &x, boost::optional &> 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 - 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(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 21c05dc2c..5c61bf7dc 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/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 diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 6a9e0cd0a..42fe5ae57 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -325,7 +325,7 @@ public: // Operator that linearizes a given range of the factors void operator()(const tbb::blocked_range& 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(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()); diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py new file mode 100644 index 000000000..562951ae5 --- /dev/null +++ b/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)