diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index d9b92b8aa3..0a05a704c0 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -27,3 +27,42 @@ struct RedirectCout { }; } + +// boost::index_sequence was introduced in 1.66, so we'll manually define an +// implementation if user has 1.65. boost::index_sequence is used to get array +// indices that align with a parameter pack. +#include +#if BOOST_VERSION >= 106600 +#include +#else +namespace boost { +namespace mp11 { +// Adapted from https://stackoverflow.com/a/32223343/9151520 +template +struct index_sequence { + using type = index_sequence; + using value_type = size_t; + static constexpr std::size_t size() noexcept { return sizeof...(Ints); } +}; +namespace detail { +template +struct _merge_and_renumber; + +template +struct _merge_and_renumber, index_sequence > + : index_sequence {}; +} // namespace detail +template +struct make_index_sequence + : detail::_merge_and_renumber< + typename make_index_sequence::type, + typename make_index_sequence::type> {}; +template <> +struct make_index_sequence<0> : index_sequence<> {}; +template <> +struct make_index_sequence<1> : index_sequence<0> {}; +template +using index_sequence_for = make_index_sequence; +} // namespace mp11 +} // namespace boost +#endif diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index 3c5342ca50..65290e0c93 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -22,7 +22,7 @@ To use GTSAM to solve your own problems, you will often have to create new facto -# The number of variables your factor involves is unknown at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError() - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel). --# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError() +-# The number of variables your factor involves is known at compile time, derive from NoiseModelFactorN (where T1, T2, ... are the types of the variables, e.g. double, Vector, Pose3) and implement \c evaluateError(). - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor. -# Derive from NonlinearFactor - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor. diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 7fafd95dfa..ad52355e42 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -25,6 +25,7 @@ #include #include #include +#include // boost::index_sequence #include #include @@ -272,68 +273,193 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { /* ************************************************************************* */ - /** - * A convenient base class for creating your own NoiseModelFactor with 1 - * variable. To derive from this class, implement evaluateError(). + * A convenient base class for creating your own NoiseModelFactor + * with n variables. To derive from this class, implement evaluateError(). + * + * For example, a 2-way factor that computes the difference in x-translation + * between a Pose3 and Point3 could be implemented like so: + * + * ~~~~~~~~~~~~~~~~~~~~{.cpp} + * class MyFactor : public NoiseModelFactorN { + * public: + * using Base = NoiseModelFactorN; + * + * MyFactor(Key pose_key, Key point_key, const SharedNoiseModel& noiseModel) + * : Base(noiseModel, pose_key, point_key) {} + * + * Vector evaluateError( + * const Pose3& T, const Point3& p, + * boost::optional H_T = boost::none, + * boost::optional H_p = boost::none) const override { + * Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T + * + * // Only compute t_H_T if needed: + * Point3 t = T.translation(H_T ? &t_H_T : 0); + * double a = t(0); // a_H_t = [1, 0, 0] + * double b = p(0); // b_H_p = [1, 0, 0] + * double error = a - b; // H_a = 1, H_b = -1 + * + * // H_T = H_a * a_H_t * t_H_T = the first row of t_H_T + * if (H_T) *H_T = (Matrix(1, 6) << t_H_T.row(0)).finished(); + * // H_p = H_b * b_H_p = -1 * [1, 0, 0] + * if (H_p) *H_p = (Matrix(1, 3) << -1., 0., 0.).finished(); * - * Templated on a values structure type. The values structures are typically - * more general than just vectors, e.g., Rot3 or Pose3, - * which are objects in non-linear manifolds (Lie groups). + * return Vector1(error); + * } + * }; + * + * // Unit Test + * TEST(NonlinearFactor, MyFactor) { + * MyFactor f(X(1), X(2), noiseModel::Unit::Create(1)); + * EXPECT_DOUBLES_EQUAL(-8., f.evaluateError(Pose3(), Point3(8., 7., 6.))(0), + * 1e-9); + * Values values; + * values.insert(X(1), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3))); + * values.insert(X(2), Point3(1, 2, 3)); + * EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); + * } + * ~~~~~~~~~~~~~~~~~~~~ + * + * These factors are templated on a values structure type. The values structures + * are typically more general than just vectors, e.g., Rot3 or Pose3, which are + * objects in non-linear manifolds (Lie groups). */ -template -class NoiseModelFactor1: public NoiseModelFactor { +template +class NoiseModelFactorN : public NoiseModelFactor { + public: + /// N is the number of variables (N-way factor) + enum { N = sizeof...(ValueTypes) }; -public: + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactorN; + + /// @name SFINAE aliases + /// @{ - // typedefs for value types pulled from keys - typedef VALUE X; + template + using IsConvertible = + typename std::enable_if::value, void>::type; -protected: + template + using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N), + void>::type; // 1-indexed! - typedef NoiseModelFactor Base; - typedef NoiseModelFactor1 This; + template + using ContainerElementType = + typename std::decay().begin())>::type; + template + using IsContainerOfKeys = IsConvertible, Key>; + + /// @} + + /* Like std::void_t, except produces `boost::optional` instead of + * `void`. Used to expand fixed-type parameter-packs with same length as + * ValueTypes. */ + template + using OptionalMatrix = boost::optional; + + /* Like std::void_t, except produces `Key` instead of `void`. Used to expand + * fixed-type parameter-packs with same length as ValueTypes. */ + template + using KeyType = Key; + + public: + /** + * The type of the I'th template param can be obtained as ValueType. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * using Factor = NoiseModelFactorN; + * Factor::ValueType<1> // Pose3 + * Factor::ValueType<2> // Point3 + * // Factor::ValueType<0> // ERROR! Will not compile. + * // Factor::ValueType<3> // ERROR! Will not compile. + * ``` + */ + template > + using ValueType = + typename std::tuple_element>::type; + + public: -public: /// @name Constructors /// @{ - /** Default constructor for I/O only */ - NoiseModelFactor1() {} - - ~NoiseModelFactor1() override {} + /// Default Constructor for I/O + NoiseModelFactorN() {} - inline Key key() const { return keys_[0]; } + /** + * Constructor. + * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) + * @param noiseModel Shared pointer to noise model. + * @param keys Keys for the variables in this factor, passed in as separate + * arguments. + */ + NoiseModelFactorN(const SharedNoiseModel& noiseModel, + KeyType... keys) + : Base(noiseModel, std::array{keys...}) {} /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param key1 by which to look up X value in Values + * Constructor. + * Example usage: `NoiseModelFactorN(noise, {key1, key2, ..., keyN})` + * Example usage: `NoiseModelFactorN(noise, keys)` where keys is a vector + * @param noiseModel Shared pointer to noise model. + * @param keys A container of keys for the variables in this factor. */ - NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1) - : Base(noiseModel, cref_list_of<1>(key1)) {} + template , + typename = IsContainerOfKeys> + NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) + : Base(noiseModel, keys) { + if (keys.size() != N) { + throw std::invalid_argument( + "NoiseModelFactorN: wrong number of keys given"); + } + } /// @} + + ~NoiseModelFactorN() override {} + + /** Returns a key. Usage: `key()` returns the I'th key. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * NoiseModelFactorN factor(noise, key1, key2); + * key<1>() // = key1 + * key<2>() // = key2 + * // key<0>() // ERROR! Will not compile + * // key<3>() // ERROR! Will not compile + * ``` + */ + template > + inline Key key() const { + return keys_[I - 1]; + } + /// @name NoiseModelFactor methods /// @{ - /** - * Calls the 1-key specific version of evaluateError below, which is pure - * virtual so must be implemented in the derived class. + /** This implements the `unwhitenedError` virtual function by calling the + * n-key specific version of evaluateError, which is pure virtual so must be + * implemented in the derived class. + * + * Example usage: + * ``` + * gtsam::Values values; + * values.insert(...) // populate values + * std::vector Hs(2); // this will be an optional output argument + * const Vector error = factor.unwhitenedError(values, Hs); + * ``` + * @param[in] x A Values object containing the values of all the variables + * used in this factor + * @param[out] H A vector of (dynamic) matrices whose size should be equal to + * n. The Jacobians w.r.t. each variable will be output in this parameter. */ Vector unwhitenedError( - const Values &x, - boost::optional &> H = boost::none) const override { - if (this->active(x)) { - const X &x1 = x.at(keys_[0]); - if (H) { - return evaluateError(x1, (*H)[0]); - } else { - return evaluateError(x1); - } - } else { - return Vector::Zero(this->dim()); - } + const Values& x, + boost::optional&> H = boost::none) const override { + return unwhitenedError(boost::mp11::index_sequence_for{}, x, + H); } /// @} @@ -341,434 +467,374 @@ class NoiseModelFactor1: public NoiseModelFactor { /// @{ /** - * Override this method to finish implementing a unary factor. - * If the optional Matrix reference argument is specified, it should compute - * both the function evaluation and its derivative in X. + * Override `evaluateError` to finish implementing an n-way factor. + * + * Both the `x` and `H` arguments are written here as parameter packs, but + * when overriding this method, you probably want to explicitly write them + * out. For example, for a 2-way factor with variable types Pose3 and Point3, + * you should implement: + * ``` + * Vector evaluateError( + * const Pose3& x1, const Point3& x2, + * boost::optional H1 = boost::none, + * boost::optional H2 = boost::none) const override { ... } + * ``` + * + * If any of the optional Matrix reference arguments are specified, it should + * compute both the function evaluation and its derivative(s) in the requested + * variables. + * + * @param x The values of the variables to evaluate the error for. Passed in + * as separate arguments. + * @param[out] H The Jacobian with respect to each variable (optional). */ - virtual Vector - evaluateError(const X &x, - boost::optional H = boost::none) const = 0; + virtual Vector evaluateError(const ValueTypes&... x, + OptionalMatrix... H) const = 0; /// @} + /// @name Convenience method overloads + /// @{ -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); - } -};// \class NoiseModelFactor1 - - -/* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 2 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor2: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor2 This; - -public: - - /** - * Default Constructor for I/O + /** No-Jacobians requested function overload. + * This specializes the version below to avoid recursive calls since this is + * commonly used. + * + * e.g. `const Vector error = factor.evaluateError(pose, point);` */ - NoiseModelFactor2() {} + inline Vector evaluateError(const ValueTypes&... x) const { + return evaluateError(x..., OptionalMatrix()...); + } - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - */ - NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel, cref_list_of<2>(j1)(j2)) {} + /** Some (but not all) optional Jacobians are omitted (function overload) + * + * e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);` + */ + template > + inline Vector evaluateError(const ValueTypes&... x, + OptionalJacArgs&&... H) const { + return evaluateError(x..., std::forward(H)..., + boost::none); + } - ~NoiseModelFactor2() override {} + /// @} - /** methods to retrieve both keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - - /** Calls the 2-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - const X1& x1 = x.at(keys_[0]); - const X2& x2 = x.at(keys_[1]); - if(H) { - return evaluateError(x1, x2, (*H)[0], (*H)[1]); + private: + /** Pack expansion with index_sequence template pattern, used to index into + * `keys_` and `H`. + * + * Example: For `NoiseModelFactorN`, the call would look like: + * `const Vector error = unwhitenedError(0, 1, values, H);` + */ + template + inline Vector unwhitenedError( + boost::mp11::index_sequence, // + const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) { + return evaluateError(x.at(keys_[Indices])..., + (*H)[Indices]...); } else { - return evaluateError(x1, x2); + return evaluateError(x.at(keys_[Indices])...); } } else { return Vector::Zero(this->dim()); } } - /** - * Override this method to finish implementing a binary factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2). - */ - virtual Vector - evaluateError(const X1&, const X2&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const = 0; - -private: - /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor2 +}; // \class NoiseModelFactorN /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 3 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor3: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor3 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor3() {} +/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 + * with ValueType<1>. + * 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, for backwards compatibility. + * Note: in your code you can probably just do: + * `using X = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X = typename NoiseModelFactor1::template ValueType<1>; + + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using This = NoiseModelFactor1; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor1() override {} - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable + /** Method to retrieve key. + * Similar to `ValueType`, you can probably do `return key<1>();` */ - NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} - - ~NoiseModelFactor3() override {} + inline Key key() const { return NoiseModelFactorN::template key<1>(); } - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - - /** Calls the 3-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); - } else { - return Vector::Zero(this->dim()); - } + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } +}; // \class NoiseModelFactor1 - /** - * Override this method to finish implementing a trinary factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 2 variables. To derive from this class, implement evaluateError(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor2 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor2::template ValueType<1>; + using X2 = typename NoiseModelFactor2::template ValueType<2>; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor2; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor2() override {} -private: + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` + */ + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor3 +}; // \class NoiseModelFactor2 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 4 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor4: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor4 This; - -public: +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 3 variables. To derive from this class, implement evaluateError(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor3 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor3::template ValueType<1>; + using X2 = typename NoiseModelFactor3::template ValueType<2>; + using X3 = typename NoiseModelFactor3::template ValueType<3>; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor3; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor3() override {} - /** - * Default Constructor for I/O + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` */ - NoiseModelFactor4() {} + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - */ - NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {} + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor3 +/* ************************************************************************* */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 4 variables. To derive from this class, implement evaluateError(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor4 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor4::template ValueType<1>; + using X2 = typename NoiseModelFactor4::template ValueType<2>; + using X3 = typename NoiseModelFactor4::template ValueType<3>; + using X4 = typename NoiseModelFactor4::template ValueType<4>; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor4; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor4() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - - /** Calls the 4-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 4-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const = 0; - -private: + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } + inline Key key4() const { return this->template key<4>(); } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor4 +}; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 5 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor5: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - typedef VALUE5 X5; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor5 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor5() {} - - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - * @param j5 key of the fifth variable - */ - NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} - +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 5 variables. To derive from this class, implement evaluateError(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor5 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor5::template ValueType<1>; + using X2 = typename NoiseModelFactor5::template ValueType<2>; + using X3 = typename NoiseModelFactor5::template ValueType<3>; + using X4 = typename NoiseModelFactor5::template ValueType<4>; + using X5 = typename NoiseModelFactor5::template ValueType<5>; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor5; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor5() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - - /** Calls the 5-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 5-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const = 0; - -private: + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } + inline Key key4() const { return this->template key<4>(); } + inline Key key5() const { return this->template key<5>(); } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor5 +}; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 6 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor6: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - typedef VALUE5 X5; - typedef VALUE6 X6; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor6 This; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor6() {} - - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - * @param j5 key of the fifth variable - * @param j6 key of the fifth variable - */ - NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} - +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with ValueType<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 6 variables. To derive from this class, implement evaluateError(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor6 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor6::template ValueType<1>; + using X2 = typename NoiseModelFactor6::template ValueType<2>; + using X3 = typename NoiseModelFactor6::template ValueType<3>; + using X4 = typename NoiseModelFactor6::template ValueType<4>; + using X5 = typename NoiseModelFactor6::template ValueType<5>; + using X6 = typename NoiseModelFactor6::template ValueType<6>; + + protected: + using Base = NoiseModelFactor; + using This = + NoiseModelFactor6; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor6() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - inline Key key6() const { return keys_[5]; } - - /** Calls the 6-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 6-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). + /** Methods to retrieve keys. + * Similar to `ValueType`, you can probably do `return key<#>();` */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const = 0; - -private: + inline Key key1() const { return this->template key<1>(); } + inline Key key2() const { return this->template key<2>(); } + inline Key key3() const { return this->template key<3>(); } + inline Key key4() const { return this->template key<4>(); } + inline Key key5() const { return this->template key<5>(); } + inline Key key6() const { return this->template key<6>(); } + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor6 - -/* ************************************************************************* */ +}; // \class NoiseModelFactor6 } // \namespace gtsam diff --git a/gtsam/nonlinear/tests/priorFactor.xml b/gtsam/nonlinear/tests/priorFactor.xml new file mode 100644 index 0000000000..0c580fb211 --- /dev/null +++ b/gtsam/nonlinear/tests/priorFactor.xml @@ -0,0 +1,77 @@ + + + + + + + + + 1 + 0 + 12345 + + + + + + + + + 6 + + + 0 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + + + + + 4.11982245665682978e-01 + -8.33737651774156818e-01 + -3.67630462924899259e-01 + -5.87266449276209815e-02 + -4.26917621276207360e-01 + 9.02381585483330806e-01 + -9.09297426825681709e-01 + -3.50175488374014632e-01 + -2.24845095366152908e-01 + + + + 4.00000000000000000e+00 + 5.00000000000000000e+00 + 6.00000000000000000e+00 + + + + + diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 4a73cbb0b4..f402656c12 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -107,8 +107,60 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } -TEST(Serialization, ISAM2) { +/** + * Test deserializing from a known serialization generated by code from commit + * 0af17f438f62f4788f3a572ecd36d06d224fd1e1 (>4.2a7) + * We only test that deserialization matches since + * (1) that's the main backward compatibility requirement and + * (2) serialized string depends on boost version + * Also note: we don't run this test when quaternions or TBB are enabled since + * serialization structures are different and the serialized strings/xml are + * hard-coded in this test. + */ +TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { + PriorFactor factor( + 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), + noiseModel::Unit::Create(6)); + + // roundtrip (sanity check) + PriorFactor factor_deserialized_str_2 = PriorFactor(); + roundtrip(factor, factor_deserialized_str_2); + EXPECT(assert_equal(factor, factor_deserialized_str_2)); + +#if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB) + // Deserialize string + std::string serialized_str = + "22 serialization::archive 15 1 0\n" + "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" + "1 1 0\n" + "2 1 0\n" + "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 6 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 " + "-8.33737651774156818e-01 -3.67630462924899259e-01 " + "-5.87266449276209815e-02 -4.26917621276207360e-01 " + "9.02381585483330806e-01 -9.09297426825681709e-01 " + "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " + "4.00000000000000000e+00 5.00000000000000000e+00 " + "6.00000000000000000e+00\n"; + + PriorFactor factor_deserialized_str = PriorFactor(); + deserializeFromString(serialized_str, factor_deserialized_str); + EXPECT(assert_equal(factor, factor_deserialized_str)); + + // Deserialize XML + PriorFactor factor_deserialized_xml = PriorFactor(); + deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR + "/../../gtsam/nonlinear/tests/priorFactor.xml", + factor_deserialized_xml); + EXPECT(assert_equal(factor, factor_deserialized_xml)); +#endif +} +TEST(Serialization, ISAM2) { gtsam::ISAM2Params parameters; gtsam::ISAM2 solver(parameters); gtsam::NonlinearFactorGraph graph; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 67a23355d6..99ec2f501e 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -329,11 +329,68 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +class TestFactor1 : public NoiseModelFactor1 { + static_assert(std::is_same::value, "Base type wrong"); + static_assert(std::is_same>::value, + "This type wrong"); + + public: + typedef NoiseModelFactor1 Base; + TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} + using Base::NoiseModelFactor1; // inherit constructors + + Vector evaluateError(const double& x1, boost::optional H1 = + boost::none) const override { + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + return (Vector(1) << x1).finished(); + } + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); + } +}; + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactor1) { + TestFactor1 tf; + Values tv; + tv.insert(L(1), double((1.0))); + EXPECT(assert_equal((Vector(1) << 1.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.25 / 2.0, tf.error(tv), 1e-9); + JacobianFactor jf( + *boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)L(1), (long)jf.keys()[0]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), + jf.getA(jf.begin()))); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5).finished(), jf.getb())); + + // Test all functions/types for backwards compatibility + static_assert(std::is_same::value, + "X type incorrect"); + EXPECT(assert_equal(tf.key(), L(1))); + std::vector 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)}); + TestFactor1 tf4(noiseModel::Unit::Create(1), gtsam::Symbol('L', 1)); +} + /* ************************************************************************* */ class TestFactor4 : public NoiseModelFactor4 { -public: + static_assert(std::is_same::value, "Base type wrong"); + static_assert( + std::is_same>::value, + "This type wrong"); + + public: typedef NoiseModelFactor4 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, @@ -347,7 +404,7 @@ class TestFactor4 : public NoiseModelFactor4 { *H3 = (Matrix(1, 1) << 3.0).finished(); *H4 = (Matrix(1, 1) << 4.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished(); } gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -363,8 +420,8 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(2), double((2.0))); tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); - EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -374,7 +431,49 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb())); + + // Test all functions/types for backwards compatibility + static_assert(std::is_same::value, + "X1 type incorrect"); + static_assert(std::is_same::value, + "X2 type incorrect"); + static_assert(std::is_same::value, + "X3 type incorrect"); + static_assert(std::is_same::value, + "X4 type incorrect"); + EXPECT(assert_equal(tf.key1(), X(1))); + EXPECT(assert_equal(tf.key2(), X(2))); + EXPECT(assert_equal(tf.key3(), X(3))); + EXPECT(assert_equal(tf.key4(), X(4))); + std::vector H = {Matrix(), Matrix(), Matrix(), Matrix()}; + EXPECT(assert_equal(Vector1(30.0), tf.unwhitenedError(tv, H))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.).finished(), H.at(0))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3))); + + // And test "forward compatibility" using `key` and `ValueType` too + 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"); + static_assert(std::is_same, double>::value, + "ValueType<4> type incorrect"); + EXPECT(assert_equal(tf.key<1>(), X(1))); + EXPECT(assert_equal(tf.key<2>(), X(2))); + EXPECT(assert_equal(tf.key<3>(), X(3))); + EXPECT(assert_equal(tf.key<4>(), 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{L(1), L(2), L(3), L(4)}); + std::vector keys = {L(1), L(2), L(3), L(4)}; + TestFactor4 tf5(noiseModel::Unit::Create(1), keys); } /* ************************************************************************* */ @@ -397,7 +496,8 @@ class TestFactor5 : public NoiseModelFactor5(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -423,7 +523,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); - EXPECT(assert_equal((Vector)(Vector(1) << -7.5).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -55.).finished(), jf.getb())); } /* ************************************************************************* */ @@ -448,7 +548,9 @@ class TestFactor6 : public NoiseModelFactor6(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -478,8 +580,98 @@ TEST(NonlinearFactor, NoiseModelFactor6) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0).finished(), jf.getA(jf.begin()+5))); - EXPECT(assert_equal((Vector)(Vector(1) << -10.5).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -91.).finished(), jf.getb())); + +} +/* ************************************************************************* */ +class TestFactorN : public NoiseModelFactorN { +public: + typedef NoiseModelFactorN Base; + using Type1 = ValueType<1>; // Test that we can use the ValueType<> template + + TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + + Vector + evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const override { + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); + if (H3) *H3 = (Matrix(1, 1) << 3.0).finished(); + if (H4) *H4 = (Matrix(1, 1) << 4.0).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished(); + } + + Key key1() const { return key<1>(); } // Test that we can use key<> template +}; + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactorN) { + TestFactorN tf; + Values tv; + tv.insert(X(1), double((1.0))); + tv.insert(X(2), double((2.0))); + tv.insert(X(3), double((3.0))); + tv.insert(X(4), double((4.0))); + EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); + LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); + LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); + LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5 * 30.).finished(), jf.getb())); + + // Test all evaluateError argument overloads to ensure backward compatibility + Matrix H1_expected, H2_expected, H3_expected, H4_expected; + Vector e_expected = tf.evaluateError(9, 8, 7, 6, H1_expected, H2_expected, + H3_expected, H4_expected); + + std::unique_ptr> base_ptr( + new TestFactorN(tf)); + Matrix H1, H2, H3, H4; + EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6))); + EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6, H1))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(e_expected, // + base_ptr->evaluateError(9, 8, 7, 6, H1, H2))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(e_expected, + base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(H3_expected, H3)); + EXPECT(assert_equal(e_expected, + base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3, H4))); + EXPECT(assert_equal(H1_expected, H1)); + 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<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"); + static_assert(std::is_same, double>::value, + "ValueType<4> type incorrect"); + static_assert(std::is_same::value, + "TestFactorN::Type1 type incorrect"); + EXPECT(assert_equal(tf.key<1>(), X(1))); + EXPECT(assert_equal(tf.key<2>(), X(2))); + EXPECT(assert_equal(tf.key<3>(), X(3))); + EXPECT(assert_equal(tf.key<4>(), X(4))); + EXPECT(assert_equal(tf.key1(), X(1))); } /* ************************************************************************* */