diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 86f35fed9..60b4a1c3d 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -163,6 +163,11 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const { return Unit::Create(maxrank); } +void Gaussian::WhitenSystem(vector& A, Vector& b) const { + BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); } + whitenInPlace(b); +} + void Gaussian::WhitenSystem(Matrix& A, Vector& b) const { WhitenInPlace(A); whitenInPlace(b); @@ -460,6 +465,24 @@ Vector Base::sqrtWeight(const Vector &error) const { /** The following three functions reweight block matrices and a vector * according to their weight implementation */ +/** Reweight n block matrices with one error vector */ +void Base::reweight(vector &A, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + BOOST_FOREACH(Matrix& Aj, A) { + Aj *= w; + } + error *= w; + } + else { + const Vector W = sqrtWeight(error); + BOOST_FOREACH(Matrix& Aj, A) { + vector_scale_inplace(W,Aj); + } + error = emul(W, error); + } +} + /** Reweight one block matrix with one error vector */ void Base::reweight(Matrix &A, Vector &error) const { if ( reweight_ == Block ) { @@ -592,6 +615,11 @@ bool Robust::equals(const Base& expected, double tol) const { return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); } +void Robust::WhitenSystem(vector& A, Vector& b) const { + noise_->WhitenSystem(A,b); + robust_->reweight(A,b); +} + void Robust::WhitenSystem(Matrix& A, Vector& b) const { noise_->WhitenSystem(A,b); robust_->reweight(A,b); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 09bcb9e24..184a599bf 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -77,6 +77,7 @@ namespace gtsam { virtual double distance(const Vector& v) const = 0; + virtual void WhitenSystem(std::vector& A, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0; @@ -185,6 +186,7 @@ namespace gtsam { /** * Whiten a system, in place as well */ + virtual void WhitenSystem(std::vector& A, Vector& b) const ; virtual void WhitenSystem(Matrix& A, Vector& b) const ; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; @@ -554,6 +556,7 @@ namespace gtsam { Vector sqrtWeight(const Vector &error) const; /** reweight block matrices and a vector according to their weight implementation */ + void reweight(std::vector &A, Vector &error) const; void reweight(Matrix &A, Vector &error) const; void reweight(Matrix &A1, Matrix &A2, Vector &error) const; void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; @@ -642,8 +645,9 @@ namespace gtsam { // TODO: these are really robust iterated re-weighting support functions - virtual void WhitenSystem(Matrix& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(std::vector& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; static shared_ptr Create( diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index 33e19f21d..e177ea4bf 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -51,36 +51,41 @@ public: /** Constructor - sets the cost function and the lagrange multipliers * @param dim is the dimension of the factor + * @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) */ - NonlinearConstraint(size_t dim, double mu = 1000.0): - Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {} + template + NonlinearConstraint(const TUPLE& keys, size_t dim, double mu = 1000.0): + Base(noiseModel::Constrained::All(dim), keys), mu_(fabs(mu)) {} virtual ~NonlinearConstraint() {} /** returns the gain mu */ double mu() const { return mu_; } /** Print */ - virtual void print(const std::string& s = "") const=0; + virtual void print(const std::string& s = "") const { + std::cout << "NonlinearConstraint " << s << std::endl; + std::cout << " "; + BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; } + std::cout << "\n"; + std::cout << "mu: " << this->mu_ << std::endl; + } /** Check if two factors are equal */ 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) && (mu_ == p->mu_); + return Base::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol); } /** error function - returns the quadratic merit function */ virtual double error(const VALUES& c) const { - const Vector error_vector = unwhitenedError(c); if (active(c)) - return mu_ * error_vector.dot(error_vector); - else return 0.0; + return mu_ * unwhitenedError(c).squaredNorm(); + else + return 0.0; } - /** Raw error vector function g(x) */ - virtual Vector unwhitenedError(const VALUES& c) const = 0; - /** * active set check, defines what type of constraint this is * @@ -95,7 +100,12 @@ public: * @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=0; + 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); + } private: @@ -138,60 +148,31 @@ public: * @param mu is the gain for the factor */ NonlinearConstraint1(const KEY& key, size_t dim, double mu = 1000.0) - : Base(dim, mu), key_(key) { - this->keys_.push_back(key); - } + : Base(make_tuple(key), dim, mu), key_(key) { } virtual ~NonlinearConstraint1() {} - /* print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearConstraint1 " << s << std::endl; - std::cout << "key: " << (std::string) key_ << std::endl; - std::cout << "mu: " << this->mu_ << std::endl; - } + /** Calls the 1-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(this->active(x)) { + const X& x1 = x[key_]; + if(H) { + return evaluateError(x1, (*H)[0]); + } else { + return evaluateError(x1); + } + } else { + return zero(this->dim()); + } + } - /** Check if two factors are equal. Note type is Factor and needs cast. */ - 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) && (key_ == p->key_); - } - - /** error function g(x), switched depending on whether the constraint is active */ - inline Vector unwhitenedError(const VALUES& x) const { - if (!active(x)) { - return zero(this->dim()); - } - const KEY& j = key_; - const X& xj = x[j]; - return evaluateError(xj); - } - - /** Linearize from config */ - boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { - if (!active(x)) { - boost::shared_ptr factor; - return factor; - } - const X& xj = x[key_]; - Matrix A; - Vector b = - evaluateError(xj, A); - Index var = ordering[key_]; - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, model)); - } - - /** g(x) with optional derivative - does not depend on active */ - virtual Vector evaluateError(const X& x, boost::optional H = - boost::none) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - return IndexFactor::shared_ptr(new IndexFactor(ordering[key_])); - } + /** + * Override this method to finish implementing a unary factor. + * If the optional Matrix reference argument is specified, it should compute + * both the function evaluation and its derivative in X. + */ + virtual Vector evaluateError(const X& x, boost::optional H = + boost::none) const = 0; private: @@ -272,73 +253,33 @@ public: * @param mu is the gain for the factor */ NonlinearConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0) : - Base(dim, mu), key1_(key1), key2_(key2) { - this->keys_.push_back(key1); - this->keys_.push_back(key2); - } + Base(make_tuple(key1, key2), dim, mu), key1_(key1), key2_(key2) { } virtual ~NonlinearConstraint2() {} - /* print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearConstraint2 " << s << std::endl; - std::cout << "key1: " << (std::string) key1_ << std::endl; - std::cout << "key2: " << (std::string) key2_ << std::endl; - std::cout << "mu: " << this->mu_ << std::endl; - } + /** Calls the 2-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(this->active(x)) { + const X1& x1 = x[key1_]; + const X2& x2 = x[key2_]; + if(H) { + return evaluateError(x1, x2, (*H)[0], (*H)[1]); + } else { + return evaluateError(x1, x2); + } + } else { + return zero(this->dim()); + } + } - /** Check if two factors are equal. Note type is Factor and needs cast. */ - 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) && (key1_ == p->key1_) && (key2_ == p->key2_); - } - - /** error function g(x), switched depending on whether the constraint is active */ - inline Vector unwhitenedError(const VALUES& x) const { - if (!active(x)) { - return zero(this->dim()); - } - const KEY1& j1 = key1_; - const KEY2& j2 = key2_; - const X1& xj1 = x[j1]; - const X2& xj2 = x[j2]; - return evaluateError(xj1, xj2); - } - - /** Linearize from config */ - boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { - if (!active(c)) { - boost::shared_ptr factor; - return factor; - } - const KEY1& j1 = key1_; const KEY2& j2 = key2_; - const X1& x1 = c[j1]; const X2& x2 = c[j2]; - Matrix grad1, grad2; - Vector g = -1.0 * evaluateError(x1, x2, grad1, grad2); - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - Index var1 = ordering[j1], var2 = ordering[j2]; - if (var1 < var2) - return GaussianFactor::shared_ptr(new JacobianFactor(var1, grad1, var2, grad2, g, model)); - else - return GaussianFactor::shared_ptr(new JacobianFactor(var2, grad2, var1, grad1, g, model)); - } - - /** g(x) with optional derivative2 - does not depend on active */ - virtual Vector evaluateError(const X1& x1, const X2& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - const Index var1 = ordering[key1_], var2 = ordering[key2_]; - if(var1 < var2) - return IndexFactor::shared_ptr(new IndexFactor(var1, var2)); - else - return IndexFactor::shared_ptr(new IndexFactor(var2, var1)); - } + /** + * Override this method to finish implementing a binary 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). + */ + virtual Vector + evaluateError(const X1&, const X2&, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const = 0; private: @@ -424,102 +365,36 @@ public: */ NonlinearConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3, size_t dim, double mu = 1000.0) : - Base(dim, mu), key1_(key1), key2_(key2), key3_(key3) { - this->keys_.push_back(key1); - this->keys_.push_back(key2); - this->keys_.push_back(key3); - } + Base(make_tuple(key1, key2, key3), dim, mu), key1_(key1), key2_(key2), key3_(key3) { } virtual ~NonlinearConstraint3() {} - /* print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearConstraint3 " << s << std::endl; - std::cout << "key1: " << (std::string) key1_ << std::endl; - std::cout << "key2: " << (std::string) key2_ << std::endl; - std::cout << "key3: " << (std::string) key3_ << std::endl; - std::cout << "mu: " << this->mu_ << std::endl; - } - - /** Check if two factors are equal. Note type is Factor and needs cast. */ - 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) && (key1_ == p->key1_) && (key2_ == p->key2_) && (key3_ == p->key3_); - } - - /** error function g(x), switched depending on whether the constraint is active */ - inline Vector unwhitenedError(const VALUES& x) const { - if (!active(x)) { - return zero(this->dim()); - } - const KEY1& j1 = key1_; - const KEY2& j2 = key2_; - const KEY3& j3 = key3_; - const X1& xj1 = x[j1]; - const X2& xj2 = x[j2]; - const X3& xj3 = x[j3]; - return evaluateError(xj1, xj2, xj3); - } - - /** Linearize from config */ - boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { - if (!active(c)) { - boost::shared_ptr factor; - return factor; - } - const KEY1& j1 = key1_; const KEY2& j2 = key2_; const KEY3& j3 = key3_; - const X1& x1 = c[j1]; const X2& x2 = c[j2]; const X3& x3 = c[j3]; - Matrix A1, A2, A3; - Vector b = -1.0 * evaluateError(x1, x2, x3, A1, A2, A3); - SharedDiagonal model = noiseModel::Constrained::All(this->dim()); - Index var1 = ordering[j1], var2 = ordering[j2], var3 = ordering[j3]; - - // perform sorting - if(var1 < var2 && var2 < var3) - return GaussianFactor::shared_ptr( - new JacobianFactor(var1, A1, var2, A2, var3, A3, b, model)); - else if(var2 < var1 && var1 < var3) - return GaussianFactor::shared_ptr( - new JacobianFactor(var2, A2, var1, A1, var3, A3, b, model)); - else if(var1 < var3 && var3 < var2) - return GaussianFactor::shared_ptr( - new JacobianFactor(var1, A1, var3, A3, var2, A2, b, model)); - else if(var2 < var3 && var3 < var1) - return GaussianFactor::shared_ptr( - new JacobianFactor(var2, A2, var3, A3, var1, A1, b, model)); - else if(var3 < var1 && var1 < var2) - return GaussianFactor::shared_ptr( - new JacobianFactor(var3, A3, var1, A1, var2, A2, b, model)); - else - return GaussianFactor::shared_ptr( - new JacobianFactor(var3, A3, var2, A2, var1, A1, b, model)); - } - - /** g(x) with optional derivative3 - does not depend on active */ - virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { - const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; - if(var1 < var2 && var2 < var3) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_])); - else if(var2 < var1 && var1 < var3) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key1_], ordering[key3_])); - else if(var1 < var3 && var3 < var2) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key3_], ordering[key2_])); - else if(var2 < var3 && var3 < var1) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key3_], ordering[key1_])); - else if(var3 < var1 && var1 < var2) - return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key1_], ordering[key2_])); - else - return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key2_], ordering[key1_])); + /** Calls the 2-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(this->active(x)) { + const X1& x1 = x[key1_]; + const X2& x2 = x[key2_]; + const X3& x3 = x[key3_]; + if(H) { + return evaluateError(x1, x2, x3, (*H)[0], (*H)[1], (*H)[2]); + } else { + return evaluateError(x1, x2, x3); + } + } else { + return zero(this->dim()); } + } + + /** + * Override this method to finish implementing a trinary 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 + evaluateError(const X1&, const X2&, const X3&, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const = 0; private: diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index aec37e0fe..dc01877ac 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -34,583 +35,735 @@ namespace gtsam { - /** - * 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). - */ - template - class NonlinearFactor: public Factor { - - protected: - - // Some handy typedefs - typedef Factor Base; - typedef NonlinearFactor This; - - public: - - typedef boost::shared_ptr > shared_ptr; - - /** Default constructor for I/O only */ - NonlinearFactor() { - } - - /** Destructor */ - virtual ~NonlinearFactor() {} - - /** - * Constructor - * @param key1 by which to look up X value in Values - */ - NonlinearFactor(const Symbol& key1) : - Factor(key1) { - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - */ - 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 std::set& keys) : - Factor(keys) { - } - - /** print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor\n"; - } - - /** - * Vector of errors, unwhitened - * This is the raw error i.e. (h(x)-z) in case of NoiseModelFactor derived class - */ - virtual Vector unwhitenedError(const VALUES& c) const = 0; - - /** - * 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 - linearize(const VALUES& c, const Ordering& ordering) const = 0; - - /** - * Create a symbolic factor using the given ordering to determine the - * variable indices. - */ - 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 { - - 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_->distance(unwhitenedError(c)); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Factor", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(noiseModel_); - } - - }; // \class NoiseModelFactor - - - /** - * A Gaussian nonlinear factor that takes 1 parameter - * implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C - * Templated on the parameter type X and the values structure Values - * There is no return type specified for h(x). Instead, we require - * the derived class implements error_vector(c) = h(x)-z \approx Ax-b - * This allows a graph to have factors with measurements of mixed type. - */ - template - class NonlinearFactor1: public NoiseModelFactor { - - public: - - // typedefs for value types pulled from keys - typedef typename KEY::Value X; - - protected: - - // The value of the key. Not const to allow serialization - KEY key_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor1 This; - - public: - - /** Default constructor for I/O only */ - NonlinearFactor1() { - } - - virtual ~NonlinearFactor1() {} - - inline const KEY& key() const { - return key_; - } - - /** - * Constructor - * @param z measurement - * @param key by which to look up X value in Values - */ - NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) : - Base(noiseModel,key1), key_(key1) { - } - - /* print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor1\n"; - std::cout << " key: " << (std::string) key_ << std::endl; - this->noiseModel_->print(" noise model: "); - } - - /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { - return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); - } - - /** error function h(x)-z, unwhitened !!! */ - inline Vector unwhitenedError(const VALUES& x) const { - const X& xj = x[key_]; - return evaluateError(xj); - } - - /** - * Linearize a non-linearFactor1 to get a GaussianFactor - * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z - * Hence b = z - h(x0) = - error_vector(x) - */ - virtual boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { - const X& xj = x[key_]; - Matrix A; - Vector b = - evaluateError(xj, A); - Index var = ordering[key_]; - // TODO pass unwhitened + noise model to Gaussian factor - SharedDiagonal constrained = - boost::shared_dynamic_cast(this->noiseModel_); - if (constrained.get() != NULL) - return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained)); - this->noiseModel_->WhitenSystem(A,b); - return GaussianFactor::shared_ptr(new JacobianFactor(var, A, 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 { - return IndexFactor::shared_ptr(new IndexFactor(ordering[key_])); - } - - /* - * Override this method to finish implementing a unary factor. - * If the optional Matrix reference argument is specified, it should compute - * both the function evaluation and its derivative in X. - */ - virtual Vector evaluateError(const X& x, boost::optional H = - boost::none) const = 0; - - private: - - /** 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)); - ar & BOOST_SERIALIZATION_NVP(key_); - } - - };// \class NonlinearFactor1 - - /** - * A Gaussian nonlinear factor that takes 2 parameters - */ - template - class NonlinearFactor2: public NoiseModelFactor { - - public: - - // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - - protected: - - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor2 This; - - public: - - /** - * Default Constructor for I/O - */ - NonlinearFactor2() { - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - */ - NonlinearFactor2(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2) : - Base(noiseModel,j1,j2), key1_(j1), key2_(j2) { - } - - virtual ~NonlinearFactor2() {} - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor2\n"; - std::cout << " key1: " << (std::string) key1_ << "\n"; - std::cout << " key2: " << (std::string) key2_ << "\n"; - this->noiseModel_->print(" noise model: "); - } - - /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { - return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) - && (key2_ == f.key2_); - } - - /** error function z-h(x1,x2) */ - inline Vector unwhitenedError(const VALUES& x) const { - const X1& x1 = x[key1_]; - const X2& x2 = x[key2_]; - return evaluateError(x1, x2); - } - - /** - * Linearize a non-linearFactor2 to get a GaussianFactor - * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z - * Hence b = z - h(x1,x2) = - error_vector(x) - */ - boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { - const X1& x1 = c[key1_]; - const X2& x2 = c[key2_]; - Matrix A1, A2; - Vector b = -evaluateError(x1, x2, A1, A2); - const Index var1 = ordering[key1_], var2 = ordering[key2_]; - // TODO pass unwhitened + noise model to Gaussian factor - SharedDiagonal constrained = - boost::shared_dynamic_cast(this->noiseModel_); - if (constrained.get() != NULL) { - return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, - A2, b, constrained)); - } - this->noiseModel_->WhitenSystem(A1,A2,b); - return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, - A2, 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 { - const Index var1 = ordering[key1_], var2 = ordering[key2_]; - return IndexFactor::shared_ptr(new IndexFactor(var1, var2)); - } - - /** methods to retrieve both keys */ - inline const KEY1& key1() const { - return key1_; - } - inline const KEY2& key2() const { - return key2_; - } - - /* - * Override this method to finish implementing a binary 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). - */ - virtual Vector - evaluateError(const X1&, const X2&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const = 0; - - private: - - /** 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)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - } - - }; // \class NonlinearFactor2 +using boost::make_tuple; /* ************************************************************************* */ +/** + * 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). + */ +template +class NonlinearFactor: public Factor { + +protected: + + // Some handy typedefs + typedef Factor Base; + typedef NonlinearFactor This; + +public: + + typedef boost::shared_ptr > shared_ptr; + + /** Default constructor for I/O only */ + NonlinearFactor() { + } + + /** Destructor */ + virtual ~NonlinearFactor() {} + + /** print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearFactor\n"; + } /** - * A Gaussian nonlinear factor that takes 3 parameters + * 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. */ - template - class NonlinearFactor3: public NoiseModelFactor { + virtual double error(const VALUES& c) const = 0; - public: + /** get the dimension of the factor (number of rows on linearization) */ + virtual size_t dim() const = 0; - // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; + /** linearize to a GaussianFactor */ + virtual boost::shared_ptr + linearize(const VALUES& c, const Ordering& ordering) const = 0; - protected: + /** + * Create a symbolic factor using the given ordering to determine the + * variable indices. + */ + virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0; - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; +}; // \class NonlinearFactor - typedef NoiseModelFactor Base; - typedef NonlinearFactor3 This; - public: +// 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 +} - /** - * Default Constructor for I/O - */ - NonlinearFactor3() { +/* ************************************************************************* */ +/** + * A nonlinear sum-of-squares factor with a zero-mean noise model + * implementing the density \f$ P(z|x) \propto exp -0.5*|z-h(x)|^2_C \f$ + * Templated on the parameter type X and the values structure Values + * There is no return type specified for h(x). Instead, we require + * the derived class implements \f$ \mathtt{error\_vector}(x) = h(x)-z \approx A \delta x - b \f$ + * This allows a graph to have factors with measurements of mixed type. + + * The noise model is typically Gaussian, but robust error models are also supported. + */ +template +class NoiseModelFactor: public NonlinearFactor { + +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 keys A boost::tuple containing the variables involved in this factor, + * 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); + } + + /** + * Constructor + * @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); + } + + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor\n"; + std::cout << " "; + BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; } + std::cout << "\n"; + this->noiseModel_->print(" noise model: "); + } + + /** Check if two factors are equal */ + virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { + return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, 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_; + } + + /** + * Error function *without* the NoiseModel, \f$ z-h(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; + + /** + * 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 { + return noiseModel_->whiten(unwhitenedError(c)); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * 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 { + return 0.5 * noiseModel_->distance(unwhitenedError(c)); + } + + /** + * 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 Ordering& ordering) const { + + // 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); + + // TODO pass unwhitened + noise model to Gaussian factor + SharedDiagonal constrained = + boost::shared_dynamic_cast(this->noiseModel_); + if(!constrained) + this->noiseModel_->WhitenSystem(A,b); + + 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]); } - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - */ - NonlinearFactor3(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) : - Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) { - this->keys_.reserve(3); - this->keys_.push_back(key1_); - this->keys_.push_back(key2_); - this->keys_.push_back(key3_); - } - - virtual ~NonlinearFactor3() {} - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor3\n"; - std::cout << " key1: " << (std::string) key1_ << "\n"; - std::cout << " key2: " << (std::string) key2_ << "\n"; - std::cout << " key3: " << (std::string) key3_ << "\n"; - this->noiseModel_->print(" noise model: "); - } - - /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor3& f, double tol = 1e-9) const { - return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) - && (key2_ == f.key2_) && (key3_ == f.key3_); - } - - /** error function z-h(x1,x2) */ - inline Vector unwhitenedError(const VALUES& x) const { - const X1& x1 = x[key1_]; - const X2& x2 = x[key2_]; - const X3& x3 = x[key3_]; - return evaluateError(x1, x2, x3); - } - - /** - * Linearize a non-linearFactor2 to get a GaussianFactor - * Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z - * Hence b = z - h(x1,x2,x3) = - error_vector(x) - */ - boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { - const X1& x1 = c[key1_]; - const X2& x2 = c[key2_]; - const X3& x3 = c[key3_]; - Matrix A1, A2, A3; - Vector b = -evaluateError(x1, x2, x3, A1, A2, A3); - const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; - // TODO pass unwhitened + noise model to Gaussian factor - SharedDiagonal constrained = - boost::shared_dynamic_cast(this->noiseModel_); - if (constrained.get() != NULL) { - return GaussianFactor::shared_ptr( - new JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained)); - } - this->noiseModel_->WhitenSystem(A1,A2,A3,b); + if(constrained) return GaussianFactor::shared_ptr( - new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size()))); - } + new JacobianFactor(terms, b, constrained)); + else + return GaussianFactor::shared_ptr( + 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 { - const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; - return IndexFactor::shared_ptr(new IndexFactor(var1, var2, var3)); - } + /** + * 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)); + } - /** methods to retrieve keys */ - inline const KEY1& key1() const { - return key1_; - } - inline const KEY2& key2() const { - return key2_; - } - inline const KEY3& key3() const { - return key3_; - } +private: - /* - * Override this method to finish implementing a trinary 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 - evaluateError(const X1&, const X2&, const X3&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(noiseModel_); + } - private: +}; // \class NoiseModelFactor - /** 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)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - } - }; // \class NonlinearFactor3 +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 1 + * variable. To derive from this class, implement evaluateError(). */ +template +class NonlinearFactor1: public NoiseModelFactor { + +public: + + // typedefs for value types pulled from keys + typedef typename KEY::Value X; + +protected: + + // The value of the key. Not const to allow serialization + KEY key_; + + typedef NoiseModelFactor Base; + typedef NonlinearFactor1 This; + +public: + + /** Default constructor for I/O only */ + NonlinearFactor1() {} + + virtual ~NonlinearFactor1() {} + + inline const KEY& key() const { return key_; } + + /** + * Constructor + * @param z measurement + * @param key by which to look up X value in Values + */ + NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) : + Base(noiseModel, make_tuple(key1)), key_(key1) { + } + + /** Calls the 1-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(H) + return evaluateError(x[key_], (*H)[0]); + else + return evaluateError(x[key_]); + } + + /** + * Override this method to finish implementing a unary factor. + * If the optional Matrix reference argument is specified, it should compute + * both the function evaluation and its derivative in X. + */ + virtual Vector evaluateError(const X& x, boost::optional H = + boost::none) const = 0; + +private: + + /** 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)); + ar & BOOST_SERIALIZATION_NVP(key_); + } +};// \class NonlinearFactor1 + + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 2 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearFactor2: public NoiseModelFactor { + +public: + + // typedefs for value types pulled from keys + typedef typename KEY1::Value X1; + typedef typename KEY2::Value X2; + +protected: + + // The values of the keys. Not const to allow serialization + KEY1 key1_; + KEY2 key2_; + + typedef NoiseModelFactor Base; + typedef NonlinearFactor2 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearFactor2() {} + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + */ + NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) : + Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {} + + virtual ~NonlinearFactor2() {} + + /** methods to retrieve both keys */ + inline const KEY1& key1() const { return key1_; } + inline const KEY2& key2() const { return key2_; } + + /** Calls the 2-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(H) + return evaluateError(x[key1_], x[key2_], (*H)[0], (*H)[1]); + else + return evaluateError(x[key1_], x[key2_]); + } + + /** + * Override this method to finish implementing a binary 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). + */ + virtual Vector + evaluateError(const X1&, const X2&, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const = 0; + +private: + + /** 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)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + } +}; // \class NonlinearFactor2 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 3 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearFactor3: public NoiseModelFactor { + +public: + + // typedefs for value types pulled from keys + typedef typename KEY1::Value X1; + typedef typename KEY2::Value X2; + typedef typename KEY3::Value X3; + +protected: + + // The values of the keys. Not const to allow serialization + KEY1 key1_; + KEY2 key2_; + KEY3 key3_; + + typedef NoiseModelFactor Base; + typedef NonlinearFactor3 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearFactor3() {} + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + * @param j3 key of the third variable + */ + NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) : + Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {} + + virtual ~NonlinearFactor3() {} + + /** methods to retrieve keys */ + inline const KEY1& key1() const { return key1_; } + inline const KEY2& key2() const { return key2_; } + inline const KEY3& key3() const { return key3_; } + + /** Calls the 3-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(H) + return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]); + else + return evaluateError(x[key1_], x[key2_], x[key3_]); + } + + /** + * Override this method to finish implementing a trinary 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 + evaluateError(const X1&, const X2&, const X3&, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const = 0; + +private: + + /** 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)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(key3_); + } +}; // \class NonlinearFactor3 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 4 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearFactor4: public NoiseModelFactor { + +public: + + // typedefs for value types pulled from keys + typedef typename KEY1::Value X1; + typedef typename KEY2::Value X2; + typedef typename KEY3::Value X3; + typedef typename KEY4::Value X4; + +protected: + + // The values of the keys. Not const to allow serialization + KEY1 key1_; + KEY2 key2_; + KEY3 key3_; + KEY4 key4_; + + typedef NoiseModelFactor Base; + typedef NonlinearFactor4 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearFactor4() {} + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + * @param j3 key of the third variable + * @param j4 key of the fourth variable + */ + NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) : + Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {} + + virtual ~NonlinearFactor4() {} + + /** methods to retrieve keys */ + inline const KEY1& key1() const { return key1_; } + inline const KEY2& key2() const { return key2_; } + inline const KEY3& key3() const { return key3_; } + inline const KEY4& key4() const { return key4_; } + + /** Calls the 4-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(H) + return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]); + else + return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]); + } + + /** + * Override this method to finish implementing a 4-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 + evaluateError(const X1&, const X2&, const X3&, const X4&, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const = 0; + +private: + + /** 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)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(key3_); + ar & BOOST_SERIALIZATION_NVP(key4_); + } +}; // \class NonlinearFactor4 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 5 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearFactor5: public NoiseModelFactor { + +public: + + // typedefs for value types pulled from keys + typedef typename KEY1::Value X1; + typedef typename KEY2::Value X2; + typedef typename KEY3::Value X3; + typedef typename KEY4::Value X4; + typedef typename KEY5::Value X5; + +protected: + + // The values of the keys. Not const to allow serialization + KEY1 key1_; + KEY2 key2_; + KEY3 key3_; + KEY4 key4_; + KEY5 key5_; + + typedef NoiseModelFactor Base; + typedef NonlinearFactor5 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearFactor5() {} + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + * @param j3 key of the third variable + * @param j4 key of the fourth variable + * @param j5 key of the fifth variable + */ + NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) : + Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {} + + virtual ~NonlinearFactor5() {} + + /** methods to retrieve keys */ + inline const KEY1& key1() const { return key1_; } + inline const KEY2& key2() const { return key2_; } + inline const KEY3& key3() const { return key3_; } + inline const KEY4& key4() const { return key4_; } + inline const KEY5& key5() const { return key5_; } + + /** Calls the 5-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(H) + return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); + else + return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]); + } + + /** + * Override this method to finish implementing a 5-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 + evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const = 0; + +private: + + /** 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)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(key3_); + ar & BOOST_SERIALIZATION_NVP(key4_); + ar & BOOST_SERIALIZATION_NVP(key5_); + } +}; // \class NonlinearFactor5 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NoiseModelFactor with 6 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearFactor6: public NoiseModelFactor { + +public: + + // typedefs for value types pulled from keys + typedef typename KEY1::Value X1; + typedef typename KEY2::Value X2; + typedef typename KEY3::Value X3; + typedef typename KEY4::Value X4; + typedef typename KEY5::Value X5; + typedef typename KEY6::Value X6; + +protected: + + // The values of the keys. Not const to allow serialization + KEY1 key1_; + KEY2 key2_; + KEY3 key3_; + KEY4 key4_; + KEY5 key5_; + KEY6 key6_; + + typedef NoiseModelFactor Base; + typedef NonlinearFactor6 This; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearFactor6() {} + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + * @param j3 key of the third variable + * @param j4 key of the fourth variable + * @param j5 key of the fifth variable + * @param j6 key of the fifth variable + */ + NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) : + Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {} + + virtual ~NonlinearFactor6() {} + + /** methods to retrieve keys */ + inline const KEY1& key1() const { return key1_; } + inline const KEY2& key2() const { return key2_; } + inline const KEY3& key3() const { return key3_; } + inline const KEY4& key4() const { return key4_; } + inline const KEY5& key5() const { return key5_; } + inline const KEY6& key6() const { return key6_; } + + /** Calls the 6-key specific version of evaluateError, which is pure virtual + * so must be implemented in the derived class. */ + virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + if(H) + return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); + else + return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]); + } + + /** + * Override this method to finish implementing a 6-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 + evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none, + boost::optional H6 = boost::none) const = 0; + +private: + + /** 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)); + ar & BOOST_SERIALIZATION_NVP(key1_); + ar & BOOST_SERIALIZATION_NVP(key2_); + ar & BOOST_SERIALIZATION_NVP(key3_); + ar & BOOST_SERIALIZATION_NVP(key4_); + ar & BOOST_SERIALIZATION_NVP(key5_); + ar & BOOST_SERIALIZATION_NVP(key6_); + } +}; // \class NonlinearFactor6 /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph-inl.h index ad3d957e9..a6f2bddb3 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph-inl.h @@ -45,17 +45,6 @@ namespace gtsam { Base::print(str); } - /* ************************************************************************* */ - template - Vector NonlinearFactorGraph::unwhitenedError(const VALUES& c) const { - list errors; - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { - if(factor) - errors.push_back(factor->unwhitenedError(c)); - } - return concatVectors(errors); - } - /* ************************************************************************* */ template double NonlinearFactorGraph::error(const VALUES& c) const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 68b5103b7..5a13a982f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -53,9 +53,6 @@ namespace gtsam { /** unnormalized error */ double error(const VALUES& c) const; - /** all individual errors */ - Vector unwhitenedError(const VALUES& c) const; - /** Unnormalized probability. O(n) */ double probPrime(const VALUES& c) const; diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 61bccef46..2ec561d96 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -160,8 +160,17 @@ TEST( planarSLAM, constructor ) double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 G.addRange(2, 3, z2, sigma); - Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22); - EXPECT(assert_equal(expected,G.unwhitenedError(c))); + Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); + Vector expected1 = Vector_(3, 0.0, 0.0, 0.0); + Vector expected2 = Vector_(1, -0.1); + Vector expected3 = Vector_(1, 0.22); + // Get NoiseModelFactors + FactorGraph > GNM = + *G.dynamicCastFactors > >(); + EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c))); + EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c))); + EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c))); + EXPECT(assert_equal(expected3, GNM[3]->unwhitenedError(c))); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 17c1e2d6f..d10a83c66 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -87,7 +87,7 @@ TEST( NonlinearFactor, NonlinearFactor ) // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] - Vector actual_e = factor->unwhitenedError(cfg); + Vector actual_e = boost::dynamic_pointer_cast >(factor)->unwhitenedError(cfg); CHECK(assert_equal(0.1*ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1