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))); } /* ************************************************************************* */