diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d053c8422..fa0a5c499 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2166,6 +2166,34 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor { Vector whitenedError(const gtsam::Values& x) const; }; +#include +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]): + * + * if not H is None: + * + * 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 class Values { Values(); diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp new file mode 100644 index 000000000..e33caed6f --- /dev/null +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CustomFactor.cpp + * @brief Class to enable arbitrary factors with runtime swappable error function. + * @author Fan Jiang + */ + +#include + +namespace gtsam { + +/* + * Calculates the unwhitened error by invoking the callback functor (i.e. from Python). + */ +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. + * As the type `std::vector` has been marked as opaque in `preamble.h`, any changes in + * Python will be immediately reflected on the C++ side. + * + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 + * H[1] = J2 + * ... + * return error + * ``` + */ + return this->error_function_(*this, x, H.get_ptr()); + } else { + /* + * 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); + } + } else { + return Vector::Zero(this->dim()); + } +} + +void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const { + std::cout << s << "CustomFactor on "; + auto keys_ = this->keys(); + bool f = false; + for (const Key &k: keys_) { + if (f) + std::cout << ", "; + std::cout << keyFormatter(k); + f = true; + } + std::cout << "\n"; + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; +} + +} diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h new file mode 100644 index 000000000..dbaf31898 --- /dev/null +++ b/gtsam/nonlinear/CustomFactor.h @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CustomFactor.h + * @brief Class to enable arbitrary factors with runtime swappable error function. + * @author Fan Jiang + */ + +#pragma once + +#include + +using namespace gtsam; + +namespace gtsam { + +using JacobianVector = std::vector; + +class CustomFactor; + +/* + * NOTE + * ========== + * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected. + * + * 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; + +/** + * @brief Custom factor that takes a std::function as the error + * @addtogroup nonlinear + * \nosubgrouping + * + * This factor is mainly for creating a custom factor in Python. + */ +class CustomFactor: public NoiseModelFactor { +protected: + CustomErrorFunction error_function_; + +protected: + + using Base = NoiseModelFactor; + using This = CustomFactor; + +public: + + /** + * Default Constructor for I/O + */ + CustomFactor() = default; + + /** + * Constructor + * @param noiseModel shared pointer to noise model + * @param keys keys of the variables + * @param errorFunction the error functional + */ + CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) : + Base(noiseModel, keys) { + this->error_function_ = errorFunction; + } + + ~CustomFactor() override = default; + + /** + * Calls the errorFunction closure, which is a std::function object + * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array + */ + 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; + + /** + * 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*/) { + ar & boost::serialization::make_nvp("CustomFactor", + boost::serialization::base_object(*this)); + } +}; + +}; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 6deaaf7fe..7fafd95df 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: */ virtual 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/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 3c234d106..28e7cce6e 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -61,7 +61,8 @@ endif() # ignoring the non-concrete types (type aliases) set(ignore gtsam::Point2 - gtsam::Point3) + gtsam::Point3 + gtsam::CustomFactor) # Wrap matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" diff --git a/python/CustomFactors.md b/python/CustomFactors.md new file mode 100644 index 000000000..a6ffa2f36 --- /dev/null +++ b/python/CustomFactors.md @@ -0,0 +1,111 @@ +# GTSAM Python-based factors + +One now can build factors purely in Python using the `CustomFactor` factor. + +## Usage + +In order to use a Python-based factor, one needs to have a Python function with the following signature: + +```python +import gtsam +import numpy as np +from typing import List + +def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + ... +``` + +`this` is a reference to the `CustomFactor` object. This is required because one can reuse the same +`error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of +**references** to the list of required Jacobians (see the corresponding C++ documentation). + +If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error` +method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`, +each entry of `H` can be assigned a `numpy` array, as the Jacobian for the corresponding variable. + +After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM: + +```python +noise_model = gtsam.noiseModel.Unit.Create(3) +# constructor(, , ) +cf = gtsam.CustomFactor(noise_model, [X(0), X(1)], error_func) +``` + +## Example + +The following is a simple `BetweenFactor` implemented in Python. + +```python +import gtsam +import numpy as np +from typing import List + +expected = Pose2(2, 2, np.pi / 2) + +def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """ + Error function that mimics a BetweenFactor + :param this: reference to the current CustomFactor being evaluated + :param v: Values object + :param H: list of references to the Jacobian arrays + :return: the non-linear error + """ + key0 = this.keys()[0] + key1 = this.keys()[1] + gT1, gT2 = v.atPose2(key0), v.atPose2(key1) + error = expected.localCoordinates(gT1.between(gT2)) + + if H is not None: + result = gT1.between(gT2) + H[0] = -result.inverse().AdjointMap() + H[1] = np.eye(3) + return error + +noise_model = gtsam.noiseModel.Unit.Create(3) +cf = gtsam.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) +``` + +In general, the Python-based factor works just like their C++ counterparts. + +## Known Issues + +Because of the `pybind11`-based translation, the performance of `CustomFactor` is not guaranteed. +Also, because `pybind11` needs to lock the Python GIL lock for evaluation of each factor, parallel +evaluation of `CustomFactor` is not possible. + +## Implementation + +`CustomFactor` is a `NonlinearFactor` that has a `std::function` as its callback. +This callback can be translated to a Python function call, thanks to `pybind11`'s functional support. + +The constructor of `CustomFactor` is +```c++ +/** +* Constructor +* @param noiseModel shared pointer to noise model +* @param keys keys of the variables +* @param errorFunction the error functional +*/ +CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) : + Base(noiseModel, keys) { + this->error_function_ = errorFunction; +} +``` + +At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object. + +Something worth special mention is this: +```c++ +/* + * NOTE + * ========== + * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected. + * + * 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; +``` + +which is not documented in `pybind11` docs. One needs to be aware of this if they wanted to implement similar +"mutable" arguments going across the Python-C++ boundary. diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py new file mode 100644 index 000000000..c7fe1e202 --- /dev/null +++ b/python/gtsam/examples/CustomFactorExample.py @@ -0,0 +1,179 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +CustomFactor demo that simulates a 1-D sensor fusion task. +Author: Fan Jiang, Frank Dellaert +""" + +import gtsam +import numpy as np + +from typing import List, Optional +from functools import partial + + +def simulate_car(): + # 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)] + + return x + + +x = simulate_car() +print(f"Simulated car trajectory: {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(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): + """GPS Factor error function + :param measurement: GPS measurement, to be filled with `partial` + :param this: gtsam.CustomFactor handle + :param values: gtsam.Values + :param jacobians: Optional list of Jacobians + :return: the unwhitened error + """ + key = this.keys()[0] + estimate = values.atVector(key) + error = estimate - measurement + if jacobians is not None: + jacobians[0] = I + + return error + + +# Add the GPS factors +for k in range(5): + gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]]))) + factor_graph.add(gf) + +# New Values container +v = gtsam.Values() + +# Add initial estimates to the Values container +for i in range(5): + v.insert(unknown[i], np.array([0.0])) + +# Initialize optimizer +params = gtsam.GaussNewtonParams() +optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + +# Optimize the factor graph +result = optimizer.optimize() + +# calculate the error from ground truth +error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + +print("Result with only GPS") +print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") + +# Adding odometry will improve things a lot +odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) + + +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): + """Odometry Factor error function + :param measurement: Odometry measurement, to be filled with `partial` + :param this: gtsam.CustomFactor handle + :param values: gtsam.Values + :param jacobians: Optional list of Jacobians + :return: the unwhitened error + """ + key1 = this.keys()[0] + key2 = this.keys()[1] + pos1, pos2 = values.atVector(key1), values.atVector(key2) + error = measurement - (pos1 - pos2) + if jacobians is not None: + jacobians[0] = I + jacobians[1] = -I + + return error + + +for k in range(4): + odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]]))) + factor_graph.add(odof) + +params = gtsam.GaussNewtonParams() +optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + +result = optimizer.optimize() + +error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + +print("Result with GPS+Odometry") +print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") + +# This is great, but GPS noise is still apparent, so now we add the two landmarks +lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) + + +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): + """Landmark Factor error function + :param measurement: Landmark measurement, to be filled with `partial` + :param this: gtsam.CustomFactor handle + :param values: gtsam.Values + :param jacobians: Optional list of Jacobians + :return: the unwhitened error + """ + key = this.keys()[0] + pos = values.atVector(key) + error = pos - measurement + if jacobians is not None: + jacobians[0] = I + + return error + + +factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0])))) +factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3])))) + +params = gtsam.GaussNewtonParams() +optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + +result = optimizer.optimize() + +error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + +print("Result with GPS+Odometry+Landmark") +print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index b56766c72..7294a6ac8 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -1,14 +1,30 @@ -// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html -// These are required to save one copy operation on Python calls +/* 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++. + */ #ifdef GTSAM_ALLOCATOR_TBB PYBIND11_MAKE_OPAQUE(std::vector>); #else PYBIND11_MAKE_OPAQUE(std::vector); #endif PYBIND11_MAKE_OPAQUE(std::vector >); +PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector + +// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this. +namespace std { + using gtsam::operator<<; +} diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 98143160e..be8eb8a6c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -1,10 +1,25 @@ -// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html -// These are required to save one copy operation on Python calls +/* 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++. + * + * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but + * without the `` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this + * allows the types to be modified with Python, and saves one copy operation. + */ #ifdef GTSAM_ALLOCATOR_TBB py::bind_vector > >(m_, "KeyVector"); +py::implicitly_convertible > >(); #else py::bind_vector >(m_, "KeyVector"); +py::implicitly_convertible >(); #endif + py::bind_vector > >(m_, "Point2Vector"); py::bind_vector >(m_, "Point3Pairs"); py::bind_vector >(m_, "Pose3Pairs"); @@ -17,3 +32,4 @@ py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector > >(m_, "CameraSetCal3_S2"); py::bind_vector > >(m_, "CameraSetCal3Bundler"); +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py new file mode 100644 index 000000000..4f0f33361 --- /dev/null +++ b/python/gtsam/tests/test_custom_factor.py @@ -0,0 +1,207 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +CustomFactor unit tests. +Author: Fan Jiang +""" +from typing import List +import unittest +from gtsam import Values, Pose2, CustomFactor + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestCustomFactor(GtsamTestCase): + def test_new(self): + """Test the creation of a new CustomFactor""" + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """Minimal error function stub""" + return np.array([1, 0, 0]) + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) + + def test_new_keylist(self): + """Test the creation of a new CustomFactor""" + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """Minimal error function stub""" + return np.array([1, 0, 0]) + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, [0], error_func) + + def test_call(self): + """Test if calling the factor works (only error)""" + expected_pose = Pose2(1, 1, 0) + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: + """Minimal error function with no Jacobian""" + key0 = this.keys()[0] + error = -v.atPose2(key0).localCoordinates(expected_pose) + return error + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, [0], error_func) + v = Values() + v.insert(0, Pose2(1, 0, 0)) + e = cf.error(v) + + self.assertEqual(e, 0.5) + + def test_jacobian(self): + """Tests if the factor result matches the GTSAM Pose2 unit test""" + + gT1 = Pose2(1, 2, np.pi / 2) + gT2 = Pose2(-1, 4, np.pi) + + expected = Pose2(2, 2, np.pi / 2) + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """ + the custom error function. One can freely use variables captured + from the outside scope. Or the variables can be acquired by indexing `v`. + Jacobian is passed by modifying the H array of numpy matrices. + """ + + key0 = this.keys()[0] + key1 = this.keys()[1] + gT1, gT2 = v.atPose2(key0), v.atPose2(key1) + error = expected.localCoordinates(gT1.between(gT2)) + + if H is not None: + result = gT1.between(gT2) + H[0] = -result.inverse().AdjointMap() + H[1] = np.eye(3) + return error + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) + v = Values() + v.insert(0, gT1) + v.insert(1, gT2) + + bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model) + + gf = cf.linearize(v) + gf_b = bf.linearize(v) + + J_cf, b_cf = gf.jacobian() + J_bf, b_bf = gf_b.jacobian() + np.testing.assert_allclose(J_cf, J_bf) + np.testing.assert_allclose(b_cf, b_bf) + + def test_printing(self): + """Tests if the factor result matches the GTSAM Pose2 unit test""" + gT1 = Pose2(1, 2, np.pi / 2) + gT2 = Pose2(-1, 4, np.pi) + + def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): + """Minimal error function stub""" + return np.array([1, 0, 0]) + + noise_model = gtsam.noiseModel.Unit.Create(3) + from gtsam.symbol_shorthand import X + cf = CustomFactor(noise_model, [X(0), X(1)], error_func) + + cf_string = """CustomFactor on x0, x1 + noise model: unit (3) +""" + self.assertEqual(cf_string, repr(cf)) + + def test_no_jacobian(self): + """Tests that we will not calculate the Jacobian if not requested""" + + gT1 = Pose2(1, 2, np.pi / 2) + gT2 = Pose2(-1, 4, np.pi) + + expected = Pose2(2, 2, np.pi / 2) + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """ + Error function that mimics a BetweenFactor + :param this: reference to the current CustomFactor being evaluated + :param v: Values object + :param H: list of references to the Jacobian arrays + :return: the non-linear error + """ + key0 = this.keys()[0] + key1 = this.keys()[1] + gT1, gT2 = v.atPose2(key0), v.atPose2(key1) + error = expected.localCoordinates(gT1.between(gT2)) + + self.assertTrue(H is None) # Should be true if we only request the error + + if H is not None: + result = gT1.between(gT2) + H[0] = -result.inverse().AdjointMap() + H[1] = np.eye(3) + return error + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) + v = Values() + v.insert(0, gT1) + v.insert(1, gT2) + + bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model) + + e_cf = cf.error(v) + e_bf = bf.error(v) + np.testing.assert_allclose(e_cf, e_bf) + + def test_optimization(self): + """Tests if a factor graph with a CustomFactor can be properly optimized""" + gT1 = Pose2(1, 2, np.pi / 2) + gT2 = Pose2(-1, 4, np.pi) + + expected = Pose2(2, 2, np.pi / 2) + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + """ + Error function that mimics a BetweenFactor + :param this: reference to the current CustomFactor being evaluated + :param v: Values object + :param H: list of references to the Jacobian arrays + :return: the non-linear error + """ + key0 = this.keys()[0] + key1 = this.keys()[1] + gT1, gT2 = v.atPose2(key0), v.atPose2(key1) + error = expected.localCoordinates(gT1.between(gT2)) + + if H is not None: + result = gT1.between(gT2) + H[0] = -result.inverse().AdjointMap() + H[1] = np.eye(3) + return error + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, [0, 1], error_func) + + fg = gtsam.NonlinearFactorGraph() + fg.add(cf) + fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model)) + + v = Values() + v.insert(0, Pose2(0, 0, 0)) + v.insert(1, Pose2(0, 0, 0)) + + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params) + result = optimizer.optimize() + + self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5) + self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5) + + +if __name__ == "__main__": + unittest.main()