From 81f1d9315840e0638da10c1397a73cec90b0744e Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 1 Dec 2021 16:04:49 -0500 Subject: [PATCH 01/49] 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 ee7bd9924..e07f45f07 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 7fafd95df..503ae7d58 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { @@ -770,5 +771,110 @@ private: }; // \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 84bba850b..eb35bf55d 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/49] 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 503ae7d58..b87d1bfaa 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -816,7 +816,7 @@ public: 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/49] 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 b87d1bfaa..249891397 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/49] 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 249891397..542c4d5f1 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -878,11 +878,24 @@ public: 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 eb35bf55d..8ecbbc397 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -418,12 +418,10 @@ public: 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/49] 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 8ecbbc397..fda65d56a 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/49] NoiseModelFactor4 implemented as derived class of NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 341 +++++++++++++----------------- 1 file changed, 151 insertions(+), 190 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 542c4d5f1..e024a41a0 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -309,13 +309,137 @@ public: /* ************************************************************************* */ /** - * 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 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor4: public NoiseModelFactor { +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; -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)) {} + 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 @@ private: } }; // \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/49] 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 e024a41a0..64ef96b85 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -316,16 +316,16 @@ public: * 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 { +template +class NoiseModelFactor1 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X = VALUE; -public: - - // typedefs for value types pulled from keys - typedef VALUE X; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor1 This; - -public: - /// @name Constructors - /// @{ - - /** 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 { +template +class NoiseModelFactor2 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; -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() {} - - /** - * 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 { +template +class NoiseModelFactor3 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; -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() {} - - /** - * 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 { +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; -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() {} - - /** - * 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 { +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; -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() {} - - /** - * 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/49] 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 64ef96b85..34e891c64 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -307,19 +307,39 @@ public: /* ************************************************************************* */ - /** - * 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(). * - * 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). + * 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) {} + * + * 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/49] avoid inheritance by conditionally defining backwards compatibility types/funcs in NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 566 ++++++++++++++++++------------ tests/testNonlinearFactor.cpp | 44 +++ 2 files changed, 385 insertions(+), 225 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 34e891c64..764f1fdf9 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -306,6 +306,76 @@ public: }; // \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 @@ public: * 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; +// template +// using NoiseModelFactor1 = NoiseModelFactorN; +// template +// using NoiseModelFactor2 = NoiseModelFactorN; +// template +// using NoiseModelFactor3 = NoiseModelFactorN; +// template +// using NoiseModelFactor4 = NoiseModelFactorN; +// template +// using NoiseModelFactor5 = +// NoiseModelFactorN; +// template +// using NoiseModelFactor6 = +// NoiseModelFactorN; - protected: - using Base = NoiseModelFactor; // grandparent, for backwards compatibility - using This = NoiseModelFactor1; +#define NoiseModelFactor1 NoiseModelFactorN +#define NoiseModelFactor2 NoiseModelFactorN +#define NoiseModelFactor3 NoiseModelFactorN +#define NoiseModelFactor4 NoiseModelFactorN +#define NoiseModelFactor5 NoiseModelFactorN +#define NoiseModelFactor6 NoiseModelFactorN - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor1() override {} +// /* ************************************************************************* */ +// /** @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; - /** method to retrieve key */ - inline Key key() const { return this->keys_[0]; } +// protected: +// using Base = NoiseModelFactor; // grandparent, for backwards compatibility +// using This = NoiseModelFactor1; - 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 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor1() override {} -/* ************************************************************************* */ -/** @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; +// /** method to retrieve key */ +// inline Key key() const { return this->keys_[0]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor2; +// 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 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor2() override {} +// /* ************************************************************************* */ +// /** @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; - /** methods to retrieve both keys */ - inline Key key1() const { return this->keys_[0]; } - inline Key key2() const { return this->keys_[1]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor2; - 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 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor2() override {} -/* ************************************************************************* */ -/** @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; +// /** methods to retrieve both keys */ +// inline Key key1() const { return this->keys_[0]; } +// inline Key key2() const { return this->keys_[1]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor3; +// 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 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor3() override {} +// /* ************************************************************************* */ +// /** @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; - /** 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]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor3; - 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 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor3() override {} -/* ************************************************************************* */ -/** @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; +// /** 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]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor4; +// 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 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor4() override {} +// /* ************************************************************************* */ +// /** @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; - /** 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]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor4; - 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 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor4() override {} -/* ************************************************************************* */ -/** @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; +// /** 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]; } - protected: - using Base = NoiseModelFactor; - using This = NoiseModelFactor5; +// 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 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor5() override {} +// /* ************************************************************************* */ +// /** @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; - /** 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]; } +// protected: +// using Base = NoiseModelFactor; +// using This = NoiseModelFactor5; - 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 +// public: +// // inherit NoiseModelFactorN's constructors +// using NoiseModelFactorN::NoiseModelFactorN; +// ~NoiseModelFactor5() override {} -/* ************************************************************************* */ -/** @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; +// /** 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]; } - protected: - using Base = NoiseModelFactor; - using This = - NoiseModelFactor6; +// 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 - public: - // inherit NoiseModelFactorN's constructors - using NoiseModelFactorN::NoiseModelFactorN; - ~NoiseModelFactor6() override {} +// /* ************************************************************************* */ +// /** @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; - /** 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]; } +// protected: +// using Base = NoiseModelFactor; +// using This = +// NoiseModelFactor6; - 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 +// 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 fda65d56a..fba7949a1 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/49] 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 764f1fdf9..b925916bb 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -314,7 +314,8 @@ public: * * 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/49] can't get "using NoiseModelFactorX = NoiseModelFactorN" to work --- gtsam/nonlinear/NonlinearFactor.h | 296 +++++------------------------- 1 file changed, 50 insertions(+), 246 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b925916bb..b55ec7856 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 +// 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, 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 +/* ************************************************************************* */ +/** @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/49] 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 b55ec7856..53a3e664d 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -305,78 +305,6 @@ public: }; // \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/49] 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 53a3e664d..099f58979 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -305,6 +305,70 @@ public: }; // \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 @@ public: * 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/49] 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 099f58979..18822f0ef 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -336,7 +336,6 @@ public: * } * ``` */ - 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/49] 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 18822f0ef..ca13434a1 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/49] 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 ca13434a1..3a59a4db3 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/49] 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 e07f45f07..e9c83da8a 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/49] 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 000000000..46752262b --- /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 4a73cbb0b..f7b524a5c 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/49] revert some template stuff with inheritance for readability NoiseModelFactor123456 are now minimal classes that inherit from NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 362 ++++++++++++++++++++---------- 1 file changed, 241 insertions(+), 121 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d7061215e..212364af3 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -309,68 +309,6 @@ public: }; // \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)); } - - 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 /* ************************************************************************* */ -/** @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 .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(). */ -#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 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 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(). + */ +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/49] 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 d9b92b8aa..22e90d275 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -27,3 +27,41 @@ private: }; } + +// 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 212364af3..4deef0d3a 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/49] 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 27b61cf89..bdc2d576d 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/49] 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 46752262b..000000000 --- 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 f7b524a5c..4a73cbb0b 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/49] 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 000000000..0c580fb21 --- /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 4a73cbb0b..023785f21 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/49] 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 023785f21..b6b5033a2 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/49] 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 b6b5033a2..63e2ae1dd 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/49] 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 b69371f16..cafb747b8 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -264,9 +264,13 @@ public: 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 63e2ae1dd..5622da07d 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/49] 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 5622da07d..95906183c 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/49] 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 cafb747b8..7dc5ea0b0 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/49] 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 7dc5ea0b0..b69371f16 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -264,13 +264,9 @@ public: 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 95906183c..860cd225b 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/49] 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 860cd225b..f402656c1 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/49] 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 b69371f16..0150b8b51 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/49] 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 22e90d275..0a05a704c 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -31,6 +31,7 @@ private: // 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/49] 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 bdc2d576d..27b61cf89 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/49] 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 0150b8b51..a54c7558b 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -302,27 +302,27 @@ public: * 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 27b61cf89..d060f663e 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/49] 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 a54c7558b..33640f0b7 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 d060f663e..349e2cd86 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/49] 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 33640f0b7..2f4b27d39 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 349e2cd86..9c4b4cff1 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 0e1c3b8cb6755577f8c8cf4891013d8931716510 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Dec 2022 07:33:51 +0530 Subject: [PATCH 37/49] rename all *Vals to *Values --- gtsam/hybrid/GaussianMixture.cpp | 14 +++--- gtsam/hybrid/GaussianMixture.h | 14 +++--- gtsam/hybrid/GaussianMixtureFactor.cpp | 13 +++--- gtsam/hybrid/GaussianMixtureFactor.h | 12 ++--- gtsam/hybrid/MixtureFactor.h | 44 ++++++++++--------- gtsam/hybrid/hybrid.i | 6 +-- .../tests/testGaussianMixtureFactor.cpp | 16 +++---- gtsam/hybrid/tests/testMixtureFactor.cpp | 8 ++-- 8 files changed, 65 insertions(+), 62 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 4819eda65..314d4fc63 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -85,8 +85,8 @@ size_t GaussianMixture::nrComponents() const { /* *******************************************************************************/ GaussianConditional::shared_ptr GaussianMixture::operator()( - const DiscreteValues &discreteVals) const { - auto &ptr = conditionals_(discreteVals); + const DiscreteValues &discreteValues) const { + auto &ptr = conditionals_(discreteValues); if (!ptr) return nullptr; auto conditional = boost::dynamic_pointer_cast(ptr); if (conditional) @@ -209,12 +209,12 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixture::error( - const VectorValues &continuousVals) const { + const VectorValues &continuousValues) const { // functor to convert from GaussianConditional to double error value. auto errorFunc = - [continuousVals](const GaussianConditional::shared_ptr &conditional) { + [continuousValues](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - return conditional->error(continuousVals); + return conditional->error(continuousValues); } else { // return arbitrarily large error return 1e50; @@ -225,10 +225,10 @@ AlgebraicDecisionTree GaussianMixture::error( } /* *******************************************************************************/ -double GaussianMixture::error(const VectorValues &continuousVals, +double GaussianMixture::error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { auto conditional = conditionals_(discreteValues); - return conditional->error(continuousVals); + return conditional->error(continuousValues); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 4bb13d298..88d5a02c0 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -122,7 +122,7 @@ class GTSAM_EXPORT GaussianMixture /// @{ GaussianConditional::shared_ptr operator()( - const DiscreteValues &discreteVals) const; + const DiscreteValues &discreteValues) const; /// Returns the total number of continuous components size_t nrComponents() const; @@ -147,21 +147,21 @@ class GTSAM_EXPORT GaussianMixture /** * @brief Compute error of the GaussianMixture as a tree. * - * @param continuousVals The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous VectorValues. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the conditionals, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousVals) const; + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** * @brief Compute the error of this Gaussian Mixture given the continuous * values and a discrete assignment. * - * @param continuousVals The continuous values at which to compute the error. + * @param continuousValues Continuous values at which to compute the error. * @param discreteValues The discrete assignment for a specific mode sequence. * @return double */ - double error(const VectorValues &continuousVals, + double error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const; /** diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 16802516e..f070fe07a 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -98,21 +98,22 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree() /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixtureFactor::error( - const VectorValues &continuousVals) const { + const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) { - return factor->error(continuousVals); - }; + auto errorFunc = + [continuousValues](const GaussianFactor::shared_ptr &factor) { + return factor->error(continuousValues); + }; DecisionTree errorTree(factors_, errorFunc); return errorTree; } /* *******************************************************************************/ double GaussianMixtureFactor::error( - const VectorValues &continuousVals, + const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { auto factor = factors_(discreteValues); - return factor->error(continuousVals); + return factor->error(continuousValues); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 2808ec49f..0b65b5aa9 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -133,21 +133,21 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { /** * @brief Compute error of the GaussianMixtureFactor as a tree. * - * @param continuousVals The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous VectorValues. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousVals) const; + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** * @brief Compute the error of this Gaussian Mixture given the continuous * values and a discrete assignment. * - * @param continuousVals The continuous values at which to compute the error. + * @param continuousValues Continuous values at which to compute the error. * @param discreteValues The discrete assignment for a specific mode sequence. * @return double */ - double error(const VectorValues &continuousVals, + double error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const; /// Add MixtureFactor to a Sum, syntactic sugar. diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 511705cf3..58a915d57 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -128,14 +128,15 @@ class MixtureFactor : public HybridFactor { /** * @brief Compute error of the MixtureFactor as a tree. * - * @param continuousVals The continuous values for which to compute the error. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous values for which to compute the + * error. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the factor, and leaf values as the error. */ - AlgebraicDecisionTree error(const Values& continuousVals) const { + AlgebraicDecisionTree error(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousVals](const sharedFactor& factor) { - return factor->error(continuousVals); + auto errorFunc = [continuousValues](const sharedFactor& factor) { + return factor->error(continuousValues); }; DecisionTree errorTree(factors_, errorFunc); return errorTree; @@ -144,19 +145,19 @@ class MixtureFactor : public HybridFactor { /** * @brief Compute error of factor given both continuous and discrete values. * - * @param continuousVals The continuous Values. - * @param discreteVals The discrete Values. + * @param continuousValues The continuous Values. + * @param discreteValues The discrete Values. * @return double The error of this factor. */ - double error(const Values& continuousVals, - const DiscreteValues& discreteVals) const { - // Retrieve the factor corresponding to the assignment in discreteVals. - auto factor = factors_(discreteVals); + double error(const Values& continuousValues, + const DiscreteValues& discreteValues) const { + // Retrieve the factor corresponding to the assignment in discreteValues. + auto factor = factors_(discreteValues); // Compute the error for the selected factor - const double factorError = factor->error(continuousVals); + const double factorError = factor->error(continuousValues); if (normalized_) return factorError; - return factorError + - this->nonlinearFactorLogNormalizingConstant(factor, continuousVals); + return factorError + this->nonlinearFactorLogNormalizingConstant( + factor, continuousValues); } size_t dim() const { @@ -212,17 +213,18 @@ class MixtureFactor : public HybridFactor { /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize( - const Values& continuousVals, const DiscreteValues& discreteVals) const { - auto factor = factors_(discreteVals); - return factor->linearize(continuousVals); + const Values& continuousValues, + const DiscreteValues& discreteValues) const { + auto factor = factors_(discreteValues); + return factor->linearize(continuousValues); } /// Linearize all the continuous factors to get a GaussianMixtureFactor. boost::shared_ptr linearize( - const Values& continuousVals) const { + const Values& continuousValues) const { // functional to linearize each factor in the decision tree - auto linearizeDT = [continuousVals](const sharedFactor& factor) { - return factor->linearize(continuousVals); + auto linearizeDT = [continuousValues](const sharedFactor& factor) { + return factor->linearize(continuousValues); }; DecisionTree linearized_factors( diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 86029a48a..90c76593e 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -204,14 +204,14 @@ class MixtureFactor : gtsam::HybridFactor { const std::vector& factors, bool normalized = false); - double error(const gtsam::Values& continuousVals, - const gtsam::DiscreteValues& discreteVals) const; + double error(const gtsam::Values& continuousValues, + const gtsam::DiscreteValues& discreteValues) const; double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor, const gtsam::Values& values) const; GaussianMixtureFactor* linearize( - const gtsam::Values& continuousVals) const; + const gtsam::Values& continuousValues) const; void print(string s = "MixtureFactor\n", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 5c25a0931..14e1b8dad 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -170,12 +170,12 @@ TEST(GaussianMixtureFactor, Error) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - VectorValues continuousVals; - continuousVals.insert(X(1), Vector2(0, 0)); - continuousVals.insert(X(2), Vector2(1, 1)); + VectorValues continuousValues; + continuousValues.insert(X(1), Vector2(0, 0)); + continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousVals); + AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; std::vector errors = {1, 4}; @@ -184,10 +184,10 @@ TEST(GaussianMixtureFactor, Error) { EXPECT(assert_equal(expected_error, error_tree)); // Test for single leaf given discrete assignment P(X|M,Z). - DiscreteValues discreteVals; - discreteVals[m1.first] = 1; - EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals), - 1e-9); + DiscreteValues discreteValues; + discreteValues[m1.first] = 1; + EXPECT_DOUBLES_EQUAL( + 4.0, mixtureFactor.error(continuousValues, discreteValues), 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index c00d70e5a..5167f6ff6 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -87,11 +87,11 @@ TEST(MixtureFactor, Error) { MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - Values continuousVals; - continuousVals.insert(X(1), 0); - continuousVals.insert(X(2), 1); + Values continuousValues; + continuousValues.insert(X(1), 0); + continuousValues.insert(X(2), 1); - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousVals); + AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; std::vector errors = {0.5, 0}; From 098d2ce4a4120a418d0987405394989db07a85ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Dec 2022 08:26:08 +0530 Subject: [PATCH 38/49] Update docstrings --- gtsam/hybrid/HybridBayesNet.h | 6 ++++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 9 ++++++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index c6ac6dcec..f8ec60911 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -145,8 +145,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** - * @brief Compute unnormalized probability for each discrete assignment, - * and return as a tree. + * @brief Compute unnormalized probability q(μ|M), + * for each discrete assignment, and return as a tree. + * q(μ|M) is the unnormalized probability at the MLE point μ, + * conditioned on the discrete variables. * * @param continuousValues Continuous values at which to compute the * probability. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index c7e9aa60d..ac9ae1a46 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -99,11 +99,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph using shared_ptr = boost::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility - using Indices = KeyVector; ///> map from keys to values + using Indices = KeyVector; ///< map from keys to values /// @name Constructors /// @{ + /// @brief Default constructor. HybridGaussianFactorGraph() = default; /** @@ -174,14 +175,16 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * @brief Compute error for each discrete assignment, * and return as a tree. * + * Error \f$ e = \Vert x - \mu \Vert_{\Sigma} \f$. + * * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ AlgebraicDecisionTree error(const VectorValues& continuousValues) const; /** - * @brief Compute unnormalized probability for each discrete assignment, - * and return as a tree. + * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ + * for each discrete assignment, and return as a tree. * * @param continuousValues Continuous values at which to compute the * probability. From d94b3199a05419020eb5a363e6c43bd573eb85f2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Dec 2022 09:22:34 +0530 Subject: [PATCH 39/49] address review comments --- gtsam/hybrid/GaussianMixture.cpp | 6 ++++-- gtsam/hybrid/GaussianMixtureFactor.cpp | 1 + gtsam/hybrid/HybridBayesNet.cpp | 7 ++++++- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 +++++ gtsam/hybrid/MixtureFactor.h | 7 ++++--- gtsam/hybrid/hybrid.i | 6 ++++-- gtsam/hybrid/tests/testGaussianMixture.cpp | 11 +++++++++-- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 1 + gtsam/hybrid/tests/testHybridBayesNet.cpp | 3 +-- gtsam/hybrid/tests/testMixtureFactor.cpp | 3 ++- 10 files changed, 37 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 314d4fc63..c0815b2d7 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -210,13 +210,14 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixture::error( const VectorValues &continuousValues) const { - // functor to convert from GaussianConditional to double error value. + // functor to calculate to double error value from GaussianConditional. auto errorFunc = [continuousValues](const GaussianConditional::shared_ptr &conditional) { if (conditional) { return conditional->error(continuousValues); } else { - // return arbitrarily large error + // Return arbitrarily large error if conditional is null + // Conditional is null if it is pruned out. return 1e50; } }; @@ -227,6 +228,7 @@ AlgebraicDecisionTree GaussianMixture::error( /* *******************************************************************************/ double GaussianMixture::error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { + // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(discreteValues); return conditional->error(continuousValues); } diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index f070fe07a..fd437f52c 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -112,6 +112,7 @@ AlgebraicDecisionTree GaussianMixtureFactor::error( double GaussianMixtureFactor::error( const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { + // Directly index to get the conditional, no need to build the whole tree. auto factor = factors_(discreteValues); return factor->error(continuousValues); } diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f0d53c416..48c4b6d50 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -244,13 +244,16 @@ AlgebraicDecisionTree HybridBayesNet::error( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree; + // Iterate over each factor. for (size_t idx = 0; idx < size(); idx++) { AlgebraicDecisionTree conditional_error; + if (factors_.at(idx)->isHybrid()) { - // If factor is hybrid, select based on assignment. + // If factor is hybrid, select based on assignment and compute error. GaussianMixture::shared_ptr gm = this->atMixture(idx); conditional_error = gm->error(continuousValues); + // Assign for the first index, add error for subsequent ones. if (idx == 0) { error_tree = conditional_error; } else { @@ -261,6 +264,7 @@ AlgebraicDecisionTree HybridBayesNet::error( // If continuous only, get the (double) error // and add it to the error_tree double error = this->atGaussian(idx)->error(continuousValues); + // Add the computed error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); @@ -273,6 +277,7 @@ AlgebraicDecisionTree HybridBayesNet::error( return error_tree; } +/* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::probPrime( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree = this->error(continuousValues); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d6937957f..32653bdec 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -428,6 +428,7 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); + // Iterate over each factor. for (size_t idx = 0; idx < size(); idx++) { AlgebraicDecisionTree factor_error; @@ -435,8 +436,10 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( // If factor is hybrid, select based on assignment. GaussianMixtureFactor::shared_ptr gaussianMixture = boost::static_pointer_cast(factors_.at(idx)); + // Compute factor error. factor_error = gaussianMixture->error(continuousValues); + // If first factor, assign error, else add it. if (idx == 0) { error_tree = factor_error; } else { @@ -450,7 +453,9 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( boost::static_pointer_cast(factors_.at(idx)); GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner(); + // Compute the error of the gaussian factor. double error = gaussian->error(continuousValues); + // Add the gaussian factor error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 58a915d57..f29a84022 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -86,11 +87,11 @@ class MixtureFactor : public HybridFactor { * elements based on the number of discrete keys and the cardinality of the * keys, so that the decision tree is constructed appropriately. * - * @tparam FACTOR The type of the factor shared pointers being passed in. Will - * be typecast to NonlinearFactor shared pointers. + * @tparam FACTOR The type of the factor shared pointers being passed in. + * Will be typecast to NonlinearFactor shared pointers. * @param keys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. - * @param factors Vector of shared pointers to factors. + * @param factors Vector of nonlinear factors. * @param normalized Flag indicating if the factor error is already * normalized. */ diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 90c76593e..899c129e0 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -196,8 +196,10 @@ class HybridNonlinearFactorGraph { #include class MixtureFactor : gtsam::HybridFactor { - MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, - const gtsam::DecisionTree& factors, bool normalized = false); + MixtureFactor( + const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const gtsam::DecisionTree& factors, + bool normalized = false); template MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 556a5f16a..310081f02 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -104,7 +104,7 @@ TEST(GaussianMixture, Error) { X(2), S2, model); // Create decision tree - DiscreteKey m1(1, 2); + DiscreteKey m1(M(1), 2); GaussianMixture::Conditionals conditionals( {m1}, vector{conditional0, conditional1}); @@ -115,12 +115,19 @@ TEST(GaussianMixture, Error) { values.insert(X(2), Vector2::Zero()); auto error_tree = mixture.error(values); + // regression std::vector discrete_keys = {m1}; std::vector leaves = {0.5, 4.3252595}; AlgebraicDecisionTree expected_error(discrete_keys, leaves); - // regression EXPECT(assert_equal(expected_error, error_tree, 1e-6)); + + // Regression for non-tree version. + DiscreteValues assignment; + assignment[M(1)] = 0; + EXPECT_DOUBLES_EQUAL(0.5, mixture.error(values, assignment), 1e-8); + assignment[M(1)] = 1; + EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), 1e-8); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 14e1b8dad..ba0622ff9 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -178,6 +178,7 @@ TEST(GaussianMixtureFactor, Error) { AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; + // Error values for regression test std::vector errors = {1, 4}; AlgebraicDecisionTree expected_error(discrete_keys, errors); diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 8b8ca976b..3593e1952 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -216,8 +216,7 @@ TEST(HybridBayesNet, Error) { // Verify error computation and check for specific error value DiscreteValues discrete_values; - discrete_values[M(0)] = 1; - discrete_values[M(1)] = 1; + insert(discrete_values)(M(0), 1)(M(1), 1); double total_error = 0; for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) { diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 5167f6ff6..fe3212eda 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -41,7 +41,8 @@ TEST(MixtureFactor, Constructor) { CHECK(it == factor.end()); } - +/* ************************************************************************* */ +// Test .print() output. TEST(MixtureFactor, Printing) { DiscreteKey m1(1, 2); double between0 = 0.0; From 23ec7eddb01c09922d38d8bc70d2a8da1763924e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Dec 2022 09:53:22 +0530 Subject: [PATCH 40/49] cleaner way of assigning discrete values --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 3593e1952..050c01aed 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -216,7 +216,7 @@ TEST(HybridBayesNet, Error) { // Verify error computation and check for specific error value DiscreteValues discrete_values; - insert(discrete_values)(M(0), 1)(M(1), 1); + boost::assign::insert(discrete_values)(M(0), 1)(M(1), 1); double total_error = 0; for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) { From 040eb63949d9a3e18ba9ba3cd0192ade10fc4b95 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 13:18:15 -0500 Subject: [PATCH 41/49] 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 2f4b27d39..1d2bff8a7 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 42/49] 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 1d2bff8a7..dfab4542c 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -277,25 +277,48 @@ public: * 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 43/49] 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 dfab4542c..ad52355e4 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 9c4b4cff1..f9c1b8b04 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 @@ public: 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 44/49] 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 f9c1b8b04..99ec2f501 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 @@ public: *H4 = (Matrix(1, 1) << 4.0).finished(); *H5 = (Matrix(1, 1) << 5.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4 + 5.0 * x5) + .finished(); } }; @@ -505,8 +510,8 @@ TEST(NonlinearFactor, NoiseModelFactor5) { tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); tv.insert(X(5), double((5.0))); - EXPECT(assert_equal((Vector(1) << 15.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 55.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 55.0 * 55.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]); @@ -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 @@ public: *H5 = (Matrix(1, 1) << 5.0).finished(); *H6 = (Matrix(1, 1) << 6.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4 + 5.0 * x5 + + 6.0 * x6) + .finished(); } }; @@ -558,8 +565,8 @@ TEST(NonlinearFactor, NoiseModelFactor6) { tv.insert(X(4), double((4.0))); tv.insert(X(5), double((5.0))); tv.insert(X(6), double((6.0))); - EXPECT(assert_equal((Vector(1) << 21.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 91.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 91.0 * 91.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]); @@ -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 @@ public: 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))); } /* ************************************************************************* */ From 885eed33d13eeb814c5e8351ef6b2a181a23a511 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 17:25:48 -0500 Subject: [PATCH 45/49] replace all NoiseModelFactor1, 2, ... with NoiseModelFactorN --- examples/CameraResectioning.cpp | 4 +- examples/LocalizationExample.cpp | 8 +-- examples/Pose3SLAMExample_changeKeys.cpp | 8 +-- examples/SolverComparer.cpp | 28 +++++------ gtsam/base/Testable.h | 2 +- gtsam/inference/graph-inl.h | 8 +-- gtsam/navigation/AHRSFactor.cpp | 4 +- gtsam/navigation/AHRSFactor.h | 5 +- gtsam/navigation/AttitudeFactor.h | 10 ++-- gtsam/navigation/BarometricFactor.h | 5 +- gtsam/navigation/CombinedImuFactor.cpp | 6 +-- gtsam/navigation/CombinedImuFactor.h | 8 +-- gtsam/navigation/ConstantVelocityFactor.h | 6 +-- gtsam/navigation/GPSFactor.h | 10 ++-- gtsam/navigation/ImuFactor.cpp | 24 ++++----- gtsam/navigation/ImuFactor.h | 10 ++-- gtsam/navigation/MagFactor.h | 16 +++--- gtsam/navigation/MagPoseFactor.h | 5 +- gtsam/nonlinear/ExtendedKalmanFilter.h | 4 +- gtsam/nonlinear/FunctorizedFactor.h | 16 +++--- gtsam/nonlinear/NonlinearEquality.h | 15 +++--- gtsam/nonlinear/NonlinearFactor.h | 49 ++++++++++++++++--- gtsam/nonlinear/PriorFactor.h | 5 +- gtsam/sfm/ShonanAveraging.cpp | 4 +- gtsam/sfm/ShonanFactor.cpp | 8 +-- gtsam/sfm/ShonanFactor.h | 4 +- gtsam/sfm/TranslationFactor.h | 4 +- gtsam/slam/BetweenFactor.h | 11 +++-- gtsam/slam/BoundingConstraint.h | 12 +++-- gtsam/slam/EssentialMatrixConstraint.cpp | 4 +- gtsam/slam/EssentialMatrixConstraint.h | 5 +- gtsam/slam/EssentialMatrixFactor.h | 12 ++--- gtsam/slam/FrobeniusFactor.h | 20 ++++---- gtsam/slam/GeneralSFMFactor.h | 16 +++--- gtsam/slam/InitializePose3.cpp | 4 +- gtsam/slam/OrientedPlane3Factor.cpp | 4 +- gtsam/slam/OrientedPlane3Factor.h | 8 +-- gtsam/slam/PoseRotationPrior.h | 5 +- gtsam/slam/PoseTranslationPrior.h | 5 +- gtsam/slam/ProjectionFactor.h | 10 ++-- gtsam/slam/ReferenceFrameFactor.h | 16 +++--- gtsam/slam/RotateFactor.h | 8 +-- gtsam/slam/StereoFactor.h | 9 ++-- gtsam/slam/TriangulationFactor.h | 4 +- gtsam/slam/dataset.cpp | 12 ++--- gtsam_unstable/dynamics/FullIMUFactor.h | 4 +- gtsam_unstable/dynamics/IMUFactor.h | 4 +- gtsam_unstable/dynamics/Pendulum.h | 16 +++--- gtsam_unstable/dynamics/SimpleHelicopter.h | 8 +-- gtsam_unstable/dynamics/VelocityConstraint.h | 4 +- gtsam_unstable/dynamics/VelocityConstraint3.h | 5 +- gtsam_unstable/slam/BiasedGPSFactor.h | 9 ++-- .../slam/EquivInertialNavFactor_GlobalVel.h | 14 +++--- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 12 ++--- .../slam/GaussMarkov1stOrderFactor.h | 8 +-- .../slam/InertialNavFactor_GlobalVelocity.h | 14 +++--- gtsam_unstable/slam/InvDepthFactor3.h | 8 +-- gtsam_unstable/slam/InvDepthFactorVariant1.h | 8 +-- gtsam_unstable/slam/InvDepthFactorVariant2.h | 8 +-- gtsam_unstable/slam/InvDepthFactorVariant3.h | 16 +++--- .../slam/LocalOrientedPlane3Factor.h | 4 +- gtsam_unstable/slam/MultiProjectionFactor.h | 4 +- gtsam_unstable/slam/PartialPriorFactor.h | 5 +- gtsam_unstable/slam/PoseBetweenFactor.h | 8 +-- gtsam_unstable/slam/PosePriorFactor.h | 4 +- gtsam_unstable/slam/PoseToPointFactor.h | 13 +++-- gtsam_unstable/slam/ProjectionFactorPPP.h | 11 +++-- gtsam_unstable/slam/ProjectionFactorPPPC.h | 8 +-- .../slam/ProjectionFactorRollingShutter.cpp | 6 +-- .../slam/ProjectionFactorRollingShutter.h | 4 +- gtsam_unstable/slam/RelativeElevationFactor.h | 5 +- gtsam_unstable/slam/TSAMFactors.h | 12 ++--- .../slam/TransformBtwRobotsUnaryFactor.h | 2 +- tests/simulated2D.h | 12 ++--- tests/simulated2DOriented.h | 8 +-- tests/simulated3D.h | 8 +-- tests/smallExample.h | 4 +- tests/testExtendedKalmanFilter.cpp | 27 +++++----- timing/timeIncremental.cpp | 12 ++--- timing/timeiSAM2Chain.cpp | 4 +- 80 files changed, 407 insertions(+), 333 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 7ac2de8b1..1dcca5244 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -30,8 +30,8 @@ using symbol_shorthand::X; * Unary factor on the unknown pose, resulting from meauring the projection of * a known 3D point in the image */ -class ResectioningFactor: public NoiseModelFactor1 { - typedef NoiseModelFactor1 Base; +class ResectioningFactor: public NoiseModelFactorN { + typedef NoiseModelFactorN Base; Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters Point3 P_; ///< 3D point on the calibration rig diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index d9b205359..7a39a4af5 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -62,10 +62,10 @@ using namespace gtsam; // // The factor will be a unary factor, affect only a single system variable. It will // also use a standard Gaussian noise model. Hence, we will derive our new factor from -// the NoiseModelFactor1. +// the NoiseModelFactorN. #include -class UnaryFactor: public NoiseModelFactor1 { +class UnaryFactor: public NoiseModelFactorN { // The factor will hold a measurement consisting of an (X,Y) location // We could this with a Point2 but here we just use two doubles double mx_, my_; @@ -76,11 +76,11 @@ class UnaryFactor: public NoiseModelFactor1 { // The constructor requires the variable key, the (X, Y) measurement value, and the noise model UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): - NoiseModelFactor1(model, j), mx_(x), my_(y) {} + NoiseModelFactorN(model, j), mx_(x), my_(y) {} ~UnaryFactor() override {} - // Using the NoiseModelFactor1 base class there are two functions that must be overridden. + // Using the NoiseModelFactorN base class there are two functions that must be overridden. // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 5d4ed6657..9aa697f14 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -71,11 +71,11 @@ int main(const int argc, const char *argv[]) { if (pose3Between){ Key key1, key2; if(add){ - key1 = pose3Between->key1() + firstKey; - key2 = pose3Between->key2() + firstKey; + key1 = pose3Between->key<1>() + firstKey; + key2 = pose3Between->key<2>() + firstKey; }else{ - key1 = pose3Between->key1() - firstKey; - key2 = pose3Between->key2() - firstKey; + key1 = pose3Between->key<1>() - firstKey; + key2 = pose3Between->key<2>() - firstKey; } NonlinearFactor::shared_ptr simpleFactor( new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->noiseModel())); diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 7bae41403..69975b620 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -69,8 +69,8 @@ namespace br = boost::range; typedef Pose2 Pose; -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; +typedef NoiseModelFactorN NM1; +typedef NoiseModelFactorN NM2; typedef BearingRangeFactor BR; double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { @@ -261,7 +261,7 @@ void runIncremental() if(BetweenFactor::shared_ptr factor = boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { - Key key1 = factor->key1(), key2 = factor->key2(); + Key key1 = factor->key<1>(), key2 = factor->key<2>(); if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); @@ -313,11 +313,11 @@ void runIncremental() boost::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps - if(factor->key1() > step || factor->key2() > step) + if(factor->key<1>() > step || factor->key<2>() > step) break; // Require that one of the nodes is the current one - if(factor->key1() != step && factor->key2() != step) + if(factor->key<1>() != step && factor->key<2>() != step) throw runtime_error("Problem in data file, out-of-sequence measurements"); // Add a new factor @@ -325,22 +325,22 @@ void runIncremental() const auto& measured = factor->measured(); // Initialize the new variable - if(factor->key1() > factor->key2()) { - if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry + if(factor->key<1>() > factor->key<2>()) { + if(!newVariables.exists(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry if(step == 1) - newVariables.insert(factor->key1(), measured.inverse()); + newVariables.insert(factor->key<1>(), measured.inverse()); else { - Pose prevPose = isam2.calculateEstimate(factor->key2()); - newVariables.insert(factor->key1(), prevPose * measured.inverse()); + Pose prevPose = isam2.calculateEstimate(factor->key<2>()); + newVariables.insert(factor->key<1>(), prevPose * measured.inverse()); } } } else { - if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry + if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry if(step == 1) - newVariables.insert(factor->key2(), measured); + newVariables.insert(factor->key<2>(), measured); else { - Pose prevPose = isam2.calculateEstimate(factor->key1()); - newVariables.insert(factor->key2(), prevPose * measured); + Pose prevPose = isam2.calculateEstimate(factor->key<1>()); + newVariables.insert(factor->key<2>(), prevPose * measured); } } } diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index f7eddd414..b68d6a97c 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -23,7 +23,7 @@ * void print(const std::string& name) const = 0; * * equality up to tolerance - * tricky to implement, see NoiseModelFactor1 for an example + * tricky to implement, see PriorFactor for an example * equals is not supposed to print out *anything*, just return true|false * bool equals(const Derived& expected, double tol) const = 0; * diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 3ea66405b..a23eda60c 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -200,8 +200,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap boost::shared_ptr factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - KEY key1 = factor->key1(); - KEY key2 = factor->key2(); + KEY key1 = factor->template key<1>(); + KEY key2 = factor->template key<2>(); PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v2 = key2vertex.find(key2)->second; @@ -270,8 +270,8 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { FACTOR2>(factor); if (!factor2) continue; - KEY key1 = factor2->key1(); - KEY key2 = factor2->key2(); + KEY key1 = factor2->template key<1>(); + KEY key2 = factor2->template key<2>(); // if the tree contains the key if ((tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0) || diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index f4db42d0f..afaaee1d4 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -105,8 +105,8 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { //------------------------------------------------------------------------------ void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; + cout << s << "AHRSFactor(" << keyFormatter(this->key<1>()) << "," + << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) << ","; _PIM_.print(" preintegrated measurements:"); noiseModel_->print(" noise model: "); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index c7d82975a..87fdd2e91 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -128,10 +128,10 @@ private: } }; -class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { +class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { typedef AHRSFactor This; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; PreintegratedAhrsMeasurements _PIM_; @@ -212,6 +212,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index e8da541b1..93495f227 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -76,9 +76,9 @@ public: * Version of AttitudeFactor for Rot3 * @ingroup navigation */ -class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { +class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public AttitudeFactor { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; public: @@ -132,6 +132,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("AttitudeFactor", @@ -149,10 +150,10 @@ template<> struct traits : public Testable, +class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, public AttitudeFactor { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; public: @@ -212,6 +213,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("AttitudeFactor", diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index b570049a5..2d793c475 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -31,9 +31,9 @@ namespace gtsam { * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html * @ingroup navigation */ -class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2 { +class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { private: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; double nT_; ///< Height Measurement based on a standard atmosphere @@ -99,6 +99,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar& boost::serialization::make_nvp( "NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 8d3a7dd31..38b3dc763 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -204,9 +204,9 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) + << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << "," + << keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << "," + << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>()) << ")\n"; _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5591bb357..3448e7794 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -255,14 +255,14 @@ public: * * @ingroup navigation */ -class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 { public: private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; PreintegratedCombinedMeasurements _PIM_; @@ -334,7 +334,9 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6); + // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility + ar& boost::serialization::make_nvp( + "NoiseModelFactor6", boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(_PIM_); } diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 6ab4c2f02..4db2b82c0 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -26,15 +26,15 @@ namespace gtsam { * Binary factor for applying a constant velocity model to a moving body represented as a NavState. * The only measurement is dt, the time delta between the states. */ -class ConstantVelocityFactor : public NoiseModelFactor2 { +class ConstantVelocityFactor : public NoiseModelFactorN { double dt_; public: - using Base = NoiseModelFactor2; + using Base = NoiseModelFactorN; public: ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) - : NoiseModelFactor2(model, i, j), dt_(dt) {} + : NoiseModelFactorN(model, i, j), dt_(dt) {} ~ConstantVelocityFactor() override{}; /** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 40d0ef22a..e515d9012 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -32,11 +32,11 @@ namespace gtsam { * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * @ingroup navigation */ -class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN { private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates @@ -99,6 +99,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -110,11 +111,11 @@ private: * Version of GPSFactor for NavState * @ingroup navigation */ -class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates @@ -163,6 +164,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9b6affaaf..1b07991e9 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -133,9 +133,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1()) - << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) - << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) + cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key<1>()) + << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) + << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>()) << ")\n"; cout << *this << endl; } @@ -184,22 +184,22 @@ PreintegratedImuMeasurements ImuFactor::Merge( ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, const shared_ptr& f12) { // IMU bias keys must be the same. - if (f01->key5() != f12->key5()) + if (f01->key<5>() != f12->key<5>()) throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); // expect intermediate pose, velocity keys to matchup. - if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) + if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>()) throw std::domain_error( "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); // return new factor auto pim02 = Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key1(), // P0 - f01->key2(), // V0 - f12->key3(), // P2 - f12->key4(), // V2 - f01->key5(), // B + return boost::make_shared(f01->key<1>(), // P0 + f01->key<2>(), // V0 + f12->key<3>(), // P2 + f12->key<4>(), // V2 + f01->key<5>(), // B pim02); } #endif @@ -230,8 +230,8 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? s : s + "\n") << "ImuFactor2(" - << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << ")\n"; + << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << "," + << keyFormatter(this->key<3>()) << ")\n"; cout << *this << endl; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index bffe3e645..feae1eb14 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -168,12 +168,12 @@ public: * * @ingroup navigation */ -class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5 { private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; PreintegratedImuMeasurements _PIM_; @@ -248,6 +248,7 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor5", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); @@ -259,11 +260,11 @@ public: * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. * @ingroup navigation */ -class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3 { +class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN { private: typedef ImuFactor2 This; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; PreintegratedImuMeasurements _PIM_; @@ -316,6 +317,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 895ac6512..9a718a5e1 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -30,7 +30,7 @@ namespace gtsam { * and assumes scale, direction, and the bias are given. * Rotation is around negative Z axis, i.e. positive is yaw to right! */ -class MagFactor: public NoiseModelFactor1 { +class MagFactor: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Point3 nM_; ///< Local magnetic field (mag output units) @@ -50,7 +50,7 @@ public: MagFactor(Key key, const Point3& measured, double scale, const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : - NoiseModelFactor1(model, key), // + NoiseModelFactorN(model, key), // measured_(measured), nM_(scale * direction), bias_(bias) { } @@ -87,7 +87,7 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and assumes scale, direction, and the bias are given */ -class MagFactor1: public NoiseModelFactor1 { +class MagFactor1: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Point3 nM_; ///< Local magnetic field (mag output units) @@ -99,7 +99,7 @@ public: MagFactor1(Key key, const Point3& measured, double scale, const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : - NoiseModelFactor1(model, key), // + NoiseModelFactorN(model, key), // measured_(measured), nM_(scale * direction), bias_(bias) { } @@ -125,7 +125,7 @@ public: * This version uses model measured bM = bRn * nM + bias * and optimizes for both nM and the bias, where nM is in units defined by magnetometer */ -class MagFactor2: public NoiseModelFactor2 { +class MagFactor2: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -135,7 +135,7 @@ public: /** Constructor */ MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor2(model, key1, key2), // + NoiseModelFactorN(model, key1, key2), // measured_(measured), bRn_(nRb.inverse()) { } @@ -166,7 +166,7 @@ public: * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor3: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -176,7 +176,7 @@ public: /** Constructor */ MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // + NoiseModelFactorN(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { } diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index da2a54ce9..c817e22b4 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -25,10 +25,10 @@ namespace gtsam { * expressed in the sensor frame. */ template -class MagPoseFactor: public NoiseModelFactor1 { +class MagPoseFactor: public NoiseModelFactorN { private: using This = MagPoseFactor; - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE. using Rot = typename POSE::Rotation; @@ -129,6 +129,7 @@ class MagPoseFactor: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index dc51de7bb..d76a6ea7e 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -53,8 +53,8 @@ class ExtendedKalmanFilter { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys - typedef NoiseModelFactor2 MotionFactor; - typedef NoiseModelFactor1 MeasurementFactor; + typedef NoiseModelFactorN MotionFactor; + typedef NoiseModelFactorN MeasurementFactor; #endif protected: diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 394b22b6b..1fb44ebf7 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -56,9 +56,9 @@ namespace gtsam { * MultiplyFunctor(multiplier)); */ template -class FunctorizedFactor : public NoiseModelFactor1 { +class FunctorizedFactor : public NoiseModelFactorN { private: - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; R measured_; ///< value that is compared with functor return value SharedNoiseModel noiseModel_; ///< noise model @@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactor1 { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" - << keyFormatter(this->key()) << ")" << std::endl; + << keyFormatter(this->template key<1>()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; @@ -120,6 +120,7 @@ class FunctorizedFactor : public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar &boost::serialization::make_nvp( "NoiseModelFactor1", boost::serialization::base_object(*this)); ar &BOOST_SERIALIZATION_NVP(measured_); @@ -155,9 +156,9 @@ FunctorizedFactor MakeFunctorizedFactor(Key key, const R &z, * @param T2: The second argument type for the functor. */ template -class FunctorizedFactor2 : public NoiseModelFactor2 { +class FunctorizedFactor2 : public NoiseModelFactorN { private: - using Base = NoiseModelFactor2; + using Base = NoiseModelFactorN; R measured_; ///< value that is compared with functor return value SharedNoiseModel noiseModel_; ///< noise model @@ -207,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactor2 { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" - << keyFormatter(this->key1()) << ", " - << keyFormatter(this->key2()) << ")" << std::endl; + << keyFormatter(this->template key<1>()) << ", " + << keyFormatter(this->template key<2>()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; @@ -227,6 +228,7 @@ class FunctorizedFactor2 : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar &boost::serialization::make_nvp( "NoiseModelFactor2", boost::serialization::base_object(*this)); ar &BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 43d30254e..d1aa58b27 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -42,7 +42,7 @@ namespace gtsam { * \nosubgrouping */ template -class NonlinearEquality: public NoiseModelFactor1 { +class NonlinearEquality: public NoiseModelFactorN { public: typedef VALUE T; @@ -62,7 +62,7 @@ private: using This = NonlinearEquality; // typedef to base class - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; public: @@ -184,6 +184,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -203,13 +204,13 @@ struct traits> : Testable> {}; * Simple unary equality constraint - fixes a value for a variable */ template -class NonlinearEquality1: public NoiseModelFactor1 { +class NonlinearEquality1: public NoiseModelFactorN { public: typedef VALUE X; protected: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef NonlinearEquality1 This; /// Default constructor to allow for serialization @@ -272,6 +273,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -290,9 +292,9 @@ struct traits > * be the same. */ template -class NonlinearEquality2 : public NoiseModelFactor2 { +class NonlinearEquality2 : public NoiseModelFactorN { protected: - using Base = NoiseModelFactor2; + using Base = NoiseModelFactorN; using This = NonlinearEquality2; GTSAM_CONCEPT_MANIFOLD_TYPE(T) @@ -337,6 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar& boost::serialization::make_nvp( "NoiseModelFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ad52355e4..e1b343218 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -14,6 +14,7 @@ * @brief Non-linear factor base classes * @author Frank Dellaert * @author Richard Roberts + * @author Gerry Chen */ // \callgraph @@ -431,8 +432,9 @@ class NoiseModelFactorN : public NoiseModelFactor { * // key<3>() // ERROR! Will not compile * ``` */ - template > + template inline Key key() const { + static_assert(I <= N, "Index out of bounds"); return keys_[I - 1]; } @@ -492,6 +494,7 @@ class NoiseModelFactorN : public NoiseModelFactor { OptionalMatrix... H) const = 0; /// @} + /// @name Convenience method overloads /// @{ @@ -550,11 +553,43 @@ class NoiseModelFactorN : public NoiseModelFactor { ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); } + + public: + /// @name Deprecated methods + /// @{ + + template 1), void>::type> + inline Key GTSAM_DEPRECATED key1() const { + return key<1>(); + } + template > + inline Key GTSAM_DEPRECATED key2() const { + return key<2>(); + } + template > + inline Key GTSAM_DEPRECATED key3() const { + return key<3>(); + } + template > + inline Key GTSAM_DEPRECATED key4() const { + return key<4>(); + } + template > + inline Key GTSAM_DEPRECATED key5() const { + return key<5>(); + } + template > + inline Key GTSAM_DEPRECATED key6() const { + return key<6>(); + } + + /// @} + }; // \class NoiseModelFactorN /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 - * with ValueType<1>. + * with ValueType<1>. If your class is templated, use `this->template key<1>()` * A convenient base class for creating your own NoiseModelFactor * with 1 variable. To derive from this class, implement evaluateError(). */ @@ -595,7 +630,7 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. + * with ValueType<1>. If your class is templated, use `this->template key<1>()` * A convenient base class for creating your own NoiseModelFactor * with 2 variables. To derive from this class, implement evaluateError(). */ @@ -639,7 +674,7 @@ class GTSAM_DEPRECATED NoiseModelFactor2 /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. + * with ValueType<1>. If your class is templated, use `this->template key<1>()` * A convenient base class for creating your own NoiseModelFactor * with 3 variables. To derive from this class, implement evaluateError(). */ @@ -685,7 +720,7 @@ class GTSAM_DEPRECATED NoiseModelFactor3 /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. + * with ValueType<1>. If your class is templated, use `this->template key<1>()` * A convenient base class for creating your own NoiseModelFactor * with 4 variables. To derive from this class, implement evaluateError(). */ @@ -733,7 +768,7 @@ class GTSAM_DEPRECATED NoiseModelFactor4 /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. + * with ValueType<1>. If your class is templated, use `this->template key<1>()` * A convenient base class for creating your own NoiseModelFactor * with 5 variables. To derive from this class, implement evaluateError(). */ @@ -784,7 +819,7 @@ class GTSAM_DEPRECATED NoiseModelFactor5 /* ************************************************************************* */ /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. + * with ValueType<1>. If your class is templated, use `this->template key<1>()` * A convenient base class for creating your own NoiseModelFactor * with 6 variables. To derive from this class, implement evaluateError(). */ diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 2d4a0ca32..d81ffbd31 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -27,14 +27,14 @@ namespace gtsam { * @ingroup nonlinear */ template - class PriorFactor: public NoiseModelFactor1 { + class PriorFactor: public NoiseModelFactorN { public: typedef VALUE T; private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; VALUE prior_; /** The measurement */ @@ -105,6 +105,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 58e98ebfa..c00669e36 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -958,7 +958,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance // because the tangent space of Pose2 is ordered as (vx, vy, w) auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); - return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + return BinaryMeasurement(f->key<1>(), f->key<2>(), f->measured().rotation(), model); } @@ -1006,7 +1006,7 @@ static BinaryMeasurement convert( // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); - return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + return BinaryMeasurement(f->key<1>(), f->key<2>(), f->measured().rotation(), model); } diff --git a/gtsam/sfm/ShonanFactor.cpp b/gtsam/sfm/ShonanFactor.cpp index b911fb5a4..a48b6e6fa 100644 --- a/gtsam/sfm/ShonanFactor.cpp +++ b/gtsam/sfm/ShonanFactor.cpp @@ -35,7 +35,7 @@ template ShonanFactor::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, const SharedNoiseModel &model, const boost::shared_ptr &G) - : NoiseModelFactor2(ConvertNoiseModel(model, p * d), j1, j2), + : NoiseModelFactorN(ConvertNoiseModel(model, p * d), j1, j2), M_(R12.matrix()), // d*d in all cases p_(p), // 4 for SO(4) pp_(p * p), // 16 for SO(4) @@ -57,8 +57,8 @@ ShonanFactor::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, template void ShonanFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const { - std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n"; + std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key<1>()) << "," + << keyFormatter(key<2>()) << ")\n"; traits::Print(M_, " M: "); noiseModel_->print(" noise model: "); } @@ -68,7 +68,7 @@ template bool ShonanFactor::equals(const NonlinearFactor &expected, double tol) const { auto e = dynamic_cast(&expected); - return e != nullptr && NoiseModelFactor2::equals(*e, tol) && + return e != nullptr && NoiseModelFactorN::equals(*e, tol) && p_ == e->p_ && M_ == e->M_; } diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h index 3c43c2c52..78cc39765 100644 --- a/gtsam/sfm/ShonanFactor.h +++ b/gtsam/sfm/ShonanFactor.h @@ -33,7 +33,7 @@ namespace gtsam { * the SO(p) matrices down to a Stiefel manifold of p*d matrices. */ template -class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2 { +class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN { Matrix M_; ///< measured rotation between R1 and R2 size_t p_, pp_; ///< dimensionality constants boost::shared_ptr G_; ///< matrix of vectorized generators @@ -66,7 +66,7 @@ public: double tol = 1e-9) const override; /// @} - /// @name NoiseModelFactor2 methods + /// @name NoiseModelFactorN methods /// @{ /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 735746b3a..8850d7d2d 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -39,9 +39,9 @@ namespace gtsam { * * */ -class TranslationFactor : public NoiseModelFactor2 { +class TranslationFactor : public NoiseModelFactorN { private: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; Point3 measured_w_aZb_; public: diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index dc72f07b2..f2b41e0a1 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -37,7 +37,7 @@ namespace gtsam { * @ingroup slam */ template - class BetweenFactor: public NoiseModelFactor2 { + class BetweenFactor: public NoiseModelFactorN { // Check that VALUE type is a testable Lie group BOOST_CONCEPT_ASSERT((IsTestable)); @@ -50,7 +50,7 @@ namespace gtsam { private: typedef BetweenFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; VALUE measured_; /** The measurement */ @@ -88,8 +88,8 @@ namespace gtsam { const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } @@ -101,7 +101,7 @@ namespace gtsam { } /// @} - /// @name NoiseModelFactor2 methods + /// @name NoiseModelFactorN methods /// @{ /// evaluate error, returns vector of errors size of tangent space @@ -136,6 +136,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index c09d4136d..79890ec94 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -30,9 +30,9 @@ namespace gtsam { * @ingroup slam */ template -struct BoundingConstraint1: public NoiseModelFactor1 { +struct BoundingConstraint1: public NoiseModelFactorN { typedef VALUE X; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr > shared_ptr; double threshold_; @@ -85,6 +85,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); @@ -97,11 +98,11 @@ private: * to implement for specific systems */ template -struct BoundingConstraint2: public NoiseModelFactor2 { +struct BoundingConstraint2: public NoiseModelFactorN { typedef VALUE1 X1; typedef VALUE2 X2; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr > shared_ptr; double threshold_; @@ -128,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactor2 { /** active when constraint *NOT* met */ bool active(const Values& c) const override { // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key1()), c.at(this->key2())); + double x = value(c.at(this->template key<1>()), c.at(this->template key<2>())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } @@ -158,6 +159,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp index 5397c2e57..c1f8b286b 100644 --- a/gtsam/slam/EssentialMatrixConstraint.cpp +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -27,8 +27,8 @@ namespace gtsam { /* ************************************************************************* */ void EssentialMatrixConstraint::print(const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) - << "," << keyFormatter(this->key2()) << ")\n"; + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key<1>()) + << "," << keyFormatter(this->key<2>()) << ")\n"; measuredE_.print(" measured: "); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index dd3c52286..9d84dfa7b 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -27,12 +27,12 @@ namespace gtsam { * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement * @ingroup slam */ -class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 { +class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN { private: typedef EssentialMatrixConstraint This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; EssentialMatrix measuredE_; /** The measurement is an essential matrix */ @@ -93,6 +93,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 5997ad224..8a0277a45 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -31,10 +31,10 @@ namespace gtsam { /** * Factor that evaluates epipolar error p'Ep for given essential matrix */ -class EssentialMatrixFactor : public NoiseModelFactor1 { +class EssentialMatrixFactor : public NoiseModelFactorN { Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef EssentialMatrixFactor This; public: @@ -106,12 +106,12 @@ class EssentialMatrixFactor : public NoiseModelFactor1 { * in image 2 is perfect, and returns re-projection error in image 1 */ class EssentialMatrixFactor2 - : public NoiseModelFactor2 { + : public NoiseModelFactorN { Point3 dP1_; ///< 3D point corresponding to measurement in image 1 Point2 pn_; ///< Measurement in image 2, in ideal coordinates double f_; ///< approximate conversion factor for error scaling - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef EssentialMatrixFactor2 This; public: @@ -321,11 +321,11 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { */ template class EssentialMatrixFactor4 - : public NoiseModelFactor2 { + : public NoiseModelFactorN { private: Point2 pA_, pB_; ///< points in pixel coordinates - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef EssentialMatrixFactor4 This; static constexpr int DimK = FixedDimension::value; diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 05e23ce6d..60f880a7a 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -48,7 +48,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, * element of SO(3) or SO(4). */ template -class FrobeniusPrior : public NoiseModelFactor1 { +class FrobeniusPrior : public NoiseModelFactorN { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; using MatrixNN = typename Rot::MatrixNN; Eigen::Matrix vecM_; ///< vectorized matrix to approximate @@ -59,7 +59,7 @@ class FrobeniusPrior : public NoiseModelFactor1 { /// Constructor FrobeniusPrior(Key j, const MatrixNN& M, const SharedNoiseModel& model = nullptr) - : NoiseModelFactor1(ConvertNoiseModel(model, Dim), j) { + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j) { vecM_ << Eigen::Map(M.data(), Dim, 1); } @@ -75,13 +75,13 @@ class FrobeniusPrior : public NoiseModelFactor1 { * The template argument can be any fixed-size SO. */ template -class FrobeniusFactor : public NoiseModelFactor2 { +class FrobeniusFactor : public NoiseModelFactorN { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: /// Constructor FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) - : NoiseModelFactor2(ConvertNoiseModel(model, Dim), j1, + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j1, j2) {} /// Error is just Frobenius norm between rotation matrices. @@ -101,7 +101,7 @@ class FrobeniusFactor : public NoiseModelFactor2 { * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. */ template -class FrobeniusBetweenFactor : public NoiseModelFactor2 { +class FrobeniusBetweenFactor : public NoiseModelFactorN { Rot R12_; ///< measured rotation between R1 and R2 Eigen::Matrix R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 @@ -116,7 +116,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { /// Construct from two keys and measured rotation FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, const SharedNoiseModel& model = nullptr) - : NoiseModelFactor2( + : NoiseModelFactorN( ConvertNoiseModel(model, Dim), j1, j2), R12_(R12), R2hat_H_R1_(R12.inverse().AdjointMap()) {} @@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name()) - << ">(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << ">(" << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; traits::Print(R12_, " R12: "); this->noiseModel_->print(" noise model: "); } @@ -140,12 +140,12 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { bool equals(const NonlinearFactor &expected, double tol = 1e-9) const override { auto e = dynamic_cast(&expected); - return e != nullptr && NoiseModelFactor2::equals(*e, tol) && + return e != nullptr && NoiseModelFactorN::equals(*e, tol) && traits::Equals(this->R12_, e->R12_, tol); } /// @} - /// @name NoiseModelFactor2 methods + /// @name NoiseModelFactorN methods /// @{ /// Error is Frobenius norm between R1*R12 and R2. diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 23f10de34..f3089bd71 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -57,7 +57,7 @@ namespace gtsam { * @ingroup slam */ template -class GeneralSFMFactor: public NoiseModelFactor2 { +class GeneralSFMFactor: public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) @@ -74,7 +74,7 @@ protected: public: typedef GeneralSFMFactor This;///< typedef for this object - typedef NoiseModelFactor2 Base;///< typedef for the base class + typedef NoiseModelFactorN Base;///< typedef for the base class // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -140,7 +140,7 @@ public: // Only linearize if the factor is active if (!this->active(values)) return boost::shared_ptr(); - const Key key1 = this->key1(), key2 = this->key2(); + const Key key1 = this->template key<1>(), key2 = this->template key<2>(); JacobianC H1; JacobianL H2; Vector2 b; @@ -184,6 +184,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); @@ -200,7 +201,7 @@ struct traits > : Testable< * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. */ template -class GeneralSFMFactor2: public NoiseModelFactor3 { +class GeneralSFMFactor2: public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) static const int DimK = FixedDimension::value; @@ -213,7 +214,7 @@ public: typedef GeneralSFMFactor2 This; typedef PinholeCamera Camera;///< typedef for camera type - typedef NoiseModelFactor3 Base;///< typedef for the base class + typedef NoiseModelFactorN Base;///< typedef for the base class // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -269,8 +270,8 @@ public: if (H1) *H1 = Matrix::Zero(2, 6); if (H2) *H2 = Matrix::Zero(2, 3); if (H3) *H3 = Matrix::Zero(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) + << " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; } return Z_2x1; } @@ -285,6 +286,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 43404df53..6bb1b0f36 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -228,7 +228,7 @@ void InitializePose3::createSymbolicGraph( Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap->emplace(factorId, Rij); - Key key1 = pose3Between->key1(); + Key key1 = pose3Between->key<1>(); if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in adjEdgesMap->at(key1).push_back(factorId); } else { @@ -236,7 +236,7 @@ void InitializePose3::createSymbolicGraph( edge_id.push_back(factorId); adjEdgesMap->emplace(key1, edge_id); } - Key key2 = pose3Between->key2(); + Key key2 = pose3Between->key<2>(); if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in adjEdgesMap->at(key2).push_back(factorId); } else { diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index d7508f6b8..7ead4ad11 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -15,8 +15,8 @@ namespace gtsam { void OrientedPlane3Factor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << (s == "" ? "" : "\n"); - cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " - << keyFormatter(key2()) << ")\n"; + cout << "OrientedPlane3Factor Factor (" << keyFormatter(key<1>()) << ", " + << keyFormatter(key<2>()) << ")\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 81bb790de..1550201ec 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -15,10 +15,10 @@ namespace gtsam { /** * Factor to measure a planar landmark from a given pose */ -class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2 { +class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN { protected: OrientedPlane3 measured_p_; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; public: /// Constructor @@ -49,10 +49,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2 { +class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN { protected: OrientedPlane3 measured_p_; /// measured plane parameters - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; public: typedef OrientedPlane3DirectionPrior This; diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 20f12dbce..759e66341 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -16,11 +16,11 @@ namespace gtsam { template -class PoseRotationPrior : public NoiseModelFactor1 { +class PoseRotationPrior : public NoiseModelFactorN { public: typedef PoseRotationPrior This; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef POSE Pose; typedef typename POSE::Translation Translation; typedef typename POSE::Rotation Rotation; @@ -92,6 +92,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 5bb3745e9..e009ace29 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -18,10 +18,10 @@ namespace gtsam { * A prior on the translation part of a pose */ template -class PoseTranslationPrior : public NoiseModelFactor1 { +class PoseTranslationPrior : public NoiseModelFactorN { public: typedef PoseTranslationPrior This; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef POSE Pose; typedef typename POSE::Translation Translation; typedef typename POSE::Rotation Rotation; @@ -91,6 +91,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 0fc1c0b54..add6c86c4 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -37,7 +37,7 @@ namespace gtsam { */ template - class GenericProjectionFactor: public NoiseModelFactor2 { + class GenericProjectionFactor: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -52,7 +52,7 @@ namespace gtsam { public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef GenericProjectionFactor This; @@ -154,10 +154,10 @@ namespace gtsam { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) - throw CheiralityException(this->key2()); + throw CheiralityException(this->template key<2>()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 40c8e29b7..74e844404 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -54,13 +54,13 @@ P transform_point( * specific classes of landmarks */ template -class ReferenceFrameFactor : public NoiseModelFactor3 { +class ReferenceFrameFactor : public NoiseModelFactorN { protected: /** default constructor for serialization only */ ReferenceFrameFactor() {} public: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; typedef ReferenceFrameFactor This; typedef POINT Point; @@ -107,16 +107,16 @@ public: void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": ReferenceFrameFactor(" - << "Global: " << keyFormatter(this->key1()) << "," - << " Transform: " << keyFormatter(this->key2()) << "," - << " Local: " << keyFormatter(this->key3()) << ")\n"; + << "Global: " << keyFormatter(this->template key<1>()) << "," + << " Transform: " << keyFormatter(this->template key<2>()) << "," + << " Local: " << keyFormatter(this->template key<3>()) << ")\n"; this->noiseModel_->print(" noise model"); } // access - convenience functions - Key global_key() const { return this->key1(); } - Key transform_key() const { return this->key2(); } - Key local_key() const { return this->key3(); } + Key global_key() const { return this->template key<1>(); } + Key transform_key() const { return this->template key<2>(); } + Key local_key() const { return this->template key<3>(); } private: /** Serialization function */ diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 9e4091111..85539e17e 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -20,11 +20,11 @@ namespace gtsam { * with z and p measured and predicted angular velocities, and hence * p = iRc * z */ -class RotateFactor: public NoiseModelFactor1 { +class RotateFactor: public NoiseModelFactorN { Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef RotateFactor This; public: @@ -64,11 +64,11 @@ public: * Factor on unknown rotation iRc that relates two directions c * Directions provide less constraints than a full rotation */ -class RotateDirectionsFactor: public NoiseModelFactor1 { +class RotateDirectionsFactor: public NoiseModelFactorN { Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef RotateDirectionsFactor This; public: diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index ca1774e31..3be255e45 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -28,7 +28,7 @@ namespace gtsam { * @ingroup slam */ template -class GenericStereoFactor: public NoiseModelFactor2 { +class GenericStereoFactor: public NoiseModelFactorN { private: // Keep a copy of measurement and calibration for I/O @@ -43,7 +43,7 @@ private: public: // shorthand for base class type - typedef NoiseModelFactor2 Base; ///< typedef for base class + typedef NoiseModelFactorN Base; ///< typedef for base class typedef GenericStereoFactor This; ///< typedef for this class (with templates) typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object typedef POSE CamPose; ///< typedef for Pose Lie Value type @@ -141,8 +141,8 @@ public: if (H1) *H1 = Matrix::Zero(3,6); if (H2) *H2 = Z_3x3; if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) throw StereoCheiralityException(this->key2()); } @@ -170,6 +170,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 80d323019..f33ba2ca2 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -30,7 +30,7 @@ namespace gtsam { * @ingroup slam */ template -class TriangulationFactor: public NoiseModelFactor1 { +class TriangulationFactor: public NoiseModelFactorN { public: @@ -40,7 +40,7 @@ public: protected: /// shorthand for base class type - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; /// shorthand for this class using This = TriangulationFactor; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 511cbd839..f25d59ab7 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -535,7 +535,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, graph->push_back(*f); // Insert vertices if pure odometry file - Key key1 = (*f)->key1(), key2 = (*f)->key2(); + Key key1 = (*f)->key<1>(), key2 = (*f)->key<2>(); if (!initial->exists(key1)) initial->insert(key1, Pose2()); if (!initial->exists(key2)) @@ -603,7 +603,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, continue; const Pose2 pose = factor->measured().inverse(); - stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " + stream << "EDGE2 " << factor->key<2>() << " " << factor->key<1>() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; @@ -691,8 +691,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE_SE2 " << index(factor->key1()) << " " - << index(factor->key2()) << " " << pose.x() << " " << pose.y() + stream << "EDGE_SE2 " << index(factor->key<1>()) << " " + << index(factor->key<2>()) << " " << pose.x() << " " << pose.y() << " " << pose.theta(); for (size_t i = 0; i < 3; i++) { for (size_t j = i; j < 3; j++) { @@ -717,8 +717,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const Pose3 pose3D = factor3D->measured(); const Point3 p = pose3D.translation(); const auto q = pose3D.rotation().toQuaternion(); - stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " " - << index(factor3D->key2()) << " " << p.x() << " " << p.y() << " " + stream << "EDGE_SE3:QUAT " << index(factor3D->key<1>()) << " " + << index(factor3D->key<2>()) << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w(); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 9f00f81d6..dcca22695 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -22,9 +22,9 @@ namespace gtsam { * assumed to be PoseRTV */ template -class FullIMUFactor : public NoiseModelFactor2 { +class FullIMUFactor : public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef FullIMUFactor This; protected: diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 9a742b4f0..4eaa3139d 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -20,9 +20,9 @@ namespace gtsam { * assumed to be PoseRTV */ template -class IMUFactor : public NoiseModelFactor2 { +class IMUFactor : public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef IMUFactor This; protected: diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 1e0ca621c..d7301a576 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -20,11 +20,11 @@ namespace gtsam { * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} */ -class PendulumFactor1: public NoiseModelFactor3 { +class PendulumFactor1: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactor1() {} @@ -66,11 +66,11 @@ public: * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) */ -class PendulumFactor2: public NoiseModelFactor3 { +class PendulumFactor2: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactor2() {} @@ -113,11 +113,11 @@ public: * \f$ p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$ * \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$ */ -class PendulumFactorPk: public NoiseModelFactor3 { +class PendulumFactorPk: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactorPk() {} @@ -169,11 +169,11 @@ public: * \f$ p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$ * \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$ */ -class PendulumFactorPk1: public NoiseModelFactor3 { +class PendulumFactorPk1: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactorPk1() {} diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 1fb895833..941b7c7ac 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -24,10 +24,10 @@ namespace gtsam { * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed * in sequential update method. */ -class Reconstruction : public NoiseModelFactor3 { +class Reconstruction : public NoiseModelFactorN { double h_; // time step - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, @@ -73,7 +73,7 @@ public: /** * Implement the Discrete Euler-Poincare' equation: */ -class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { +class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN { double h_; /// time step Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] @@ -86,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index d24d06e79..0fa3d9cb7 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -32,9 +32,9 @@ typedef enum { * NOTE: this approximation is insufficient for large timesteps, but is accurate * if timesteps are small. */ -class VelocityConstraint : public gtsam::NoiseModelFactor2 { +class VelocityConstraint : public gtsam::NoiseModelFactorN { public: - typedef gtsam::NoiseModelFactor2 Base; + typedef gtsam::NoiseModelFactorN Base; protected: diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index c9f05cf98..2880b9c8c 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -10,11 +10,11 @@ namespace gtsam { -class VelocityConstraint3 : public NoiseModelFactor3 { +class VelocityConstraint3 : public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ VelocityConstraint3() {} @@ -53,6 +53,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index 250c15b83..6f0aa605b 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -27,12 +27,12 @@ namespace gtsam { * common-mode errors and that can be partially corrected if other sensors are used * @ingroup slam */ - class BiasedGPSFactor: public NoiseModelFactor2 { + class BiasedGPSFactor: public NoiseModelFactorN { private: typedef BiasedGPSFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; Point3 measured_; /** The measurement */ @@ -57,8 +57,8 @@ namespace gtsam { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BiasedGPSFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n" + << keyFormatter(this->key<1>()) << "," + << keyFormatter(this->key<2>()) << ")\n" << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } @@ -96,6 +96,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index cabbfdbe8..43001fbc2 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -88,12 +88,12 @@ namespace gtsam { */ template -class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 { +class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorN { private: typedef EquivInertialNavFactor_GlobalVel This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactorN Base; Vector delta_pos_in_t0_; Vector delta_vel_in_t0_; @@ -136,11 +136,11 @@ public: /** print */ void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << "," + << keyFormatter(this->template key<3>()) << "," + << keyFormatter(this->template key<4>()) << "," + << keyFormatter(this->template key<5>()) << "\n"; std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index b053b13f8..6423fabda 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -87,12 +87,12 @@ namespace gtsam { */ template -class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4 { +class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactorN { private: typedef EquivInertialNavFactor_GlobalVel_NoBias This; - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; Vector delta_pos_in_t0_; Vector delta_vel_in_t0_; @@ -136,10 +136,10 @@ public: const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "\n"; + << keyFormatter(this->key<1>()) << "," + << keyFormatter(this->key<2>()) << "," + << keyFormatter(this->key<3>()) << "," + << keyFormatter(this->key<4>()) << "\n"; std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl; diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 52e4f44cb..c3682e536 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -42,12 +42,12 @@ namespace gtsam { * T is the measurement type, by default the same */ template -class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { +class GaussMarkov1stOrderFactor: public NoiseModelFactorN { private: typedef GaussMarkov1stOrderFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; double dt_; Vector tau_; @@ -73,8 +73,8 @@ public: /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "GaussMarkov1stOrderFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; this->noiseModel_->print(" noise model"); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 0828fbd08..efaf71d22 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -77,12 +77,12 @@ namespace gtsam { * vehicle */ template -class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5 { +class InertialNavFactor_GlobalVelocity : public NoiseModelFactorN { private: typedef InertialNavFactor_GlobalVelocity This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactorN Base; Vector measurement_acc_; Vector measurement_gyro_; @@ -117,11 +117,11 @@ public: /** print */ void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << "," + << keyFormatter(this->template key<3>()) << "," + << keyFormatter(this->template key<4>()) << "," + << keyFormatter(this->template key<5>()) << "\n"; std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; std::cout << "dt: " << this->dt_ << std::endl; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 44d3b8fd0..59a834f0b 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -24,7 +24,7 @@ namespace gtsam { * Ternary factor representing a visual measurement that includes inverse depth */ template -class InvDepthFactor3: public NoiseModelFactor3 { +class InvDepthFactor3: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactor3 This; @@ -93,8 +93,8 @@ public: if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,5); if (H3) *H3 = Matrix::Zero(2,1); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 40c09416c..a41a06ccd 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -24,7 +24,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant1: public NoiseModelFactor2 { +class InvDepthFactorVariant1: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant1 This; @@ -93,8 +93,8 @@ public: return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" - << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]" << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index b1169580e..d120eff32 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -25,7 +25,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant2: public NoiseModelFactor2 { +class InvDepthFactorVariant2: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -36,7 +36,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant2 This; @@ -96,8 +96,8 @@ public: return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" - << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]" << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 98f2db2f3..cade280f0 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -24,7 +24,7 @@ namespace gtsam { /** * Binary factor representing the first visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3a: public NoiseModelFactor2 { +class InvDepthFactorVariant3a: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant3a This; @@ -96,8 +96,8 @@ public: return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" - << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]" << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } @@ -150,7 +150,7 @@ private: /** * Ternary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3b: public NoiseModelFactor3 { +class InvDepthFactorVariant3b: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -160,7 +160,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant3b This; @@ -222,8 +222,8 @@ public: return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" - << " moved behind camera " << DefaultKeyFormatter(this->key2()) + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]" + << " moved behind camera " << DefaultKeyFormatter(this->key<2>()) << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index f81c18bfa..f2c5dd3a9 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -35,10 +35,10 @@ namespace gtsam { * optimized in x1 frame in the optimization. */ class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor - : public NoiseModelFactor3 { + : public NoiseModelFactorN { protected: OrientedPlane3 measured_p_; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; public: /// Constructor LocalOrientedPlane3Factor() {} diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 91d79f208..34ab51d15 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -187,8 +187,8 @@ namespace gtsam { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) << + " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl; if (throwCheirality_) throw e; } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 52a57b945..b49b49131 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -35,7 +35,7 @@ namespace gtsam { * @tparam VALUE is the type of variable the prior effects */ template - class PartialPriorFactor: public NoiseModelFactor1 { + class PartialPriorFactor: public NoiseModelFactorN { public: typedef VALUE T; @@ -44,7 +44,7 @@ namespace gtsam { // Concept checks on the variable type - currently requires Lie GTSAM_CONCEPT_LIE_TYPE(VALUE) - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef PartialPriorFactor This; Vector prior_; ///< Measurement on tangent space parameters, in compressed form. @@ -141,6 +141,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index b47dcaf33..676e011de 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -29,12 +29,12 @@ namespace gtsam { * @ingroup slam */ template - class PoseBetweenFactor: public NoiseModelFactor2 { + class PoseBetweenFactor: public NoiseModelFactorN { private: typedef PoseBetweenFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; POSE measured_; /** The measurement */ boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -68,8 +68,8 @@ namespace gtsam { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; measured_.print(" measured: "); if(this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index b0cb25cfa..c7a094bda 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -26,12 +26,12 @@ namespace gtsam { * @ingroup slam */ template - class PosePriorFactor: public NoiseModelFactor1 { + class PosePriorFactor: public NoiseModelFactorN { private: typedef PosePriorFactor This; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; POSE prior_; /** The measurement */ boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 8d183015e..ad1ba5fbe 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -21,10 +21,10 @@ namespace gtsam { * @ingroup slam */ template -class PoseToPointFactor : public NoiseModelFactor2 { +class PoseToPointFactor : public NoiseModelFactorN { private: typedef PoseToPointFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; POINT measured_; /** the point measurement in local coordinates */ @@ -47,8 +47,9 @@ class PoseToPointFactor : public NoiseModelFactor2 { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n" + std::cout << s << "PoseToPointFactor(" + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n" << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } @@ -91,8 +92,10 @@ class PoseToPointFactor : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar& boost::serialization::make_nvp( - "NoiseModelFactor2", boost::serialization::base_object(*this)); + "NoiseModelFactor2", + boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(measured_); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index d6f643c6c..8962b31b2 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -31,7 +31,7 @@ namespace gtsam { * @ingroup slam */ template - class ProjectionFactorPPP: public NoiseModelFactor3 { + class ProjectionFactorPPP: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -45,7 +45,7 @@ namespace gtsam { public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef ProjectionFactorPPP This; @@ -142,8 +142,11 @@ namespace gtsam { if (H2) *H2 = Matrix::Zero(2,6); if (H3) *H3 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->template key<2>()) + << " moved behind camera " + << DefaultKeyFormatter(this->template key<1>()) + << std::endl; if (throwCheirality_) throw e; } diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 5c9a8c691..afbf85838 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -34,7 +34,7 @@ namespace gtsam { */ template class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC - : public NoiseModelFactor4 { + : public NoiseModelFactorN { protected: Point2 measured_; ///< 2D measurement @@ -44,7 +44,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC public: /// shorthand for base class type - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef ProjectionFactorPPPC This; @@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC if (H3) *H3 = Matrix::Zero(2,3); if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) throw e; } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index c92a13daf..8173c9dbd 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -54,9 +54,9 @@ Vector ProjectionFactorRollingShutter::evaluateError( if (H3) *H3 = Matrix::Zero(2, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) throw CheiralityException(this->key2()); + << DefaultKeyFormatter(this->key<2>()) << " moved behind camera " + << DefaultKeyFormatter(this->key<1>()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key<2>()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 597d894da..806f54fa5 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -42,7 +42,7 @@ namespace gtsam { */ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter - : public NoiseModelFactor3 { + : public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O Point2 measured_; ///< 2D measurement @@ -60,7 +60,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef ProjectionFactorRollingShutter This; diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index ebf91d1f7..c6273ff4b 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -25,13 +25,13 @@ namespace gtsam { * * TODO: enable use of a Pose3 for the target as well */ -class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2 { +class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactorN { private: double measured_; /** measurement */ typedef RelativeElevationFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; public: @@ -66,6 +66,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 6f98a2dbd..2e024c5bb 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -26,11 +26,11 @@ namespace gtsam { /** * DeltaFactor: relative 2D measurement between Pose2 and Point2 */ -class DeltaFactor: public NoiseModelFactor2 { +class DeltaFactor: public NoiseModelFactorN { public: typedef DeltaFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr shared_ptr; private: @@ -55,11 +55,11 @@ public: /** * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes */ -class DeltaFactorBase: public NoiseModelFactor4 { +class DeltaFactorBase: public NoiseModelFactorN { public: typedef DeltaFactorBase This; - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr shared_ptr; private: @@ -110,11 +110,11 @@ public: /** * OdometryFactorBase: Pose2 odometry, with Basenodes */ -class OdometryFactorBase: public NoiseModelFactor4 { +class OdometryFactorBase: public NoiseModelFactorN { public: typedef OdometryFactorBase This; - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr shared_ptr; private: diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 3afa83f45..afe731bd5 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -32,7 +32,7 @@ namespace gtsam { * @ingroup slam */ template - class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ? + class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactorN ? public: diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 097dbd3fe..2cd0d0f39 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -124,9 +124,9 @@ namespace simulated2D { * Unary factor encoding a soft prior on a vector */ template - class GenericPrior: public NoiseModelFactor1 { + class GenericPrior: public NoiseModelFactorN { public: - typedef NoiseModelFactor1 Base; ///< base class + typedef NoiseModelFactorN Base; ///< base class typedef GenericPrior This; typedef boost::shared_ptr > shared_ptr; typedef VALUE Pose; ///< shortcut to Pose type @@ -168,9 +168,9 @@ namespace simulated2D { * Binary factor simulating "odometry" between two Vectors */ template - class GenericOdometry: public NoiseModelFactor2 { + class GenericOdometry: public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; ///< base class + typedef NoiseModelFactorN Base; ///< base class typedef GenericOdometry This; typedef boost::shared_ptr > shared_ptr; typedef VALUE Pose; ///< shortcut to Pose type @@ -214,9 +214,9 @@ namespace simulated2D { * Binary factor simulating "measurement" between two Vectors */ template - class GenericMeasurement: public NoiseModelFactor2 { + class GenericMeasurement: public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; ///< base class + typedef NoiseModelFactorN Base; ///< base class typedef GenericMeasurement This; typedef boost::shared_ptr > shared_ptr; typedef POSE Pose; ///< shortcut to Pose type diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 924a5fe4e..086da7cad 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -80,13 +80,13 @@ namespace simulated2DOriented { /// Unary factor encoding a soft prior on a vector template - struct GenericPosePrior: public NoiseModelFactor1 { + struct GenericPosePrior: public NoiseModelFactorN { Pose2 measured_; ///< measurement /// Create generic pose prior GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) : - NoiseModelFactor1(model, key), measured_(measured) { + NoiseModelFactorN(model, key), measured_(measured) { } /// Evaluate error and optionally derivative @@ -101,7 +101,7 @@ namespace simulated2DOriented { * Binary factor simulating "odometry" between two Vectors */ template - struct GenericOdometry: public NoiseModelFactor2 { + struct GenericOdometry: public NoiseModelFactorN { Pose2 measured_; ///< Between measurement for odometry factor typedef GenericOdometry This; @@ -111,7 +111,7 @@ namespace simulated2DOriented { */ GenericOdometry(const Pose2& measured, const SharedNoiseModel& model, Key i1, Key i2) : - NoiseModelFactor2(model, i1, i2), measured_(measured) { + NoiseModelFactorN(model, i1, i2), measured_(measured) { } ~GenericOdometry() override {} diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 3b4afb106..4a20acd15 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -68,7 +68,7 @@ Point3 mea(const Point3& x, const Point3& l, /** * A prior factor on a single linear robot pose */ -struct PointPrior3D: public NoiseModelFactor1 { +struct PointPrior3D: public NoiseModelFactorN { Point3 measured_; ///< The prior pose value for the variable attached to this factor @@ -79,7 +79,7 @@ struct PointPrior3D: public NoiseModelFactor1 { * @param key is the key for the pose */ PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) : - NoiseModelFactor1 (model, key), measured_(measured) { + NoiseModelFactorN (model, key), measured_(measured) { } /** @@ -98,7 +98,7 @@ struct PointPrior3D: public NoiseModelFactor1 { /** * Models a linear 3D measurement between 3D points */ -struct Simulated3DMeasurement: public NoiseModelFactor2 { +struct Simulated3DMeasurement: public NoiseModelFactorN { Point3 measured_; ///< Linear displacement between a pose and landmark @@ -110,7 +110,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { * @param pointKey is the point key for the landmark */ Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) : - NoiseModelFactor2(model, i, j), measured_(measured) {} + NoiseModelFactorN(model, i, j), measured_(measured) {} /** * Error function with optional derivatives diff --git a/tests/smallExample.h b/tests/smallExample.h index ca9a8580f..38b202c41 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -329,12 +329,12 @@ inline Matrix H(const Point2& v) { 0.0, cos(v.y())).finished(); } -struct UnaryFactor: public gtsam::NoiseModelFactor1 { +struct UnaryFactor: public gtsam::NoiseModelFactorN { Point2 z_; UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : - gtsam::NoiseModelFactor1(model, key), z_(z) { + gtsam::NoiseModelFactorN(model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional A = boost::none) const override { diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index f0c1b4b70..88da7006d 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -91,9 +91,9 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor -class NonlinearMotionModel : public NoiseModelFactor2 { +class NonlinearMotionModel : public NoiseModelFactorN { - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef NonlinearMotionModel This; protected: @@ -155,14 +155,14 @@ public: /* print */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": NonlinearMotionModel\n"; - std::cout << " TestKey1: " << keyFormatter(key1()) << std::endl; - std::cout << " TestKey2: " << keyFormatter(key2()) << std::endl; + std::cout << " TestKey1: " << keyFormatter(key<1>()) << std::endl; + std::cout << " TestKey2: " << keyFormatter(key<2>()) << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This *e = dynamic_cast (&f); - return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); + return (e != nullptr) && (key<1>() == e->key<1>()) && (key<2>() == e->key<2>()); } /** @@ -181,7 +181,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c.at(key1()))*unwhitenedError(c); + return QInvSqrt(c.at(key<1>()))*unwhitenedError(c); } /** @@ -190,14 +190,16 @@ public: * Hence b = z - h(x1,x2) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c) const override { - const X1& x1 = c.at(key1()); - const X2& x2 = c.at(key2()); + using X1 = ValueType<1>; + using X2 = ValueType<2>; + const X1& x1 = c.at(key<1>()); + const X2& x2 = c.at(key<2>()); Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != nullptr) { - return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), + return JacobianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(), A2, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor @@ -205,7 +207,7 @@ public: A1 = Qinvsqrt*A1; A2 = Qinvsqrt*A2; b = Qinvsqrt*b; - return GaussianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), + return GaussianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(), A2, b, noiseModel::Unit::Create(b.size()))); } @@ -232,9 +234,9 @@ public: }; // Create Measurement Model Factor -class NonlinearMeasurementModel : public NoiseModelFactor1 { +class NonlinearMeasurementModel : public NoiseModelFactorN { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef NonlinearMeasurementModel This; protected: @@ -320,6 +322,7 @@ public: * Hence b = z - h(x1) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c) const override { + using X = ValueType<1>; const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 5e3fc9189..2b5ec474d 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -35,8 +35,8 @@ using namespace gtsam::symbol_shorthand; typedef Pose2 Pose; -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; +typedef NoiseModelFactorN NM1; +typedef NoiseModelFactorN NM2; typedef BearingRangeFactor BR; //GTSAM_VALUE_EXPORT(Value); @@ -107,18 +107,18 @@ int main(int argc, char *argv[]) { boost::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps - if(measurement->key1() > step || measurement->key2() > step) + if(measurement->key<1>() > step || measurement->key<2>() > step) break; // Require that one of the nodes is the current one - if(measurement->key1() != step && measurement->key2() != step) + if(measurement->key<1>() != step && measurement->key<2>() != step) throw runtime_error("Problem in data file, out-of-sequence measurements"); // Add a new factor newFactors.push_back(measurement); // Initialize the new variable - if(measurement->key1() == step && measurement->key2() == step-1) { + if(measurement->key<1>() == step && measurement->key<2>() == step-1) { if(step == 1) newVariables.insert(step, measurement->measured().inverse()); else { @@ -126,7 +126,7 @@ int main(int argc, char *argv[]) { newVariables.insert(step, prevPose * measurement->measured().inverse()); } // cout << "Initializing " << step << endl; - } else if(measurement->key2() == step && measurement->key1() == step-1) { + } else if(measurement->key<2>() == step && measurement->key<1>() == step-1) { if(step == 1) newVariables.insert(step, measurement->measured()); else { diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index a056bde24..dcb42f763 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -36,8 +36,8 @@ using namespace gtsam::symbol_shorthand; typedef Pose2 Pose; -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; +typedef NoiseModelFactorN NM1; +typedef NoiseModelFactorN NM2; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); int main(int argc, char *argv[]) { From e5ec0071853e290c07506e8fadec9be8825fcd06 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 17:55:25 -0500 Subject: [PATCH 46/49] readability --- gtsam/nonlinear/NonlinearFactor.h | 114 +++++++++++++----------------- 1 file changed, 50 insertions(+), 64 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index e1b343218..c58179db3 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -555,31 +555,38 @@ class NoiseModelFactorN : public NoiseModelFactor { } public: - /// @name Deprecated methods + /// @name Deprecated methods. Use `key<1>()`, `key<2>()`, ... instead of old + /// `key1()`, `key2()`, ... + /// If your class is templated AND you are trying to call `key<1>` inside your + /// class, due to dependent types you need to do `this->template key<1>()`. /// @{ - template 1), void>::type> inline Key GTSAM_DEPRECATED key1() const { return key<1>(); } - template > + template inline Key GTSAM_DEPRECATED key2() const { + static_assert(I <= N, "Index out of bounds"); return key<2>(); } - template > + template inline Key GTSAM_DEPRECATED key3() const { + static_assert(I <= N, "Index out of bounds"); return key<3>(); } - template > + template inline Key GTSAM_DEPRECATED key4() const { + static_assert(I <= N, "Index out of bounds"); return key<4>(); } - template > + template inline Key GTSAM_DEPRECATED key5() const { + static_assert(I <= N, "Index out of bounds"); return key<5>(); } - template > + template inline Key GTSAM_DEPRECATED key6() const { + static_assert(I <= N, "Index out of bounds"); return key<6>(); } @@ -588,8 +595,12 @@ class NoiseModelFactorN : public NoiseModelFactor { }; // \class NoiseModelFactorN /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 - * with ValueType<1>. If your class is templated, use `this->template key<1>()` +/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1>() and X1 + * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 1 variable. To derive from this class, implement evaluateError(). */ @@ -613,11 +624,6 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} - /** 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 */ friend class boost::serialization::access; @@ -629,8 +635,12 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { }; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. If your class is templated, use `this->template key<1>()` +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 + * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 2 variables. To derive from this class, implement evaluateError(). */ @@ -656,12 +666,6 @@ class GTSAM_DEPRECATED NoiseModelFactor2 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor2() override {} - /** 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 */ friend class boost::serialization::access; @@ -673,8 +677,12 @@ class GTSAM_DEPRECATED NoiseModelFactor2 }; // \class NoiseModelFactor2 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. If your class is templated, use `this->template key<1>()` +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 + * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 3 variables. To derive from this class, implement evaluateError(). */ @@ -701,13 +709,6 @@ class GTSAM_DEPRECATED NoiseModelFactor3 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor3() override {} - /** 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 */ friend class boost::serialization::access; @@ -719,8 +720,12 @@ class GTSAM_DEPRECATED NoiseModelFactor3 }; // \class NoiseModelFactor3 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. If your class is templated, use `this->template key<1>()` +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 + * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 4 variables. To derive from this class, implement evaluateError(). */ @@ -748,14 +753,6 @@ class GTSAM_DEPRECATED NoiseModelFactor4 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor4() override {} - /** 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 */ friend class boost::serialization::access; @@ -767,8 +764,12 @@ class GTSAM_DEPRECATED NoiseModelFactor4 }; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. If your class is templated, use `this->template key<1>()` +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 + * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 5 variables. To derive from this class, implement evaluateError(). */ @@ -798,15 +799,6 @@ class GTSAM_DEPRECATED NoiseModelFactor5 VALUE5>::NoiseModelFactorN; ~NoiseModelFactor5() override {} - /** 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 */ friend class boost::serialization::access; @@ -818,8 +810,12 @@ class GTSAM_DEPRECATED NoiseModelFactor5 }; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 - * with ValueType<1>. If your class is templated, use `this->template key<1>()` +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 + * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 6 variables. To derive from this class, implement evaluateError(). */ @@ -852,16 +848,6 @@ class GTSAM_DEPRECATED NoiseModelFactor6 VALUE6>::NoiseModelFactorN; ~NoiseModelFactor6() override {} - /** 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 */ friend class boost::serialization::access; From 581c2d5ebd3b926ea391ebecb101797d52b4f3d2 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 23:13:30 -0500 Subject: [PATCH 47/49] Use new key version in Barometric --- gtsam/navigation/BarometricFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 2f0ff7436..2e87986ae 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -26,8 +26,8 @@ namespace gtsam { void BarometricFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "Barometric Factor on " - << keyFormatter(key1()) << "Barometric Bias on " - << keyFormatter(key2()) << "\n"; + << keyFormatter(key<1>()) << "Barometric Bias on " + << keyFormatter(key<2>()) << "\n"; cout << " Baro measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); From a3e314f3f7fed464c7624e26c5ef8f22c68a9072 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 22 Dec 2022 23:56:46 -0500 Subject: [PATCH 48/49] suppress warnings in backwards compatibility unit tests --- gtsam/base/types.h | 39 +++++++++++++++++++++++++++++++---- tests/testNonlinearFactor.cpp | 23 +++++++++++++++++++++ 2 files changed, 58 insertions(+), 4 deletions(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index cb8412cdf..b9f4b0a3e 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -46,18 +46,49 @@ #include #endif +/* Define macros for ignoring compiler warnings. + * Usage Example: + * ``` + * CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") + * GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") + * MSVC_DIAGNOSTIC_PUSH_IGNORE(4996) + * // ... code you want to suppress deprecation warnings for ... + * DIAGNOSTIC_POP() + * ``` + */ +#define DO_PRAGMA(x) _Pragma (#x) #ifdef __clang__ # define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ _Pragma("clang diagnostic push") \ - _Pragma("clang diagnostic ignored \"" diag "\"") + DO_PRAGMA(clang diagnostic ignored diag) #else # define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) #endif -#ifdef __clang__ -# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") +#ifdef __GNUC__ +# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \ + _Pragma("GCC diagnostic push") \ + DO_PRAGMA(GCC diagnostic ignored diag) #else -# define CLANG_DIAGNOSTIC_POP() +# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) +#endif + +#ifdef _MSC_VER +# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) \ + _Pragma("warning ( push )") \ + DO_PRAGMA(warning ( disable : code )) +#else +# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) +#endif + +#if defined(__clang__) +# define DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") +#elif defined(__GNUC__) +# define DIAGNOSTIC_POP() _Pragma("GCC diagnostic pop") +#elif defined(_MSC_VER) +# define DIAGNOSTIC_POP() _Pragma("warning ( pop )") +#else +# define DIAGNOSTIC_POP() #endif namespace gtsam { diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 99ec2f501..c536a48c3 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -330,6 +330,13 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) } /* ************************************************************************* */ +// Suppress deprecation warnings while we are testing backwards compatibility +#define IGNORE_DEPRECATED_PUSH \ + CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \ + GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \ + MSVC_DIAGNOSTIC_PUSH_IGNORE(4996) +/* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor1 : public NoiseModelFactor1 { static_assert(std::is_same::value, "Base type wrong"); static_assert(std::is_same>::value, @@ -351,6 +358,7 @@ class TestFactor1 : public NoiseModelFactor1 { gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor1) { @@ -380,6 +388,7 @@ TEST(NonlinearFactor, NoiseModelFactor1) { } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor4 : public NoiseModelFactor4 { static_assert(std::is_same::value, "Base type wrong"); static_assert( @@ -411,6 +420,7 @@ class TestFactor4 : public NoiseModelFactor4 { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor4) { @@ -434,6 +444,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb())); // Test all functions/types for backwards compatibility + IGNORE_DEPRECATED_PUSH static_assert(std::is_same::value, "X1 type incorrect"); static_assert(std::is_same::value, @@ -452,6 +463,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { 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))); + DIAGNOSTIC_POP() // And test "forward compatibility" using `key` and `ValueType` too static_assert(std::is_same, double>::value, @@ -477,6 +489,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; @@ -500,6 +513,7 @@ public: .finished(); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor5) { @@ -527,6 +541,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 Base; @@ -554,6 +569,7 @@ public: } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor6) { @@ -656,6 +672,13 @@ TEST(NonlinearFactor, NoiseModelFactorN) { EXPECT(assert_equal(H3_expected, H3)); EXPECT(assert_equal(H4_expected, H4)); + // Test all functions/types for backwards compatibility + IGNORE_DEPRECATED_PUSH + static_assert(std::is_same::value, + "X1 type incorrect"); + EXPECT(assert_equal(tf.key3(), X(3))); + DIAGNOSTIC_POP() + // Test using `key` and `ValueType` static_assert(std::is_same, double>::value, "ValueType<1> type incorrect"); From 28b118ccda00988f5d9c9059ce17ca791ba42112 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Dec 2022 11:09:35 +0530 Subject: [PATCH 49/49] GaussianConditional logDeterminant function to remove code duplication. --- gtsam/linear/GaussianBayesNet.cpp | 9 +-------- gtsam/linear/GaussianBayesTree-inl.h | 2 +- gtsam/linear/GaussianBayesTree.cpp | 10 +--------- gtsam/linear/GaussianConditional.cpp | 14 ++++++++++++++ gtsam/linear/GaussianConditional.h | 22 ++++++++++++++++++++++ 5 files changed, 39 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 6dcf662a9..41a734b34 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -217,14 +217,7 @@ namespace gtsam { double GaussianBayesNet::logDeterminant() const { double logDet = 0.0; for (const sharedConditional& cg : *this) { - if (cg->get_model()) { - Vector diag = cg->R().diagonal(); - cg->get_model()->whitenInPlace(diag); - logDet += diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - logDet += - cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - } + logDet += cg->logDeterminant(); } return logDet; } diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index d781533e6..e73420644 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) { double result = 0.0; // this clique - result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + result += clique->conditional()->logDeterminant(); // sum of children for(const typename BAYESTREE::sharedClique& child: clique->children_) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index a83475e26..b35252e72 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -50,15 +50,7 @@ namespace gtsam { const GaussianBayesTreeClique::shared_ptr& clique, LogDeterminantData& parentSum) { auto cg = clique->conditional(); - double logDet; - if (cg->get_model()) { - Vector diag = cg->R().diagonal(); - cg->get_model()->whitenInPlace(diag); - logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - logDet = - cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - } + double logDet = cg->logDeterminant(); // Add the current clique's log-determinant to the overall sum (*parentSum.logDet) += logDet; return parentSum; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 951577641..60ddb1b7d 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -155,6 +155,20 @@ namespace gtsam { } } +/* ************************************************************************* */ +double GaussianConditional::logDeterminant() const { + double logDet; + if (this->get_model()) { + Vector diag = this->R().diagonal(); + this->get_model()->whitenInPlace(diag); + logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); + } else { + logDet = + this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + } + return logDet; +} + /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 4822e3eaf..8af7f6602 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -133,6 +133,28 @@ namespace gtsam { /** Get a view of the r.h.s. vector d */ const constBVector d() const { return BaseFactor::getb(); } + /** + * @brief Compute the log determinant of the Gaussian conditional. + * The determinant is computed using the R matrix, which is upper + * triangular. + * For numerical stability, the determinant is computed in log + * form, so it is a summation rather than a multiplication. + * + * @return double + */ + double logDeterminant() const; + + /** + * @brief Compute the determinant of the conditional from the + * upper-triangular R matrix. + * + * The determinant is computed in log form (hence summation) for numerical + * stability and then exponentiated. + * + * @return double + */ + double determinant() const { return exp(this->logDeterminant()); } + /** * Solves a conditional Gaussian and writes the solution into the entries of * \c x for each frontal variable of the conditional. The parents are