diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index ee7bd9924..e07f45f07 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -16,7 +16,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 and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError(). If the number of variables is greater than 6, derive from NoiseModelFactorN. - 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 7fafd95df..503ae7d58 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { @@ -770,5 +771,110 @@ 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(std::size(keys) == 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) */ + Vector evaluateError(const VALUES&... x) const { + return evaluateError(x..., optional_matrix_type()...); + } + + 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 diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 84bba850b..eb35bf55d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -406,6 +406,50 @@ TEST(NonlinearFactor, NoiseModelFactor6) { } +/* ************************************************************************* */ +class TestFactorN : public NoiseModelFactorN { +public: + typedef NoiseModelFactorN Base; + 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(); + *H2 = (Matrix(1, 1) << 2.0).finished(); + *H3 = (Matrix(1, 1) << 3.0).finished(); + *H4 = (Matrix(1, 1) << 4.0).finished(); + } + return (Vector(1) << x1 + x2 + x3 + x4).finished(); + } +}; + +/* ************************************ */ +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) << 10.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(25.0/2.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) << -5.0).finished(), jf.getb())); +} + /* ************************************************************************* */ TEST( NonlinearFactor, clone_rekey ) {