diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 64ef96b85..34e891c64 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -307,19 +307,39 @@ public: /* ************************************************************************* */ - /** - * A convenient base class for creating your own NoiseModelFactor with n - * variables. 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). + * For example, a 2-way factor could be implemented like so: + * + * ~~~~~~~~~~~~~~~~~~~~{.cpp} + * class MyFactor : public NoiseModelFactorN { + * public: + * using Base = NoiseModelFactorN; + * + * MyFactor(Key key1, Key key2, const SharedNoiseModel& noiseModel) + * : Base(noiseModel, key1, key2) {} + * + * Vector evaluateError( + * const double& x1, const double& x2, + * boost::optional H1 = boost::none, + * boost::optional H2 = boost::none) const override { + * if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + * if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); + * return (Vector(1) << x1 + 2 * x2).finished(); + * } + * }; + * ~~~~~~~~~~~~~~~~~~~~ + * + * 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 NoiseModelFactorN : public NoiseModelFactor { public: - /** The type of the N'th template param can be obtained with VALUE */ + /** The type of the N'th template param can be obtained as VALUE */ template using VALUE = typename std::tuple_element>::type; @@ -338,6 +358,9 @@ class NoiseModelFactorN : public NoiseModelFactor { using key_type = Key; public: + /// @name Constructors + /// @{ + /** * Default Constructor for I/O */ @@ -346,8 +369,9 @@ class NoiseModelFactorN : public NoiseModelFactor { /** * Constructor. * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) - * @param noiseModel shared pointer to noise model - * @param keys... keys for the variables in this factor + * @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, key_type... keys) @@ -355,8 +379,8 @@ class NoiseModelFactorN : public NoiseModelFactor { /** * Constructor. - * @param noiseModel shared pointer to noise model - * @param keys a container of keys for the variables in this factor + * @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) @@ -364,6 +388,8 @@ class NoiseModelFactorN : public NoiseModelFactor { assert(keys.size() == sizeof...(VALUES)); } + /// @} + ~NoiseModelFactorN() override {} /** Method to retrieve keys */ @@ -372,26 +398,59 @@ class NoiseModelFactorN : public NoiseModelFactor { return keys_[N]; } + /// @name NoiseModelFactor methods + /// @{ + /** Calls the n-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ + * so must be implemented in the derived class. + * @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 { return unwhitenedError(boost::mp11::index_sequence_for{}, x, H); } + /// @} + /// @name Virtual methods + /// @{ /** * Override this method 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 double: + * ``` + * Vector evaluateError(const Pose3& x1, const double& 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 VALUES&... x, optional_matrix_type... H) const = 0; + /// @} + /// @name Convenience method overloads + /// @{ + /** 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. */ + * since this is commonly used. + * + * e.g. `Vector error = factor.evaluateError(x1, x2, x3);` + */ inline Vector evaluateError(const VALUES&... x) const { return evaluateError(x..., optional_matrix_type()...); } @@ -408,10 +467,14 @@ class NoiseModelFactorN : public NoiseModelFactor { boost::none); } + /// @} + private: - /** Pack expansion with index_sequence template pattern */ + /** Pack expansion with index_sequence template pattern, used to index into + * `keys_` and `H` + */ template - Vector unwhitenedError( + inline Vector unwhitenedError( boost::mp11::index_sequence, // const Values& x, boost::optional&> H = boost::none) const { @@ -436,8 +499,11 @@ class NoiseModelFactorN : public NoiseModelFactor { }; // \class NoiseModelFactorN /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 1 - * variable. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 1 variable. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor1 : public NoiseModelFactorN { public: @@ -453,6 +519,7 @@ class NoiseModelFactor1 : public NoiseModelFactorN { using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} + /** method to retrieve key */ inline Key key() const { return this->keys_[0]; } private: @@ -466,8 +533,11 @@ class NoiseModelFactor1 : public NoiseModelFactorN { }; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 2 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 2 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor2 : public NoiseModelFactorN { public: @@ -499,8 +569,11 @@ class NoiseModelFactor2 : public NoiseModelFactorN { }; // \class NoiseModelFactor2 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 3 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 3 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor3 : public NoiseModelFactorN { public: @@ -534,8 +607,11 @@ class NoiseModelFactor3 : public NoiseModelFactorN { }; // \class NoiseModelFactor3 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 4 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 4 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor4 : public NoiseModelFactorN { @@ -572,8 +648,11 @@ class NoiseModelFactor4 }; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 5 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 5 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor5 : public NoiseModelFactorN { @@ -613,8 +692,11 @@ class NoiseModelFactor5 }; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 6 - * variables. To derive from this class, implement evaluateError(). */ +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 + * with VALUE<1>. + * A convenient base class for creating your own NoiseModelFactor + * with 6 variables. To derive from this class, implement evaluateError(). + */ template class NoiseModelFactor6