From 81f1d9315840e0638da10c1397a73cec90b0744e Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 1 Dec 2021 16:04:49 -0500 Subject: [PATCH 01/40] NoiseModelFactorN - fixed-number of variables >6 --- gtsam/mainpage.dox | 2 +- gtsam/nonlinear/NonlinearFactor.h | 106 ++++++++++++++++++++++++++++++ tests/testNonlinearFactor.cpp | 44 +++++++++++++ 3 files changed, 151 insertions(+), 1 deletion(-) diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index ee7bd9924c..e07f45f079 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -16,7 +16,7 @@ To use GTSAM to solve your own problems, you will often have to create new facto -# The number of variables your factor involves is unknown at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError() - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel). --# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError() +-# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError(). If the number of variables is greater than 6, derive from NoiseModelFactorN. - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor. -# Derive from NonlinearFactor - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor. diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 7fafd95dfa..503ae7d58f 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { @@ -770,5 +771,110 @@ class NoiseModelFactor6: public NoiseModelFactor { }; // \class NoiseModelFactor6 /* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with N + * variables. To derive from this class, implement evaluateError(). */ +template +class NoiseModelFactorN: public NoiseModelFactor { + +protected: + + typedef NoiseModelFactor Base; + typedef NoiseModelFactorN This; + + /* "Dummy templated" alias is used to expand fixed-type parameter packs with + * same length as VALUES. This ignores the template parameter. */ + template + using optional_matrix_type = boost::optional; + + /* "Dummy templated" alias is used to expand fixed-type parameter packs with + * same length as VALUES. This ignores the template parameter. */ + template + using key_type = Key; + +public: + + /** + * Default Constructor for I/O + */ + NoiseModelFactorN() {} + + /** + * Constructor. + * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) + * @param noiseModel shared pointer to noise model + * @param keys... keys for the variables in this factor + */ + NoiseModelFactorN(const SharedNoiseModel& noiseModel, + key_type... keys) + : Base(noiseModel, std::array{keys...}) {} + + /** + * Constructor. + * @param noiseModel shared pointer to noise model + * @param keys a container of keys for the variables in this factor + */ + template + NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) + : Base(noiseModel, keys) { + assert(std::size(keys) == sizeof...(VALUES)); + } + + ~NoiseModelFactorN() override {} + + /** Method to retrieve keys */ + template + inline Key key() const { return keys_[N]; } + + /** Calls the n-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + Vector unwhitenedError( + const Values& x, + boost::optional&> H = boost::none) const override { + return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); + } + + /** + * Override this method to finish implementing an n-way factor. + * If any of the optional Matrix reference arguments are specified, it should + * compute both the function evaluation and its derivative(s) in the requested + * variables. + */ + virtual Vector evaluateError( + const VALUES& ... x, + optional_matrix_type ... H) const = 0; + + /** No-jacobians requested function overload (since parameter packs can't have + * default args) */ + Vector evaluateError(const VALUES&... x) const { + return evaluateError(x..., optional_matrix_type()...); + } + + private: + + /** Pack expansion with index_sequence template pattern */ + template + Vector unwhitenedError( + boost::mp11::index_sequence, // + const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) { + return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); + } else { + return evaluateError(x.at(keys_[Inds])...); + } + } else { + return Vector::Zero(this->dim()); + } + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactorN } // \namespace gtsam diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 84bba850b3..eb35bf55da 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -406,6 +406,50 @@ TEST(NonlinearFactor, NoiseModelFactor6) { } +/* ************************************************************************* */ +class TestFactorN : public NoiseModelFactorN { +public: + typedef NoiseModelFactorN Base; + TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + + Vector + evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const override { + if (H1) { + *H1 = (Matrix(1, 1) << 1.0).finished(); + *H2 = (Matrix(1, 1) << 2.0).finished(); + *H3 = (Matrix(1, 1) << 3.0).finished(); + *H4 = (Matrix(1, 1) << 4.0).finished(); + } + return (Vector(1) << x1 + x2 + x3 + x4).finished(); + } +}; + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactorN) { + TestFactorN tf; + Values tv; + tv.insert(X(1), double((1.0))); + tv.insert(X(2), double((2.0))); + tv.insert(X(3), double((3.0))); + tv.insert(X(4), double((4.0))); + EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); + LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); + LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); + LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); +} + /* ************************************************************************* */ TEST( NonlinearFactor, clone_rekey ) { From e037fa1bdbf49a7c07aeb226caaca11e43fa4be5 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 1 Dec 2021 16:20:51 -0500 Subject: [PATCH 02/40] c++11 doesn't support std::size so use obj.size() instead --- gtsam/nonlinear/NonlinearFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 503ae7d58f..b87d1bfaaa 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -816,7 +816,7 @@ class NoiseModelFactorN: public NoiseModelFactor { template NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { - assert(std::size(keys) == sizeof...(VALUES)); + assert(keys.size() == sizeof...(VALUES)); } ~NoiseModelFactorN() override {} From d9c8ce2721c076805b541746d268e69a99e71bd9 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 2 Dec 2021 23:26:34 -0500 Subject: [PATCH 03/40] alternate make_index_sequence impl if no boost::mp11 --- gtsam/nonlinear/NonlinearFactor.h | 34 +++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b87d1bfaaa..2498913970 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -28,7 +28,41 @@ #include #include + +#if BOOST_VERSION >= 106600 #include +#else +namespace boost { +namespace mp11 { +// Adapted from https://stackoverflow.com/a/32223343/9151520 +template +struct index_sequence { + using type = index_sequence; + using value_type = size_t; + static constexpr std::size_t size() noexcept { return sizeof...(Ints); } +}; +namespace detail { +template +struct _merge_and_renumber; + +template +struct _merge_and_renumber, index_sequence > + : index_sequence {}; +} // namespace detail +template +struct make_index_sequence + : detail::_merge_and_renumber< + typename make_index_sequence::type, + typename make_index_sequence::type> {}; +template <> +struct make_index_sequence<0> : index_sequence<> {}; +template <> +struct make_index_sequence<1> : index_sequence<0> {}; +template +using index_sequence_for = make_index_sequence; +} // namespace mp11 +} // namespace boost +#endif namespace gtsam { From 2aecaf325805f14e690fcfb83a4a20b579527467 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 00:39:10 -0500 Subject: [PATCH 04/40] optional jacobian overloads backwards compatibility --- gtsam/nonlinear/NonlinearFactor.h | 17 ++++++++++++-- tests/testNonlinearFactor.cpp | 37 ++++++++++++++++++++++++++----- 2 files changed, 46 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2498913970..542c4d5f1b 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -878,11 +878,24 @@ class NoiseModelFactorN: public NoiseModelFactor { optional_matrix_type ... H) const = 0; /** No-jacobians requested function overload (since parameter packs can't have - * default args) */ - Vector evaluateError(const VALUES&... x) const { + * default args). This specializes the version below to avoid recursive calls + * since this is commonly used. */ + inline Vector evaluateError(const VALUES&... x) const { return evaluateError(x..., optional_matrix_type()...); } + /** Some optional jacobians omitted function overload */ + template 0) && + (sizeof...(OptionalJacArgs) < + sizeof...(VALUES)), + bool>::type = true> + inline Vector evaluateError(const VALUES&... x, + OptionalJacArgs&&... H) const { + return evaluateError(x..., std::forward(H)..., + boost::none); + } + private: /** Pack expansion with index_sequence template pattern */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index eb35bf55da..8ecbbc397f 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -418,12 +418,10 @@ class TestFactorN : public NoiseModelFactorN { boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none) const override { - if (H1) { - *H1 = (Matrix(1, 1) << 1.0).finished(); - *H2 = (Matrix(1, 1) << 2.0).finished(); - *H3 = (Matrix(1, 1) << 3.0).finished(); - *H4 = (Matrix(1, 1) << 4.0).finished(); - } + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); + if (H3) *H3 = (Matrix(1, 1) << 3.0).finished(); + if (H4) *H4 = (Matrix(1, 1) << 4.0).finished(); return (Vector(1) << x1 + x2 + x3 + x4).finished(); } }; @@ -448,6 +446,33 @@ TEST(NonlinearFactor, NoiseModelFactorN) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); + + // Test all evaluateError argument overloads to ensure backward compatibility + Matrix H1_expected, H2_expected, H3_expected, H4_expected; + Vector e_expected = tf.evaluateError(9, 8, 7, 6, H1_expected, H2_expected, + H3_expected, H4_expected); + + std::unique_ptr> base_ptr( + new TestFactorN(tf)); + Matrix H1, H2, H3, H4; + EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6))); + EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6, H1))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(e_expected, // + base_ptr->evaluateError(9, 8, 7, 6, H1, H2))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(e_expected, + base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(H3_expected, H3)); + EXPECT(assert_equal(e_expected, + base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3, H4))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(H3_expected, H3)); + EXPECT(assert_equal(H4_expected, H4)); } /* ************************************************************************* */ From 8fe7e48258d69f4cb131c6af7e15504806ebbd4b Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 00:46:23 -0500 Subject: [PATCH 05/40] backward compatibility unit tests for NoiseModelFactor4 --- tests/testNonlinearFactor.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 8ecbbc397f..fda65d56a3 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -255,7 +255,13 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) /* ************************************************************************* */ class TestFactor4 : public NoiseModelFactor4 { -public: + static_assert(std::is_same::value, "Base type wrong"); + static_assert( + std::is_same>::value, + "This type wrong"); + + public: typedef NoiseModelFactor4 Base; TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} @@ -299,6 +305,22 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); + + // Test all functions/types for backwards compatibility + static_assert(std::is_same::value, + "X1 type incorrect"); + static_assert(std::is_same::value, + "X2 type incorrect"); + static_assert(std::is_same::value, + "X3 type incorrect"); + static_assert(std::is_same::value, + "X4 type incorrect"); + EXPECT(assert_equal(tf.key1(), X(1))); + EXPECT(assert_equal(tf.key2(), X(2))); + EXPECT(assert_equal(tf.key3(), X(3))); + EXPECT(assert_equal(tf.key4(), X(4))); + std::vector H = {Matrix(), Matrix(), Matrix(), Matrix()}; + EXPECT(assert_equal(Vector1(10.0), tf.unwhitenedError(tv, H))); } /* ************************************************************************* */ From bee4eeefdd358d07fb7ed02911a58dd5ec3eba33 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 02:25:11 -0500 Subject: [PATCH 06/40] NoiseModelFactor4 implemented as derived class of NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 345 +++++++++++++----------------- 1 file changed, 153 insertions(+), 192 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 542c4d5f1b..e024a41a09 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -309,13 +309,137 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { /* ************************************************************************* */ /** - * A convenient base class for creating your own NoiseModelFactor with 1 - * variable. To derive from this class, implement evaluateError(). + * A convenient base class for creating your own NoiseModelFactor with n + * variables. To derive from this class, implement evaluateError(). * * Templated on a values structure type. The values structures are typically * more general than just vectors, e.g., Rot3 or Pose3, * which are objects in non-linear manifolds (Lie groups). */ +template +class NoiseModelFactorN: public NoiseModelFactor { + public: + /** The type of the N'th template param can be obtained with VALUE */ + template + using VALUE = typename std::tuple_element>::type; + + protected: + typedef NoiseModelFactor Base; + typedef NoiseModelFactorN This; + + /* "Dummy templated" alias is used to expand fixed-type parameter packs with + * same length as VALUES. This ignores the template parameter. */ + template + using optional_matrix_type = boost::optional; + + /* "Dummy templated" alias is used to expand fixed-type parameter packs with + * same length as VALUES. This ignores the template parameter. */ + template + using key_type = Key; + + public: + /** + * Default Constructor for I/O + */ + NoiseModelFactorN() {} + + /** + * Constructor. + * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) + * @param noiseModel shared pointer to noise model + * @param keys... keys for the variables in this factor + */ + NoiseModelFactorN(const SharedNoiseModel& noiseModel, + key_type... keys) + : Base(noiseModel, std::array{keys...}) {} + + /** + * Constructor. + * @param noiseModel shared pointer to noise model + * @param keys a container of keys for the variables in this factor + */ + template + NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) + : Base(noiseModel, keys) { + assert(keys.size() == sizeof...(VALUES)); + } + + ~NoiseModelFactorN() override {} + + // /** Method to retrieve keys */ + // template + // inline Key key() const { return keys_[N]; } + + /** Calls the n-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + Vector unwhitenedError( + const Values& x, + boost::optional&> H = boost::none) const override { + return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); + } + + /** + * Override this method to finish implementing an n-way factor. + * If any of the optional Matrix reference arguments are specified, it should + * compute both the function evaluation and its derivative(s) in the requested + * variables. + */ + virtual Vector evaluateError( + const VALUES& ... x, + optional_matrix_type ... H) const = 0; + + /** No-jacobians requested function overload (since parameter packs can't have + * default args). This specializes the version below to avoid recursive calls + * since this is commonly used. */ + inline Vector evaluateError(const VALUES&... x) const { + return evaluateError(x..., optional_matrix_type()...); + } + + /** Some optional jacobians omitted function overload */ + template 0) && + (sizeof...(OptionalJacArgs) < + sizeof...(VALUES)), + bool>::type = true> + inline Vector evaluateError(const VALUES&... x, + OptionalJacArgs&&... H) const { + return evaluateError(x..., std::forward(H)..., + boost::none); + } + + private: + + /** Pack expansion with index_sequence template pattern */ + template + Vector unwhitenedError( + boost::mp11::index_sequence, // + const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) { + return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); + } else { + return evaluateError(x.at(keys_[Inds])...); + } + } else { + return Vector::Zero(this->dim()); + } + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactorN + + + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 1 + * variable. To derive from this class, implement evaluateError(). */ template class NoiseModelFactor1: public NoiseModelFactor { @@ -552,83 +676,40 @@ class NoiseModelFactor3: public NoiseModelFactor { /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor4: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor4 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor4() {} - - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - */ - NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {} - +template +class NoiseModelFactor4 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using This = NoiseModelFactor4; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor4() override {} /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - - /** Calls the 4-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 4-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const = 0; - -private: + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor4 +}; // \class NoiseModelFactor4 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 5 @@ -804,124 +885,4 @@ class NoiseModelFactor6: public NoiseModelFactor { } }; // \class NoiseModelFactor6 -/* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with N - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactorN: public NoiseModelFactor { - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactorN This; - - /* "Dummy templated" alias is used to expand fixed-type parameter packs with - * same length as VALUES. This ignores the template parameter. */ - template - using optional_matrix_type = boost::optional; - - /* "Dummy templated" alias is used to expand fixed-type parameter packs with - * same length as VALUES. This ignores the template parameter. */ - template - using key_type = Key; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactorN() {} - - /** - * Constructor. - * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) - * @param noiseModel shared pointer to noise model - * @param keys... keys for the variables in this factor - */ - NoiseModelFactorN(const SharedNoiseModel& noiseModel, - key_type... keys) - : Base(noiseModel, std::array{keys...}) {} - - /** - * Constructor. - * @param noiseModel shared pointer to noise model - * @param keys a container of keys for the variables in this factor - */ - template - NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) - : Base(noiseModel, keys) { - assert(keys.size() == sizeof...(VALUES)); - } - - ~NoiseModelFactorN() override {} - - /** Method to retrieve keys */ - template - inline Key key() const { return keys_[N]; } - - /** Calls the n-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError( - const Values& x, - boost::optional&> H = boost::none) const override { - return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); - } - - /** - * Override this method to finish implementing an n-way factor. - * If any of the optional Matrix reference arguments are specified, it should - * compute both the function evaluation and its derivative(s) in the requested - * variables. - */ - virtual Vector evaluateError( - const VALUES& ... x, - optional_matrix_type ... H) const = 0; - - /** No-jacobians requested function overload (since parameter packs can't have - * default args). This specializes the version below to avoid recursive calls - * since this is commonly used. */ - inline Vector evaluateError(const VALUES&... x) const { - return evaluateError(x..., optional_matrix_type()...); - } - - /** Some optional jacobians omitted function overload */ - template 0) && - (sizeof...(OptionalJacArgs) < - sizeof...(VALUES)), - bool>::type = true> - inline Vector evaluateError(const VALUES&... x, - OptionalJacArgs&&... H) const { - return evaluateError(x..., std::forward(H)..., - boost::none); - } - - private: - - /** Pack expansion with index_sequence template pattern */ - template - Vector unwhitenedError( - boost::mp11::index_sequence, // - const Values& x, - boost::optional&> H = boost::none) const { - if (this->active(x)) { - if (H) { - return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); - } else { - return evaluateError(x.at(keys_[Inds])...); - } - } else { - return Vector::Zero(this->dim()); - } - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactorN - } // \namespace gtsam From ed07edbfe4ab40fa1d583fe47bfd762818a97ae1 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 03:00:47 -0500 Subject: [PATCH 07/40] converted all NoiseModelFactorX to inherit from NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 502 ++++++++---------------------- 1 file changed, 137 insertions(+), 365 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index e024a41a09..64ef96b854 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -316,16 +316,16 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * more general than just vectors, e.g., Rot3 or Pose3, * which are objects in non-linear manifolds (Lie groups). */ -template -class NoiseModelFactorN: public NoiseModelFactor { +template +class NoiseModelFactorN : public NoiseModelFactor { public: /** The type of the N'th template param can be obtained with VALUE */ template using VALUE = typename std::tuple_element>::type; protected: - typedef NoiseModelFactor Base; - typedef NoiseModelFactorN This; + using Base = NoiseModelFactor; + using This = NoiseModelFactorN; /* "Dummy templated" alias is used to expand fixed-type parameter packs with * same length as VALUES. This ignores the template parameter. */ @@ -366,9 +366,11 @@ class NoiseModelFactorN: public NoiseModelFactor { ~NoiseModelFactorN() override {} - // /** Method to retrieve keys */ - // template - // inline Key key() const { return keys_[N]; } + /** Method to retrieve keys */ + template + inline Key key() const { + return keys_[N]; + } /** Calls the n-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ @@ -384,9 +386,8 @@ class NoiseModelFactorN: public NoiseModelFactor { * compute both the function evaluation and its derivative(s) in the requested * variables. */ - virtual Vector evaluateError( - const VALUES& ... x, - optional_matrix_type ... H) const = 0; + virtual Vector evaluateError(const VALUES&... x, + optional_matrix_type... H) const = 0; /** No-jacobians requested function overload (since parameter packs can't have * default args). This specializes the version below to avoid recursive calls @@ -408,7 +409,6 @@ class NoiseModelFactorN: public NoiseModelFactor { } private: - /** Pack expansion with index_sequence template pattern */ template Vector unwhitenedError( @@ -428,250 +428,110 @@ class NoiseModelFactorN: public NoiseModelFactor { /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactorN - - +}; // \class NoiseModelFactorN /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 1 * variable. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor1: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE X; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor1 This; - -public: - /// @name Constructors - /// @{ +template +class NoiseModelFactor1 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X = VALUE; - /** Default constructor for I/O only */ - NoiseModelFactor1() {} + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using This = NoiseModelFactor1; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} - inline Key key() const { return keys_[0]; } + inline Key key() const { return this->keys_[0]; } - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param key1 by which to look up X value in Values - */ - NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1) - : Base(noiseModel, cref_list_of<1>(key1)) {} - - /// @} - /// @name NoiseModelFactor methods - /// @{ - - /** - * Calls the 1-key specific version of evaluateError below, which is pure - * virtual so must be implemented in the derived class. - */ - Vector unwhitenedError( - const Values &x, - boost::optional &> H = boost::none) const override { - if (this->active(x)) { - const X &x1 = x.at(keys_[0]); - if (H) { - return evaluateError(x1, (*H)[0]); - } else { - return evaluateError(x1); - } - } else { - return Vector::Zero(this->dim()); - } - } - - /// @} - /// @name Virtual methods - /// @{ - - /** - * Override this method to finish implementing a unary factor. - * If the optional Matrix reference argument is specified, it should compute - * both the function evaluation and its derivative in X. - */ - virtual Vector - evaluateError(const X &x, - boost::optional H = boost::none) const = 0; - - /// @} - -private: + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -};// \class NoiseModelFactor1 - +}; // \class NoiseModelFactor1 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 2 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor2: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor2 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor2() {} +template +class NoiseModelFactor2 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - */ - NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel, cref_list_of<2>(j1)(j2)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor2; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor2() override {} /** methods to retrieve both keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - - /** Calls the 2-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - const X1& x1 = x.at(keys_[0]); - const X2& x2 = x.at(keys_[1]); - if(H) { - return evaluateError(x1, x2, (*H)[0], (*H)[1]); - } else { - return evaluateError(x1, x2); - } - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a binary factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2). - */ - virtual Vector - evaluateError(const X1&, const X2&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const = 0; - -private: + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor2 +}; // \class NoiseModelFactor2 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor3: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor3 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor3() {} +template +class NoiseModelFactor3 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - */ - NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor3; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor3() override {} /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - - /** Calls the 3-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a trinary factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; - -private: + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor3 +}; // \class NoiseModelFactor3 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 @@ -687,7 +547,7 @@ class NoiseModelFactor4 using X4 = VALUE4; protected: - using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using Base = NoiseModelFactor; using This = NoiseModelFactor4; public: @@ -714,175 +574,87 @@ class NoiseModelFactor4 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 5 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor5: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - typedef VALUE5 X5; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor5 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor5() {} +template +class NoiseModelFactor5 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + using X5 = VALUE5; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - * @param j5 key of the fifth variable - */ - NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor5; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor5() override {} /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - - /** Calls the 5-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 5-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const = 0; - -private: + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + inline Key key5() const { return this->keys_[4]; } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor5 +}; // \class NoiseModelFactor5 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor6: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - typedef VALUE5 X5; - typedef VALUE6 X6; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor6 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor6() {} +template +class NoiseModelFactor6 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + using X5 = VALUE5; + using X6 = VALUE6; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - * @param j5 key of the fifth variable - * @param j6 key of the fifth variable - */ - NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} + protected: + using Base = NoiseModelFactor; + using This = + NoiseModelFactor6; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor6() override {} /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - inline Key key6() const { return keys_[5]; } - - /** Calls the 6-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 6-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const = 0; - -private: + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + inline Key key5() const { return this->keys_[4]; } + inline Key key6() const { return this->keys_[5]; } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor6 +}; // \class NoiseModelFactor6 } // \namespace gtsam From ea7d769476e897f786799c5f774846460e880e19 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 05:49:36 -0500 Subject: [PATCH 08/40] documentation --- gtsam/nonlinear/NonlinearFactor.h | 136 ++++++++++++++++++++++++------ 1 file changed, 109 insertions(+), 27 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 64ef96b854..34e891c643 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -307,19 +307,39 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { /* ************************************************************************* */ - /** - * A convenient base class for creating your own NoiseModelFactor with n - * variables. To derive from this class, implement evaluateError(). + * A convenient base class for creating your own NoiseModelFactor + * with n variables. To derive from this class, implement evaluateError(). + * + * For example, a 2-way factor could be implemented like so: + * + * ~~~~~~~~~~~~~~~~~~~~{.cpp} + * class MyFactor : public NoiseModelFactorN { + * public: + * using Base = NoiseModelFactorN; + * + * MyFactor(Key key1, Key key2, const SharedNoiseModel& noiseModel) + * : Base(noiseModel, key1, key2) {} * - * Templated on a values structure type. The values structures are typically - * more general than just vectors, e.g., Rot3 or Pose3, - * which are objects in non-linear manifolds (Lie groups). + * Vector evaluateError( + * const double& x1, const double& x2, + * boost::optional H1 = boost::none, + * boost::optional H2 = boost::none) const override { + * if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + * if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); + * return (Vector(1) << x1 + 2 * x2).finished(); + * } + * }; + * ~~~~~~~~~~~~~~~~~~~~ + * + * These factors are templated on a values structure type. The values structures + * are typically more general than just vectors, e.g., Rot3 or Pose3, which are + * objects in non-linear manifolds (Lie groups). */ template class NoiseModelFactorN : public NoiseModelFactor { public: - /** The type of the N'th template param can be obtained with VALUE */ + /** The type of the N'th template param can be obtained as VALUE */ template using VALUE = typename std::tuple_element>::type; @@ -338,6 +358,9 @@ class NoiseModelFactorN : public NoiseModelFactor { using key_type = Key; public: + /// @name Constructors + /// @{ + /** * Default Constructor for I/O */ @@ -346,8 +369,9 @@ class NoiseModelFactorN : public NoiseModelFactor { /** * Constructor. * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) - * @param noiseModel shared pointer to noise model - * @param keys... keys for the variables in this factor + * @param noiseModel Shared pointer to noise model. + * @param keys Keys for the variables in this factor, passed in as separate + * arguments. */ NoiseModelFactorN(const SharedNoiseModel& noiseModel, key_type... keys) @@ -355,8 +379,8 @@ class NoiseModelFactorN : public NoiseModelFactor { /** * Constructor. - * @param noiseModel shared pointer to noise model - * @param keys a container of keys for the variables in this factor + * @param noiseModel Shared pointer to noise model. + * @param keys A container of keys for the variables in this factor. */ template NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) @@ -364,6 +388,8 @@ class NoiseModelFactorN : public NoiseModelFactor { assert(keys.size() == sizeof...(VALUES)); } + /// @} + ~NoiseModelFactorN() override {} /** Method to retrieve keys */ @@ -372,26 +398,59 @@ class NoiseModelFactorN : public NoiseModelFactor { return keys_[N]; } + /// @name NoiseModelFactor methods + /// @{ + /** Calls the n-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ + * so must be implemented in the derived class. + * @param[in] x A Values object containing the values of all the variables + * used in this factor + * @param[out] H A vector of (dynamic) matrices whose size should be equal to + * n. The jacobians w.r.t. each variable will be output in this parameter. + */ Vector unwhitenedError( const Values& x, boost::optional&> H = boost::none) const override { return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); } + /// @} + /// @name Virtual methods + /// @{ /** * Override this method to finish implementing an n-way factor. + * + * Both the `x` and `H` arguments are written here as parameter packs, but + * when overriding this method, you probably want to explicitly write them + * out. For example, for a 2-way factor with variable types Pose3 and double: + * ``` + * Vector evaluateError(const Pose3& x1, const double& x2, + * boost::optional H1 = boost::none, + * boost::optional H2 = boost::none) const + * override {...} + * ``` + * * If any of the optional Matrix reference arguments are specified, it should * compute both the function evaluation and its derivative(s) in the requested * variables. + * + * @param x The values of the variables to evaluate the error for. Passed in + * as separate arguments. + * @param[out] H The Jacobian with respect to each variable (optional). */ virtual Vector evaluateError(const VALUES&... x, optional_matrix_type... H) const = 0; + /// @} + /// @name Convenience method overloads + /// @{ + /** No-jacobians requested function overload (since parameter packs can't have * default args). This specializes the version below to avoid recursive calls - * since this is commonly used. */ + * since this is commonly used. + * + * e.g. `Vector error = factor.evaluateError(x1, x2, x3);` + */ inline Vector evaluateError(const VALUES&... x) const { return evaluateError(x..., optional_matrix_type()...); } @@ -408,10 +467,14 @@ class NoiseModelFactorN : public NoiseModelFactor { boost::none); } + /// @} + private: - /** Pack expansion with index_sequence template pattern */ + /** Pack expansion with index_sequence template pattern, used to index into + * `keys_` and `H` + */ template - Vector unwhitenedError( + inline Vector unwhitenedError( boost::mp11::index_sequence, // const Values& x, boost::optional&> H = boost::none) const { @@ -436,8 +499,11 @@ class NoiseModelFactorN : public NoiseModelFactor { }; // \class NoiseModelFactorN /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 1 - * variable. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 1 variable. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor1 : public NoiseModelFactorN { public: @@ -453,6 +519,7 @@ class NoiseModelFactor1 : public NoiseModelFactorN { using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} + /** method to retrieve key */ inline Key key() const { return this->keys_[0]; } private: @@ -466,8 +533,11 @@ class NoiseModelFactor1 : public NoiseModelFactorN { }; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 2 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 2 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor2 : public NoiseModelFactorN { public: @@ -499,8 +569,11 @@ class NoiseModelFactor2 : public NoiseModelFactorN { }; // \class NoiseModelFactor2 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 3 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 3 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor3 : public NoiseModelFactorN { public: @@ -534,8 +607,11 @@ class NoiseModelFactor3 : public NoiseModelFactorN { }; // \class NoiseModelFactor3 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 4 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 4 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor4 : public NoiseModelFactorN { @@ -572,8 +648,11 @@ class NoiseModelFactor4 }; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 5 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 5 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor5 : public NoiseModelFactorN { @@ -613,8 +692,11 @@ class NoiseModelFactor5 }; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 6 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 6 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor6 From ba3cc85701aaee6277ae5446e98027f9a1267295 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 06:17:14 -0500 Subject: [PATCH 09/40] avoid inheritance by conditionally defining backwards compatibility types/funcs in NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 624 ++++++++++++++++++------------ tests/testNonlinearFactor.cpp | 44 +++ 2 files changed, 414 insertions(+), 254 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 34e891c643..764f1fdf98 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -306,6 +306,76 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { }; // \class NoiseModelFactor +/* ************************************************************************* */ +/* We need some helper structs to help us with NoiseModelFactorN - specifically + * we need to alias X1, X2, X3, ... in the templated NoiseModelFactorN class to + * maintain backwards compatibility with NoiseModelFactor1, NoiseModelFactor2, + * NoiseModelFactor3, ... + * + * The tricky part is that we want to _conditionally_ alias these only if the + * `sizeof...(VALUES)` is greater than the index we want to alias (e.g. a 3-way + * factor should only have up to X3). + * + * The approach we use is to create structs which use template specialization to + * conditionally typedef X1, X2, ... for us, then inherit from them to inherit + * the aliases. + * + * Usage: + * ``` + * template + * class MyClass : public AliasX3 { ... }; + * ``` + * This will only typedef X3 if VALUES has at least 3 template parameters. So + * then we can do something like: + * ``` + * int main { + * MyClass::X3 a; // variable a will have type double + * // MyClass::X3 b; // this won't compile + * MyClass::X3 c; // variable c will have type char + * } + * ``` + */ + +// convenience macro extracts the type for the i'th VALUE in a parameter pack +#define GET_VALUE_I(VALUES, I) \ + typename std::tuple_element>::type + +namespace detail { + +// First handle `typedef X`. By default, we do not alias X (empty struct). +template +struct AliasX_ {}; +// But if `1 == sizeof...(VALUES)` is true, then we do alias X by specializing +// for when the first template parameter is true. +template +struct AliasX_<(1 == sizeof...(VALUES)), VALUES...> { + using X = GET_VALUE_I(VALUES, 0); +}; +// We'll alias the "true" template version for convenience. +template +using AliasX = AliasX_; + +// Now do the same thing for X1, X2, ... using a macro. +#define ALIAS_HELPER_X(N) \ + template \ + struct AliasX##N##_ {}; \ + template \ + struct AliasX##N##_<(N <= sizeof...(VALUES)), VALUES...> { \ + using X##N = GET_VALUE_I(VALUES, N - 1); \ + }; \ + template \ + using AliasX##N = AliasX##N##_; +ALIAS_HELPER_X(1); +ALIAS_HELPER_X(2); +ALIAS_HELPER_X(3); +ALIAS_HELPER_X(4); +ALIAS_HELPER_X(5); +ALIAS_HELPER_X(6); +#undef ALIAS_HELPER_X +#undef GET_VALUE_I + +} // namespace detail + /* ************************************************************************* */ /** * A convenient base class for creating your own NoiseModelFactor @@ -337,11 +407,21 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * objects in non-linear manifolds (Lie groups). */ template -class NoiseModelFactorN : public NoiseModelFactor { +class NoiseModelFactorN : public NoiseModelFactor, + public detail::AliasX, + public detail::AliasX1, + public detail::AliasX2, + public detail::AliasX3, + public detail::AliasX4, + public detail::AliasX5, + public detail::AliasX6 { public: - /** The type of the N'th template param can be obtained as VALUE */ - template - using VALUE = typename std::tuple_element>::type; + /// N is the number of variables (N-way factor) + enum { N = sizeof...(VALUES) }; + + /** The type of the i'th template param can be obtained as VALUE */ + template ::type = true> + using VALUE = typename std::tuple_element>::type; protected: using Base = NoiseModelFactor; @@ -375,28 +455,42 @@ class NoiseModelFactorN : public NoiseModelFactor { */ NoiseModelFactorN(const SharedNoiseModel& noiseModel, key_type... keys) - : Base(noiseModel, std::array{keys...}) {} + : Base(noiseModel, std::array{keys...}) {} /** - * Constructor. + * Constructor. Only enabled for n-ary factors where n > 1. * @param noiseModel Shared pointer to noise model. * @param keys A container of keys for the variables in this factor. */ - template + template 1), bool>::type = true> NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { - assert(keys.size() == sizeof...(VALUES)); + assert(keys.size() == N); } /// @} ~NoiseModelFactorN() override {} - /** Method to retrieve keys */ - template - inline Key key() const { - return keys_[N]; + /** Methods to retrieve keys */ +#define SUB(Old, New) template // to delay template deduction +#define KEY_IF_TRUE(Enable) typename std::enable_if<(Enable), Key>::type + // templated version of `key()` + template + inline KEY_IF_TRUE(I < N) key() const { + return keys_[I]; } + // backwards-compatibility functions + SUB(T, N) inline KEY_IF_TRUE(T == 1) key() const { return keys_[0]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 1) key1() const { return keys_[0]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 2) key2() const { return keys_[1]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 3) key3() const { return keys_[2]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 4) key4() const { return keys_[3]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 5) key5() const { return keys_[4]; } + SUB(T, N) inline KEY_IF_TRUE(T >= 6) key6() const { return keys_[5]; } +#undef SUB +#undef KEY_IF_TRUE /// @name NoiseModelFactor methods /// @{ @@ -458,8 +552,7 @@ class NoiseModelFactorN : public NoiseModelFactor { /** Some optional jacobians omitted function overload */ template 0) && - (sizeof...(OptionalJacArgs) < - sizeof...(VALUES)), + (sizeof...(OptionalJacArgs) < N), bool>::type = true> inline Vector evaluateError(const VALUES&... x, OptionalJacArgs&&... H) const { @@ -498,245 +591,268 @@ class NoiseModelFactorN : public NoiseModelFactor { } }; // \class NoiseModelFactorN -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 1 variable. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor1 : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X = VALUE; - - protected: - using Base = NoiseModelFactor; // grandparent, for backwards compatibility - using This = NoiseModelFactor1; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor1() override {} - - /** method to retrieve key */ - inline Key key() const { return this->keys_[0]; } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor1 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 2 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor2 : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor2; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor2() override {} - - /** methods to retrieve both keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor2 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 3 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor3 : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor3; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor3() override {} - - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor3 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 4 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor4 - : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor4; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor4() override {} - - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor4 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 5 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor5 - : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - using X5 = VALUE5; - - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor5; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor5() override {} - - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - inline Key key5() const { return this->keys_[4]; } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor5 - -/* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with VALUE<1>. - * A convenient base class for creating your own NoiseModelFactor - * with 6 variables. To derive from this class, implement evaluateError(). - */ -template -class NoiseModelFactor6 - : public NoiseModelFactorN { - public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - using X5 = VALUE5; - using X6 = VALUE6; - - protected: - using Base = NoiseModelFactor; - using This = - NoiseModelFactor6; - - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor6() override {} - - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - inline Key key5() const { return this->keys_[4]; } - inline Key key6() const { return this->keys_[5]; } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); - } -}; // \class NoiseModelFactor6 +// template +// using NoiseModelFactor1 = NoiseModelFactorN; +// template +// using NoiseModelFactor2 = NoiseModelFactorN; +// template +// using NoiseModelFactor3 = NoiseModelFactorN; +// template +// using NoiseModelFactor4 = NoiseModelFactorN; +// template +// using NoiseModelFactor5 = +// NoiseModelFactorN; +// template +// using NoiseModelFactor6 = +// NoiseModelFactorN; + +#define NoiseModelFactor1 NoiseModelFactorN +#define NoiseModelFactor2 NoiseModelFactorN +#define NoiseModelFactor3 NoiseModelFactorN +#define NoiseModelFactor4 NoiseModelFactorN +#define NoiseModelFactor5 NoiseModelFactorN +#define NoiseModelFactor6 NoiseModelFactorN + +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 1 variable. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor1 : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X = VALUE; + +// protected: +// using Base = NoiseModelFactor; // grandparent, for backwards compatibility +// using This = NoiseModelFactor1; + +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor1() override {} + +// /** method to retrieve key */ +// inline Key key() const { return this->keys_[0]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor1 + +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 2 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor2 : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; + +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor2; + +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor2() override {} + +// /** methods to retrieve both keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor2 + +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 3 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor3 : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; + +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor3; + +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor3() override {} + +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor3 + +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 4 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor4 +// : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; +// using X4 = VALUE4; + +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor4; + +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor4() override {} + +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } +// inline Key key4() const { return this->keys_[3]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor4 + +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 5 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor5 +// : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; +// using X4 = VALUE4; +// using X5 = VALUE5; + +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor5; + +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor5() override {} + +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } +// inline Key key4() const { return this->keys_[3]; } +// inline Key key5() const { return this->keys_[4]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor5 + +// /* ************************************************************************* */ +// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +// * with VALUE<1>. +// * A convenient base class for creating your own NoiseModelFactor +// * with 6 variables. To derive from this class, implement evaluateError(). +// */ +// template +// class NoiseModelFactor6 +// : public NoiseModelFactorN { +// public: +// // aliases for value types pulled from keys +// using X1 = VALUE1; +// using X2 = VALUE2; +// using X3 = VALUE3; +// using X4 = VALUE4; +// using X5 = VALUE5; +// using X6 = VALUE6; + +// protected: +// using Base = NoiseModelFactor; +// using This = +// NoiseModelFactor6; + +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor6() override {} + +// /** methods to retrieve keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } +// inline Key key3() const { return this->keys_[2]; } +// inline Key key4() const { return this->keys_[3]; } +// inline Key key5() const { return this->keys_[4]; } +// inline Key key6() const { return this->keys_[5]; } + +// private: +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { +// ar& boost::serialization::make_nvp( +// "NoiseModelFactor", boost::serialization::base_object(*this)); +// } +// }; // \class NoiseModelFactor6 } // \namespace gtsam diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index fda65d56a3..fba7949a12 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -253,6 +253,50 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +class TestFactor1 : public NoiseModelFactor1 { + static_assert(std::is_same::value, "Base type wrong"); + static_assert(std::is_same>::value, + "This type wrong"); + + public: + typedef NoiseModelFactor1 Base; + TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} + + Vector evaluateError(const double& x1, boost::optional H1 = + boost::none) const override { + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + return (Vector(1) << x1).finished(); + } + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); + } +}; + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactor1) { + TestFactor1 tf; + Values tv; + tv.insert(L(1), double((1.0))); + EXPECT(assert_equal((Vector(1) << 1.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.25 / 2.0, tf.error(tv), 1e-9); + JacobianFactor jf( + *boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)L(1), (long)jf.keys()[0]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), + jf.getA(jf.begin()))); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5).finished(), jf.getb())); + + // Test all functions/types for backwards compatibility + static_assert(std::is_same::value, + "X type incorrect"); + EXPECT(assert_equal(tf.key(), L(1))); + std::vector H = {Matrix()}; + EXPECT(assert_equal(Vector1(1.0), tf.unwhitenedError(tv, H))); +} + /* ************************************************************************* */ class TestFactor4 : public NoiseModelFactor4 { static_assert(std::is_same::value, "Base type wrong"); From ddcca4cdae0220bcbfa299837bbbe68099062f15 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 06:46:26 -0500 Subject: [PATCH 10/40] switch template bool specialization order --- gtsam/nonlinear/NonlinearFactor.h | 31 ++++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 764f1fdf98..b925916bb2 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -314,7 +314,8 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * * The tricky part is that we want to _conditionally_ alias these only if the * `sizeof...(VALUES)` is greater than the index we want to alias (e.g. a 3-way - * factor should only have up to X3). + * factor should only have up to X3). SFINAE doesn't work in this case with + * aliases so we have to come up with a different approach. * * The approach we use is to create structs which use template specialization to * conditionally typedef X1, X2, ... for us, then inherit from them to inherit @@ -345,26 +346,26 @@ namespace detail { // First handle `typedef X`. By default, we do not alias X (empty struct). template struct AliasX_ {}; -// But if `1 == sizeof...(VALUES)` is true, then we do alias X by specializing -// for when the first template parameter is true. +// But if the first template is true, then we do alias X by specializing. template -struct AliasX_<(1 == sizeof...(VALUES)), VALUES...> { +struct AliasX_ { using X = GET_VALUE_I(VALUES, 0); }; -// We'll alias the "true" template version for convenience. +// We'll alias (for convenience) the correct version based on whether or not +// `1 == sizeof...(VALUES)` is true template -using AliasX = AliasX_; +using AliasX = AliasX_<(1 == sizeof...(VALUES)), VALUES...>; // Now do the same thing for X1, X2, ... using a macro. -#define ALIAS_HELPER_X(N) \ - template \ - struct AliasX##N##_ {}; \ - template \ - struct AliasX##N##_<(N <= sizeof...(VALUES)), VALUES...> { \ - using X##N = GET_VALUE_I(VALUES, N - 1); \ - }; \ - template \ - using AliasX##N = AliasX##N##_; +#define ALIAS_HELPER_X(N) \ + template \ + struct AliasX##N##_ {}; \ + template \ + struct AliasX##N##_ { \ + using X##N = GET_VALUE_I(VALUES, N - 1); \ + }; \ + template \ + using AliasX##N = AliasX##N##_<(N <= sizeof...(VALUES)), VALUES...>; ALIAS_HELPER_X(1); ALIAS_HELPER_X(2); ALIAS_HELPER_X(3); From 280acde2fca26ff09df8acd40b10fae0a774cd1c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 07:43:42 -0500 Subject: [PATCH 11/40] can't get "using NoiseModelFactorX = NoiseModelFactorN" to work --- gtsam/nonlinear/NonlinearFactor.h | 298 +++++------------------------- 1 file changed, 51 insertions(+), 247 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b925916bb2..b55ec78567 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -592,6 +592,7 @@ class NoiseModelFactorN : public NoiseModelFactor, } }; // \class NoiseModelFactorN +// // `using` does not work for some reason // template // using NoiseModelFactor1 = NoiseModelFactorN; // template @@ -608,252 +609,55 @@ class NoiseModelFactorN : public NoiseModelFactor, // using NoiseModelFactor6 = // NoiseModelFactorN; -#define NoiseModelFactor1 NoiseModelFactorN -#define NoiseModelFactor2 NoiseModelFactorN -#define NoiseModelFactor3 NoiseModelFactorN -#define NoiseModelFactor4 NoiseModelFactorN -#define NoiseModelFactor5 NoiseModelFactorN -#define NoiseModelFactor6 NoiseModelFactorN - -// /* ************************************************************************* */ -// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 -// * with VALUE<1>. -// * A convenient base class for creating your own NoiseModelFactor -// * with 1 variable. To derive from this class, implement evaluateError(). -// */ -// template -// class NoiseModelFactor1 : public NoiseModelFactorN { -// public: -// // aliases for value types pulled from keys -// using X = VALUE; - -// protected: -// using Base = NoiseModelFactor; // grandparent, for backwards compatibility -// using This = NoiseModelFactor1; - -// public: -// // inherit NoiseModelFactorN's constructors -// using NoiseModelFactorN::NoiseModelFactorN; -// ~NoiseModelFactor1() override {} - -// /** method to retrieve key */ -// inline Key key() const { return this->keys_[0]; } - -// private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { -// ar& boost::serialization::make_nvp( -// "NoiseModelFactor", boost::serialization::base_object(*this)); -// } -// }; // \class NoiseModelFactor1 - -// /* ************************************************************************* */ -// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 -// * with VALUE<1>. -// * A convenient base class for creating your own NoiseModelFactor -// * with 2 variables. To derive from this class, implement evaluateError(). -// */ -// template -// class NoiseModelFactor2 : public NoiseModelFactorN { -// public: -// // aliases for value types pulled from keys -// using X1 = VALUE1; -// using X2 = VALUE2; - -// protected: -// using Base = NoiseModelFactor; -// using This = NoiseModelFactor2; - -// public: -// // inherit NoiseModelFactorN's constructors -// using NoiseModelFactorN::NoiseModelFactorN; -// ~NoiseModelFactor2() override {} - -// /** methods to retrieve both keys */ -// inline Key key1() const { return this->keys_[0]; } -// inline Key key2() const { return this->keys_[1]; } - -// private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { -// ar& boost::serialization::make_nvp( -// "NoiseModelFactor", boost::serialization::base_object(*this)); -// } -// }; // \class NoiseModelFactor2 - -// /* ************************************************************************* */ -// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 -// * with VALUE<1>. -// * A convenient base class for creating your own NoiseModelFactor -// * with 3 variables. To derive from this class, implement evaluateError(). -// */ -// template -// class NoiseModelFactor3 : public NoiseModelFactorN { -// public: -// // aliases for value types pulled from keys -// using X1 = VALUE1; -// using X2 = VALUE2; -// using X3 = VALUE3; - -// protected: -// using Base = NoiseModelFactor; -// using This = NoiseModelFactor3; - -// public: -// // inherit NoiseModelFactorN's constructors -// using NoiseModelFactorN::NoiseModelFactorN; -// ~NoiseModelFactor3() override {} - -// /** methods to retrieve keys */ -// inline Key key1() const { return this->keys_[0]; } -// inline Key key2() const { return this->keys_[1]; } -// inline Key key3() const { return this->keys_[2]; } - -// private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { -// ar& boost::serialization::make_nvp( -// "NoiseModelFactor", boost::serialization::base_object(*this)); -// } -// }; // \class NoiseModelFactor3 - -// /* ************************************************************************* */ -// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 -// * with VALUE<1>. -// * A convenient base class for creating your own NoiseModelFactor -// * with 4 variables. To derive from this class, implement evaluateError(). -// */ -// template -// class NoiseModelFactor4 -// : public NoiseModelFactorN { -// public: -// // aliases for value types pulled from keys -// using X1 = VALUE1; -// using X2 = VALUE2; -// using X3 = VALUE3; -// using X4 = VALUE4; - -// protected: -// using Base = NoiseModelFactor; -// using This = NoiseModelFactor4; - -// public: -// // inherit NoiseModelFactorN's constructors -// using NoiseModelFactorN::NoiseModelFactorN; -// ~NoiseModelFactor4() override {} - -// /** methods to retrieve keys */ -// inline Key key1() const { return this->keys_[0]; } -// inline Key key2() const { return this->keys_[1]; } -// inline Key key3() const { return this->keys_[2]; } -// inline Key key4() const { return this->keys_[3]; } - -// private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { -// ar& boost::serialization::make_nvp( -// "NoiseModelFactor", boost::serialization::base_object(*this)); -// } -// }; // \class NoiseModelFactor4 - -// /* ************************************************************************* */ -// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 -// * with VALUE<1>. -// * A convenient base class for creating your own NoiseModelFactor -// * with 5 variables. To derive from this class, implement evaluateError(). -// */ -// template -// class NoiseModelFactor5 -// : public NoiseModelFactorN { -// public: -// // aliases for value types pulled from keys -// using X1 = VALUE1; -// using X2 = VALUE2; -// using X3 = VALUE3; -// using X4 = VALUE4; -// using X5 = VALUE5; - -// protected: -// using Base = NoiseModelFactor; -// using This = NoiseModelFactor5; - -// public: -// // inherit NoiseModelFactorN's constructors -// using NoiseModelFactorN::NoiseModelFactorN; -// ~NoiseModelFactor5() override {} - -// /** methods to retrieve keys */ -// inline Key key1() const { return this->keys_[0]; } -// inline Key key2() const { return this->keys_[1]; } -// inline Key key3() const { return this->keys_[2]; } -// inline Key key4() const { return this->keys_[3]; } -// inline Key key5() const { return this->keys_[4]; } - -// private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { -// ar& boost::serialization::make_nvp( -// "NoiseModelFactor", boost::serialization::base_object(*this)); -// } -// }; // \class NoiseModelFactor5 - -// /* ************************************************************************* */ -// /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 -// * with VALUE<1>. -// * A convenient base class for creating your own NoiseModelFactor -// * with 6 variables. To derive from this class, implement evaluateError(). -// */ -// template -// class NoiseModelFactor6 -// : public NoiseModelFactorN { -// public: -// // aliases for value types pulled from keys -// using X1 = VALUE1; -// using X2 = VALUE2; -// using X3 = VALUE3; -// using X4 = VALUE4; -// using X5 = VALUE5; -// using X6 = VALUE6; - -// protected: -// using Base = NoiseModelFactor; -// using This = -// NoiseModelFactor6; - -// public: -// // inherit NoiseModelFactorN's constructors -// using NoiseModelFactorN::NoiseModelFactorN; -// ~NoiseModelFactor6() override {} - -// /** methods to retrieve keys */ -// inline Key key1() const { return this->keys_[0]; } -// inline Key key2() const { return this->keys_[1]; } -// inline Key key3() const { return this->keys_[2]; } -// inline Key key4() const { return this->keys_[3]; } -// inline Key key5() const { return this->keys_[4]; } -// inline Key key6() const { return this->keys_[5]; } - -// private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(ARCHIVE& ar, const unsigned int /*version*/) { -// ar& boost::serialization::make_nvp( -// "NoiseModelFactor", boost::serialization::base_object(*this)); -// } -// }; // \class NoiseModelFactor6 +// this is visually ugly +template +struct NoiseModelFactor1 : NoiseModelFactorN { + using NoiseModelFactorN::NoiseModelFactorN; + using This = NoiseModelFactor1; +}; +template +struct NoiseModelFactor2 : NoiseModelFactorN { + using NoiseModelFactorN::NoiseModelFactorN; + using This = NoiseModelFactor2; +}; +template +struct NoiseModelFactor3 : NoiseModelFactorN { + using NoiseModelFactorN::NoiseModelFactorN; + using This = NoiseModelFactor3; +}; +template +struct NoiseModelFactor4 : NoiseModelFactorN { + using NoiseModelFactorN::NoiseModelFactorN; + using This = NoiseModelFactor4; +}; +template +struct NoiseModelFactor5 + : NoiseModelFactorN { + using NoiseModelFactorN::NoiseModelFactorN; + using This = NoiseModelFactor5; +}; +template +struct NoiseModelFactor6 + : NoiseModelFactorN { + using NoiseModelFactorN::NoiseModelFactorN; + using This = + NoiseModelFactor6; +}; + +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN + * Convenient base classes for creating your own NoiseModelFactors with 1-6 + * variables. To derive from these classes, implement evaluateError(). + */ +// // This has the side-effect that you could e.g. NoiseModelFactor6 +// #define NoiseModelFactor1 NoiseModelFactorN +// #define NoiseModelFactor2 NoiseModelFactorN +// #define NoiseModelFactor3 NoiseModelFactorN +// #define NoiseModelFactor4 NoiseModelFactorN +// #define NoiseModelFactor5 NoiseModelFactorN +// #define NoiseModelFactor6 NoiseModelFactorN } // \namespace gtsam From 89b4340530db96c6bfbfe750fbe129c9f9e998fb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 8 Dec 2021 18:29:52 -0500 Subject: [PATCH 12/40] alternate option for typedef-ing X1, X2, ... --- gtsam/nonlinear/NonlinearFactor.h | 100 ++++++------------------------ 1 file changed, 20 insertions(+), 80 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b55ec78567..53a3e664d4 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -305,78 +305,6 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { }; // \class NoiseModelFactor - -/* ************************************************************************* */ -/* We need some helper structs to help us with NoiseModelFactorN - specifically - * we need to alias X1, X2, X3, ... in the templated NoiseModelFactorN class to - * maintain backwards compatibility with NoiseModelFactor1, NoiseModelFactor2, - * NoiseModelFactor3, ... - * - * The tricky part is that we want to _conditionally_ alias these only if the - * `sizeof...(VALUES)` is greater than the index we want to alias (e.g. a 3-way - * factor should only have up to X3). SFINAE doesn't work in this case with - * aliases so we have to come up with a different approach. - * - * The approach we use is to create structs which use template specialization to - * conditionally typedef X1, X2, ... for us, then inherit from them to inherit - * the aliases. - * - * Usage: - * ``` - * template - * class MyClass : public AliasX3 { ... }; - * ``` - * This will only typedef X3 if VALUES has at least 3 template parameters. So - * then we can do something like: - * ``` - * int main { - * MyClass::X3 a; // variable a will have type double - * // MyClass::X3 b; // this won't compile - * MyClass::X3 c; // variable c will have type char - * } - * ``` - */ - -// convenience macro extracts the type for the i'th VALUE in a parameter pack -#define GET_VALUE_I(VALUES, I) \ - typename std::tuple_element>::type - -namespace detail { - -// First handle `typedef X`. By default, we do not alias X (empty struct). -template -struct AliasX_ {}; -// But if the first template is true, then we do alias X by specializing. -template -struct AliasX_ { - using X = GET_VALUE_I(VALUES, 0); -}; -// We'll alias (for convenience) the correct version based on whether or not -// `1 == sizeof...(VALUES)` is true -template -using AliasX = AliasX_<(1 == sizeof...(VALUES)), VALUES...>; - -// Now do the same thing for X1, X2, ... using a macro. -#define ALIAS_HELPER_X(N) \ - template \ - struct AliasX##N##_ {}; \ - template \ - struct AliasX##N##_ { \ - using X##N = GET_VALUE_I(VALUES, N - 1); \ - }; \ - template \ - using AliasX##N = AliasX##N##_<(N <= sizeof...(VALUES)), VALUES...>; -ALIAS_HELPER_X(1); -ALIAS_HELPER_X(2); -ALIAS_HELPER_X(3); -ALIAS_HELPER_X(4); -ALIAS_HELPER_X(5); -ALIAS_HELPER_X(6); -#undef ALIAS_HELPER_X -#undef GET_VALUE_I - -} // namespace detail - /* ************************************************************************* */ /** * A convenient base class for creating your own NoiseModelFactor @@ -408,14 +336,7 @@ ALIAS_HELPER_X(6); * objects in non-linear manifolds (Lie groups). */ template -class NoiseModelFactorN : public NoiseModelFactor, - public detail::AliasX, - public detail::AliasX1, - public detail::AliasX2, - public detail::AliasX3, - public detail::AliasX4, - public detail::AliasX5, - public detail::AliasX6 { +class NoiseModelFactorN : public NoiseModelFactor { public: /// N is the number of variables (N-way factor) enum { N = sizeof...(VALUES) }; @@ -424,6 +345,25 @@ class NoiseModelFactorN : public NoiseModelFactor, template ::type = true> using VALUE = typename std::tuple_element>::type; + private: + template + struct VALUE_OR_VOID { + using type = void; + }; + template + struct VALUE_OR_VOID::type> { + using type = VALUE; + }; + + public: + using X = typename VALUE_OR_VOID<0>::type; + using X1 = typename VALUE_OR_VOID<0>::type; + using X2 = typename VALUE_OR_VOID<1>::type; + using X3 = typename VALUE_OR_VOID<2>::type; + using X4 = typename VALUE_OR_VOID<3>::type; + using X5 = typename VALUE_OR_VOID<4>::type; + using X6 = typename VALUE_OR_VOID<5>::type; + protected: using Base = NoiseModelFactor; using This = NoiseModelFactorN; From 5004c47944f1e50234aa02c4c879771e272be7d2 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 8 Dec 2021 18:56:09 -0500 Subject: [PATCH 13/40] revert typedef X1, X2, ... to old version, and clean up a little --- gtsam/nonlinear/NonlinearFactor.h | 94 ++++++++++++++++++++++++------- 1 file changed, 74 insertions(+), 20 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 53a3e664d4..099f589792 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -305,6 +305,70 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { }; // \class NoiseModelFactor + +/* ************************************************************************* */ +/* We need some helper structs to help us with NoiseModelFactorN - specifically + * we need to alias X1, X2, X3, ... in the templated NoiseModelFactorN class to + * maintain backwards compatibility with NoiseModelFactor1, NoiseModelFactor2, + * NoiseModelFactor3, ... + * + * The tricky part is that we want to _conditionally_ alias these only if the + * `sizeof...(VALUES)` is greater than the index we want to alias (e.g. a 3-way + * factor should only have up to X3). SFINAE doesn't work in this case with + * aliases so we have to come up with a different approach. + * + * The approach we use is to inherit from structs that conditionally typedef + * these types for us (using template specialization). Note: std::conditional + * doesn't work because it requires that both types exist at compile time. + * + * Usage: + * ``` + * template + * class MyClass : public AliasX3 { ... }; + * ``` + * This will only typedef X3 if VALUES has at least 3 template parameters. So + * then we can do something like: + * ``` + * int main { + * MyClass::X3 a; // variable a will have type double + * // MyClass::X3 b; // this won't compile + * MyClass::X3 c; // variable c will have type char + * } + * ``` + */ + +namespace detail { + +// By default, we do not alias X (empty struct). +#define ALIAS_FALSE_X(NAME) \ + template \ + struct Alias##NAME##_ {}; +// But if the first template is true, then we do alias X by specializing. +#define ALIAS_TRUE_X(NAME, N) \ + template \ + struct Alias##NAME##_ { \ + using NAME = typename std::tuple_element>::type; \ + }; +// Finally, alias a convenience struct that chooses the right version. +#define ALIAS_X(NAME, N, CONDITION) \ + ALIAS_FALSE_X(NAME) \ + ALIAS_TRUE_X(NAME, N) \ + template \ + using Alias##NAME = Alias##NAME##_<(CONDITION), VALUES...>; + +ALIAS_X(X, 0, 0 == sizeof...(VALUES)); +ALIAS_X(X1, 0, 0 < sizeof...(VALUES)); +ALIAS_X(X2, 1, 1 < sizeof...(VALUES)); +ALIAS_X(X3, 2, 2 < sizeof...(VALUES)); +ALIAS_X(X4, 3, 3 < sizeof...(VALUES)); +ALIAS_X(X5, 4, 4 < sizeof...(VALUES)); +ALIAS_X(X6, 5, 5 < sizeof...(VALUES)); +#undef ALIAS_FALSE_X +#undef ALIAS_TRUE_X +#undef ALIAS_X + +} // namespace detail + /* ************************************************************************* */ /** * A convenient base class for creating your own NoiseModelFactor @@ -336,7 +400,16 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * objects in non-linear manifolds (Lie groups). */ template -class NoiseModelFactorN : public NoiseModelFactor { +class NoiseModelFactorN + : public NoiseModelFactor, + public detail::AliasX, // using X = VALUE1 + public detail::AliasX1, // using X1 = VALUE1 + public detail::AliasX2, // using X2 = VALUE2 + public detail::AliasX3, // using X3 = VALUE3 + public detail::AliasX4, // using X4 = VALUE4 + public detail::AliasX5, // using X5 = VALUE5 + public detail::AliasX6 // using X6 = VALUE6 +{ public: /// N is the number of variables (N-way factor) enum { N = sizeof...(VALUES) }; @@ -345,25 +418,6 @@ class NoiseModelFactorN : public NoiseModelFactor { template ::type = true> using VALUE = typename std::tuple_element>::type; - private: - template - struct VALUE_OR_VOID { - using type = void; - }; - template - struct VALUE_OR_VOID::type> { - using type = VALUE; - }; - - public: - using X = typename VALUE_OR_VOID<0>::type; - using X1 = typename VALUE_OR_VOID<0>::type; - using X2 = typename VALUE_OR_VOID<1>::type; - using X3 = typename VALUE_OR_VOID<2>::type; - using X4 = typename VALUE_OR_VOID<3>::type; - using X5 = typename VALUE_OR_VOID<4>::type; - using X6 = typename VALUE_OR_VOID<5>::type; - protected: using Base = NoiseModelFactor; using This = NoiseModelFactorN; From 018213ec855db1dcbe0c7a7b754c178e49442379 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 8 Dec 2021 19:09:35 -0500 Subject: [PATCH 14/40] switch `using NoiseModelFactorX = ...` to `#define ...`. Reasoning is given as comments. --- gtsam/nonlinear/NonlinearFactor.h | 91 ++++++++----------------------- 1 file changed, 23 insertions(+), 68 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 099f589792..18822f0ef3 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -336,7 +336,6 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * } * ``` */ - namespace detail { // By default, we do not alias X (empty struct). @@ -356,7 +355,7 @@ namespace detail { template \ using Alias##NAME = Alias##NAME##_<(CONDITION), VALUES...>; -ALIAS_X(X, 0, 0 == sizeof...(VALUES)); +ALIAS_X(X, 0, 1 == sizeof...(VALUES)); ALIAS_X(X1, 0, 0 < sizeof...(VALUES)); ALIAS_X(X2, 1, 1 < sizeof...(VALUES)); ALIAS_X(X3, 2, 2 < sizeof...(VALUES)); @@ -422,13 +421,13 @@ class NoiseModelFactorN using Base = NoiseModelFactor; using This = NoiseModelFactorN; - /* "Dummy templated" alias is used to expand fixed-type parameter packs with - * same length as VALUES. This ignores the template parameter. */ + /* Like std::void_t, except produces `boost::optional` instead. Used + * to expand fixed-type parameter-packs with same length as VALUES */ template using optional_matrix_type = boost::optional; - /* "Dummy templated" alias is used to expand fixed-type parameter packs with - * same length as VALUES. This ignores the template parameter. */ + /* Like std::void_t, except produces `Key` instead. Used to expand fixed-type + * parameter-packs with same length as VALUES */ template using key_type = Key; @@ -586,72 +585,28 @@ class NoiseModelFactorN } }; // \class NoiseModelFactorN -// // `using` does not work for some reason -// template -// using NoiseModelFactor1 = NoiseModelFactorN; -// template -// using NoiseModelFactor2 = NoiseModelFactorN; -// template -// using NoiseModelFactor3 = NoiseModelFactorN; -// template -// using NoiseModelFactor4 = NoiseModelFactorN; -// template -// using NoiseModelFactor5 = -// NoiseModelFactorN; -// template -// using NoiseModelFactor6 = -// NoiseModelFactorN; - -// this is visually ugly -template -struct NoiseModelFactor1 : NoiseModelFactorN { - using NoiseModelFactorN::NoiseModelFactorN; - using This = NoiseModelFactor1; -}; -template -struct NoiseModelFactor2 : NoiseModelFactorN { - using NoiseModelFactorN::NoiseModelFactorN; - using This = NoiseModelFactor2; -}; -template -struct NoiseModelFactor3 : NoiseModelFactorN { - using NoiseModelFactorN::NoiseModelFactorN; - using This = NoiseModelFactor3; -}; -template -struct NoiseModelFactor4 : NoiseModelFactorN { - using NoiseModelFactorN::NoiseModelFactorN; - using This = NoiseModelFactor4; -}; -template -struct NoiseModelFactor5 - : NoiseModelFactorN { - using NoiseModelFactorN::NoiseModelFactorN; - using This = NoiseModelFactor5; -}; -template -struct NoiseModelFactor6 - : NoiseModelFactorN { - using NoiseModelFactorN::NoiseModelFactorN; - using This = - NoiseModelFactor6; -}; - /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN * Convenient base classes for creating your own NoiseModelFactors with 1-6 * variables. To derive from these classes, implement evaluateError(). + * + * Note: we cannot use `using NoiseModelFactor1 = NoiseModelFactorN` due + * to class name injection making backwards compatibility difficult. + * + * Note: This has the side-effect that you could e.g. NoiseModelFactor6. + * That is, there is nothing stopping you from using any number of template + * arguments with any `NoiseModelFactorX`. */ -// // This has the side-effect that you could e.g. NoiseModelFactor6 -// #define NoiseModelFactor1 NoiseModelFactorN -// #define NoiseModelFactor2 NoiseModelFactorN -// #define NoiseModelFactor3 NoiseModelFactorN -// #define NoiseModelFactor4 NoiseModelFactorN -// #define NoiseModelFactor5 NoiseModelFactorN -// #define NoiseModelFactor6 NoiseModelFactorN +#define NoiseModelFactor1 NoiseModelFactorN +/** @deprecated */ +#define NoiseModelFactor2 NoiseModelFactorN +/** @deprecated */ +#define NoiseModelFactor3 NoiseModelFactorN +/** @deprecated */ +#define NoiseModelFactor4 NoiseModelFactorN +/** @deprecated */ +#define NoiseModelFactor5 NoiseModelFactorN +/** @deprecated */ +#define NoiseModelFactor6 NoiseModelFactorN } // \namespace gtsam From 84e873ebb0d14aaf0058d5a3e09d94aa38776847 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 9 Dec 2021 00:50:36 -0500 Subject: [PATCH 15/40] fix Windows CI issue: VALUE happens to have the same name in PriorFactor --- gtsam/nonlinear/NonlinearFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 18822f0ef3..ca13434a16 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -413,9 +413,9 @@ class NoiseModelFactorN /// N is the number of variables (N-way factor) enum { N = sizeof...(VALUES) }; - /** The type of the i'th template param can be obtained as VALUE */ + /** The type of the i'th template param can be obtained as X_ */ template ::type = true> - using VALUE = typename std::tuple_element>::type; + using X_ = typename std::tuple_element>::type; protected: using Base = NoiseModelFactor; From 40e585bb117eb24d3e591a97d06362b9acfa63a3 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 9 Dec 2021 02:26:57 -0500 Subject: [PATCH 16/40] review comments --- gtsam/nonlinear/NonlinearFactor.h | 46 +++++++++++++++++++++---------- 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ca13434a16..3a59a4db3e 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -29,6 +29,9 @@ #include #include +// boost::index_sequence was introduced in 1.66, so we'll manually define an +// implementation if user has 1.65. boost::index_sequence is used to get array +// indices that align with a parameter pack. #if BOOST_VERSION >= 106600 #include #else @@ -467,24 +470,11 @@ class NoiseModelFactorN ~NoiseModelFactorN() override {} - /** Methods to retrieve keys */ -#define SUB(Old, New) template // to delay template deduction -#define KEY_IF_TRUE(Enable) typename std::enable_if<(Enable), Key>::type - // templated version of `key()` + /** Returns a key. Usage: `key()` returns the I'th key. */ template - inline KEY_IF_TRUE(I < N) key() const { + inline typename std::enable_if<(I < N), Key>::type key() const { return keys_[I]; } - // backwards-compatibility functions - SUB(T, N) inline KEY_IF_TRUE(T == 1) key() const { return keys_[0]; } - SUB(T, N) inline KEY_IF_TRUE(T >= 1) key1() const { return keys_[0]; } - SUB(T, N) inline KEY_IF_TRUE(T >= 2) key2() const { return keys_[1]; } - SUB(T, N) inline KEY_IF_TRUE(T >= 3) key3() const { return keys_[2]; } - SUB(T, N) inline KEY_IF_TRUE(T >= 4) key4() const { return keys_[3]; } - SUB(T, N) inline KEY_IF_TRUE(T >= 5) key5() const { return keys_[4]; } - SUB(T, N) inline KEY_IF_TRUE(T >= 6) key6() const { return keys_[5]; } -#undef SUB -#undef KEY_IF_TRUE /// @name NoiseModelFactor methods /// @{ @@ -583,6 +573,32 @@ class NoiseModelFactorN ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); } + + public: + /** @defgroup deprecated key access + * Functions to retrieve keys (deprecated). Use the templated version: + * `key()` instead. + * @{ + */ +#define SUB(Old, New) template // to delay template deduction +#define KEY_IF_TRUE(Enable) typename std::enable_if<(Enable), Key>::type + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T == 1) key() const { return keys_[0]; } + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T >= 1) key1() const { return keys_[0]; } + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T >= 2) key2() const { return keys_[1]; } + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T >= 3) key3() const { return keys_[2]; } + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T >= 4) key4() const { return keys_[3]; } + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T >= 5) key5() const { return keys_[4]; } + /** @deprecated */ + SUB(T, N) inline KEY_IF_TRUE(T >= 6) key6() const { return keys_[5]; } +#undef SUB +#undef KEY_IF_TRUE + /** @} */ }; // \class NoiseModelFactorN /* ************************************************************************* */ From 11fd8612269690c9d95eba4a80ac5db705906aa1 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 9 Dec 2021 02:30:51 -0500 Subject: [PATCH 17/40] update doxygen (review comment) --- gtsam/mainpage.dox | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index e07f45f079..e9c83da8a8 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -16,7 +16,7 @@ To use GTSAM to solve your own problems, you will often have to create new facto -# The number of variables your factor involves is unknown at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError() - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel). --# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError(). If the number of variables is greater than 6, derive from NoiseModelFactorN. +-# The number of variables your factor involves is known at compile time, derive from NoiseModelFactorN (where T1, T2, ... are the types of the variables, e.g. double, Vector, Pose3) and implement \c evaluateError(). - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor. -# Derive from NonlinearFactor - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor. From c9dbb6e04085dad6dc7e48c99dbe6b5624a4575f Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 30 Jan 2022 17:23:02 -0500 Subject: [PATCH 18/40] create backwards compatibility unit test for NoiseModelFactor1 --- gtsam/nonlinear/tests/priorFactor.xml | 77 +++++++++++++++++++ .../tests/testSerializationNonlinear.cpp | 53 +++++++++++++ 2 files changed, 130 insertions(+) create mode 100644 gtsam/nonlinear/tests/priorFactor.xml diff --git a/gtsam/nonlinear/tests/priorFactor.xml b/gtsam/nonlinear/tests/priorFactor.xml new file mode 100644 index 0000000000..46752262b9 --- /dev/null +++ b/gtsam/nonlinear/tests/priorFactor.xml @@ -0,0 +1,77 @@ + + + + + + + + + 1 + 0 + 12345 + + + + + + + + + 6 + + + 0 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + + + + + 4.11982245665682978e-01 + -8.33737651774156818e-01 + -3.67630462924899259e-01 + -5.87266449276209815e-02 + -4.26917621276207360e-01 + 9.02381585483330806e-01 + -9.09297426825681709e-01 + -3.50175488374014632e-01 + -2.24845095366152908e-01 + + + + 4.00000000000000000e+00 + 5.00000000000000000e+00 + 6.00000000000000000e+00 + + + + + diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 4a73cbb0b4..f7b524a5c7 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -107,6 +107,59 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } +bool str_assert_equal(const string& expected, const string& actual) { + if (actual == expected) return true; + printf("Not equal:\n"); + std::cout << "expected:\n" << expected << std::endl; + std::cout << "actual:\n" << actual << std::endl; + return false; +} +TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { + PriorFactor factor( + 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), + noiseModel::Unit::Create(6)); + + // String + std::string actual_str = serialize(factor); + // Generated from hash 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4) + std::string expected_str = + "22 serialization::archive 19 1 0\n" + "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" + "1 1 0\n" + "2 1 0\n" + "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 6 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 " + "-8.33737651774156818e-01 -3.67630462924899259e-01 " + "-5.87266449276209815e-02 -4.26917621276207360e-01 " + "9.02381585483330806e-01 -9.09297426825681709e-01 " + "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " + "4.00000000000000000e+00 5.00000000000000000e+00 " + "6.00000000000000000e+00\n"; + EXPECT(str_assert_equal(expected_str, actual_str)); + PriorFactor factor_deserialized_str = PriorFactor(); + deserializeFromString(expected_str, factor_deserialized_str); + EXPECT(assert_equal(factor, factor_deserialized_str)); + + // XML + std::string actual_xml = serializeXML(factor); + std::string expected_xml; + { // read from file + // Generated from hash 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4) + std::ifstream f("priorFactor.xml"); + std::stringstream buffer; + buffer << f.rdbuf(); + expected_xml = buffer.str(); + } + EXPECT(str_assert_equal(expected_xml, actual_xml)); + PriorFactor factor_deserialized_xml = PriorFactor(); + deserializeFromXMLFile("priorFactor.xml", factor_deserialized_xml); + EXPECT(assert_equal(factor, factor_deserialized_xml)); +} + TEST(Serialization, ISAM2) { gtsam::ISAM2Params parameters; From bb33be58b36ec4aa4f00d56064b6286901710e96 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 30 Jan 2022 17:25:20 -0500 Subject: [PATCH 19/40] revert some template stuff with inheritance for readability NoiseModelFactor123456 are now minimal classes that inherit from NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 360 ++++++++++++++++++++---------- 1 file changed, 240 insertions(+), 120 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d7061215ed..212364af3c 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -309,68 +309,6 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { }; // \class NoiseModelFactor -/* ************************************************************************* */ -/* We need some helper structs to help us with NoiseModelFactorN - specifically - * we need to alias X1, X2, X3, ... in the templated NoiseModelFactorN class to - * maintain backwards compatibility with NoiseModelFactor1, NoiseModelFactor2, - * NoiseModelFactor3, ... - * - * The tricky part is that we want to _conditionally_ alias these only if the - * `sizeof...(VALUES)` is greater than the index we want to alias (e.g. a 3-way - * factor should only have up to X3). SFINAE doesn't work in this case with - * aliases so we have to come up with a different approach. - * - * The approach we use is to inherit from structs that conditionally typedef - * these types for us (using template specialization). Note: std::conditional - * doesn't work because it requires that both types exist at compile time. - * - * Usage: - * ``` - * template - * class MyClass : public AliasX3 { ... }; - * ``` - * This will only typedef X3 if VALUES has at least 3 template parameters. So - * then we can do something like: - * ``` - * int main { - * MyClass::X3 a; // variable a will have type double - * // MyClass::X3 b; // this won't compile - * MyClass::X3 c; // variable c will have type char - * } - * ``` - */ -namespace detail { - -// By default, we do not alias X (empty struct). -#define ALIAS_FALSE_X(NAME) \ - template \ - struct Alias##NAME##_ {}; -// But if the first template is true, then we do alias X by specializing. -#define ALIAS_TRUE_X(NAME, N) \ - template \ - struct Alias##NAME##_ { \ - using NAME = typename std::tuple_element>::type; \ - }; -// Finally, alias a convenience struct that chooses the right version. -#define ALIAS_X(NAME, N, CONDITION) \ - ALIAS_FALSE_X(NAME) \ - ALIAS_TRUE_X(NAME, N) \ - template \ - using Alias##NAME = Alias##NAME##_<(CONDITION), VALUES...>; - -ALIAS_X(X, 0, 1 == sizeof...(VALUES)); -ALIAS_X(X1, 0, 0 < sizeof...(VALUES)); -ALIAS_X(X2, 1, 1 < sizeof...(VALUES)); -ALIAS_X(X3, 2, 2 < sizeof...(VALUES)); -ALIAS_X(X4, 3, 3 < sizeof...(VALUES)); -ALIAS_X(X5, 4, 4 < sizeof...(VALUES)); -ALIAS_X(X6, 5, 5 < sizeof...(VALUES)); -#undef ALIAS_FALSE_X -#undef ALIAS_TRUE_X -#undef ALIAS_X - -} // namespace detail - /* ************************************************************************* */ /** * A convenient base class for creating your own NoiseModelFactor @@ -402,23 +340,14 @@ ALIAS_X(X6, 5, 5 < sizeof...(VALUES)); * objects in non-linear manifolds (Lie groups). */ template -class GTSAM_EXPORT NoiseModelFactorN - : public NoiseModelFactor, - public detail::AliasX, // using X = VALUE1 - public detail::AliasX1, // using X1 = VALUE1 - public detail::AliasX2, // using X2 = VALUE2 - public detail::AliasX3, // using X3 = VALUE3 - public detail::AliasX4, // using X4 = VALUE4 - public detail::AliasX5, // using X5 = VALUE5 - public detail::AliasX6 // using X6 = VALUE6 -{ +class GTSAM_EXPORT NoiseModelFactorN : public NoiseModelFactor { public: /// N is the number of variables (N-way factor) enum { N = sizeof...(VALUES) }; - /** The type of the i'th template param can be obtained as X_ */ + /** The type of the i'th template param can be obtained as X */ template ::type = true> - using X_ = typename std::tuple_element>::type; + using X = typename std::tuple_element>::type; protected: using Base = NoiseModelFactor; @@ -573,56 +502,247 @@ class GTSAM_EXPORT NoiseModelFactorN ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); } +}; // \class NoiseModelFactorN +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 + * with X<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 1 variable. To derive from this class, implement evaluateError(). + */ +template +class NoiseModelFactor1 : public NoiseModelFactorN { public: - /** @defgroup deprecated key access - * Functions to retrieve keys (deprecated). Use the templated version: - * `key()` instead. - * @{ - */ -#define SUB(Old, New) template // to delay template deduction -#define KEY_IF_TRUE(Enable) typename std::enable_if<(Enable), Key>::type - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T == 1) key() const { return keys_[0]; } - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T >= 1) key1() const { return keys_[0]; } - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T >= 2) key2() const { return keys_[1]; } - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T >= 3) key3() const { return keys_[2]; } - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T >= 4) key4() const { return keys_[3]; } - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T >= 5) key5() const { return keys_[4]; } - /** @deprecated */ - SUB(T, N) inline KEY_IF_TRUE(T >= 6) key6() const { return keys_[5]; } -#undef SUB -#undef KEY_IF_TRUE - /** @} */ -}; // \class NoiseModelFactorN + // aliases for value types pulled from keys + using X = VALUE; + + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using This = NoiseModelFactor1; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor1() override {} + + /** method to retrieve key */ + inline Key key() const { return this->keys_[0]; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN - * Convenient base classes for creating your own NoiseModelFactors with 1-6 - * variables. To derive from these classes, implement evaluateError(). - * - * Note: we cannot use `using NoiseModelFactor1 = NoiseModelFactorN` due - * to class name injection making backwards compatibility difficult. - * - * Note: This has the side-effect that you could e.g. NoiseModelFactor6. - * That is, there is nothing stopping you from using any number of template - * arguments with any `NoiseModelFactorX`. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with X<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 2 variables. To derive from this class, implement evaluateError(). + */ +template +class NoiseModelFactor2 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor2; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor2() override {} + + /** methods to retrieve both keys */ + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor2 + +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with X<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 3 variables. To derive from this class, implement evaluateError(). */ -#define NoiseModelFactor1 NoiseModelFactorN -/** @deprecated */ -#define NoiseModelFactor2 NoiseModelFactorN -/** @deprecated */ -#define NoiseModelFactor3 NoiseModelFactorN -/** @deprecated */ -#define NoiseModelFactor4 NoiseModelFactorN -/** @deprecated */ -#define NoiseModelFactor5 NoiseModelFactorN -/** @deprecated */ -#define NoiseModelFactor6 NoiseModelFactorN +template +class NoiseModelFactor3 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor3; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor3() override {} + + /** methods to retrieve keys */ + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor3 + +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with X<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 4 variables. To derive from this class, implement evaluateError(). + */ +template +class NoiseModelFactor4 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor4; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor4() override {} + + /** methods to retrieve keys */ + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor4 + +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with X<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 5 variables. To derive from this class, implement evaluateError(). + */ +template +class NoiseModelFactor5 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + using X5 = VALUE5; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor5; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor5() override {} + + /** methods to retrieve keys */ + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + inline Key key5() const { return this->keys_[4]; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor5 + +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with X<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 6 variables. To derive from this class, implement evaluateError(). + */ +template +class NoiseModelFactor6 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + using X5 = VALUE5; + using X6 = VALUE6; + + protected: + using Base = NoiseModelFactor; + using This = + NoiseModelFactor6; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor6() override {} + + /** methods to retrieve keys */ + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + inline Key key5() const { return this->keys_[4]; } + inline Key key6() const { return this->keys_[5]; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor6 } // \namespace gtsam From 82e0d205190577e7f3fc669829b9ff1b364bcb67 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 30 Jan 2022 17:26:54 -0500 Subject: [PATCH 20/40] move boost::index_sequence stuff to utilities file --- gtsam/base/utilities.h | 38 ++++++++++++++++++++++++++++++ gtsam/nonlinear/NonlinearFactor.h | 39 +------------------------------ 2 files changed, 39 insertions(+), 38 deletions(-) diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index d9b92b8aa3..22e90d2754 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -27,3 +27,41 @@ struct RedirectCout { }; } + +// boost::index_sequence was introduced in 1.66, so we'll manually define an +// implementation if user has 1.65. boost::index_sequence is used to get array +// indices that align with a parameter pack. +#if BOOST_VERSION >= 106600 +#include +#else +namespace boost { +namespace mp11 { +// Adapted from https://stackoverflow.com/a/32223343/9151520 +template +struct index_sequence { + using type = index_sequence; + using value_type = size_t; + static constexpr std::size_t size() noexcept { return sizeof...(Ints); } +}; +namespace detail { +template +struct _merge_and_renumber; + +template +struct _merge_and_renumber, index_sequence > + : index_sequence {}; +} // namespace detail +template +struct make_index_sequence + : detail::_merge_and_renumber< + typename make_index_sequence::type, + typename make_index_sequence::type> {}; +template <> +struct make_index_sequence<0> : index_sequence<> {}; +template <> +struct make_index_sequence<1> : index_sequence<0> {}; +template +using index_sequence_for = make_index_sequence; +} // namespace mp11 +} // namespace boost +#endif diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 212364af3c..4deef0d3ac 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -25,48 +25,11 @@ #include #include #include +#include // boost::index_sequence #include #include -// boost::index_sequence was introduced in 1.66, so we'll manually define an -// implementation if user has 1.65. boost::index_sequence is used to get array -// indices that align with a parameter pack. -#if BOOST_VERSION >= 106600 -#include -#else -namespace boost { -namespace mp11 { -// Adapted from https://stackoverflow.com/a/32223343/9151520 -template -struct index_sequence { - using type = index_sequence; - using value_type = size_t; - static constexpr std::size_t size() noexcept { return sizeof...(Ints); } -}; -namespace detail { -template -struct _merge_and_renumber; - -template -struct _merge_and_renumber, index_sequence > - : index_sequence {}; -} // namespace detail -template -struct make_index_sequence - : detail::_merge_and_renumber< - typename make_index_sequence::type, - typename make_index_sequence::type> {}; -template <> -struct make_index_sequence<0> : index_sequence<> {}; -template <> -struct make_index_sequence<1> : index_sequence<0> {}; -template -using index_sequence_for = make_index_sequence; -} // namespace mp11 -} // namespace boost -#endif - namespace gtsam { using boost::assign::cref_list_of; From d62033a856f4e78ce0625ff9dd43539d30e39a2a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 30 Jan 2022 17:38:42 -0500 Subject: [PATCH 21/40] fix namespace collision with symbol_shorthand::X in unit test --- tests/testNonlinearFactor.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 27b61cf894..bdc2d576d0 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -382,8 +382,9 @@ class TestFactor4 : public NoiseModelFactor4 { "This type wrong"); public: + static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactor4 Base; - TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4)) {} Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, @@ -446,8 +447,9 @@ TEST(NonlinearFactor, NoiseModelFactor4) { /* ************************************************************************* */ class TestFactor5 : public NoiseModelFactor5 { public: + static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactor5 Base; - TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {} + TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4), X_(5)) {} Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -495,8 +497,9 @@ TEST(NonlinearFactor, NoiseModelFactor5) { /* ************************************************************************* */ class TestFactor6 : public NoiseModelFactor6 { public: + static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactor6 Base; - TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {} + TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4), X_(5), X_(6)) {} Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -551,8 +554,9 @@ TEST(NonlinearFactor, NoiseModelFactor6) { /* ************************************************************************* */ class TestFactorN : public NoiseModelFactorN { public: + static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactorN Base; - TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4)) {} Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, From a2fb0e49d51c9c822ad13f017c2521e572d6274c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 30 Jan 2022 21:30:03 -0500 Subject: [PATCH 22/40] Revert "create backwards compatibility unit test for NoiseModelFactor1" This reverts commit c9dbb6e04085dad6dc7e48c99dbe6b5624a4575f. --- gtsam/nonlinear/tests/priorFactor.xml | 77 ------------------- .../tests/testSerializationNonlinear.cpp | 53 ------------- 2 files changed, 130 deletions(-) delete mode 100644 gtsam/nonlinear/tests/priorFactor.xml diff --git a/gtsam/nonlinear/tests/priorFactor.xml b/gtsam/nonlinear/tests/priorFactor.xml deleted file mode 100644 index 46752262b9..0000000000 --- a/gtsam/nonlinear/tests/priorFactor.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - 1 - 0 - 12345 - - - - - - - - - 6 - - - 0 - - - - 6 - - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - - - - 6 - - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - 1.00000000000000000e+00 - - - - 1.00000000000000000e+00 - 1.00000000000000000e+00 - - - - - - - - 4.11982245665682978e-01 - -8.33737651774156818e-01 - -3.67630462924899259e-01 - -5.87266449276209815e-02 - -4.26917621276207360e-01 - 9.02381585483330806e-01 - -9.09297426825681709e-01 - -3.50175488374014632e-01 - -2.24845095366152908e-01 - - - - 4.00000000000000000e+00 - 5.00000000000000000e+00 - 6.00000000000000000e+00 - - - - - diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index f7b524a5c7..4a73cbb0b4 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -107,59 +107,6 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } -bool str_assert_equal(const string& expected, const string& actual) { - if (actual == expected) return true; - printf("Not equal:\n"); - std::cout << "expected:\n" << expected << std::endl; - std::cout << "actual:\n" << actual << std::endl; - return false; -} -TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { - PriorFactor factor( - 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), - noiseModel::Unit::Create(6)); - - // String - std::string actual_str = serialize(factor); - // Generated from hash 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4) - std::string expected_str = - "22 serialization::archive 19 1 0\n" - "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" - "1 1 0\n" - "2 1 0\n" - "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " - "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " - "1.00000000000000000e+00 6 1.00000000000000000e+00 " - "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " - "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " - "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 " - "-8.33737651774156818e-01 -3.67630462924899259e-01 " - "-5.87266449276209815e-02 -4.26917621276207360e-01 " - "9.02381585483330806e-01 -9.09297426825681709e-01 " - "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " - "4.00000000000000000e+00 5.00000000000000000e+00 " - "6.00000000000000000e+00\n"; - EXPECT(str_assert_equal(expected_str, actual_str)); - PriorFactor factor_deserialized_str = PriorFactor(); - deserializeFromString(expected_str, factor_deserialized_str); - EXPECT(assert_equal(factor, factor_deserialized_str)); - - // XML - std::string actual_xml = serializeXML(factor); - std::string expected_xml; - { // read from file - // Generated from hash 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4) - std::ifstream f("priorFactor.xml"); - std::stringstream buffer; - buffer << f.rdbuf(); - expected_xml = buffer.str(); - } - EXPECT(str_assert_equal(expected_xml, actual_xml)); - PriorFactor factor_deserialized_xml = PriorFactor(); - deserializeFromXMLFile("priorFactor.xml", factor_deserialized_xml); - EXPECT(assert_equal(factor, factor_deserialized_xml)); -} - TEST(Serialization, ISAM2) { gtsam::ISAM2Params parameters; From 1a427cd96c3c00e20c5a4518256523ab966f7270 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 30 Jan 2022 23:48:22 -0500 Subject: [PATCH 23/40] Serialize test strings generated with Boost 1.65 --- gtsam/nonlinear/tests/priorFactor.xml | 77 +++++++++++++++++++ .../tests/testSerializationNonlinear.cpp | 47 +++++++++++ 2 files changed, 124 insertions(+) create mode 100644 gtsam/nonlinear/tests/priorFactor.xml diff --git a/gtsam/nonlinear/tests/priorFactor.xml b/gtsam/nonlinear/tests/priorFactor.xml new file mode 100644 index 0000000000..0c580fb211 --- /dev/null +++ b/gtsam/nonlinear/tests/priorFactor.xml @@ -0,0 +1,77 @@ + + + + + + + + + 1 + 0 + 12345 + + + + + + + + + 6 + + + 0 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + + + + + 4.11982245665682978e-01 + -8.33737651774156818e-01 + -3.67630462924899259e-01 + -5.87266449276209815e-02 + -4.26917621276207360e-01 + 9.02381585483330806e-01 + -9.09297426825681709e-01 + -3.50175488374014632e-01 + -2.24845095366152908e-01 + + + + 4.00000000000000000e+00 + 5.00000000000000000e+00 + 6.00000000000000000e+00 + + + + + diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 4a73cbb0b4..023785f219 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -107,6 +107,53 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } +/** + * Test deserializing from a known serialization generated by code from commit + * 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4) + * We only test that deserialization matches since + * (1) that's the main backward compatibility requirement and + * (2) serialized string depends on boost version + */ +TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { + PriorFactor factor( + 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), + noiseModel::Unit::Create(6)); + + // String + std::string expected_str = + "22 serialization::archive 15 1 0\n" + "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" + "1 1 0\n" + "2 1 0\n" + "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 6 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 " + "-8.33737651774156818e-01 -3.67630462924899259e-01 " + "-5.87266449276209815e-02 -4.26917621276207360e-01 " + "9.02381585483330806e-01 -9.09297426825681709e-01 " + "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " + "4.00000000000000000e+00 5.00000000000000000e+00 " + "6.00000000000000000e+00\n"; + PriorFactor factor_deserialized_str = PriorFactor(); + deserializeFromString(expected_str, factor_deserialized_str); + EXPECT(assert_equal(factor, factor_deserialized_str)); + + // XML + std::string expected_xml; + { // read from file + std::ifstream f("priorFactor.xml"); + std::stringstream buffer; + buffer << f.rdbuf(); + expected_xml = buffer.str(); + } + PriorFactor factor_deserialized_xml = PriorFactor(); + deserializeFromXMLFile("priorFactor.xml", factor_deserialized_xml); + EXPECT(assert_equal(factor, factor_deserialized_xml)); +} + TEST(Serialization, ISAM2) { gtsam::ISAM2Params parameters; From 6653d666ef6e23eab9366545344e689492287edf Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 31 Jan 2022 01:08:47 -0500 Subject: [PATCH 24/40] fix test xml file path --- gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 023785f219..b6b5033a2e 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -142,15 +142,10 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { EXPECT(assert_equal(factor, factor_deserialized_str)); // XML - std::string expected_xml; - { // read from file - std::ifstream f("priorFactor.xml"); - std::stringstream buffer; - buffer << f.rdbuf(); - expected_xml = buffer.str(); - } PriorFactor factor_deserialized_xml = PriorFactor(); - deserializeFromXMLFile("priorFactor.xml", factor_deserialized_xml); + deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR + "/../../gtsam/nonlinear/tests/priorFactor.xml", + factor_deserialized_xml); EXPECT(assert_equal(factor, factor_deserialized_xml)); } From 782a8949882cc851c02f339ce27673185768cd33 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 21 Apr 2022 15:41:44 -0400 Subject: [PATCH 25/40] fix expected serialization string --- gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index b6b5033a2e..63e2ae1dd9 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -120,9 +120,9 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { noiseModel::Unit::Create(6)); // String - std::string expected_str = + std::string serialized_str = "22 serialization::archive 15 1 0\n" - "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" + "0 0 0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" "1 1 0\n" "2 1 0\n" "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " @@ -135,10 +135,9 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { "-5.87266449276209815e-02 -4.26917621276207360e-01 " "9.02381585483330806e-01 -9.09297426825681709e-01 " "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " - "4.00000000000000000e+00 5.00000000000000000e+00 " - "6.00000000000000000e+00\n"; + "4.00000000000000000e+00 5.00000000000000000e+00 6.00000000000000000e+00"; PriorFactor factor_deserialized_str = PriorFactor(); - deserializeFromString(expected_str, factor_deserialized_str); + deserializeFromString(serialized_str, factor_deserialized_str); EXPECT(assert_equal(factor, factor_deserialized_str)); // XML From 71767a4c2bf694a4665eaed1d54b7c9b5598ed3b Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jul 2022 14:57:15 -0400 Subject: [PATCH 26/40] serialization debugging (from stash) --- gtsam/nonlinear/NonlinearFactor.h | 13 +++++++- .../tests/testSerializationNonlinear.cpp | 33 +++++++++++++++++-- 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b69371f162..cafb747b80 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -264,9 +264,13 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + std::cout << "noise model base open " << std::endl; ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); + std::cout << "noise model itself begin" << std::endl; ar & BOOST_SERIALIZATION_NVP(noiseModel_); + std::cout << "noise model itself end" << std::endl; + std::cout << "noise model base close " << std::endl; } }; // \class NoiseModelFactor @@ -462,8 +466,10 @@ class NoiseModelFactorN : public NoiseModelFactor { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + std::cout << "checkpoint N open" << std::endl; ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); + std::cout << "checkpoint N close" << std::endl; } }; // \class NoiseModelFactorN @@ -496,8 +502,13 @@ class NoiseModelFactor1 : public NoiseModelFactorN { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + std::cout << "checkpoint a open " << std::endl; + // ar& boost::serialization::make_nvp( + // "NoiseModelFactor", boost::serialization::base_object(*this)); ar& boost::serialization::make_nvp( - "NoiseModelFactor", boost::serialization::base_object(*this)); + "NoiseModelFactorN", + boost::serialization::base_object>(*this)); + std::cout << "checkpoint a close" << std::endl; } }; // \class NoiseModelFactor1 diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 63e2ae1dd9..5622da07de 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -88,6 +88,7 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { + std::cout << "templatedValues open" << std::endl; EXPECT(equalsObj(pt3)); GenericValue chv1(pt3); EXPECT(equalsObj(chv1)); @@ -105,6 +106,7 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); + std::cout << "templatedValues close" << std::endl; } /** @@ -115,14 +117,21 @@ TEST (Serialization, TemplatedValues) { * (2) serialized string depends on boost version */ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { + std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; PriorFactor factor( 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), noiseModel::Unit::Create(6)); + std::cout << "Serialized: \n" + << serializeToString(factor) << "\nend serialization" << std::endl; + + std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; + // String + std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; std::string serialized_str = "22 serialization::archive 15 1 0\n" - "0 0 0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" + "0 0 0 0 0 0 0 0 0 1 0 12345 0 1 7 21 gtsam_noiseModel_Unit 1 0\n" "1 1 0\n" "2 1 0\n" "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " @@ -135,20 +144,39 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { "-5.87266449276209815e-02 -4.26917621276207360e-01 " "9.02381585483330806e-01 -9.09297426825681709e-01 " "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " - "4.00000000000000000e+00 5.00000000000000000e+00 6.00000000000000000e+00"; + "4.00000000000000000e+00 5.00000000000000000e+00 " + "6.00000000000000000e+00\n"; + + // // serialized_str = serializeToString(factor); + // { + std::cout << "roundtrip start" << std::endl; + PriorFactor factor_deserialized_str_2 = PriorFactor(); + roundtrip(factor, factor_deserialized_str_2); + EXPECT(assert_equal(factor, factor_deserialized_str_2)); + std::cout << "roundtrip end" << std::endl; + // } + PriorFactor factor_deserialized_str = PriorFactor(); + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; deserializeFromString(serialized_str, factor_deserialized_str); + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; EXPECT(assert_equal(factor, factor_deserialized_str)); + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; // XML + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; PriorFactor factor_deserialized_xml = PriorFactor(); + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR "/../../gtsam/nonlinear/tests/priorFactor.xml", factor_deserialized_xml); + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; EXPECT(assert_equal(factor, factor_deserialized_xml)); + std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; } TEST(Serialization, ISAM2) { + std::cout << "ISAM2 open" << std::endl; gtsam::ISAM2Params parameters; gtsam::ISAM2 solver(parameters); @@ -201,6 +229,7 @@ TEST(Serialization, ISAM2) { EXPECT(false); } EXPECT(assert_equal(p1, p2)); + std::cout << "ISAM close" << std::endl; } /* ************************************************************************* */ From 00cf13bd4bed640b10ded2896ce0678b5dcc1e5c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jul 2022 17:19:04 -0400 Subject: [PATCH 27/40] update serialized string --- gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 5622da07de..95906183c9 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -131,7 +131,7 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; std::string serialized_str = "22 serialization::archive 15 1 0\n" - "0 0 0 0 0 0 0 0 0 1 0 12345 0 1 7 21 gtsam_noiseModel_Unit 1 0\n" + "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" "1 1 0\n" "2 1 0\n" "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " From ea6e32de82ae29f95de90cffb5943da4b48b193a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jul 2022 17:19:47 -0400 Subject: [PATCH 28/40] bugfix on serialization --- gtsam/nonlinear/NonlinearFactor.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index cafb747b80..7dc5ea0b0c 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -503,11 +503,8 @@ class NoiseModelFactor1 : public NoiseModelFactorN { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { std::cout << "checkpoint a open " << std::endl; - // ar& boost::serialization::make_nvp( - // "NoiseModelFactor", boost::serialization::base_object(*this)); ar& boost::serialization::make_nvp( - "NoiseModelFactorN", - boost::serialization::base_object>(*this)); + "NoiseModelFactor", boost::serialization::base_object(*this)); std::cout << "checkpoint a close" << std::endl; } }; // \class NoiseModelFactor1 From 83276853b58c64bb222c93ff73005acbb82c7079 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jul 2022 21:41:45 -0400 Subject: [PATCH 29/40] remove debug statements --- gtsam/nonlinear/NonlinearFactor.h | 8 ----- .../tests/testSerializationNonlinear.cpp | 35 ++++--------------- 2 files changed, 6 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 7dc5ea0b0c..b69371f162 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -264,13 +264,9 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - std::cout << "noise model base open " << std::endl; ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); - std::cout << "noise model itself begin" << std::endl; ar & BOOST_SERIALIZATION_NVP(noiseModel_); - std::cout << "noise model itself end" << std::endl; - std::cout << "noise model base close " << std::endl; } }; // \class NoiseModelFactor @@ -466,10 +462,8 @@ class NoiseModelFactorN : public NoiseModelFactor { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - std::cout << "checkpoint N open" << std::endl; ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); - std::cout << "checkpoint N close" << std::endl; } }; // \class NoiseModelFactorN @@ -502,10 +496,8 @@ class NoiseModelFactor1 : public NoiseModelFactorN { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - std::cout << "checkpoint a open " << std::endl; ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); - std::cout << "checkpoint a close" << std::endl; } }; // \class NoiseModelFactor1 diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 95906183c9..860cd225bd 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -88,7 +88,6 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { - std::cout << "templatedValues open" << std::endl; EXPECT(equalsObj(pt3)); GenericValue chv1(pt3); EXPECT(equalsObj(chv1)); @@ -106,7 +105,6 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); - std::cout << "templatedValues close" << std::endl; } /** @@ -117,18 +115,16 @@ TEST (Serialization, TemplatedValues) { * (2) serialized string depends on boost version */ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { - std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; PriorFactor factor( 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), noiseModel::Unit::Create(6)); - std::cout << "Serialized: \n" - << serializeToString(factor) << "\nend serialization" << std::endl; + // roundtrip (sanity check) + PriorFactor factor_deserialized_str_2 = PriorFactor(); + roundtrip(factor, factor_deserialized_str_2); + EXPECT(assert_equal(factor, factor_deserialized_str_2)); - std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; - - // String - std::cout << "checkpoint: __FILE__, __LINE__" << std::endl; + // Deserialize string std::string serialized_str = "22 serialization::archive 15 1 0\n" "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" @@ -147,37 +143,19 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { "4.00000000000000000e+00 5.00000000000000000e+00 " "6.00000000000000000e+00\n"; - // // serialized_str = serializeToString(factor); - // { - std::cout << "roundtrip start" << std::endl; - PriorFactor factor_deserialized_str_2 = PriorFactor(); - roundtrip(factor, factor_deserialized_str_2); - EXPECT(assert_equal(factor, factor_deserialized_str_2)); - std::cout << "roundtrip end" << std::endl; - // } - PriorFactor factor_deserialized_str = PriorFactor(); - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; deserializeFromString(serialized_str, factor_deserialized_str); - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; EXPECT(assert_equal(factor, factor_deserialized_str)); - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; - // XML - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; + // Deserialize XML PriorFactor factor_deserialized_xml = PriorFactor(); - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR "/../../gtsam/nonlinear/tests/priorFactor.xml", factor_deserialized_xml); - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; EXPECT(assert_equal(factor, factor_deserialized_xml)); - std::cout << "checkpoint: " << __FILE__ << ", " << __LINE__ << std::endl; } TEST(Serialization, ISAM2) { - std::cout << "ISAM2 open" << std::endl; - gtsam::ISAM2Params parameters; gtsam::ISAM2 solver(parameters); gtsam::NonlinearFactorGraph graph; @@ -229,7 +207,6 @@ TEST(Serialization, ISAM2) { EXPECT(false); } EXPECT(assert_equal(p1, p2)); - std::cout << "ISAM close" << std::endl; } /* ************************************************************************* */ From fa196aa638de2b19004d07dd6aa4f69221cd1c10 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 20 Jul 2022 00:15:58 -0400 Subject: [PATCH 30/40] turn off backwards compatibility test with quaternions or TBB since serialization structure is different --- gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 860cd225bd..f402656c12 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -109,10 +109,13 @@ TEST (Serialization, TemplatedValues) { /** * Test deserializing from a known serialization generated by code from commit - * 3ba65669113f41d0e56a03b4b314047776bab5c4 (>4.2a4) + * 0af17f438f62f4788f3a572ecd36d06d224fd1e1 (>4.2a7) * We only test that deserialization matches since * (1) that's the main backward compatibility requirement and * (2) serialized string depends on boost version + * Also note: we don't run this test when quaternions or TBB are enabled since + * serialization structures are different and the serialized strings/xml are + * hard-coded in this test. */ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { PriorFactor factor( @@ -124,6 +127,7 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { roundtrip(factor, factor_deserialized_str_2); EXPECT(assert_equal(factor, factor_deserialized_str_2)); +#if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB) // Deserialize string std::string serialized_str = "22 serialization::archive 15 1 0\n" @@ -153,6 +157,7 @@ TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { "/../../gtsam/nonlinear/tests/priorFactor.xml", factor_deserialized_xml); EXPECT(assert_equal(factor, factor_deserialized_xml)); +#endif } TEST(Serialization, ISAM2) { From 322e5551f706f876d0cb5226690492eaffe245de Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 16 Nov 2022 14:56:18 -0500 Subject: [PATCH 31/40] address review comments --- gtsam/nonlinear/NonlinearFactor.h | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b69371f162..0150b8b511 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -308,7 +308,7 @@ class NoiseModelFactorN : public NoiseModelFactor { /// N is the number of variables (N-way factor) enum { N = sizeof...(VALUES) }; - /** The type of the i'th template param can be obtained as X */ + /// The type of the i'th template param can be obtained as X template ::type = true> using X = typename std::tuple_element>::type; @@ -330,9 +330,7 @@ class NoiseModelFactorN : public NoiseModelFactor { /// @name Constructors /// @{ - /** - * Default Constructor for I/O - */ + /// Default Constructor for I/O NoiseModelFactorN() {} /** @@ -362,7 +360,7 @@ class NoiseModelFactorN : public NoiseModelFactor { ~NoiseModelFactorN() override {} - /** Returns a key. Usage: `key()` returns the I'th key. */ + /// Returns a key. Usage: `key()` returns the I'th key. template inline typename std::enable_if<(I < N), Key>::type key() const { return keys_[I]; @@ -474,7 +472,7 @@ class NoiseModelFactorN : public NoiseModelFactor { * with 1 variable. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor1 : public NoiseModelFactorN { +class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { public: // aliases for value types pulled from keys using X = VALUE; @@ -508,7 +506,7 @@ class NoiseModelFactor1 : public NoiseModelFactorN { * with 2 variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor2 : public NoiseModelFactorN { +class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN { public: // aliases for value types pulled from keys using X1 = VALUE1; @@ -544,7 +542,7 @@ class NoiseModelFactor2 : public NoiseModelFactorN { * with 3 variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor3 : public NoiseModelFactorN { +class GTSAM_DEPRECATED NoiseModelFactor3 : public NoiseModelFactorN { public: // aliases for value types pulled from keys using X1 = VALUE1; @@ -582,7 +580,7 @@ class NoiseModelFactor3 : public NoiseModelFactorN { * with 4 variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor4 +class GTSAM_DEPRECATED NoiseModelFactor4 : public NoiseModelFactorN { public: // aliases for value types pulled from keys @@ -623,7 +621,7 @@ class NoiseModelFactor4 * with 5 variables. To derive from this class, implement evaluateError(). */ template -class NoiseModelFactor5 +class GTSAM_DEPRECATED NoiseModelFactor5 : public NoiseModelFactorN { public: // aliases for value types pulled from keys @@ -668,7 +666,7 @@ class NoiseModelFactor5 */ template -class NoiseModelFactor6 +class GTSAM_DEPRECATED NoiseModelFactor6 : public NoiseModelFactorN { public: // aliases for value types pulled from keys From 94865c456203ca54312b1e6db4a29380c928d983 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 19 Dec 2022 17:20:58 -0500 Subject: [PATCH 32/40] fix boost 1.65 patch bug --- gtsam/base/utilities.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index 22e90d2754..0a05a704c0 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -31,6 +31,7 @@ struct RedirectCout { // boost::index_sequence was introduced in 1.66, so we'll manually define an // implementation if user has 1.65. boost::index_sequence is used to get array // indices that align with a parameter pack. +#include #if BOOST_VERSION >= 106600 #include #else From 63950b952bdc4cf3a9e0510854488e6f84475a95 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 19 Dec 2022 17:41:21 -0500 Subject: [PATCH 33/40] Revert "fix namespace collision with symbol_shorthand::X in unit test" This reverts commit d62033a856f4e78ce0625ff9dd43539d30e39a2a. --- tests/testNonlinearFactor.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index bdc2d576d0..27b61cf894 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -382,9 +382,8 @@ class TestFactor4 : public NoiseModelFactor4 { "This type wrong"); public: - static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactor4 Base; - TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4)) {} + TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, @@ -447,9 +446,8 @@ TEST(NonlinearFactor, NoiseModelFactor4) { /* ************************************************************************* */ class TestFactor5 : public NoiseModelFactor5 { public: - static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactor5 Base; - TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4), X_(5)) {} + TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {} Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -497,9 +495,8 @@ TEST(NonlinearFactor, NoiseModelFactor5) { /* ************************************************************************* */ class TestFactor6 : public NoiseModelFactor6 { public: - static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactor6 Base; - TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4), X_(5), X_(6)) {} + TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {} Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -554,9 +551,8 @@ TEST(NonlinearFactor, NoiseModelFactor6) { /* ************************************************************************* */ class TestFactorN : public NoiseModelFactorN { public: - static constexpr auto X_ = symbol_shorthand::X; // collision with X<1> typedef NoiseModelFactorN Base; - TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X_(1), X_(2), X_(3), X_(4)) {} + TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, From 0ebc6e881d92eecce566cd1b2f8d7cdc18d55070 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 19 Dec 2022 18:06:34 -0500 Subject: [PATCH 34/40] Change `X` to `ValueType` and `VALUES` -> `ValueTypes` --- gtsam/nonlinear/NonlinearFactor.h | 58 +++++++++++++++---------------- tests/testNonlinearFactor.cpp | 28 +++++++++++++++ 2 files changed, 57 insertions(+), 29 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 0150b8b511..a54c7558b3 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -302,27 +302,27 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * are typically more general than just vectors, e.g., Rot3 or Pose3, which are * objects in non-linear manifolds (Lie groups). */ -template +template class NoiseModelFactorN : public NoiseModelFactor { public: /// N is the number of variables (N-way factor) - enum { N = sizeof...(VALUES) }; + enum { N = sizeof...(ValueTypes) }; - /// The type of the i'th template param can be obtained as X + /// The type of the i'th template param can be obtained as ValueType template ::type = true> - using X = typename std::tuple_element>::type; + using ValueType = typename std::tuple_element>::type; protected: using Base = NoiseModelFactor; - using This = NoiseModelFactorN; + using This = NoiseModelFactorN; /* Like std::void_t, except produces `boost::optional` instead. Used - * to expand fixed-type parameter-packs with same length as VALUES */ + * to expand fixed-type parameter-packs with same length as ValueTypes */ template using optional_matrix_type = boost::optional; /* Like std::void_t, except produces `Key` instead. Used to expand fixed-type - * parameter-packs with same length as VALUES */ + * parameter-packs with same length as ValueTypes */ template using key_type = Key; @@ -341,7 +341,7 @@ class NoiseModelFactorN : public NoiseModelFactor { * arguments. */ NoiseModelFactorN(const SharedNoiseModel& noiseModel, - key_type... keys) + key_type... keys) : Base(noiseModel, std::array{keys...}) {} /** @@ -379,7 +379,7 @@ class NoiseModelFactorN : public NoiseModelFactor { Vector unwhitenedError( const Values& x, boost::optional&> H = boost::none) const override { - return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); + return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); } /// @} @@ -406,8 +406,8 @@ class NoiseModelFactorN : public NoiseModelFactor { * as separate arguments. * @param[out] H The Jacobian with respect to each variable (optional). */ - virtual Vector evaluateError(const VALUES&... x, - optional_matrix_type... H) const = 0; + virtual Vector evaluateError(const ValueTypes&... x, + optional_matrix_type... H) const = 0; /// @} /// @name Convenience method overloads @@ -419,8 +419,8 @@ class NoiseModelFactorN : public NoiseModelFactor { * * e.g. `Vector error = factor.evaluateError(x1, x2, x3);` */ - inline Vector evaluateError(const VALUES&... x) const { - return evaluateError(x..., optional_matrix_type()...); + inline Vector evaluateError(const ValueTypes&... x) const { + return evaluateError(x..., optional_matrix_type()...); } /** Some optional jacobians omitted function overload */ @@ -428,7 +428,7 @@ class NoiseModelFactorN : public NoiseModelFactor { typename std::enable_if<(sizeof...(OptionalJacArgs) > 0) && (sizeof...(OptionalJacArgs) < N), bool>::type = true> - inline Vector evaluateError(const VALUES&... x, + inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const { return evaluateError(x..., std::forward(H)..., boost::none); @@ -447,9 +447,9 @@ class NoiseModelFactorN : public NoiseModelFactor { boost::optional&> H = boost::none) const { if (this->active(x)) { if (H) { - return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); + return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); } else { - return evaluateError(x.at(keys_[Inds])...); + return evaluateError(x.at(keys_[Inds])...); } } else { return Vector::Zero(this->dim()); @@ -466,15 +466,15 @@ class NoiseModelFactorN : public NoiseModelFactor { }; // \class NoiseModelFactorN /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 - * with X<1>. +/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<0> and X1 + * with ValueType<0>. * A convenient base class for creating your own NoiseModelFactor * with 1 variable. To derive from this class, implement evaluateError(). */ template class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { public: - // aliases for value types pulled from keys + // aliases for value types pulled from keys, for backwards compatibility using X = VALUE; protected: @@ -500,8 +500,8 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { }; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with X<1>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 + * with ValueType<0>. * A convenient base class for creating your own NoiseModelFactor * with 2 variables. To derive from this class, implement evaluateError(). */ @@ -536,8 +536,8 @@ class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN and X1 - * with X<1>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 + * with ValueType<0>. * A convenient base class for creating your own NoiseModelFactor * with 3 variables. To derive from this class, implement evaluateError(). */ @@ -574,8 +574,8 @@ class GTSAM_DEPRECATED NoiseModelFactor3 : public NoiseModelFactorN and X1 - * with X<1>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 + * with ValueType<0>. * A convenient base class for creating your own NoiseModelFactor * with 4 variables. To derive from this class, implement evaluateError(). */ @@ -615,8 +615,8 @@ class GTSAM_DEPRECATED NoiseModelFactor4 }; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with X<1>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 + * with ValueType<0>. * A convenient base class for creating your own NoiseModelFactor * with 5 variables. To derive from this class, implement evaluateError(). */ @@ -659,8 +659,8 @@ class GTSAM_DEPRECATED NoiseModelFactor5 }; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with X<1>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 + * with ValueType<0>. * A convenient base class for creating your own NoiseModelFactor * with 6 variables. To derive from this class, implement evaluateError(). */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 27b61cf894..d060f663e6 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -441,6 +441,20 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal(tf.key4(), X(4))); std::vector H = {Matrix(), Matrix(), Matrix(), Matrix()}; EXPECT(assert_equal(Vector1(10.0), tf.unwhitenedError(tv, H))); + + // And test "forward compatibility" using `key` and `ValueType` too + static_assert(std::is_same, double>::value, + "ValueType<0> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<1> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<2> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<3> type incorrect"); + EXPECT(assert_equal(tf.key<0>(), X(1))); + EXPECT(assert_equal(tf.key<1>(), X(2))); + EXPECT(assert_equal(tf.key<2>(), X(3))); + EXPECT(assert_equal(tf.key<3>(), X(4))); } /* ************************************************************************* */ @@ -615,6 +629,20 @@ TEST(NonlinearFactor, NoiseModelFactorN) { EXPECT(assert_equal(H2_expected, H2)); EXPECT(assert_equal(H3_expected, H3)); EXPECT(assert_equal(H4_expected, H4)); + + // Test using `key` and `ValueType` + static_assert(std::is_same, double>::value, + "ValueType<0> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<1> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<2> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<3> type incorrect"); + EXPECT(assert_equal(tf.key<0>(), X(1))); + EXPECT(assert_equal(tf.key<1>(), X(2))); + EXPECT(assert_equal(tf.key<2>(), X(3))); + EXPECT(assert_equal(tf.key<3>(), X(4))); } /* ************************************************************************* */ From b24511fb18080db085338b2630b206c61cbd91a0 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 19 Dec 2022 18:49:20 -0500 Subject: [PATCH 35/40] address review comments --- gtsam/nonlinear/NonlinearFactor.h | 29 +++++++++++++++++------------ tests/testNonlinearFactor.cpp | 14 ++++++++++++++ 2 files changed, 31 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index a54c7558b3..33640f0b71 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -310,7 +310,8 @@ class NoiseModelFactorN : public NoiseModelFactor { /// The type of the i'th template param can be obtained as ValueType template ::type = true> - using ValueType = typename std::tuple_element>::type; + using ValueType = + typename std::tuple_element>::type; protected: using Base = NoiseModelFactor; @@ -319,12 +320,12 @@ class NoiseModelFactorN : public NoiseModelFactor { /* Like std::void_t, except produces `boost::optional` instead. Used * to expand fixed-type parameter-packs with same length as ValueTypes */ template - using optional_matrix_type = boost::optional; + using OptionalMatrix = boost::optional; /* Like std::void_t, except produces `Key` instead. Used to expand fixed-type * parameter-packs with same length as ValueTypes */ template - using key_type = Key; + using KeyType = Key; public: /// @name Constructors @@ -341,16 +342,17 @@ class NoiseModelFactorN : public NoiseModelFactor { * arguments. */ NoiseModelFactorN(const SharedNoiseModel& noiseModel, - key_type... keys) + KeyType... keys) : Base(noiseModel, std::array{keys...}) {} /** - * Constructor. Only enabled for n-ary factors where n > 1. + * Constructor. + * Example usage: NoiseModelFactorN(noise, {key1, key2, ..., keyN}) + * Example usage: NoiseModelFactorN(noise, keys); where keys is a vector * @param noiseModel Shared pointer to noise model. * @param keys A container of keys for the variables in this factor. */ - template 1), bool>::type = true> + template > NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { assert(keys.size() == N); @@ -379,7 +381,8 @@ class NoiseModelFactorN : public NoiseModelFactor { Vector unwhitenedError( const Values& x, boost::optional&> H = boost::none) const override { - return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); + return unwhitenedError(boost::mp11::index_sequence_for{}, x, + H); } /// @} @@ -407,7 +410,7 @@ class NoiseModelFactorN : public NoiseModelFactor { * @param[out] H The Jacobian with respect to each variable (optional). */ virtual Vector evaluateError(const ValueTypes&... x, - optional_matrix_type... H) const = 0; + OptionalMatrix... H) const = 0; /// @} /// @name Convenience method overloads @@ -420,7 +423,7 @@ class NoiseModelFactorN : public NoiseModelFactor { * e.g. `Vector error = factor.evaluateError(x1, x2, x3);` */ inline Vector evaluateError(const ValueTypes&... x) const { - return evaluateError(x..., optional_matrix_type()...); + return evaluateError(x..., OptionalMatrix()...); } /** Some optional jacobians omitted function overload */ @@ -506,7 +509,8 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { * with 2 variables. To derive from this class, implement evaluateError(). */ template -class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN { +class GTSAM_DEPRECATED NoiseModelFactor2 + : public NoiseModelFactorN { public: // aliases for value types pulled from keys using X1 = VALUE1; @@ -542,7 +546,8 @@ class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN -class GTSAM_DEPRECATED NoiseModelFactor3 : public NoiseModelFactorN { +class GTSAM_DEPRECATED NoiseModelFactor3 + : public NoiseModelFactorN { public: // aliases for value types pulled from keys using X1 = VALUE1; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index d060f663e6..349e2cd86e 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -338,6 +338,7 @@ class TestFactor1 : public NoiseModelFactor1 { public: typedef NoiseModelFactor1 Base; TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} + using Base::NoiseModelFactor1; // inherit constructors Vector evaluateError(const double& x1, boost::optional H1 = boost::none) const override { @@ -371,6 +372,10 @@ TEST(NonlinearFactor, NoiseModelFactor1) { EXPECT(assert_equal(tf.key(), L(1))); std::vector H = {Matrix()}; EXPECT(assert_equal(Vector1(1.0), tf.unwhitenedError(tv, H))); + + // Test constructors + TestFactor1 tf2(noiseModel::Unit::Create(1), L(1)); + TestFactor1 tf3(noiseModel::Unit::Create(1), {L(1)}); } /* ************************************************************************* */ @@ -384,6 +389,7 @@ class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 Base; TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + using Base::NoiseModelFactor4; // inherit constructors Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, @@ -455,6 +461,14 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal(tf.key<1>(), X(2))); EXPECT(assert_equal(tf.key<2>(), X(3))); EXPECT(assert_equal(tf.key<3>(), X(4))); + + // Test constructors + TestFactor4 tf2(noiseModel::Unit::Create(1), L(1), L(2), L(3), L(4)); + TestFactor4 tf3(noiseModel::Unit::Create(1), {L(1), L(2), L(3), L(4)}); + TestFactor4 tf4(noiseModel::Unit::Create(1), + std::array{L(1), L(2), L(3), L(4)}); + std::vector keys = {L(1), L(2), L(3), L(4)}; + TestFactor4 tf5(noiseModel::Unit::Create(1), keys); } /* ************************************************************************* */ From e8ddbbebff6cb114fcfc986cd9b43cbfbdfabe29 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 19 Dec 2022 20:14:12 -0500 Subject: [PATCH 36/40] Check type of CONTAINER constructor tparam This is a byproduct of the overload resolution problem when N=1, then it can be hard to differentiate between: NoiseModelFactorN(noise, key) NoiseModelFactorN(noise, {key}) --- gtsam/nonlinear/NonlinearFactor.h | 7 ++++++- tests/testNonlinearFactor.cpp | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 33640f0b71..2f4b27d39d 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -352,7 +352,12 @@ class NoiseModelFactorN : public NoiseModelFactor { * @param noiseModel Shared pointer to noise model. * @param keys A container of keys for the variables in this factor. */ - template > + template , + // check that CONTAINER is a container of Keys: + typename T = typename std::decay< + decltype(*std::declval().begin())>::type, + typename std::enable_if::value, + bool>::type = true> NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { assert(keys.size() == N); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 349e2cd86e..9c4b4cff17 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -376,6 +376,7 @@ TEST(NonlinearFactor, NoiseModelFactor1) { // Test constructors TestFactor1 tf2(noiseModel::Unit::Create(1), L(1)); TestFactor1 tf3(noiseModel::Unit::Create(1), {L(1)}); + TestFactor1 tf4(noiseModel::Unit::Create(1), gtsam::Symbol('L', 1)); } /* ************************************************************************* */ From 040eb63949d9a3e18ba9ba3cd0192ade10fc4b95 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 13:18:15 -0500 Subject: [PATCH 37/40] make SFINAE templates more readable --- gtsam/nonlinear/NonlinearFactor.h | 86 ++++++++++++++++++++++--------- 1 file changed, 61 insertions(+), 25 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2f4b27d39d..1d2bff8a7f 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -308,26 +308,58 @@ class NoiseModelFactorN : public NoiseModelFactor { /// N is the number of variables (N-way factor) enum { N = sizeof...(ValueTypes) }; - /// The type of the i'th template param can be obtained as ValueType - template ::type = true> - using ValueType = - typename std::tuple_element>::type; - protected: using Base = NoiseModelFactor; using This = NoiseModelFactorN; - /* Like std::void_t, except produces `boost::optional` instead. Used - * to expand fixed-type parameter-packs with same length as ValueTypes */ + /// @name SFINAE aliases + /// @{ + + template + using IsConvertible = + typename std::enable_if::value, void>::type; + + template + using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N), + void>::type; // 1-indexed! + + template + using ContainerElementType = + typename std::decay().begin())>::type; + template + using IsContainerOfKeys = IsConvertible, Key>; + + /// @} + + /* Like std::void_t, except produces `boost::optional` instead of + * `void`. Used to expand fixed-type parameter-packs with same length as + * ValueTypes. */ template using OptionalMatrix = boost::optional; - /* Like std::void_t, except produces `Key` instead. Used to expand fixed-type - * parameter-packs with same length as ValueTypes */ + /* Like std::void_t, except produces `Key` instead of `void`. Used to expand + * fixed-type parameter-packs with same length as ValueTypes. */ template using KeyType = Key; public: + /** + * The type of the I'th template param can be obtained as ValueType. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * using Factor = NoiseModelFactorN; + * Factor::ValueType<1> // Pose3 + * Factor::ValueType<2> // Point3 + * // Factor::ValueType<0> // ERROR! Will not compile. + * // Factor::ValueType<3> // ERROR! Will not compile. + * ``` + */ + template > + using ValueType = + typename std::tuple_element>::type; + + public: + /// @name Constructors /// @{ @@ -353,11 +385,7 @@ class NoiseModelFactorN : public NoiseModelFactor { * @param keys A container of keys for the variables in this factor. */ template , - // check that CONTAINER is a container of Keys: - typename T = typename std::decay< - decltype(*std::declval().begin())>::type, - typename std::enable_if::value, - bool>::type = true> + typename = IsContainerOfKeys> NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { assert(keys.size() == N); @@ -367,10 +395,19 @@ class NoiseModelFactorN : public NoiseModelFactor { ~NoiseModelFactorN() override {} - /// Returns a key. Usage: `key()` returns the I'th key. - template - inline typename std::enable_if<(I < N), Key>::type key() const { - return keys_[I]; + /** Returns a key. Usage: `key()` returns the I'th key. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * NoiseModelFactorN factor(noise, key1, key2); + * key<1>() // = key1 + * key<2>() // = key2 + * // key<0>() // ERROR! Will not compile + * // key<3>() // ERROR! Will not compile + * ``` + */ + template > + inline Key key() const { + return keys_[I - 1]; } /// @name NoiseModelFactor methods @@ -433,9 +470,7 @@ class NoiseModelFactorN : public NoiseModelFactor { /** Some optional jacobians omitted function overload */ template 0) && - (sizeof...(OptionalJacArgs) < N), - bool>::type = true> + typename = IndexIsValid> inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const { return evaluateError(x..., std::forward(H)..., @@ -448,16 +483,17 @@ class NoiseModelFactorN : public NoiseModelFactor { /** Pack expansion with index_sequence template pattern, used to index into * `keys_` and `H` */ - template + template inline Vector unwhitenedError( - boost::mp11::index_sequence, // + boost::mp11::index_sequence, // const Values& x, boost::optional&> H = boost::none) const { if (this->active(x)) { if (H) { - return evaluateError(x.at(keys_[Inds])..., (*H)[Inds]...); + return evaluateError(x.at(keys_[Indices])..., + (*H)[Indices]...); } else { - return evaluateError(x.at(keys_[Inds])...); + return evaluateError(x.at(keys_[Indices])...); } } else { return Vector::Zero(this->dim()); From d16d26394ed6f8137bf545f3c8d102559be801c6 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 14:26:16 -0500 Subject: [PATCH 38/40] better docstrings w/ usage examples --- gtsam/nonlinear/NonlinearFactor.h | 129 ++++++++++++++++++++---------- 1 file changed, 86 insertions(+), 43 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 1d2bff8a7f..dfab4542ce 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -277,25 +277,48 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { * A convenient base class for creating your own NoiseModelFactor * with n variables. To derive from this class, implement evaluateError(). * - * For example, a 2-way factor could be implemented like so: + * For example, a 2-way factor that computes the difference in x-translation + * between a Pose3 and Point3 could be implemented like so: * * ~~~~~~~~~~~~~~~~~~~~{.cpp} - * class MyFactor : public NoiseModelFactorN { + * class MyFactor : public NoiseModelFactorN { * public: - * using Base = NoiseModelFactorN; + * using Base = NoiseModelFactorN; * - * MyFactor(Key key1, Key key2, const SharedNoiseModel& noiseModel) - * : Base(noiseModel, key1, key2) {} + * MyFactor(Key pose_key, Key point_key, const SharedNoiseModel& noiseModel) + * : Base(noiseModel, pose_key, point_key) {} * * Vector evaluateError( - * const double& x1, const double& x2, - * boost::optional H1 = boost::none, - * boost::optional H2 = boost::none) const override { - * if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); - * if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); - * return (Vector(1) << x1 + 2 * x2).finished(); + * const Pose3& T, const Point3& p, + * boost::optional H_T = boost::none, + * boost::optional H_p = boost::none) const override { + * Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T + * + * // Only compute t_H_T if needed: + * Point3 t = T.translation(H_T ? &t_H_T : 0); + * double a = t(0); // a_H_t = [1, 0, 0] + * double b = p(0); // b_H_p = [1, 0, 0] + * double error = a - b; // H_a = 1, H_b = -1 + * + * // H_T = H_a * a_H_t * t_H_T = the first row of t_H_T + * if (H_T) *H_T = (Matrix(1, 6) << t_H_T.row(0)).finished(); + * // H_p = H_b * b_H_p = -1 * [1, 0, 0] + * if (H_p) *H_p = (Matrix(1, 3) << -1., 0., 0.).finished(); + * + * return Vector1(error); * } * }; + * + * // Unit Test + * TEST(NonlinearFactor, MyFactor) { + * MyFactor f(X(1), X(2), noiseModel::Unit::Create(1)); + * EXPECT_DOUBLES_EQUAL(-8., f.evaluateError(Pose3(), Point3(8., 7., 6.))(0), + * 1e-9); + * Values values; + * values.insert(X(1), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3))); + * values.insert(X(2), Point3(1, 2, 3)); + * EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); + * } * ~~~~~~~~~~~~~~~~~~~~ * * These factors are templated on a values structure type. The values structures @@ -379,8 +402,8 @@ class NoiseModelFactorN : public NoiseModelFactor { /** * Constructor. - * Example usage: NoiseModelFactorN(noise, {key1, key2, ..., keyN}) - * Example usage: NoiseModelFactorN(noise, keys); where keys is a vector + * Example usage: `NoiseModelFactorN(noise, {key1, key2, ..., keyN})` + * Example usage: `NoiseModelFactorN(noise, keys)` where keys is a vector * @param noiseModel Shared pointer to noise model. * @param keys A container of keys for the variables in this factor. */ @@ -388,7 +411,10 @@ class NoiseModelFactorN : public NoiseModelFactor { typename = IsContainerOfKeys> NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) : Base(noiseModel, keys) { - assert(keys.size() == N); + if (keys.size() != N) { + throw std::invalid_argument( + "NoiseModelFactorN: wrong number of keys given"); + } } /// @} @@ -413,12 +439,21 @@ class NoiseModelFactorN : public NoiseModelFactor { /// @name NoiseModelFactor methods /// @{ - /** Calls the n-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. + /** This implements the `unwhitenedError` virtual function by calling the + * n-key specific version of evaluateError, which is pure virtual so must be + * implemented in the derived class. + * + * Example usage: + * ``` + * gtsam::Values values; + * values.insert(...) // populate values + * std::vector Hs(2); // this will be an optional output argument + * const Vector error = factor.unwhitenedError(values, Hs); + * ``` * @param[in] x A Values object containing the values of all the variables * used in this factor * @param[out] H A vector of (dynamic) matrices whose size should be equal to - * n. The jacobians w.r.t. each variable will be output in this parameter. + * n. The Jacobians w.r.t. each variable will be output in this parameter. */ Vector unwhitenedError( const Values& x, @@ -430,20 +465,22 @@ class NoiseModelFactorN : public NoiseModelFactor { /// @} /// @name Virtual methods /// @{ + /** - * Override this method to finish implementing an n-way factor. + * Override `evaluateError` to finish implementing an n-way factor. * - * Both the `x` and `H` arguments are written here as parameter packs, but + * Both the `x` and `H` arguments are written here as parameter packs, but * when overriding this method, you probably want to explicitly write them - * out. For example, for a 2-way factor with variable types Pose3 and double: + * out. For example, for a 2-way factor with variable types Pose3 and Point3, + * you should implement: * ``` - * Vector evaluateError(const Pose3& x1, const double& x2, - * boost::optional H1 = boost::none, - * boost::optional H2 = boost::none) const - * override {...} + * Vector evaluateError( + * const Pose3& x1, const Point3& x2, + * boost::optional H1 = boost::none, + * boost::optional H2 = boost::none) const override { ... } * ``` * - * If any of the optional Matrix reference arguments are specified, it should + * If any of the optional Matrix reference arguments are specified, it should * compute both the function evaluation and its derivative(s) in the requested * variables. * @@ -458,17 +495,20 @@ class NoiseModelFactorN : public NoiseModelFactor { /// @name Convenience method overloads /// @{ - /** No-jacobians requested function overload (since parameter packs can't have - * default args). This specializes the version below to avoid recursive calls - * since this is commonly used. + /** No-Jacobians requested function overload. + * This specializes the version below to avoid recursive calls since this is + * commonly used. * - * e.g. `Vector error = factor.evaluateError(x1, x2, x3);` + * e.g. `const Vector error = factor.evaluateError(pose, point);` */ inline Vector evaluateError(const ValueTypes&... x) const { return evaluateError(x..., OptionalMatrix()...); } - /** Some optional jacobians omitted function overload */ + /** Some (but not all) optional Jacobians are omitted (function overload) + * + * e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);` + */ template > inline Vector evaluateError(const ValueTypes&... x, @@ -481,7 +521,10 @@ class NoiseModelFactorN : public NoiseModelFactor { private: /** Pack expansion with index_sequence template pattern, used to index into - * `keys_` and `H` + * `keys_` and `H`. + * + * Example: For `NoiseModelFactorN`, the call would look like: + * `const Vector error = unwhitenedError(0, 1, values, H);` */ template inline Vector unwhitenedError( @@ -510,8 +553,8 @@ class NoiseModelFactorN : public NoiseModelFactor { }; // \class NoiseModelFactorN /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<0> and X1 - * with ValueType<0>. +/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 + * with ValueType<1>. * A convenient base class for creating your own NoiseModelFactor * with 1 variable. To derive from this class, implement evaluateError(). */ @@ -544,8 +587,8 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { }; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 - * with ValueType<0>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. * A convenient base class for creating your own NoiseModelFactor * with 2 variables. To derive from this class, implement evaluateError(). */ @@ -581,8 +624,8 @@ class GTSAM_DEPRECATED NoiseModelFactor2 }; // \class NoiseModelFactor2 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 - * with ValueType<0>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. * A convenient base class for creating your own NoiseModelFactor * with 3 variables. To derive from this class, implement evaluateError(). */ @@ -620,8 +663,8 @@ class GTSAM_DEPRECATED NoiseModelFactor3 }; // \class NoiseModelFactor3 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 - * with ValueType<0>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. * A convenient base class for creating your own NoiseModelFactor * with 4 variables. To derive from this class, implement evaluateError(). */ @@ -661,8 +704,8 @@ class GTSAM_DEPRECATED NoiseModelFactor4 }; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 - * with ValueType<0>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. * A convenient base class for creating your own NoiseModelFactor * with 5 variables. To derive from this class, implement evaluateError(). */ @@ -705,8 +748,8 @@ class GTSAM_DEPRECATED NoiseModelFactor5 }; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1 - * with ValueType<0>. +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. * A convenient base class for creating your own NoiseModelFactor * with 6 variables. To derive from this class, implement evaluateError(). */ From 4b93970b3490101b181ce6e25d352bb151390bad Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 14:29:15 -0500 Subject: [PATCH 39/40] Change backwards-compatibility defs to utilize new style --- gtsam/nonlinear/NonlinearFactor.h | 150 +++++++++++++++++++----------- tests/testNonlinearFactor.cpp | 4 + 2 files changed, 100 insertions(+), 54 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index dfab4542ce..ad52355e42 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -561,8 +561,13 @@ class NoiseModelFactorN : public NoiseModelFactor { template class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { public: - // aliases for value types pulled from keys, for backwards compatibility - using X = VALUE; + /** Aliases for value types pulled from keys, for backwards compatibility. + * Note: in your code you can probably just do: + * `using X = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X = typename NoiseModelFactor1::template ValueType<1>; protected: using Base = NoiseModelFactor; // grandparent, for backwards compatibility @@ -573,8 +578,10 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} - /** method to retrieve key */ - inline Key key() const { return this->keys_[0]; } + /** Method to retrieve key. + * Similar to `ValueType`, you can probably do `return key<1>();` + */ + inline Key key() const { return NoiseModelFactorN::template key<1>(); } private: /** Serialization function */ @@ -596,9 +603,14 @@ template class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN { public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor2::template ValueType<1>; + using X2 = typename NoiseModelFactor2::template ValueType<2>; protected: using Base = NoiseModelFactor; @@ -609,9 +621,11 @@ class GTSAM_DEPRECATED NoiseModelFactor2 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor2() override {} - /** methods to retrieve both keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` + */ + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } private: /** Serialization function */ @@ -633,10 +647,15 @@ template class GTSAM_DEPRECATED NoiseModelFactor3 : public NoiseModelFactorN { public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor3::template ValueType<1>; + using X2 = typename NoiseModelFactor3::template ValueType<2>; + using X3 = typename NoiseModelFactor3::template ValueType<3>; protected: using Base = NoiseModelFactor; @@ -647,10 +666,12 @@ class GTSAM_DEPRECATED NoiseModelFactor3 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor3() override {} - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` + */ + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } private: /** Serialization function */ @@ -672,11 +693,16 @@ template class GTSAM_DEPRECATED NoiseModelFactor4 : public NoiseModelFactorN { public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor4::template ValueType<1>; + using X2 = typename NoiseModelFactor4::template ValueType<2>; + using X3 = typename NoiseModelFactor4::template ValueType<3>; + using X4 = typename NoiseModelFactor4::template ValueType<4>; protected: using Base = NoiseModelFactor; @@ -687,11 +713,13 @@ class GTSAM_DEPRECATED NoiseModelFactor4 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor4() override {} - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` + */ + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } + inline Key key4() const { return this->template key<4>(); } private: /** Serialization function */ @@ -713,12 +741,17 @@ template class GTSAM_DEPRECATED NoiseModelFactor5 : public NoiseModelFactorN { public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - using X5 = VALUE5; + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor5::template ValueType<1>; + using X2 = typename NoiseModelFactor5::template ValueType<2>; + using X3 = typename NoiseModelFactor5::template ValueType<3>; + using X4 = typename NoiseModelFactor5::template ValueType<4>; + using X5 = typename NoiseModelFactor5::template ValueType<5>; protected: using Base = NoiseModelFactor; @@ -730,12 +763,14 @@ class GTSAM_DEPRECATED NoiseModelFactor5 VALUE5>::NoiseModelFactorN; ~NoiseModelFactor5() override {} - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - inline Key key5() const { return this->keys_[4]; } + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` + */ + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } + inline Key key4() const { return this->template key<4>(); } + inline Key key5() const { return this->template key<5>(); } private: /** Serialization function */ @@ -758,13 +793,18 @@ template { public: - // aliases for value types pulled from keys - using X1 = VALUE1; - using X2 = VALUE2; - using X3 = VALUE3; - using X4 = VALUE4; - using X5 = VALUE5; - using X6 = VALUE6; + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor6::template ValueType<1>; + using X2 = typename NoiseModelFactor6::template ValueType<2>; + using X3 = typename NoiseModelFactor6::template ValueType<3>; + using X4 = typename NoiseModelFactor6::template ValueType<4>; + using X5 = typename NoiseModelFactor6::template ValueType<5>; + using X6 = typename NoiseModelFactor6::template ValueType<6>; protected: using Base = NoiseModelFactor; @@ -777,13 +817,15 @@ class GTSAM_DEPRECATED NoiseModelFactor6 VALUE6>::NoiseModelFactorN; ~NoiseModelFactor6() override {} - /** methods to retrieve keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } - inline Key key3() const { return this->keys_[2]; } - inline Key key4() const { return this->keys_[3]; } - inline Key key5() const { return this->keys_[4]; } - inline Key key6() const { return this->keys_[5]; } + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` + */ + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } + inline Key key4() const { return this->template key<4>(); } + inline Key key5() const { return this->template key<5>(); } + inline Key key6() const { return this->template key<6>(); } private: /** Serialization function */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 9c4b4cff17..f9c1b8b04c 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -581,6 +581,8 @@ TEST(NonlinearFactor, NoiseModelFactor6) { class TestFactorN : public NoiseModelFactorN { public: typedef NoiseModelFactorN Base; + using Type1 = ValueType<1>; // Test that we can use the ValueType<> template + TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} Vector @@ -595,6 +597,8 @@ class TestFactorN : public NoiseModelFactorN { if (H4) *H4 = (Matrix(1, 1) << 4.0).finished(); return (Vector(1) << x1 + x2 + x3 + x4).finished(); } + + Key key1() const { return key<1>(); } // Test that we can use key<> template }; /* ************************************ */ From 19215aff98e8c8b40a50e0f98fb3417228f04c1a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 14:29:38 -0500 Subject: [PATCH 40/40] update and fix unit tests --- tests/testNonlinearFactor.cpp | 68 ++++++++++++++++++++--------------- 1 file changed, 39 insertions(+), 29 deletions(-) diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index f9c1b8b04c..99ec2f501e 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -404,7 +404,7 @@ class TestFactor4 : public NoiseModelFactor4 { *H3 = (Matrix(1, 1) << 3.0).finished(); *H4 = (Matrix(1, 1) << 4.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished(); } gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -420,8 +420,8 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(2), double((2.0))); tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); - EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -431,7 +431,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb())); // Test all functions/types for backwards compatibility static_assert(std::is_same::value, @@ -447,21 +447,25 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal(tf.key3(), X(3))); EXPECT(assert_equal(tf.key4(), X(4))); std::vector H = {Matrix(), Matrix(), Matrix(), Matrix()}; - EXPECT(assert_equal(Vector1(10.0), tf.unwhitenedError(tv, H))); + EXPECT(assert_equal(Vector1(30.0), tf.unwhitenedError(tv, H))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.).finished(), H.at(0))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3))); // And test "forward compatibility" using `key` and `ValueType` too - static_assert(std::is_same, double>::value, - "ValueType<0> type incorrect"); static_assert(std::is_same, double>::value, "ValueType<1> type incorrect"); static_assert(std::is_same, double>::value, "ValueType<2> type incorrect"); static_assert(std::is_same, double>::value, "ValueType<3> type incorrect"); - EXPECT(assert_equal(tf.key<0>(), X(1))); - EXPECT(assert_equal(tf.key<1>(), X(2))); - EXPECT(assert_equal(tf.key<2>(), X(3))); - EXPECT(assert_equal(tf.key<3>(), X(4))); + static_assert(std::is_same, double>::value, + "ValueType<4> type incorrect"); + EXPECT(assert_equal(tf.key<1>(), X(1))); + EXPECT(assert_equal(tf.key<2>(), X(2))); + EXPECT(assert_equal(tf.key<3>(), X(3))); + EXPECT(assert_equal(tf.key<4>(), X(4))); // Test constructors TestFactor4 tf2(noiseModel::Unit::Create(1), L(1), L(2), L(3), L(4)); @@ -492,7 +496,8 @@ class TestFactor5 : public NoiseModelFactor5(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -518,7 +523,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); - EXPECT(assert_equal((Vector)(Vector(1) << -7.5).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -55.).finished(), jf.getb())); } /* ************************************************************************* */ @@ -543,7 +548,9 @@ class TestFactor6 : public NoiseModelFactor6(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -573,7 +580,7 @@ TEST(NonlinearFactor, NoiseModelFactor6) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0).finished(), jf.getA(jf.begin()+5))); - EXPECT(assert_equal((Vector)(Vector(1) << -10.5).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -91.).finished(), jf.getb())); } @@ -595,7 +602,7 @@ class TestFactorN : public NoiseModelFactorN { if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); if (H3) *H3 = (Matrix(1, 1) << 3.0).finished(); if (H4) *H4 = (Matrix(1, 1) << 4.0).finished(); - return (Vector(1) << x1 + x2 + x3 + x4).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished(); } Key key1() const { return key<1>(); } // Test that we can use key<> template @@ -609,8 +616,8 @@ TEST(NonlinearFactor, NoiseModelFactorN) { tv.insert(X(2), double((2.0))); tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); - EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -620,7 +627,7 @@ TEST(NonlinearFactor, NoiseModelFactorN) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5 * 30.).finished(), jf.getb())); // Test all evaluateError argument overloads to ensure backward compatibility Matrix H1_expected, H2_expected, H3_expected, H4_expected; @@ -650,18 +657,21 @@ TEST(NonlinearFactor, NoiseModelFactorN) { EXPECT(assert_equal(H4_expected, H4)); // Test using `key` and `ValueType` - static_assert(std::is_same, double>::value, - "ValueType<0> type incorrect"); static_assert(std::is_same, double>::value, "ValueType<1> type incorrect"); static_assert(std::is_same, double>::value, "ValueType<2> type incorrect"); static_assert(std::is_same, double>::value, "ValueType<3> type incorrect"); - EXPECT(assert_equal(tf.key<0>(), X(1))); - EXPECT(assert_equal(tf.key<1>(), X(2))); - EXPECT(assert_equal(tf.key<2>(), X(3))); - EXPECT(assert_equal(tf.key<3>(), X(4))); + static_assert(std::is_same, double>::value, + "ValueType<4> type incorrect"); + static_assert(std::is_same::value, + "TestFactorN::Type1 type incorrect"); + EXPECT(assert_equal(tf.key<1>(), X(1))); + EXPECT(assert_equal(tf.key<2>(), X(2))); + EXPECT(assert_equal(tf.key<3>(), X(3))); + EXPECT(assert_equal(tf.key<4>(), X(4))); + EXPECT(assert_equal(tf.key1(), X(1))); } /* ************************************************************************* */