address review comments
parent
0ebc6e881d
commit
b24511fb18
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
Loading…
Reference in New Issue