diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 542c4d5f1..e024a41a0 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -309,13 +309,137 @@ public: /* ************************************************************************* */ /** - * 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(). * * 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 NoiseModelFactorN: public NoiseModelFactor { + public: + /** The type of the N'th template param can be obtained with VALUE */ + template + using VALUE = typename std::tuple_element>::type; + + protected: + typedef NoiseModelFactor Base; + typedef NoiseModelFactorN This; + + /* "Dummy templated" alias is used to expand fixed-type parameter packs with + * same length as VALUES. This ignores the template parameter. */ + template + using optional_matrix_type = boost::optional; + + /* "Dummy templated" alias is used to expand fixed-type parameter packs with + * same length as VALUES. This ignores the template parameter. */ + template + using key_type = Key; + + public: + /** + * Default Constructor for I/O + */ + NoiseModelFactorN() {} + + /** + * Constructor. + * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) + * @param noiseModel shared pointer to noise model + * @param keys... keys for the variables in this factor + */ + NoiseModelFactorN(const SharedNoiseModel& noiseModel, + key_type... keys) + : Base(noiseModel, std::array{keys...}) {} + + /** + * Constructor. + * @param noiseModel shared pointer to noise model + * @param keys a container of keys for the variables in this factor + */ + template + NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) + : Base(noiseModel, keys) { + assert(keys.size() == sizeof...(VALUES)); + } + + ~NoiseModelFactorN() override {} + + // /** Method to retrieve keys */ + // template + // inline Key key() const { return keys_[N]; } + + /** Calls the n-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 { + return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); + } + + /** + * Override this method to finish implementing an n-way factor. + * 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. + */ + virtual Vector evaluateError( + const VALUES& ... x, + optional_matrix_type ... H) const = 0; + + /** No-jacobians requested function overload (since parameter packs can't have + * default args). This specializes the version below to avoid recursive calls + * since this is commonly used. */ + inline Vector evaluateError(const VALUES&... x) const { + return evaluateError(x..., optional_matrix_type()...); + } + + /** Some optional jacobians omitted function overload */ + template 0) && + (sizeof...(OptionalJacArgs) < + sizeof...(VALUES)), + bool>::type = true> + inline Vector evaluateError(const VALUES&... x, + OptionalJacArgs&&... H) const { + return evaluateError(x..., std::forward(H)..., + boost::none); + } + + private: + + /** Pack expansion with index_sequence template pattern */ + template + 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_[Inds])..., (*H)[Inds]...); + } else { + return evaluateError(x.at(keys_[Inds])...); + } + } else { + return Vector::Zero(this->dim()); + } + } + + /** 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 NoiseModelFactorN + + + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 1 + * variable. To derive from this class, implement evaluateError(). */ template class NoiseModelFactor1: public NoiseModelFactor { @@ -552,83 +676,40 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor4: public NoiseModelFactor { +template +class NoiseModelFactor4 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; -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: - - /** - * Default Constructor for I/O - */ - NoiseModelFactor4() {} - - /** - * 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)) {} + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + 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). - */ - 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->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + inline Key key3() const { return this->keys_[2]; } + inline Key key4() const { return this->keys_[3]; } + 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 @@ -804,124 +885,4 @@ private: } }; // \class NoiseModelFactor6 -/* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with N - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactorN: public NoiseModelFactor { - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactorN This; - - /* "Dummy templated" alias is used to expand fixed-type parameter packs with - * same length as VALUES. This ignores the template parameter. */ - template - using optional_matrix_type = boost::optional; - - /* "Dummy templated" alias is used to expand fixed-type parameter packs with - * same length as VALUES. This ignores the template parameter. */ - template - using key_type = Key; - -public: - - /** - * Default Constructor for I/O - */ - NoiseModelFactorN() {} - - /** - * Constructor. - * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) - * @param noiseModel shared pointer to noise model - * @param keys... keys for the variables in this factor - */ - NoiseModelFactorN(const SharedNoiseModel& noiseModel, - key_type... keys) - : Base(noiseModel, std::array{keys...}) {} - - /** - * Constructor. - * @param noiseModel shared pointer to noise model - * @param keys a container of keys for the variables in this factor - */ - template - NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) - : Base(noiseModel, keys) { - assert(keys.size() == sizeof...(VALUES)); - } - - ~NoiseModelFactorN() override {} - - /** Method to retrieve keys */ - template - inline Key key() const { return keys_[N]; } - - /** Calls the n-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 { - return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); - } - - /** - * Override this method to finish implementing an n-way factor. - * 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. - */ - virtual Vector evaluateError( - const VALUES& ... x, - optional_matrix_type ... H) const = 0; - - /** No-jacobians requested function overload (since parameter packs can't have - * default args). This specializes the version below to avoid recursive calls - * since this is commonly used. */ - inline Vector evaluateError(const VALUES&... x) const { - return evaluateError(x..., optional_matrix_type()...); - } - - /** Some optional jacobians omitted function overload */ - template 0) && - (sizeof...(OptionalJacArgs) < - sizeof...(VALUES)), - bool>::type = true> - inline Vector evaluateError(const VALUES&... x, - OptionalJacArgs&&... H) const { - return evaluateError(x..., std::forward(H)..., - boost::none); - } - - private: - - /** Pack expansion with index_sequence template pattern */ - template - 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_[Inds])..., (*H)[Inds]...); - } else { - return evaluateError(x.at(keys_[Inds])...); - } - } else { - return Vector::Zero(this->dim()); - } - } - - /** 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 NoiseModelFactorN - } // \namespace gtsam