From 866d6b1fa1a68a8f4afdb084178fda07493d914d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 14 May 2021 16:24:31 -0400 Subject: [PATCH] Working CustomFactor --- gtsam/gtsam.i | 6 ++ gtsam/nonlinear/CustomFactor.cpp | 35 ++++++++++ gtsam/nonlinear/CustomFactor.h | 84 +++++++++++++++++++++++ matlab/CMakeLists.txt | 3 +- python/gtsam/preamble.h | 8 +++ python/gtsam/specializations.h | 1 + python/gtsam/tests/test_custom_factor.py | 86 ++++++++++++++++++++++++ 7 files changed, 222 insertions(+), 1 deletion(-) create mode 100644 gtsam/nonlinear/CustomFactor.cpp create mode 100644 gtsam/nonlinear/CustomFactor.h create mode 100644 python/gtsam/tests/test_custom_factor.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d053c8422..295d5237f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2166,6 +2166,12 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor { Vector whitenedError(const gtsam::Values& x) const; }; +#include +virtual class CustomFactor: gtsam::NoiseModelFactor { + CustomFactor(); + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction); +}; + #include class Values { Values(); diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp new file mode 100644 index 000000000..0f2c542bc --- /dev/null +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + + * 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 { + +Vector CustomFactor::unwhitenedError(const Values& x, boost::optional&> H) const { + if(this->active(x)) { + if(H) { + return this->errorFunction(*this, x, H.get_ptr()); + } else { + JacobianVector dummy; + return this->errorFunction(*this, x, &dummy); + } + } else { + return Vector::Zero(this->dim()); + } +} + +} diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h new file mode 100644 index 000000000..9d9db9eda --- /dev/null +++ b/gtsam/nonlinear/CustomFactor.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 { + +typedef std::vector JacobianVector; + +class CustomFactor; + +typedef std::function CustomErrorFunction; + +/** + * @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 { +public: + CustomErrorFunction errorFunction; + +protected: + + typedef NoiseModelFactor Base; + typedef CustomFactor This; + +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->errorFunction = 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; + +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/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/gtsam/preamble.h b/python/gtsam/preamble.h index b56766c72..dd1b342eb 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -6,9 +6,17 @@ PYBIND11_MAKE_OPAQUE(std::vector>); 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..c7f45fc0f 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -17,3 +17,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..2ad7b929d --- /dev/null +++ b/python/gtsam/tests/test_custom_factor.py @@ -0,0 +1,86 @@ +""" +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): + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + return np.array([1, 0, 0]) + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) + + def test_call(self): + + expected_pose = Pose2(1, 1, 0) + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: + key0 = this.keys()[0] + error = -v.atPose2(key0).localCoordinates(expected_pose) + return error + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, gtsam.KeyVector([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]): + # print(f"{this = },\n{v = },\n{len(H) = }") + + key0 = this.keys()[0] + key1 = this.keys()[1] + gT1, gT2 = v.atPose2(key0), v.atPose2(key1) + error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2)) + + if len(H) > 0: + 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, Pose2(0, 0, 0), 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) + +if __name__ == "__main__": + unittest.main()