From b24511fb18080db085338b2630b206c61cbd91a0 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 19 Dec 2022 18:49:20 -0500 Subject: [PATCH] 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); } /* ************************************************************************* */