From 04e41f8c38712d9c71afea840ad8109106a353ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 3 Sep 2011 03:46:19 +0000 Subject: [PATCH] Split off NoiseModelFactor (a NonlinearFactor with a NoiseModel) from NonlinearFactor (a Factor with dim, linearize, and symbolic methods). NonlinearConstraint derives from NoiseModelFactor. --- gtsam/nonlinear/NonlinearConstraint.h | 4 +- gtsam/nonlinear/NonlinearFactor.h | 201 ++++++++++++++++++-------- tests/testNonlinearFactor.cpp | 7 +- 3 files changed, 144 insertions(+), 68 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index d7b8586a6..33e19f21d 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -36,11 +36,11 @@ namespace gtsam { * significant */ template -class NonlinearConstraint : public NonlinearFactor { +class NonlinearConstraint : public NoiseModelFactor { protected: typedef NonlinearConstraint This; - typedef NonlinearFactor Base; + typedef NoiseModelFactor Base; /** default constructor to allow for serialization */ NonlinearConstraint() {} diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index bb7508093..87a586d37 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearFactor.h + * @file NoiseModelFactor.h * @brief Non-linear factor class * @author Frank Dellaert * @author Richard Roberts @@ -35,8 +35,7 @@ namespace gtsam { /** - * Nonlinear factor which assumes zero-mean Gaussian noise on the - * on a measurement predicted by a non-linear function h. + * Nonlinear factor base class * * Templated on a values structure type. The values structures are typically * more general than just vectors, e.g., Rot3 or Pose3, @@ -47,10 +46,10 @@ namespace gtsam { protected: + // Some handy typedefs + typedef Factor Base; typedef NonlinearFactor This; - SharedNoiseModel noiseModel_; /** Noise model */ - public: typedef boost::shared_ptr > shared_ptr; @@ -59,23 +58,15 @@ namespace gtsam { NonlinearFactor() { } -// virtual ~NonlinearFactor() {} + /** Destructor */ + virtual ~NonlinearFactor() {} /** * Constructor - * @param noiseModel shared pointer to a noise model + * @param key1 by which to look up X value in Values */ - NonlinearFactor(const SharedNoiseModel& noiseModel) : - noiseModel_(noiseModel) { - } - - /** - * Constructor - * @param z measurement - * @param key by which to look up X value in Values - */ - NonlinearFactor(const SharedNoiseModel& noiseModel, const Symbol& key1) : - Factor(key1), noiseModel_(noiseModel) { + NonlinearFactor(const Symbol& key1) : + Factor(key1) { } /** @@ -83,54 +74,38 @@ namespace gtsam { * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) : - Factor(j1,j2), noiseModel_(noiseModel) { + NonlinearFactor(const Symbol& j1, const Symbol& j2) : + Factor(j1,j2) { } /** * Constructor - arbitrary number of keys * @param keys is the set of Symbols in the factor */ - NonlinearFactor(const SharedNoiseModel& noiseModel, const std::set& keys) : - Factor(keys), noiseModel_(noiseModel) { + NonlinearFactor(const std::set& keys) : + Factor(keys) { } /** print */ virtual void print(const std::string& s = "") const { std::cout << s << ": NonlinearFactor\n"; - noiseModel_->print(" noise model"); - } - - /** Check if two NonlinearFactor objects are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - return noiseModel_->equals(*f.noiseModel_, tol); } /** - * calculate the error of the factor - * Override for systems with unusual noise models + * Vector of errors, unwhitened + * This is the raw error i.e. (h(x)-z) in case of NoiseModelFactor derived class */ - virtual double error(const VALUES& c) const { - return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); - } - - /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const { - return noiseModel_->dim(); - } - - /** access to the noise model */ - SharedNoiseModel get_noiseModel() const { - return noiseModel_; - } - - /** Vector of errors, unwhitened ! */ virtual Vector unwhitenedError(const VALUES& c) const = 0; - /** Vector of errors, whitened ! */ - Vector whitenedError(const VALUES& c) const { - return noiseModel_->whiten(unwhitenedError(c)); - } + /** + * Calculate the error of the factor + * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. + * You can override this for systems with unusual noise models. + */ + virtual double error(const VALUES& c) const = 0; + + /** get the dimension of the factor (number of rows on linearization) */ + virtual size_t dim() const = 0; /** linearize to a GaussianFactor */ virtual boost::shared_ptr @@ -142,6 +117,109 @@ namespace gtsam { */ virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0; + }; // \class NonlinearFactor + + + /** + * Nonlinear factor which assumes a zero-mean noise model + * on a measurement predicted by a non-linear function h. + * The noise model is typically Gaussian, but robust error models are also supported. + */ + template + class NoiseModelFactor: public NonlinearFactor, public Testable > { + + protected: + + // handy typedefs + typedef NonlinearFactor Base; + typedef NoiseModelFactor This; + + SharedNoiseModel noiseModel_; /** Noise model */ + + public: + + typedef boost::shared_ptr > shared_ptr; + + /** Default constructor for I/O only */ + NoiseModelFactor() { + } + + /** Destructor */ + virtual ~NoiseModelFactor() {} + + /** + * Constructor + * @param noiseModel shared pointer to a noise model + */ + NoiseModelFactor(const SharedNoiseModel& noiseModel) : + noiseModel_(noiseModel) { + } + + /** + * Constructor + * @param z measurement + * @param key by which to look up X value in Values + */ + NoiseModelFactor(const SharedNoiseModel& noiseModel, const Symbol& key1) : + Base(key1), noiseModel_(noiseModel) { + } + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + */ + NoiseModelFactor(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) : + Base(j1,j2), noiseModel_(noiseModel) { + } + + /** + * Constructor - arbitrary number of keys + * @param keys is the set of Symbols in the factor + */ + NoiseModelFactor(const SharedNoiseModel& noiseModel, const std::set& keys) : + Base(keys), noiseModel_(noiseModel) { + } + + /** print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor\n"; + noiseModel_->print(" noise model"); + } + + /** Check if two NoiseModelFactor objects are equal */ + virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { + return noiseModel_->equals(*f.noiseModel_, tol); + } + + /** get the dimension of the factor (number of rows on linearization) */ + virtual size_t dim() const { + return noiseModel_->dim(); + } + + /** access to the noise model */ + SharedNoiseModel get_noiseModel() const { + return noiseModel_; + } + + /** + * Vector of errors, whitened + * This is the raw error, i.e., i.e. (h(x)-z)/sigma in case of a Gaussian + */ + Vector whitenedError(const VALUES& c) const { + return noiseModel_->whiten(unwhitenedError(c)); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. + * In this class, we take the raw prediction error h(x)-z, ask the noise model + * to transform it to (h(x)-z)^2/sigma^2, and then multiply by 0.5. + */ + virtual double error(const VALUES& c) const { + return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); + } + private: /** Serialization function */ @@ -149,11 +227,11 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("Factor", - boost::serialization::base_object >(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(noiseModel_); } - }; // \class NonlinearFactor + }; // \class NoiseModelFactor /** @@ -165,7 +243,7 @@ namespace gtsam { * This allows a graph to have factors with measurements of mixed type. */ template - class NonlinearFactor1: public NonlinearFactor { + class NonlinearFactor1: public NoiseModelFactor { public: @@ -177,7 +255,7 @@ namespace gtsam { // The value of the key. Not const to allow serialization KEY key_; - typedef NonlinearFactor Base; + typedef NoiseModelFactor Base; typedef NonlinearFactor1 This; public: @@ -215,8 +293,7 @@ namespace gtsam { /** error function h(x)-z, unwhitened !!! */ inline Vector unwhitenedError(const VALUES& x) const { - const KEY& j = key_; - const X& xj = x[j]; + const X& xj = x[key_]; return evaluateError(xj); } @@ -262,7 +339,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor", + ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key_); } @@ -273,7 +350,7 @@ namespace gtsam { * A Gaussian nonlinear factor that takes 2 parameters */ template - class NonlinearFactor2: public NonlinearFactor { + class NonlinearFactor2: public NoiseModelFactor { public: @@ -287,7 +364,7 @@ namespace gtsam { KEY1 key1_; KEY2 key2_; - typedef NonlinearFactor Base; + typedef NoiseModelFactor Base; typedef NonlinearFactor2 This; public: @@ -385,7 +462,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor", + ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key2_); @@ -399,7 +476,7 @@ namespace gtsam { * A Gaussian nonlinear factor that takes 3 parameters */ template - class NonlinearFactor3: public NonlinearFactor { + class NonlinearFactor3: public NoiseModelFactor { public: @@ -415,7 +492,7 @@ namespace gtsam { KEY2 key2_; KEY3 key3_; - typedef NonlinearFactor Base; + typedef NoiseModelFactor Base; typedef NonlinearFactor3 This; public: @@ -526,7 +603,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor", + ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key2_); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 183f8a4e7..17c1e2d6f 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -86,10 +86,9 @@ TEST( NonlinearFactor, NonlinearFactor ) Graph::sharedFactor factor = fg[0]; // calculate the error_vector from the factor "f1" - // the expected value for the whitened error from the factor - // error_vector / sigma = [0.1 0.1]/0.1 = [1;1] - Vector actual_e = factor->whitenedError(cfg); - CHECK(assert_equal(ones(2),actual_e)); + // error_vector = [0.1 0.1] + Vector actual_e = factor->unwhitenedError(cfg); + CHECK(assert_equal(0.1*ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 double expected = 1.0;