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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/nonlinear/NonlinearFactor.h
View File

@ -310,7 +310,8 @@ class NoiseModelFactorN : public NoiseModelFactor {
/// 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>
using ValueType = typename std::tuple_element<I, std::tuple<ValueTypes...>>::type;
using ValueType =
typename std::tuple_element<I, std::tuple<ValueTypes...>>::type;
protected:
using Base = NoiseModelFactor;
@ -319,12 +320,12 @@ class NoiseModelFactorN : public NoiseModelFactor {
/* Like std::void_t, except produces `boost::optional<Matrix&>` instead. Used
* to expand fixed-type parameter-packs with same length as ValueTypes */
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
* parameter-packs with same length as ValueTypes */
template <typename T>
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<ValueTypes>... keys)
KeyType<ValueTypes>... 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 keys A container of keys for the variables in this factor.
*/
template <typename CONTAINER, // use "dummy" parameter T to delay deduction
size_t T = N, typename std::enable_if<(T > 1), bool>::type = true>
template <typename CONTAINER = std::initializer_list<Key>>
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<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).
*/
virtual Vector evaluateError(const ValueTypes&... x,
optional_matrix_type<ValueTypes>... H) const = 0;
OptionalMatrix<ValueTypes>... 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<ValueTypes>()...);
return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
}
/** 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().
*/
template <class VALUE1, class VALUE2>
class GTSAM_DEPRECATED NoiseModelFactor2 : public NoiseModelFactorN<VALUE1, VALUE2> {
class GTSAM_DEPRECATED NoiseModelFactor2
: public NoiseModelFactorN<VALUE1, VALUE2> {
public:
// aliases for value types pulled from keys
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().
*/
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:
// aliases for value types pulled from keys
using X1 = VALUE1;

14
tests/testNonlinearFactor.cpp
View File

@ -338,6 +338,7 @@ class TestFactor1 : public NoiseModelFactor1<double> {
public:
typedef NoiseModelFactor1<double> Base;
TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
using Base::NoiseModelFactor1; // inherit constructors
Vector evaluateError(const double& x1, boost::optional<Matrix&> H1 =
boost::none) const override {
@ -371,6 +372,10 @@ TEST(NonlinearFactor, NoiseModelFactor1) {
EXPECT(assert_equal(tf.key(), L(1)));
std::vector<Matrix> 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<double, double, double, double> {
public:
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)) {}
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<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);
}
/* ************************************************************************* */