address review comments

release/4.3a0
Gerry Chen 2022-12-19 18:49:20 -05:00
parent 0ebc6e881d
commit b24511fb18
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
2 changed files with 31 additions and 12 deletions

View File

@ -310,7 +310,8 @@ class NoiseModelFactorN : public NoiseModelFactor {
/// The type of the i'th template param can be obtained as ValueType<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 ValueType = typename std::tuple_element<I, std::tuple<ValueTypes...>>::type; using ValueType =
typename std::tuple_element<I, std::tuple<ValueTypes...>>::type;
protected: protected:
using Base = NoiseModelFactor; using Base = NoiseModelFactor;
@ -319,12 +320,12 @@ class NoiseModelFactorN : public NoiseModelFactor {
/* 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 ValueTypes */ * 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 OptionalMatrix = 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 ValueTypes */ * parameter-packs with same length as ValueTypes */
template <typename T> template <typename T>
using key_type = Key; using KeyType = Key;
public: public:
/// @name Constructors /// @name Constructors
@ -341,16 +342,17 @@ class NoiseModelFactorN : public NoiseModelFactor {
* arguments. * arguments.
*/ */
NoiseModelFactorN(const SharedNoiseModel& noiseModel, NoiseModelFactorN(const SharedNoiseModel& noiseModel,
key_type<ValueTypes>... keys) KeyType<ValueTypes>... keys)
: Base(noiseModel, std::array<Key, N>{keys...}) {} : Base(noiseModel, std::array<Key, N>{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<Key>
* @param noiseModel Shared pointer to noise model. * @param noiseModel Shared pointer to noise model.
* @param keys A container of keys for the variables in this factor. * @param keys A container of keys for the variables in this factor.
*/ */
template <typename CONTAINER, // use "dummy" parameter T to delay deduction template <typename CONTAINER = std::initializer_list<Key>>
size_t T = N, typename std::enable_if<(T > 1), bool>::type = true>
NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
: Base(noiseModel, keys) { : Base(noiseModel, keys) {
assert(keys.size() == N); assert(keys.size() == N);
@ -379,7 +381,8 @@ 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<ValueTypes...>{}, x, H); return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
H);
} }
/// @} /// @}
@ -407,7 +410,7 @@ class NoiseModelFactorN : public NoiseModelFactor {
* @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 ValueTypes&... x, virtual Vector evaluateError(const ValueTypes&... x,
optional_matrix_type<ValueTypes>... H) const = 0; OptionalMatrix<ValueTypes>... H) const = 0;
/// @} /// @}
/// @name Convenience method overloads /// @name Convenience method overloads
@ -420,7 +423,7 @@ 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 ValueTypes&... x) const { inline Vector evaluateError(const ValueTypes&... x) const {
return evaluateError(x..., optional_matrix_type<ValueTypes>()...); return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
} }
/** Some optional jacobians omitted function overload */ /** Some optional jacobians omitted function overload */
@ -506,7 +509,8 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
* with 2 variables. To derive from this class, implement evaluateError(). * with 2 variables. To derive from this class, implement evaluateError().
*/ */
template <class VALUE1, class VALUE2> template <class VALUE1, class VALUE2>
class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN<VALUE1, VALUE2> { class GTSAM_DEPRECATED NoiseModelFactor2
: public NoiseModelFactorN<VALUE1, VALUE2> {
public: public:
// aliases for value types pulled from keys // aliases for value types pulled from keys
using X1 = VALUE1; using X1 = VALUE1;
@ -542,7 +546,8 @@ class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN<VALUE1, VALU
* with 3 variables. To derive from this class, implement evaluateError(). * with 3 variables. To derive from this class, implement evaluateError().
*/ */
template <class VALUE1, class VALUE2, class VALUE3> template <class VALUE1, class VALUE2, class VALUE3>
class GTSAM_DEPRECATED NoiseModelFactor3 : public NoiseModelFactorN<VALUE1, VALUE2, VALUE3> { class GTSAM_DEPRECATED NoiseModelFactor3
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3> {
public: public:
// aliases for value types pulled from keys // aliases for value types pulled from keys
using X1 = VALUE1; using X1 = VALUE1;

View File

@ -338,6 +338,7 @@ class TestFactor1 : public NoiseModelFactor1<double> {
public: public:
typedef NoiseModelFactor1<double> Base; typedef NoiseModelFactor1<double> Base;
TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
using Base::NoiseModelFactor1; // inherit constructors
Vector evaluateError(const double& x1, boost::optional<Matrix&> H1 = Vector evaluateError(const double& x1, boost::optional<Matrix&> H1 =
boost::none) const override { boost::none) const override {
@ -371,6 +372,10 @@ TEST(NonlinearFactor, NoiseModelFactor1) {
EXPECT(assert_equal(tf.key(), L(1))); EXPECT(assert_equal(tf.key(), L(1)));
std::vector<Matrix> H = {Matrix()}; std::vector<Matrix> H = {Matrix()};
EXPECT(assert_equal(Vector1(1.0), tf.unwhitenedError(tv, H))); 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<double, double, double, double> {
public: public:
typedef NoiseModelFactor4<double, double, double, double> Base; typedef NoiseModelFactor4<double, double, double, double> 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)) {}
using Base::NoiseModelFactor4; // inherit constructors
Vector Vector
evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, 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<1>(), X(2)));
EXPECT(assert_equal(tf.key<2>(), X(3))); EXPECT(assert_equal(tf.key<2>(), X(3)));
EXPECT(assert_equal(tf.key<3>(), X(4))); 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<Key, 4>{L(1), L(2), L(3), L(4)});
std::vector<Key> keys = {L(1), L(2), L(3), L(4)};
TestFactor4 tf5(noiseModel::Unit::Create(1), keys);
} }
/* ************************************************************************* */ /* ************************************************************************* */