From c01c756d697749c0e96bda552bdaa5ffe433f44d Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 1 Oct 2014 13:14:55 +0200 Subject: [PATCH] Created implementation file for NonlinearFactor and moved non-templated code there --- gtsam/nonlinear/NonlinearFactor.cpp | 141 ++++++++++++++++++++++++++++ gtsam/nonlinear/NonlinearFactor.h | 132 ++++---------------------- 2 files changed, 160 insertions(+), 113 deletions(-) create mode 100644 gtsam/nonlinear/NonlinearFactor.cpp diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..f6a910795 --- /dev/null +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearFactor.cpp + * @brief Nonlinear Factor base classes + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ + +void NonlinearFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " keys = { "; + BOOST_FOREACH(Key key, this->keys()) { + std::cout << keyFormatter(key) << " "; + } + std::cout << "}" << std::endl; +} + +bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const { + return Base::equals(f); +} + +NonlinearFactor::shared_ptr NonlinearFactor::rekey( + const std::map& rekey_mapping) const { + shared_ptr new_factor = clone(); + for (size_t i = 0; i < new_factor->size(); ++i) { + Key& cur_key = new_factor->keys()[i]; + std::map::const_iterator mapping = rekey_mapping.find(cur_key); + if (mapping != rekey_mapping.end()) + cur_key = mapping->second; + } + return new_factor; +} + +NonlinearFactor::shared_ptr NonlinearFactor::rekey( + const std::vector& new_keys) const { + assert(new_keys.size() == this->keys().size()); + shared_ptr new_factor = clone(); + new_factor->keys() = new_keys; + return new_factor; +} + +/* ************************************************************************* */ + +void NoiseModelFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + this->noiseModel_->print(" noise model: "); +} + +bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { + const NoiseModelFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) + && ((!noiseModel_ && !e->noiseModel_) + || (noiseModel_ && e->noiseModel_ + && noiseModel_->equals(*e->noiseModel_, tol))); +} + +Vector NoiseModelFactor::whitenedError(const Values& c) const { + const Vector unwhitenedErrorVec = unwhitenedError(c); + if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) + throw std::invalid_argument( + "This factor was created with a NoiseModel of incorrect dimension."); + return noiseModel_->whiten(unwhitenedErrorVec); +} + +double NoiseModelFactor::error(const Values& c) const { + if (this->active(c)) { + const Vector unwhitenedErrorVec = unwhitenedError(c); + if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) + throw std::invalid_argument( + "This factor was created with a NoiseModel of incorrect dimension."); + return 0.5 * noiseModel_->distance(unwhitenedErrorVec); + } else { + return 0.0; + } +} + +boost::shared_ptr NoiseModelFactor::linearize( + const Values& x) const { + + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + + // Call evaluate error to get Jacobians and RHS vector b + std::vector A(this->size()); + Vector b = -unwhitenedError(x, A); + + // If a noiseModel is given, whiten the corresponding system now + if (noiseModel_) { + if ((size_t) b.size() != noiseModel_->dim()) + throw std::invalid_argument( + "This factor was created with a NoiseModel of incorrect dimension."); + this->noiseModel_->WhitenSystem(A, b); + } + + // Fill in terms, needed to create JacobianFactor below + std::vector > terms(this->size()); + for (size_t j = 0; j < this->size(); ++j) { + terms[j].first = this->keys()[j]; + terms[j].second.swap(A[j]); + } + + // TODO pass unwhitened + noise model to Gaussian factor + // For now, only linearized constrained factors have noise model at linear level!!! + if (noiseModel_) { + noiseModel::Constrained::shared_ptr constrained = + boost::dynamic_pointer_cast(this->noiseModel_); + if (constrained) { + // Create a factor of reduced row dimension d_ + size_t d_ = terms[0].second.rows() - constrained->dim(); + Vector zero_ = zero(d_); + Vector b_ = concatVectors(2, &b, &zero_); + Constrained::shared_ptr model = constrained->unit(d_); + return boost::make_shared(terms, b_, model); + } else + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); + } else + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); +} + +/* ************************************************************************* */ + +} // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 1092d8ac2..fc831d7d4 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -43,12 +43,10 @@ namespace gtsam { using boost::assign::cref_list_of; /* ************************************************************************* */ + /** * 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, - * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ class NonlinearFactor: public Factor { @@ -82,18 +80,10 @@ public: /** print */ virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - { - std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } - std::cout << "}" << std::endl; - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - return Base::equals(f); - } - + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; /// @} /// @name Standard Interface /// @{ @@ -128,17 +118,6 @@ public: virtual boost::shared_ptr linearize(const Values& c) const = 0; - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - //virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - // std::vector indices(this->size()); - // for(size_t j=0; jsize(); ++j) - // indices[j] = ordering[this->keys()[j]]; - // return IndexFactor::shared_ptr(new IndexFactor(indices)); - //} - /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow * for subclasses @@ -155,28 +134,13 @@ public: * Creates a shared_ptr clone of the factor with different keys using * a map from old->new keys */ - shared_ptr rekey(const std::map& rekey_mapping) const { - shared_ptr new_factor = clone(); - for (size_t i=0; isize(); ++i) { - Key& cur_key = new_factor->keys()[i]; - std::map::const_iterator mapping = rekey_mapping.find(cur_key); - if (mapping != rekey_mapping.end()) - cur_key = mapping->second; - } - return new_factor; - } + shared_ptr rekey(const std::map& rekey_mapping) const; /** * Clones a factor and fully replaces its keys * @param new_keys is the full replacement set of keys */ - shared_ptr rekey(const std::vector& new_keys) const { - assert(new_keys.size() == this->keys().size()); - shared_ptr new_factor = clone(); - new_factor->keys() = new_keys; - return new_factor; - } - + shared_ptr rekey(const std::vector& new_keys) const; }; // \class NonlinearFactor @@ -229,19 +193,10 @@ public: /** Print */ virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - { - Base::print(s, keyFormatter); - this->noiseModel_->print(" noise model: "); - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - const NoiseModelFactor* e = dynamic_cast(&f); - return e && Base::equals(f, tol) && - ((!noiseModel_ && !e->noiseModel_) || - (noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol))); - } + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; /** get the dimension of the factor (number of rows on linearization) */ virtual size_t dim() const { @@ -265,12 +220,7 @@ public: * Vector of errors, whitened * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian */ - Vector whitenedError(const Values& c) const { - const Vector unwhitenedErrorVec = unwhitenedError(c); - if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - return noiseModel_->whiten(unwhitenedErrorVec); - } + Vector whitenedError(const Values& c) const; /** * Calculate the error of the factor. @@ -278,65 +228,14 @@ public: * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ - virtual double error(const Values& c) const { - if (this->active(c)) { - const Vector unwhitenedErrorVec = unwhitenedError(c); - if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - return 0.5 * noiseModel_->distance(unwhitenedErrorVec); - } else { - return 0.0; - } - } + virtual double error(const Values& c) const; /** * Linearize a non-linearFactorN to get a GaussianFactor, * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x) const { - // Only linearize if the factor is active - if (!this->active(x)) - return boost::shared_ptr(); - - // Create the set of terms - Jacobians for each index - Vector b; - // Call evaluate error to get Jacobians and b vector - std::vector A(this->size()); - b = -unwhitenedError(x, A); - if(noiseModel_) - { - if((size_t) b.size() != noiseModel_->dim()) - throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); - - this->noiseModel_->WhitenSystem(A,b); - } - - std::vector > terms(this->size()); - // Fill in terms - for(size_t j=0; jsize(); ++j) { - terms[j].first = this->keys()[j]; - terms[j].second.swap(A[j]); - } - - // TODO pass unwhitened + noise model to Gaussian factor - // For now, only linearized constrained factors have noise model at linear level!!! - if(noiseModel_) - { - noiseModel::Constrained::shared_ptr constrained = - boost::dynamic_pointer_cast(this->noiseModel_); - if(constrained) { - size_t augmentedDim = terms[0].second.rows() - constrained->dim(); - Vector augmentedZero = zero(augmentedDim); - Vector augmentedb = concatVectors(2, &b, &augmentedZero); - return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim))); - } - else - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); - } - else - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); - } + boost::shared_ptr linearize(const Values& x) const; private: @@ -353,8 +252,15 @@ private: /* ************************************************************************* */ -/** 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 1 + * variable. 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 NoiseModelFactor1: public NoiseModelFactor {