From ed07edbfe4ab40fa1d583fe47bfd762818a97ae1 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 3 Dec 2021 03:00:47 -0500 Subject: [PATCH] converted all NoiseModelFactorX to inherit from NoiseModelFactorN --- gtsam/nonlinear/NonlinearFactor.h | 502 ++++++++---------------------- 1 file changed, 137 insertions(+), 365 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index e024a41a0..64ef96b85 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -316,16 +316,16 @@ public: * more general than just vectors, e.g., Rot3 or Pose3, * which are objects in non-linear manifolds (Lie groups). */ -template -class NoiseModelFactorN: public NoiseModelFactor { +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; + using Base = NoiseModelFactor; + using This = NoiseModelFactorN; /* "Dummy templated" alias is used to expand fixed-type parameter packs with * same length as VALUES. This ignores the template parameter. */ @@ -366,9 +366,11 @@ class NoiseModelFactorN: public NoiseModelFactor { ~NoiseModelFactorN() override {} - // /** Method to retrieve keys */ - // template - // inline Key key() const { return keys_[N]; } + /** 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. */ @@ -384,9 +386,8 @@ class NoiseModelFactorN: public NoiseModelFactor { * 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; + 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 @@ -408,7 +409,6 @@ class NoiseModelFactorN: public NoiseModelFactor { } private: - /** Pack expansion with index_sequence template pattern */ template Vector unwhitenedError( @@ -428,250 +428,110 @@ class NoiseModelFactorN: public NoiseModelFactor { /** 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 NoiseModelFactorN - - +}; // \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 { +template +class NoiseModelFactor1 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X = VALUE; -public: - - // typedefs for value types pulled from keys - typedef VALUE X; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor1 This; - -public: - /// @name Constructors - /// @{ - - /** Default constructor for I/O only */ - NoiseModelFactor1() {} + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using This = NoiseModelFactor1; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} - inline Key key() const { return keys_[0]; } + inline Key key() const { return this->keys_[0]; } - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param key1 by which to look up X value in Values - */ - NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1) - : Base(noiseModel, cref_list_of<1>(key1)) {} - - /// @} - /// @name NoiseModelFactor methods - /// @{ - - /** - * Calls the 1-key specific version of evaluateError below, 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 X &x1 = x.at(keys_[0]); - if (H) { - return evaluateError(x1, (*H)[0]); - } else { - return evaluateError(x1); - } - } else { - return Vector::Zero(this->dim()); - } - } - - /// @} - /// @name Virtual methods - /// @{ - - /** - * 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. - */ - virtual Vector - evaluateError(const X &x, - boost::optional H = boost::none) const = 0; - - /// @} - -private: + 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 NoiseModelFactor1 - +}; // \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 { +template +class NoiseModelFactor2 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; -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 - */ - NoiseModelFactor2() {} - - /** - * 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)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor2; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~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]); - } else { - return evaluateError(x1, x2); - } - } 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: + inline Key key1() const { return this->keys_[0]; } + inline Key key2() const { return this->keys_[1]; } + 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 NoiseModelFactor2 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor3: public NoiseModelFactor { +template +class NoiseModelFactor3 : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; -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() {} - - /** - * 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 - */ - NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor3; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor3() 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]; } - - /** 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()); - } - } - - /** - * 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; - -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]; } + 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 NoiseModelFactor3 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 @@ -687,7 +547,7 @@ class NoiseModelFactor4 using X4 = VALUE4; protected: - using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using Base = NoiseModelFactor; using This = NoiseModelFactor4; public: @@ -714,175 +574,87 @@ 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 { +template +class NoiseModelFactor5 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + using X5 = VALUE5; -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)) {} + 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). - */ - 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->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]; } + inline Key key5() const { return this->keys_[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 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 { +template +class NoiseModelFactor6 + : public NoiseModelFactorN { + public: + // aliases for value types pulled from keys + using X1 = VALUE1; + using X2 = VALUE2; + using X3 = VALUE3; + using X4 = VALUE4; + using X5 = VALUE5; + using X6 = VALUE6; -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)) {} + 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). - */ - 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->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]; } + inline Key key5() const { return this->keys_[4]; } + inline Key key6() const { return this->keys_[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 NoiseModelFactor6 +}; // \class NoiseModelFactor6 } // \namespace gtsam