From c14d69326a39384eb02c8d2ccf8da58e677a05d8 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 7 Oct 2011 20:51:47 +0000 Subject: [PATCH] NonlinearConstraint now derives from NonlinearFactor rather than NoiseModelFactor --- gtsam/nonlinear/NonlinearConstraint.h | 73 +++++++++++++++++++++----- gtsam/nonlinear/NonlinearFactor.h | 74 +++++++++++++++------------ 2 files changed, 101 insertions(+), 46 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index e177ea4bf..2ad147d8d 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -36,16 +35,18 @@ namespace gtsam { * significant */ template -class NonlinearConstraint : public NoiseModelFactor { +class NonlinearConstraint : public NonlinearFactor { protected: typedef NonlinearConstraint This; - typedef NoiseModelFactor Base; + typedef NonlinearFactor Base; + typedef Factor BaseFactor; /** default constructor to allow for serialization */ NonlinearConstraint() {} double mu_; /// gain for quadratic merit function + size_t dim_; /// dimension of the constraint public: @@ -54,14 +55,30 @@ public: * @param keys is a boost::tuple containing the keys, e.g. \c make_tuple(key1,key2,key3) * @param mu is the gain used at error evaluation (forced to be positive) */ - template - NonlinearConstraint(const TUPLE& keys, size_t dim, double mu = 1000.0): - Base(noiseModel::Constrained::All(dim), keys), mu_(fabs(mu)) {} + template + NonlinearConstraint(const boost::tuples::cons& keys, size_t dim, double mu = 1000.0) + : Base(keys), mu_(fabs(mu)), dim_(dim) { + } + + /** + * Constructor + * @param keys The variables involved in this factor + */ + template + NonlinearConstraint(ITERATOR beginKeys, ITERATOR endKeys, size_t dim, double mu = 1000.0) + : Base(beginKeys, endKeys), mu_(fabs(mu)), dim_(dim) { + } + virtual ~NonlinearConstraint() {} /** returns the gain mu */ double mu() const { return mu_; } + /** get the dimension of the factor (number of rows on linearization) */ + virtual size_t dim() const { + return dim_; + } + /** Print */ virtual void print(const std::string& s = "") const { std::cout << "NonlinearConstraint " << s << std::endl; @@ -75,7 +92,7 @@ public: virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const { const This* p = dynamic_cast (&f); if (p == NULL) return false; - return Base::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol); + return BaseFactor::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol) && dim_ == p->dim_; } /** error function - returns the quadratic merit function */ @@ -95,16 +112,38 @@ public: */ virtual bool active(const VALUES& c) const=0; + /** + * Error function \f$ z-c(x) \f$. + * 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 X1 (and/or X2, X3...). + */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const = 0; + /** * Linearizes around a given config * @param config is the values structure * @return a combined linear factor containing both the constraint and the constraint factor */ virtual boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { - if (!active(c)) - return boost::shared_ptr(); - else - return Base::linearize(c, ordering); + if (!active(c)) + 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(c, A); + + std::vector > terms(this->size()); + // Fill in terms + for(size_t j=0; jsize(); ++j) { + terms[j].first = ordering[this->keys()[j]]; + terms[j].second.swap(A[j]); + } + + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, b, noiseModel::Constrained::All(dim_))); } private: @@ -116,10 +155,11 @@ private: ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(mu_); + ar & BOOST_SERIALIZATION_NVP(dim_); } }; - +/* ************************************************************************* */ /** * A unary constraint that defaults to an equality constraint */ @@ -186,6 +226,7 @@ private: } }; +/* ************************************************************************* */ /** * Unary Equality constraint - simply forces the value of active() to true */ @@ -222,6 +263,7 @@ private: }; +/* ************************************************************************* */ /** * A binary constraint with arbitrary cost and jacobian functions */ @@ -294,6 +336,7 @@ private: } }; +/* ************************************************************************* */ /** * Binary Equality constraint - simply forces the value of active() to true */ @@ -330,6 +373,7 @@ private: } }; +/* ************************************************************************* */ /** * A ternary constraint */ @@ -410,6 +454,7 @@ private: } }; +/* ************************************************************************* */ /** * Ternary Equality constraint - simply forces the value of active() to true */ @@ -448,7 +493,7 @@ private: } }; - +/* ************************************************************************* */ /** * Simple unary equality constraint - fixes a value for a variable */ @@ -493,7 +538,7 @@ private: } }; - +/* ************************************************************************* */ /** * Simple binary equality constraint - this constraint forces two factors to * be the same. This constraint requires the underlying type to a Lie type diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 260bd3940..c3dacb85f 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -37,6 +37,17 @@ namespace gtsam { using boost::make_tuple; +// Helper function to fill a vector from a tuple function of any length +template +inline void __fill_from_tuple(std::vector& vector, size_t position, const CONS& tuple) { + vector[position] = tuple.get_head(); + __fill_from_tuple(vector, position+1, tuple.get_tail()); +} +template<> +inline void __fill_from_tuple(std::vector& vector, size_t position, const boost::tuples::null_type& tuple) { + // Do nothing +} + /* ************************************************************************* */ /** * Nonlinear factor base class @@ -62,6 +73,27 @@ public: NonlinearFactor() { } + /** + * Constructor + * @param keys A boost::tuple containing the variables involved in this factor, + * example: NonlinearFactor(make_tuple(symbol1, symbol2, symbol3)) + */ + template + NonlinearFactor(const boost::tuples::cons& keys) { + this->keys_.resize(boost::tuples::length >::value); + // Use helper function to fill key vector, using 'cons' representation of tuple + __fill_from_tuple(this->keys(), 0, keys); + } + + /** + * Constructor + * @param keys The variables involved in this factor + */ + template + NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) { + this->keys_.insert(this->keys_.end(), beginKeys, endKeys); + } + /** Destructor */ virtual ~NonlinearFactor() {} @@ -88,22 +120,15 @@ public: * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0; + 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)); + } }; // \class NonlinearFactor - -// Helper function to fill a vector from a tuple function of any length -template -inline void __fill_from_tuple(std::vector& vector, size_t position, const CONS& tuple) { - vector[position] = tuple.get_head(); - __fill_from_tuple(vector, position+1, tuple.get_tail()); -} -template<> -inline void __fill_from_tuple(std::vector& vector, size_t position, const boost::tuples::null_type& tuple) { - // Do nothing -} - /* ************************************************************************* */ /** * A nonlinear sum-of-squares factor with a zero-mean noise model @@ -143,11 +168,8 @@ public: * example: NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3) */ template - NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons& keys) : - noiseModel_(noiseModel) { - this->keys_.resize(boost::tuples::length >::value); - // Use helper function to fill key vector, using 'cons' representation of tuple - __fill_from_tuple(this->keys(), 0, keys); + NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons& keys) + : Base(keys), noiseModel_(noiseModel) { } /** @@ -155,9 +177,8 @@ public: * @param keys The variables involved in this factor */ template - NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) : - Base(noiseModel) { - this->keys_.insert(this->keys_.end(), beginKeys, endKeys); + NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) + : Base(beginKeys, endKeys), noiseModel_(noiseModel) { } protected: @@ -253,17 +274,6 @@ public: new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size()))); } - /** - * 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)); - } - private: /** Serialization function */