12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Change `X<N>` to `ValueType<N>` and `VALUES` -> `ValueTypes`

release/4.3a0
Gerry Chen 2022-12-19 18:06:34 -05:00
parent 63950b952b
commit 0ebc6e881d
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
2 changed files with 57 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/nonlinear/NonlinearFactor.h
View File

@ -302,27 +302,27 @@ public:
* are typically more general than just vectors, e.g., Rot3 or Pose3, which are * are typically more general than just vectors, e.g., Rot3 or Pose3, which are
* objects in non-linear manifolds (Lie groups). * objects in non-linear manifolds (Lie groups).
*/ */
template <class... VALUES> template <class... ValueTypes>
class NoiseModelFactorN : public NoiseModelFactor { class NoiseModelFactorN : public NoiseModelFactor {
public: public:
/// N is the number of variables (N-way factor) /// 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<I> /// The type of the i'th template param can be obtained as ValueType<I>
template <int I, typename std::enable_if<(I < N), bool>::type = true> template <int I, typename std::enable_if<(I < N), bool>::type = true>
using X = typename std::tuple_element<I, std::tuple<VALUES...>>::type; using ValueType = typename std::tuple_element<I, std::tuple<ValueTypes...>>::type;
protected: protected:
using Base = NoiseModelFactor; using Base = NoiseModelFactor;
using This = NoiseModelFactorN<VALUES...>; using This = NoiseModelFactorN<ValueTypes...>;
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead. Used /* Like std::void_t, except produces `boost::optional<Matrix&>` 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 <typename T> template <typename T>
using optional_matrix_type = boost::optional<Matrix&>; using optional_matrix_type = boost::optional<Matrix&>;
/* Like std::void_t, except produces `Key` instead. Used to expand fixed-type /* 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 <typename T> template <typename T>
using key_type = Key; using key_type = Key;
@ -341,7 +341,7 @@ class NoiseModelFactorN : public NoiseModelFactor {
* arguments. * arguments.
*/ */
NoiseModelFactorN(const SharedNoiseModel& noiseModel, NoiseModelFactorN(const SharedNoiseModel& noiseModel,
key_type<VALUES>... keys) key_type<ValueTypes>... keys)
: Base(noiseModel, std::array<Key, N>{keys...}) {} : Base(noiseModel, std::array<Key, N>{keys...}) {}
/** /**
@ -379,7 +379,7 @@ class NoiseModelFactorN : public NoiseModelFactor {
Vector unwhitenedError( Vector unwhitenedError(
const Values& x, const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const override { boost::optional<std::vector<Matrix>&> H = boost::none) const override {
return unwhitenedError(boost::mp11::index_sequence_for<VALUES...>{}, x, H); return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x, H);
} }
/// @} /// @}
@ -406,8 +406,8 @@ class NoiseModelFactorN : public NoiseModelFactor {
* as separate arguments. * as separate arguments.
* @param[out] H The Jacobian with respect to each variable (optional). * @param[out] H The Jacobian with respect to each variable (optional).
*/ */
virtual Vector evaluateError(const VALUES&... x, virtual Vector evaluateError(const ValueTypes&... x,
optional_matrix_type<VALUES>... H) const = 0; optional_matrix_type<ValueTypes>... H) const = 0;
/// @} /// @}
/// @name Convenience method overloads /// @name Convenience method overloads
@ -419,8 +419,8 @@ class NoiseModelFactorN : public NoiseModelFactor {
* *
* e.g. `Vector error = factor.evaluateError(x1, x2, x3);` * e.g. `Vector error = factor.evaluateError(x1, x2, x3);`
*/ */
inline Vector evaluateError(const VALUES&... x) const { inline Vector evaluateError(const ValueTypes&... x) const {
return evaluateError(x..., optional_matrix_type<VALUES>()...); return evaluateError(x..., optional_matrix_type<ValueTypes>()...);
} }
/** Some optional jacobians omitted function overload */ /** Some optional jacobians omitted function overload */
@ -428,7 +428,7 @@ class NoiseModelFactorN : public NoiseModelFactor {
typename std::enable_if<(sizeof...(OptionalJacArgs) > 0) && typename std::enable_if<(sizeof...(OptionalJacArgs) > 0) &&
(sizeof...(OptionalJacArgs) < N), (sizeof...(OptionalJacArgs) < N),
bool>::type = true> bool>::type = true>
inline Vector evaluateError(const VALUES&... x, inline Vector evaluateError(const ValueTypes&... x,
OptionalJacArgs&&... H) const { OptionalJacArgs&&... H) const {
return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
boost::none); boost::none);
@ -447,9 +447,9 @@ class NoiseModelFactorN : public NoiseModelFactor {
boost::optional<std::vector<Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (this->active(x)) { if (this->active(x)) {
if (H) { if (H) {
return evaluateError(x.at<VALUES>(keys_[Inds])..., (*H)[Inds]...); return evaluateError(x.at<ValueTypes>(keys_[Inds])..., (*H)[Inds]...);
} else { } else {
return evaluateError(x.at<VALUES>(keys_[Inds])...); return evaluateError(x.at<ValueTypes>(keys_[Inds])...);
} }
} else { } else {
return Vector::Zero(this->dim()); return Vector::Zero(this->dim());
@ -466,15 +466,15 @@ class NoiseModelFactorN : public NoiseModelFactor {
}; // \class NoiseModelFactorN }; // \class NoiseModelFactorN
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key() with .key<0> and X1
* with X<1>. * with ValueType<0>.
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 1 variable. To derive from this class, implement evaluateError(). * with 1 variable. To derive from this class, implement evaluateError().
*/ */
template <class VALUE> template <class VALUE>
class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN<VALUE> { class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
public: public:
// aliases for value types pulled from keys // aliases for value types pulled from keys, for backwards compatibility
using X = VALUE; using X = VALUE;
protected: protected:
@ -500,8 +500,8 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
}; // \class NoiseModelFactor1 }; // \class NoiseModelFactor1
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1
* with X<1>. * with ValueType<0>.
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 2 variables. To derive from this class, implement evaluateError(). * with 2 variables. To derive from this class, implement evaluateError().
*/ */
@ -536,8 +536,8 @@ class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN<VALUE1, VALU
}; // \class NoiseModelFactor2 }; // \class NoiseModelFactor2
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1
* with X<1>. * with ValueType<0>.
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 3 variables. To derive from this class, implement evaluateError(). * with 3 variables. To derive from this class, implement evaluateError().
*/ */
@ -574,8 +574,8 @@ class GTSAM_DEPRECATED NoiseModelFactor3 : public NoiseModelFactorN<VALUE1, VALU
}; // \class NoiseModelFactor3 }; // \class NoiseModelFactor3
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1
* with X<1>. * with ValueType<0>.
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 4 variables. To derive from this class, implement evaluateError(). * with 4 variables. To derive from this class, implement evaluateError().
*/ */
@ -615,8 +615,8 @@ class GTSAM_DEPRECATED NoiseModelFactor4
}; // \class NoiseModelFactor4 }; // \class NoiseModelFactor4
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1
* with X<1>. * with ValueType<0>.
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 5 variables. To derive from this class, implement evaluateError(). * with 5 variables. To derive from this class, implement evaluateError().
*/ */
@ -659,8 +659,8 @@ class GTSAM_DEPRECATED NoiseModelFactor5
}; // \class NoiseModelFactor5 }; // \class NoiseModelFactor5
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<0> and X1
* with X<1>. * with ValueType<0>.
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 6 variables. To derive from this class, implement evaluateError(). * with 6 variables. To derive from this class, implement evaluateError().
*/ */

28
tests/testNonlinearFactor.cpp
View File

@ -441,6 +441,20 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
EXPECT(assert_equal(tf.key4(), X(4))); EXPECT(assert_equal(tf.key4(), X(4)));
std::vector<Matrix> H = {Matrix(), Matrix(), Matrix(), Matrix()}; std::vector<Matrix> H = {Matrix(), Matrix(), Matrix(), Matrix()};
EXPECT(assert_equal(Vector1(10.0), tf.unwhitenedError(tv, H))); EXPECT(assert_equal(Vector1(10.0), tf.unwhitenedError(tv, H)));
// And test "forward compatibility" using `key<N>` and `ValueType<N>` too
static_assert(std::is_same<TestFactor4::ValueType<0>, double>::value,
"ValueType<0> type incorrect");
static_assert(std::is_same<TestFactor4::ValueType<1>, double>::value,
"ValueType<1> type incorrect");
static_assert(std::is_same<TestFactor4::ValueType<2>, double>::value,
"ValueType<2> type incorrect");
static_assert(std::is_same<TestFactor4::ValueType<3>, 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(H2_expected, H2));
EXPECT(assert_equal(H3_expected, H3)); EXPECT(assert_equal(H3_expected, H3));
EXPECT(assert_equal(H4_expected, H4)); EXPECT(assert_equal(H4_expected, H4));
// Test using `key<N>` and `ValueType<N>`
static_assert(std::is_same<TestFactorN::ValueType<0>, double>::value,
"ValueType<0> type incorrect");
static_assert(std::is_same<TestFactorN::ValueType<1>, double>::value,
"ValueType<1> type incorrect");
static_assert(std::is_same<TestFactorN::ValueType<2>, double>::value,
"ValueType<2> type incorrect");
static_assert(std::is_same<TestFactorN::ValueType<3>, 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)));
} }
/* ************************************************************************* */ /* ************************************************************************* */