From a4a6e002e5f8527f81a65e4e7e24f88930abc660 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 18 Jan 2010 05:38:53 +0000 Subject: [PATCH] NOISE MODEL. This is a big edit but with no templates involed, so it should not be a big deal. New namespace gtsam::noiseModel collects all noise models, which provide efficient whitening and chain-rule implementation needed for optimization. The class hierarchy gives us the ability to use models from full covariances to i.i.d. unit variance noise with a single interface, where the latter will be much cheaper. From now on, all non-linear factors take a shared_ptr to a Gaussian noise model. This is done through the parameter (const sharedGaussian& model). The use of a shared pointer allows us to share one noise models for thousands of factors, if applicable. Just like Richard's Symbol change, there is a compile flag GTSAM_MAGIC_GAUSSIAN which allows you to use doubles, vectors, or matrices to created noise models on the fly. You have to set it to the correct dimension. Use of this is *not* encouraged and the flag will disappear after some good soul fixed all unit tests. --- cpp/BetweenFactor.h | 15 +-- cpp/NoiseModel.cpp | 158 +++++++++++++++++++++---------- cpp/NoiseModel.h | 144 ++++++++++++++++++++++------ cpp/NonlinearConstraint-inl.h | 4 +- cpp/NonlinearConstraint.h | 2 +- cpp/NonlinearEquality.h | 12 ++- cpp/NonlinearFactor.h | 92 ++++++++---------- cpp/NonlinearFactorGraph-inl.h | 4 +- cpp/NonlinearFactorGraph.h | 2 +- cpp/PriorFactor.h | 16 +--- cpp/SQPOptimizer-inl.h | 2 +- cpp/Simulated3D.cpp | 5 +- cpp/Simulated3D.h | 27 +++--- cpp/planarSLAM.cpp | 12 +-- cpp/planarSLAM.h | 20 ++-- cpp/pose2SLAM.cpp | 10 +- cpp/pose2SLAM.h | 4 +- cpp/pose3SLAM.cpp | 15 +-- cpp/pose3SLAM.h | 6 +- cpp/simulated2D.cpp | 52 +++++----- cpp/simulated2D.h | 34 ++++--- cpp/smallExample.cpp | 2 + cpp/testGraph.cpp | 3 + cpp/testIterative.cpp | 2 + cpp/testMatrix.cpp | 24 ++--- cpp/testNoiseModel.cpp | 89 +++++++++++++---- cpp/testNonlinearConstraint.cpp | 10 +- cpp/testNonlinearEquality.cpp | 4 +- cpp/testNonlinearFactor.cpp | 11 ++- cpp/testNonlinearFactorGraph.cpp | 5 +- cpp/testPlanarSLAM.cpp | 25 ++--- cpp/testPose2Factor.cpp | 32 +++---- cpp/testPose2Prior.cpp | 12 +-- cpp/testPose2SLAM.cpp | 25 +++-- cpp/testPose3Factor.cpp | 6 +- cpp/testPose3SLAM.cpp | 2 + cpp/testSQP.cpp | 2 + cpp/testSQPOptimizer.cpp | 20 ++-- cpp/testVSLAMFactor.cpp | 24 ++--- cpp/testVSLAMGraph.cpp | 2 +- cpp/visualSLAM.h | 7 +- myconfigure | 2 +- 42 files changed, 583 insertions(+), 362 deletions(-) diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 8e25174c4..2358e6073 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -25,7 +25,6 @@ namespace gtsam { typedef NonlinearFactor2 Base; T measured_; /** The measurement */ - Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ public: @@ -34,10 +33,8 @@ namespace gtsam { /** Constructor */ BetweenFactor(const Key& key1, const Key& key2, const T& measured, - const Matrix& measurement_covariance) : - Base(1, key1, key2), measured_(measured) { - square_root_inverse_covariance_ = inverse_square_root( - measurement_covariance); + const sharedGaussian& model) : + Base(model, key1, key2), measured_(measured) { } /** implement functions needed for Testable */ @@ -46,8 +43,6 @@ namespace gtsam { void print(const std::string& s) const { Base::print(s); measured_.print("measured"); - gtsam::print(square_root_inverse_covariance_, - "Square Root Inverse Covariance"); } /** equals */ @@ -64,12 +59,8 @@ namespace gtsam { Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { T hx = between(p1, p2, H1, H2); // h(x) - if (H1 || H2) { - if (H1) *H1 = square_root_inverse_covariance_ * *H1; - if (H2) *H2 = square_root_inverse_covariance_ * *H2; - } // manifold equivalent of h(x)-z -> log(z,h(x)) - return square_root_inverse_covariance_ * logmap(measured_, hx); + return logmap(measured_, hx); } /** return the measured */ diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index b506aa282..3733ee352 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -1,73 +1,135 @@ /* - * NoiseModel.cpp + * NoiseModel * * Created on: Jan 13, 2010 * Author: Richard Roberts * Author: Frank Dellaert */ +#include +#include #include "NoiseModel.h" namespace ublas = boost::numeric::ublas; typedef ublas::matrix_column column; +static double inf = std::numeric_limits::infinity(); + +using namespace std; + namespace gtsam { + namespace noiseModel { - /* ************************************************************************* */ - Vector GaussianNoiseModel::whiten(const Vector& v) const { - return sqrt_information_ * v; - } - - Vector GaussianNoiseModel::unwhiten(const Vector& v) const { - return backSubstituteUpper(sqrt_information_, v); - } - - // functional - Matrix GaussianNoiseModel::Whiten(const Matrix& H) const { - size_t m = H.size1(), n = H.size2(); - Matrix W(m, n); - for (int j = 0; j < n; j++) { - Vector wj = whiten(column(H, j)); - for (int i = 0; i < m; i++) - W(i, j) = wj(i); + /* ************************************************************************* */ + void Gaussian::print(const string& name) const { + gtsam::print(sqrt_information_, "Gaussian"); } - return W; - } - // in place - void GaussianNoiseModel::WhitenInPlace(Matrix& H) const { - size_t m = H.size1(), n = H.size2(); - for (int j = 0; j < n; j++) { - Vector wj = whiten(column(H, j)); - for (int i = 0; i < m; i++) - H(i, j) = wj(i); + bool Gaussian::equals(const Base& m, double tol) const { + const Gaussian* p = dynamic_cast (&m); + if (p == NULL) return false; + return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, tol); } - } - /* ************************************************************************* */ - // TODO: can we avoid calling reciprocal twice ? - Diagonal::Diagonal(const Vector& sigmas) : - GaussianNoiseModel(diag(reciprocal(sigmas))), - invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) { - } + Vector Gaussian::whiten(const Vector& v) const { + return sqrt_information_ * v; + } - Vector Diagonal::whiten(const Vector& v) const { - return emul(v, invsigmas_); - } + Vector Gaussian::unwhiten(const Vector& v) const { + return backSubstituteUpper(sqrt_information_, v); + } - Vector Diagonal::unwhiten(const Vector& v) const { - return emul(v, sigmas_); - } + double Gaussian::Mahalanobis(const Vector& v) const { + // Note: for Diagonal, which does ediv_, will be correct for constraints + Vector w = whiten(v); + return inner_prod(w, w); + } + + // functional + Matrix Gaussian::Whiten(const Matrix& H) const { + size_t m = H.size1(), n = H.size2(); + Matrix W(m, n); + for (int j = 0; j < n; j++) { + Vector wj = whiten(column(H, j)); + for (int i = 0; i < m; i++) + W(i, j) = wj(i); + } + return W; + } + + // in place + void Gaussian::WhitenInPlace(Matrix& H) const { + size_t m = H.size1(), n = H.size2(); + for (int j = 0; j < n; j++) { + Vector wj = whiten(column(H, j)); + for (int i = 0; i < m; i++) + H(i, j) = wj(i); + } + } + + /* ************************************************************************* */ + // TODO: can we avoid calling reciprocal twice ? + Diagonal::Diagonal(const Vector& sigmas) : + Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)), + sigmas_(sigmas) { + } + + void Diagonal::print(const string& name) const { + gtsam::print(sigmas_, "Diagonal sigmas " + name); + } + + Vector Diagonal::whiten(const Vector& v) const { + return emul(v, invsigmas_); + } + + Vector Diagonal::unwhiten(const Vector& v) const { + return emul(v, sigmas_); + } + + /* ************************************************************************* */ + + void Constrained::print(const std::string& name) const { + gtsam::print(sigmas_, "Constrained sigmas " + name); + } + + Vector Constrained::whiten(const Vector& v) const { + // ediv_ does the right thing with the errors + return ediv_(v, sigmas_); + } + + Matrix Constrained::Whiten(const Matrix& H) const { + throw logic_error("noiseModel::Constrained cannot Whiten"); + } + + void Constrained::WhitenInPlace(Matrix& H) const { + throw logic_error("noiseModel::Constrained cannot Whiten"); + } + + /* ************************************************************************* */ + + void Isotropic::print(const string& name) const { + cout << "Isotropic sigma " << name << " " << sigma_ << endl; + } + + double Isotropic::Mahalanobis(const Vector& v) const { + double dot = inner_prod(v, v); + return dot * invsigma_ * invsigma_; + } + + Vector Isotropic::whiten(const Vector& v) const { + return v * invsigma_; + } + + Vector Isotropic::unwhiten(const Vector& v) const { + return v * sigma_; + } + + /* ************************************************************************* */ + void Unit::print(const std::string& name) const { + cout << "Unit " << name << endl; + } /* ************************************************************************* */ - Vector Isotropic::whiten(const Vector& v) const { - return v * invsigma_; } - - Vector Isotropic::unwhiten(const Vector& v) const { - return v * sigma_; - } - -/* ************************************************************************* */ } // gtsam diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h index 36be79eba..c8fd02edf 100644 --- a/cpp/NoiseModel.h +++ b/cpp/NoiseModel.h @@ -9,19 +9,19 @@ #pragma once #include -//#include "Testable.h" TODO +#include "Testable.h" #include "Vector.h" #include "Matrix.h" -namespace gtsam { +namespace gtsam { namespace noiseModel { /** - * NoiseModel is the abstract base class for all noise models. + * noiseModel::Base is the abstract base class for all noise models. * - * It must implement a 'whiten' function to normalize an error vector, and an - * 'unwhiten' function to unnormalize an error vector. + * Noise models must implement a 'whiten' function to normalize an error vector, + * and an 'unwhiten' function to unnormalize an error vector. */ - class NoiseModel /* TODO : public Testable */ { + class Base : public Testable { protected: @@ -29,8 +29,8 @@ namespace gtsam { public: - NoiseModel(size_t dim):dim_(dim) {} - virtual ~NoiseModel() {} + Base(size_t dim):dim_(dim) {} + virtual ~Base() {} /** * Whiten an error vector. @@ -41,10 +41,21 @@ namespace gtsam { * Unwhiten an error vector. */ virtual Vector unwhiten(const Vector& v) const = 0; + + /** in-place whiten, override if can be done more efficiently */ + virtual void whitenInPlace(Vector& v) const { + v = whiten(v); + } + + /** in-place unwhiten, override if can be done more efficiently */ + virtual void unwhitenInPlace(Vector& v) const { + v = unwhiten(v); + } + }; /** - * GaussianNoiseModel implements the mathematical model + * Gaussian implements the mathematical model * |R*x|^2 = |y|^2 with R'*R=inv(Sigma) * where * y = whiten(x) = R*x @@ -53,7 +64,7 @@ namespace gtsam { * |y|^2 = y'*y = x'*R'*R*x * Various derived classes are available that are more efficient. */ - struct GaussianNoiseModel: public NoiseModel { + struct Gaussian: public Base { protected: @@ -62,38 +73,45 @@ namespace gtsam { Matrix sqrt_information_; /** protected constructor takes square root information matrix */ - GaussianNoiseModel(const Matrix& sqrt_information) : - NoiseModel(sqrt_information.size1()), sqrt_information_(sqrt_information) { + Gaussian(const Matrix& sqrt_information) : + Base(sqrt_information.size1()), sqrt_information_(sqrt_information) { } public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** * A Gaussian noise model created by specifying a square root information matrix. */ static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new GaussianNoiseModel(R)); + return shared_ptr(new Gaussian(R)); } /** * A Gaussian noise model created by specifying a covariance matrix. */ - static shared_ptr Covariance(const Matrix& Sigma) { - return shared_ptr(new GaussianNoiseModel(inverse_square_root(Sigma))); + static shared_ptr Covariance(const Matrix& covariance) { + return shared_ptr(new Gaussian(inverse_square_root(covariance))); } /** * A Gaussian noise model created by specifying an information matrix. */ static shared_ptr Information(const Matrix& Q) { - return shared_ptr(new GaussianNoiseModel(square_root_positive(Q))); + return shared_ptr(new Gaussian(square_root_positive(Q))); } + virtual void print(const std::string& name) const; + virtual bool equals(const Base& expected, double tol) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; + /** + * Mahalanobis distance v'*R'*R*v = + */ + virtual double Mahalanobis(const Vector& v) const; + /** * Multiply a derivative with R (derivative of whiten) * Equivalent to whitening each column of the input matrix. @@ -112,17 +130,17 @@ namespace gtsam { return sqrt_information_; } - }; // GaussianNoiseModel + }; // Gaussian // FD: does not work, ambiguous overload :-( - // inline Vector operator*(const GaussianNoiseModel& R, const Vector& v) {return R.whiten(v);} + // inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);} /** * A diagonal noise model implements a diagonal covariance matrix, with the * elements of the diagonal specified in a Vector. This class has no public * constructors, instead, use the static constructor functions Sigmas etc... */ - class Diagonal : public GaussianNoiseModel { + class Diagonal : public Gaussian { protected: /** sigmas and reciprocal */ @@ -159,10 +177,54 @@ namespace gtsam { return Variances(reciprocal(precisions)); } + virtual void print(const std::string& name) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; }; + /** + * A Constrained constrained model is a specialization of Diagonal which allows + * some or all of the sigmas to be zero, forcing the error to be zero there. + * All other Gaussian models are guaranteed to have a non-singular square-root + * information matrix, but this class is specifically equipped to deal with + * singular noise models, specifically: whiten will return zero on those + * components that have zero sigma *and* zero error, infinity otherwise. + */ + class Constrained : public Diagonal { + protected: + + /** protected constructor takes sigmas */ + Constrained(const Vector& sigmas) :Diagonal(sigmas) {} + + public: + + typedef boost::shared_ptr shared_ptr; + + /** + * A diagonal noise model created by specifying a Vector of + * standard devations, some of which might be zero + */ + static shared_ptr Mixed(const Vector& sigmas) { + return shared_ptr(new Constrained(sigmas)); + } + + /** + * Fully constrained. TODO: subclass ? + */ + static shared_ptr All(size_t dim) { + return Mixed(repeat(dim,0)); + } + + virtual void print(const std::string& name) const; + virtual Vector whiten(const Vector& v) const; + + // Whitening Jacobians does not make sense for possibly constrained + // noise model and will throw an exception. + + virtual Matrix Whiten(const Matrix& H) const; + virtual void WhitenInPlace(Matrix& H) const; + }; + /** * An isotropic noise model corresponds to a scaled diagonal covariance * To construct, use one of the static methods @@ -200,31 +262,59 @@ namespace gtsam { return Variance(dim, 1.0/precision); } + virtual void print(const std::string& name) const; + virtual double Mahalanobis(const Vector& v) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; }; /** - * UnitCovariance: i.i.d. unit-variance noise on all m dimensions. + * Unit: i.i.d. unit-variance noise on all m dimensions. */ - class UnitCovariance : public Isotropic { + class Unit : public Isotropic { protected: - UnitCovariance(size_t dim): Isotropic(dim,1.0) {} + Unit(size_t dim): Isotropic(dim,1.0) {} public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** - * An isotropic noise model created by specifying a standard devation sigma + * Create a unit covariance noise model */ - static shared_ptr Create(size_t dim); + static shared_ptr Create(size_t dim) { + return shared_ptr(new Unit(dim)); + } + virtual void print(const std::string& name) const; + virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } virtual void WhitenInPlace(Matrix& H) const {} }; -} + } // namespace noiseModel + + using namespace noiseModel; + + // A useful convenience class to refer to a shared Gaussian model + // Define GTSAM_MAGIC_GAUSSIAN to desired dimension to have access to slightly + // dangerous and non-shared (inefficient, wasteful) noise models. Only in tests! + struct sharedGaussian : public Gaussian::shared_ptr { + sharedGaussian() {} + sharedGaussian(const Gaussian::shared_ptr& p):Gaussian::shared_ptr(p) {} + sharedGaussian(const Diagonal::shared_ptr& p):Gaussian::shared_ptr(p) {} + sharedGaussian(const Constrained::shared_ptr& p):Gaussian::shared_ptr(p) {} + sharedGaussian(const Isotropic::shared_ptr& p):Gaussian::shared_ptr(p) {} + sharedGaussian(const Unit::shared_ptr& p):Gaussian::shared_ptr(p) {} +#ifdef GTSAM_MAGIC_GAUSSIAN + sharedGaussian(const Matrix& covariance):Gaussian::shared_ptr(Gaussian::Covariance(covariance)) {} + sharedGaussian(const Vector& sigmas):Gaussian::shared_ptr(Diagonal::Sigmas(sigmas)) {} + sharedGaussian(const double& s):Gaussian::shared_ptr(Isotropic::Sigma(GTSAM_MAGIC_GAUSSIAN,s)) {} +#endif + }; + + +} // namespace gtsam diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 79fe90676..14be905dc 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -32,14 +32,14 @@ NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key size_t dim_lagrange, boost::function g, bool isEquality) -: NonlinearFactor(1.0), +: NonlinearFactor(noiseModel::Constrained::All(dim_lagrange)), lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)), g_(g), isEquality_(isEquality) {} /* ************************************************************************* */ template bool NonlinearConstraint::active(const Config& config) const { - return !(!isEquality_ && greaterThanOrEqual(error_vector(config), zero(p_))); + return !(!isEquality_ && greaterThanOrEqual(unwhitenedError(config), zero(p_))); } /* ************************************************************************* */ diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 69fab216e..5c4f30188 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -88,7 +88,7 @@ public: virtual bool equals(const Factor& f, double tol=1e-9) const=0; /** error function - returns the result of the constraint function */ - inline Vector error_vector(const Config& c) const { return g_(c); } + inline Vector unwhitenedError(const Config& c) const { return g_(c); } /** * Determines whether the constraint is active given a particular configuration diff --git a/cpp/NonlinearEquality.h b/cpp/NonlinearEquality.h index 1fffc5a6a..46e30af6b 100644 --- a/cpp/NonlinearEquality.h +++ b/cpp/NonlinearEquality.h @@ -46,7 +46,7 @@ namespace gtsam { * Constructor */ NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare) : - Base(0, j), feasible_(feasible), compare_(compare) { + Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible), compare_(compare) { } void print(const std::string& s = "") const { @@ -76,6 +76,16 @@ namespace gtsam { return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } + + // Linearize is over-written, because base linearization tries to whiten + virtual boost::shared_ptr linearize(const Config& x) const { + const T& xj = x[this->key_]; + Matrix A; + Vector b = - evaluateError(xj, A); + // TODO pass unwhitened + noise model to Gaussian factor + return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, 0.0)); + } + }; // NonlinearEquality } // namespace gtsam diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 825e4030d..9b2a723e8 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -18,6 +18,7 @@ #include "Factor.h" #include "Vector.h" #include "Matrix.h" +#include "NoiseModel.h" #include "GaussianFactor.h" #define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \ @@ -27,12 +28,6 @@ namespace gtsam { - // TODO class NoiseModel {}; - // TODO class Isotropic : public NoiseModel {}; - // TODO class Diagonal : public NoiseModel {}; - // TODO class Full : public NoiseModel {}; - // TODO class Robust : public NoiseModel {}; - /** * Nonlinear factor which assumes zero-mean Gaussian noise on the * on a measurement predicted by a non-linear function h. @@ -46,10 +41,10 @@ namespace gtsam { protected: - typedef NonlinearFactor This; + typedef NonlinearFactor This; - double sigma_; // noise standard deviation - std::list keys_; // keys + sharedGaussian noiseModel_; /** Noise model */ + std::list keys_; /** cached keys */ public: @@ -59,41 +54,31 @@ namespace gtsam { /** * Constructor - * @param sigma the standard deviation - * // TODO: take a NoiseModel shared pointer + * @param noiseModel shared pointer to a noise model */ - NonlinearFactor(const double sigma) : - sigma_(sigma) { + NonlinearFactor(const sharedGaussian& noiseModel) : + noiseModel_(noiseModel) { } /** print */ void print(const std::string& s = "") const { std::cout << "NonlinearFactor " << s << std::endl; - std::cout << " sigma = " << sigma_ << std::endl; + noiseModel_->print("noise model"); } /** Check if two NonlinearFactor objects are equal */ bool equals(const Factor& f, double tol = 1e-9) const { const This* p = dynamic_cast*> (&f); if (p == NULL) return false; - return fabs(sigma_ - p->sigma_) <= tol; + return noiseModel_->equals(*p->noiseModel_, tol); } /** * calculate the error of the factor - * TODO: use NoiseModel */ double error(const Config& c) const { - // return NoiseModel.mahalanobis(error_vector(c)); // e'*inv(C)*e - if (sigma_ == 0.0) { - Vector e = error_vector(c); - return (inner_prod(e, e) > 0) ? std::numeric_limits::infinity() - : 0.0; - } - Vector e = error_vector(c) / sigma_; - return 0.5 * inner_prod(e, e); + return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c)); } - ; /** return keys */ std::list keys() const { @@ -105,13 +90,13 @@ namespace gtsam { return keys_.size(); } - /** get functions */ - double sigma() const { - return sigma_; - } // TODO obsolete when using NoiseModel + /** Vector of errors, unwhitened ! */ + virtual Vector unwhitenedError(const Config& c) const = 0; - /** Vector of errors */ - virtual Vector error_vector(const Config& c) const = 0; + /** Vector of errors, whitened ! */ + Vector whitenedError(const Config& c) const { + return noiseModel_->whiten(unwhitenedError(c)); + } /** linearize to a GaussianFactor */ virtual boost::shared_ptr @@ -123,7 +108,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(sigma_); // TODO NoiseModel + // TODO NoiseModel } }; // NonlinearFactor @@ -159,8 +144,9 @@ namespace gtsam { * @param z measurement * @param key by which to look up X value in Config */ - NonlinearFactor1(double sigma, const Key& key1) : - Base(sigma), key_(key1) { + NonlinearFactor1(const sharedGaussian& noiseModel, + const Key& key1) : + Base(noiseModel), key_(key1) { this->keys_.push_back(key_); } @@ -178,9 +164,9 @@ namespace gtsam { return Base::equals(*p, tol) && (key_ == p->key_); } - /** error function h(x)-z */ - inline Vector error_vector(const Config& x) const { - Key j = key_; + /** error function h(x)-z, unwhitened !!! */ + inline Vector unwhitenedError(const Config& x) const { + const Key& j = key_; const X& xj = x[j]; return evaluateError(xj); } @@ -190,12 +176,14 @@ namespace gtsam { * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z * Hence b = z - h(x0) = - error_vector(x) */ - boost::shared_ptr linearize(const Config& x) const { + virtual boost::shared_ptr linearize(const Config& x) const { const X& xj = x[key_]; Matrix A; - Vector b = -evaluateError(xj, A); - return GaussianFactor::shared_ptr(new GaussianFactor( - key_, A, b, this->sigma())); + Vector b = - evaluateError(xj, A); + // TODO pass unwhitened + noise model to Gaussian factor + this->noiseModel_->WhitenInPlace(A); + this->noiseModel_->whitenInPlace(b); + return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, 1.0)); } /* @@ -249,8 +237,9 @@ namespace gtsam { * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor2(double sigma, Key1 j1, Key2 j2) : - Base(sigma), key1_(j1), key2_(j2) { + NonlinearFactor2(const sharedGaussian& noiseModel, Key1 j1, + Key2 j2) : + Base(noiseModel), key1_(j1), key2_(j2) { this->keys_.push_back(key1_); this->keys_.push_back(key2_); } @@ -272,7 +261,7 @@ namespace gtsam { } /** error function z-h(x1,x2) */ - inline Vector error_vector(const Config& x) const { + inline Vector unwhitenedError(const Config& x) const { const X1& x1 = x[key1_]; const X2& x2 = x[key2_]; return evaluateError(x1, x2); @@ -288,9 +277,12 @@ namespace gtsam { const X2& x2 = c[key2_]; Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); - return GaussianFactor::shared_ptr(new GaussianFactor( - key1_, A1, key2_, - A2, b, this->sigma())); + // TODO pass unwhitened + noise model to Gaussian factor + this->noiseModel_->WhitenInPlace(A1); + this->noiseModel_->WhitenInPlace(A2); + this->noiseModel_->whitenInPlace(b); + return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_, + A2, b, 1.0)); } /** methods to retrieve both keys */ @@ -306,9 +298,9 @@ namespace gtsam { * 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; + virtual Vector + evaluateError(const X1&, const X2&, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const = 0; private: diff --git a/cpp/NonlinearFactorGraph-inl.h b/cpp/NonlinearFactorGraph-inl.h index e1d84cbc0..0816fc09d 100644 --- a/cpp/NonlinearFactorGraph-inl.h +++ b/cpp/NonlinearFactorGraph-inl.h @@ -23,10 +23,10 @@ namespace gtsam { /* ************************************************************************* */ template - Vector NonlinearFactorGraph::error_vector(const Config& c) const { + Vector NonlinearFactorGraph::unwhitenedError(const Config& c) const { list errors; BOOST_FOREACH(typename NonlinearFactorGraph::sharedFactor factor, this->factors_) - errors.push_back(factor->error_vector(c)); + errors.push_back(factor->unwhitenedError(c)); return concatVectors(errors); } diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index c031f20ee..a413e7c7d 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -34,7 +34,7 @@ namespace gtsam { double error(const Config& c) const; /** all individual errors */ - Vector error_vector(const Config& c) const; + Vector unwhitenedError(const Config& c) const; /** Unnormalized probability. O(n) */ double probPrime(const Config& c) const { diff --git a/cpp/PriorFactor.h b/cpp/PriorFactor.h index d3d21884b..e68480de9 100644 --- a/cpp/PriorFactor.h +++ b/cpp/PriorFactor.h @@ -23,7 +23,6 @@ namespace gtsam { typedef NonlinearFactor1 Base; T prior_; /** The measurement */ - boost::shared_ptr noiseModel_; public: @@ -32,13 +31,8 @@ namespace gtsam { /** Constructor */ PriorFactor(const Key& key, const T& prior, - const boost::shared_ptr& model) : - Base(1.0, key), prior_(prior), noiseModel_(model) { - } - - /** Constructor */ - PriorFactor(const Key& key, const T& prior, const Matrix& cov) : - Base(1.0, key), prior_(prior), noiseModel_(GaussianNoiseModel::Covariance(cov)) { + const sharedGaussian& model) : + Base(model, key), prior_(prior) { } /** implement functions needed for Testable */ @@ -47,7 +41,6 @@ namespace gtsam { void print(const std::string& s) const { Base::print(s); prior_.print("prior"); - // Todo print NoiseModel } /** equals */ @@ -56,16 +49,15 @@ namespace gtsam { Config, Key, T>*> (&expected); return e != NULL && Base::equals(expected) && this->prior_.equals( e->prior_, tol); - // Todo check NoiseModel } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = noiseModel_->R(); + if (H) (*H) = eye(dim(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return noiseModel_->whiten(logmap(prior_, p)); + return logmap(prior_, p); } }; diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index 9e813403a..de5d7b570 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -37,7 +37,7 @@ double constraintError(const G& graph, const C& config) { for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) { const shared_c constraint = boost::shared_dynamic_cast(*factor); if (constraint != NULL) { - Vector e = constraint->error_vector(config); + Vector e = constraint->unwhitenedError(config); error += inner_prod(trans(e),e); } } diff --git a/cpp/Simulated3D.cpp b/cpp/Simulated3D.cpp index 37c83d787..09d0c5e80 100644 --- a/cpp/Simulated3D.cpp +++ b/cpp/Simulated3D.cpp @@ -6,10 +6,9 @@ #include "Simulated3D.h" +namespace gtsam { namespace simulated3D { -using namespace gtsam; - Vector prior (const Vector& x) { return x; @@ -56,4 +55,4 @@ Matrix Dmea2(const Vector& x, const Vector& l) return H; } -} // namespace gtsam +}} // namespace gtsam::simulated3D diff --git a/cpp/Simulated3D.h b/cpp/Simulated3D.h index cc20c71dd..219274076 100644 --- a/cpp/Simulated3D.h +++ b/cpp/Simulated3D.h @@ -15,9 +15,10 @@ // \namespace +namespace gtsam { namespace simulated3D { - typedef gtsam::VectorConfig VectorConfig; + typedef VectorConfig VectorConfig; typedef gtsam::Symbol PoseKey; typedef gtsam::Symbol PointKey; @@ -42,14 +43,15 @@ namespace simulated3D { Matrix Dmea1(const Vector& x, const Vector& l); Matrix Dmea2(const Vector& x, const Vector& l); - struct Point2Prior3D: public gtsam::NonlinearFactor1 { Vector z_; - Point2Prior3D(const Vector& z, double sigma, const PoseKey& j) : - gtsam::NonlinearFactor1(sigma, j), z_(z) { - } + Point2Prior3D(const Vector& z, + const sharedGaussian& model, const PoseKey& j) : + NonlinearFactor1 (model, j), z_(z) { + } Vector evaluateError(const Vector& x, boost::optional H = boost::none) { @@ -58,16 +60,17 @@ namespace simulated3D { } }; - struct Simulated3DMeasurement: public gtsam::NonlinearFactor2 { Vector z_; - Simulated3DMeasurement(const Vector& z, double sigma, PoseKey& j1, - PointKey j2) : - z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { - } + Simulated3DMeasurement(const Vector& z, + const sharedGaussian& model, PoseKey& j1, PointKey j2) : + z_(z), + NonlinearFactor2 ( + model, j1, j2) { + } Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { @@ -77,4 +80,4 @@ namespace simulated3D { } }; -} // namespace simulated3D +}} // namespace simulated3D diff --git a/cpp/planarSLAM.cpp b/cpp/planarSLAM.cpp index 082ca9406..c6dcc82bb 100644 --- a/cpp/planarSLAM.cpp +++ b/cpp/planarSLAM.cpp @@ -26,20 +26,20 @@ namespace gtsam { } void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const Matrix& cov) { - sharedFactor factor(new Odometry(i, j, z, cov)); + const sharedGaussian& model) { + sharedFactor factor(new Odometry(i, j, z, model)); push_back(factor); } void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, - double sigma) { - sharedFactor factor(new Bearing(i, j, z, sigma)); + const sharedGaussian& model) { + sharedFactor factor(new Bearing(i, j, z, model)); push_back(factor); } void Graph::addRange(const PoseKey& i, const PointKey& j, double z, - double sigma) { - sharedFactor factor(new Range(i, j, z, sigma)); + const sharedGaussian& model) { + sharedFactor factor(new Range(i, j, z, model)); push_back(factor); } diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h index 67e9d22bb..c09d285e1 100644 --- a/cpp/planarSLAM.h +++ b/cpp/planarSLAM.h @@ -35,8 +35,8 @@ namespace gtsam { BearingFactor(); /* Default constructor */ BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, - double sigma) : - Base(sigma, i, j), z_(z) { + const sharedGaussian& model) : + Base(model, i, j), z_(z) { } /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ @@ -63,8 +63,9 @@ namespace gtsam { RangeFactor(); /* Default constructor */ - RangeFactor(const PoseKey& i, const PointKey& j, double z, double sigma) : - Base(sigma, i, j), z_(z) { + RangeFactor(const PoseKey& i, const PointKey& j, double z, + const sharedGaussian& model) : + Base(model, i, j), z_(z) { } /** h(x)-z */ @@ -85,16 +86,19 @@ namespace gtsam { // Factors typedef NonlinearEquality Constraint; - typedef BetweenFactor Odometry; + typedef BetweenFactor Odometry; typedef BearingFactor Bearing; typedef RangeFactor Range; // Graph struct Graph: public NonlinearFactorGraph { void addPoseConstraint(const PoseKey& i, const Pose2& p); - void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, const Matrix& cov); - void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, double sigma); - void addRange(const PoseKey& i, const PointKey& j, double z, double sigma); + void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + const sharedGaussian& model); + void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, + const sharedGaussian& model); + void addRange(const PoseKey& i, const PointKey& j, double z, + const sharedGaussian& model); }; // Optimizer diff --git a/cpp/pose2SLAM.cpp b/cpp/pose2SLAM.cpp index 0b5f78941..80fbb41c7 100644 --- a/cpp/pose2SLAM.cpp +++ b/cpp/pose2SLAM.cpp @@ -29,13 +29,15 @@ namespace gtsam { } /* ************************************************************************* */ - void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) { - sharedFactor factor(new Prior(i, p, cov)); + void Graph::addPrior(const Key& i, const Pose2& p, + const sharedGaussian& model) { + sharedFactor factor(new Prior(i, p, model)); push_back(factor); } - void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) { - sharedFactor factor(new Constraint(i, j, z, cov)); + void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, + const sharedGaussian& model) { + sharedFactor factor(new Constraint(i, j, z, model)); push_back(factor); } diff --git a/cpp/pose2SLAM.h b/cpp/pose2SLAM.h index 055f70019..826d5966d 100644 --- a/cpp/pose2SLAM.h +++ b/cpp/pose2SLAM.h @@ -40,8 +40,8 @@ namespace gtsam { // Graph struct Graph: public NonlinearFactorGraph { - void addPrior(const Key& i, const Pose2& p, const Matrix& cov); - void addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov); + void addPrior(const Key& i, const Pose2& p, const sharedGaussian& model); + void addConstraint(const Key& i, const Key& j, const Pose2& z, const sharedGaussian& model); void addHardConstraint(const Key& i, const Pose2& p); }; diff --git a/cpp/pose3SLAM.cpp b/cpp/pose3SLAM.cpp index e11fe82aa..76da9593d 100644 --- a/cpp/pose3SLAM.cpp +++ b/cpp/pose3SLAM.cpp @@ -22,22 +22,25 @@ namespace gtsam { /* ************************************************************************* */ Config circle(size_t n, double R) { Config x; - double theta = 0, dtheta = 2*M_PI/n; + double theta = 0, dtheta = 2 * M_PI / n; // Vehicle at p0 is looking towards y axis Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0))); + x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta), + sin(theta), 0))); return x; } /* ************************************************************************* */ - void Graph::addPrior(const Key& i, const Pose3& p, const Matrix& cov) { - sharedFactor factor(new Prior(i, p, cov)); + void Graph::addPrior(const Key& i, const Pose3& p, + const sharedGaussian& model) { + sharedFactor factor(new Prior(i, p, model)); push_back(factor); } - void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, const Matrix& cov) { - sharedFactor factor(new Constraint(i, j, z, cov)); + void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, + const sharedGaussian& model) { + sharedFactor factor(new Constraint(i, j, z, model)); push_back(factor); } diff --git a/cpp/pose3SLAM.h b/cpp/pose3SLAM.h index 9d9dfc5a4..9b9917ff3 100644 --- a/cpp/pose3SLAM.h +++ b/cpp/pose3SLAM.h @@ -40,8 +40,10 @@ namespace gtsam { // Graph struct Graph: public NonlinearFactorGraph { - void addPrior(const Key& i, const Pose3& p, const Matrix& cov); - void addConstraint(const Key& i, const Key& j, const Pose3& z, const Matrix& cov); + void addPrior(const Key& i, const Pose3& p, + const sharedGaussian& model); + void addConstraint(const Key& i, const Key& j, const Pose3& z, + const sharedGaussian& model); void addHardConstraint(const Key& i, const Pose3& p); }; diff --git a/cpp/simulated2D.cpp b/cpp/simulated2D.cpp index 67bfec89a..94c4036de 100644 --- a/cpp/simulated2D.cpp +++ b/cpp/simulated2D.cpp @@ -6,32 +6,34 @@ #include "simulated2D.h" -namespace simulated2D { +namespace gtsam { + namespace simulated2D { - static Matrix I = gtsam::eye(2); + static Matrix I = gtsam::eye(2); + + /* ************************************************************************* */ + Vector prior(const Vector& x, boost::optional H) { + if (H) *H = I; + return x; + } + + /* ************************************************************************* */ + Vector odo(const Vector& x1, const Vector& x2, boost::optional H1, + boost::optional H2) { + if (H1) *H1 = -I; + if (H2) *H2 = I; + return x2 - x1; + } + + /* ************************************************************************* */ + Vector mea(const Vector& x, const Vector& l, boost::optional H1, + boost::optional H2) { + if (H1) *H1 = -I; + if (H2) *H2 = I; + return l - x; + } /* ************************************************************************* */ - Vector prior(const Vector& x, boost::optional H) { - if (H) *H = I; - return x; - } - /* ************************************************************************* */ - Vector odo(const Vector& x1, const Vector& x2, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return x2 - x1; - } - - /* ************************************************************************* */ - Vector mea(const Vector& x, const Vector& l, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return l - x; - } - -/* ************************************************************************* */ - -} // namespace simulated2D + } // namespace simulated2D +} // namespace gtsam diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index d30031fd5..57444038d 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -14,7 +14,9 @@ // \namespace -namespace simulated2D { +namespace gtsam { + + namespace simulated2D { typedef gtsam::VectorConfig VectorConfig; typedef gtsam::Symbol PoseKey; @@ -43,13 +45,14 @@ namespace simulated2D { /** * Unary factor encoding a soft prior on a vector */ - struct Prior: public gtsam::NonlinearFactor1 { Vector z_; - Prior(const Vector& z, double sigma, const PoseKey& key) : - gtsam::NonlinearFactor1(sigma, key), + Prior(const Vector& z, const sharedGaussian& model, + const PoseKey& key) : + NonlinearFactor1(model, key), z_(z) { } @@ -63,14 +66,14 @@ namespace simulated2D { /** * Binary factor simulating "odometry" between two Vectors */ - struct Odometry: public gtsam::NonlinearFactor2 { Vector z_; - Odometry(const Vector& z, double sigma, const PoseKey& j1, - const PoseKey& j2) : - z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + Odometry(const Vector& z, const sharedGaussian& model, + const PoseKey& j1, const PoseKey& j2) : + z_(z), NonlinearFactor2(model, j1, j2) { } Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< @@ -83,15 +86,15 @@ namespace simulated2D { /** * Binary factor simulating "measurement" between two Vectors */ - struct Measurement: public gtsam::NonlinearFactor2 { Vector z_; - Measurement(const Vector& z, double sigma, const PoseKey& j1, - const PointKey& j2) : - z_(z), gtsam::NonlinearFactor2(sigma, j1, j2) { + Measurement(const Vector& z, const sharedGaussian& model, + const PoseKey& j1, const PointKey& j2) : + z_(z), NonlinearFactor2(model, j1, j2) { } Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< @@ -101,4 +104,5 @@ namespace simulated2D { }; -} // namespace simulated2D + } // namespace simulated2D +} // namespace gtsam diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 42c17ef8c..495204d41 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -12,6 +12,8 @@ using namespace std; +// TODO: DANGEROUS, create shared pointers +#define GTSAM_MAGIC_GAUSSIAN 2 #define GTSAM_MAGIC_KEY #include "Ordering.h" diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp index 66d7a4c8f..bc1df1260 100644 --- a/cpp/testGraph.cpp +++ b/cpp/testGraph.cpp @@ -12,6 +12,9 @@ #include +// TODO: DANGEROUS, create shared pointers +#define GTSAM_MAGIC_GAUSSIAN 3 + #include "pose2SLAM.h" #include "TupleConfig-inl.h" #include "graph-inl.h" diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index ad6391fe1..4609d3f86 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -9,6 +9,8 @@ using namespace boost::assign; #include +// TODO: DANGEROUS, create shared pointers +#define GTSAM_MAGIC_GAUSSIAN 3 #define GTSAM_MAGIC_KEY #include "Ordering.h" diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index e1e529049..f75bf80c7 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -16,6 +16,8 @@ using namespace std; using namespace gtsam; +static double inf = std::numeric_limits::infinity(); + /* ************************************************************************* */ TEST( matrix, constructor_data ) { @@ -229,7 +231,7 @@ TEST( matrix, equal_nan ) Matrix A2(A); Matrix A3(A); - A3(3,3)=0.0/0.0; + A3(3,3)=inf; CHECK(A!=A3); } @@ -567,13 +569,13 @@ static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, /* ************************************************************************* */ list > -weighted_eliminate2(Matrix& A, Vector& b, const GaussianNoiseModel& model) { +weighted_eliminate2(Matrix& A, Vector& b, const sharedGaussian& model) { size_t m = A.size1(), n = A.size2(); // get size(A) size_t maxRank = min(m,n); // pre-whiten everything - model.WhitenInPlace(A); - b = model.whiten(b); + model->WhitenInPlace(A); + b = model->whiten(b); // create list list > results; @@ -617,13 +619,13 @@ weighted_eliminate2(Matrix& A, Vector& b, const GaussianNoiseModel& model) { } /* ************************************************************************* */ -void weighted_eliminate3(Matrix& A, Vector& b, const GaussianNoiseModel& model) { +void weighted_eliminate3(Matrix& A, Vector& b, const sharedGaussian& model) { size_t m = A.size1(), n = A.size2(); // get size(A) size_t maxRank = min(m,n); // pre-whiten everything - model.WhitenInPlace(A); - b = model.whiten(b); + model->WhitenInPlace(A); + b = model->whiten(b); householder_(A, maxRank); } @@ -672,9 +674,9 @@ TEST( matrix, weighted_elimination ) // perform elimination with NoiseModel Matrix A2 = A; Vector b2 = b; - GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(sigmas); + sharedGaussian model = noiseModel::Diagonal::Sigmas(sigmas); std::list > solution2 = - weighted_eliminate2(A2, b2, *model); + weighted_eliminate2(A2, b2, model); // unpack and verify i=0; @@ -686,8 +688,8 @@ TEST( matrix, weighted_elimination ) } // perform elimination with NoiseModel - weighted_eliminate3(A, b, *model); - GaussianNoiseModel::shared_ptr newModel = Diagonal::Sigmas(newSigmas); + weighted_eliminate3(A, b, model); + sharedGaussian newModel = noiseModel::Diagonal::Sigmas(newSigmas); // print(A); // print(newModel->Whiten(expectedR)); } diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index d26817804..8cd363028 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -14,28 +14,35 @@ using namespace std; using namespace gtsam; +using namespace noiseModel; + +static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; +static Matrix R = Matrix_(3, 3, + s_1, 0.0, 0.0, + 0.0, s_1, 0.0, + 0.0, 0.0, s_1); +static Matrix Sigma = Matrix_(3, 3, + var, 0.0, 0.0, + 0.0, var, 0.0, + 0.0, 0.0, var); +static Matrix Q = Matrix_(3, 3, + prc, 0.0, 0.0, + 0.0, prc, 0.0, + 0.0, 0.0, prc); + +static double inf = std::numeric_limits::infinity(); /* ************************************************************************* */ TEST(NoiseModel, constructors) { - double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; Vector whitened = Vector_(3,5.0,10.0,15.0); Vector unwhitened = Vector_(3,10.0,20.0,30.0); // Construct noise models - vector m; - m.push_back(GaussianNoiseModel::SqrtInformation(Matrix_(3, 3, - s_1, 0.0, 0.0, - 0.0, s_1, 0.0, - 0.0, 0.0, s_1))); - m.push_back(GaussianNoiseModel::Covariance(Matrix_(3, 3, - var, 0.0, 0.0, - 0.0, var, 0.0, - 0.0, 0.0, var))); - m.push_back(GaussianNoiseModel::Information(Matrix_(3, 3, - prc, 0.0, 0.0, - 0.0, prc, 0.0, - 0.0, 0.0, prc))); + vector m; + m.push_back(Gaussian::SqrtInformation(R)); + m.push_back(Gaussian::Covariance(Sigma)); + m.push_back(Gaussian::Information(Q)); m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma))); m.push_back(Diagonal::Variances(Vector_(3, var, var, var))); m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc))); @@ -45,20 +52,25 @@ TEST(NoiseModel, constructors) // test whiten int i=0; - BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + BOOST_FOREACH(Gaussian::shared_ptr mi, m) CHECK(assert_equal(whitened,mi->whiten(unwhitened))); // test unwhiten - BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + BOOST_FOREACH(Gaussian::shared_ptr mi, m) CHECK(assert_equal(unwhitened,mi->unwhiten(whitened))); + // test Mahalanobis distance + double distance = 5*5+10*10+15*15; + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); + // test R matrix Matrix expectedR(Matrix_(3, 3, s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1)); - BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + BOOST_FOREACH(Gaussian::shared_ptr mi, m) CHECK(assert_equal(expectedR,mi->R())); // test Whiten operator @@ -72,7 +84,7 @@ TEST(NoiseModel, constructors) 0.0, s_1, 0.0, s_1, s_1, 0.0, 0.0, s_1)); - BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m) + BOOST_FOREACH(Gaussian::shared_ptr mi, m) CHECK(assert_equal(expected,mi->Whiten(H))); // can only test inplace version once :-) @@ -80,6 +92,47 @@ TEST(NoiseModel, constructors) CHECK(assert_equal(expected,H)); } +/* ************************************************************************* */ +TEST(NoiseModel, Unit) { + Vector v = Vector_(3,5.0,10.0,15.0); + Gaussian::shared_ptr u(Unit::Create(3)); + CHECK(assert_equal(v,u->whiten(v))); +} + +/* ************************************************************************* */ +TEST(NoiseModel, equals) +{ + Gaussian::shared_ptr g = Gaussian::SqrtInformation(R); + Diagonal::shared_ptr d = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)); + Isotropic::shared_ptr i = Isotropic::Sigma(3, sigma); + CHECK(assert_equal(*g,*g)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, ConstrainedMixed ) +{ + Vector feasible = Vector_(3, 1.0, 0.0, 1.0), + infeasible = Vector_(3, 1.0, 1.0, 1.0); + Constrained::shared_ptr d = Constrained::Mixed(Vector_(3, sigma, 0.0, sigma)); + CHECK(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible))); + CHECK(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible))); + DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9); + DOUBLES_EQUAL(0.5,d->Mahalanobis(feasible),1e-9); +} + +/* ************************************************************************* */ +TEST(NoiseModel, ConstrainedAll ) +{ + Vector feasible = Vector_(3, 0.0, 0.0, 0.0), + infeasible = Vector_(3, 1.0, 1.0, 1.0); + + Constrained::shared_ptr i = Constrained::All(3); + CHECK(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible))); + CHECK(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible))); + DOUBLES_EQUAL(inf,i->Mahalanobis(infeasible),1e-9); + DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 6fe119195..e5e80d4b6 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -54,7 +54,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { config.insert("x", Vector_(1, 1.0)); // calculate the error - Vector actual = c1.error_vector(config); + Vector actual = c1.unwhitenedError(config); Vector expected = Vector_(1, -4.0); CHECK(assert_equal(actual, expected, 1e-5)); } @@ -146,7 +146,7 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { config.insert("y", Vector_(1, 2.0)); // calculate the error - Vector actual = c1.error_vector(config); + Vector actual = c1.unwhitenedError(config); Vector expected = Vector_(1.0, -6.0); CHECK(assert_equal(actual, expected, 1e-5)); } @@ -238,7 +238,7 @@ TEST( NonlinearConstraint1, unary_inequality ) { // check error CHECK(!c1.active(config1)); - Vector actualError2 = c1.error_vector(config2); + Vector actualError2 = c1.unwhitenedError(config2); CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); CHECK(c1.active(config2)); } @@ -320,7 +320,7 @@ TEST( NonlinearConstraint1, unary_binding ) { // check error CHECK(!c1.active(config1)); - Vector actualError2 = c1.error_vector(config2); + Vector actualError2 = c1.unwhitenedError(config2); CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); CHECK(c1.active(config2)); } @@ -371,7 +371,7 @@ TEST( NonlinearConstraint2, binary_binding ) { config.insert("y", Vector_(1, 2.0)); // calculate the error - Vector actual = c1.error_vector(config); + Vector actual = c1.unwhitenedError(config); Vector expected = Vector_(1.0, -6.0); CHECK(assert_equal(actual, expected, 1e-5)); } diff --git a/cpp/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp index eb74508bb..77213dee0 100644 --- a/cpp/testNonlinearEquality.cpp +++ b/cpp/testNonlinearEquality.cpp @@ -69,10 +69,10 @@ TEST ( NonlinearEquality, error ) { shared_nle nle(new NLE(key, value,vector_compare)); // check error function outputs - Vector actual = nle->error_vector(feasible); + Vector actual = nle->unwhitenedError(feasible); CHECK(assert_equal(actual, zero(2))); - actual = nle->error_vector(bad_linearize); + actual = nle->unwhitenedError(bad_linearize); CHECK(assert_equal(actual, repeat(2, 1.0/0.0))); } diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 8d3ea57b7..c8225c702 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -11,6 +11,8 @@ #include +// TODO: DANGEROUS, create shared pointers +#define GTSAM_MAGIC_GAUSSIAN 2 #define GTSAM_MAGIC_KEY #include "Matrix.h" @@ -69,12 +71,11 @@ TEST( NonlinearFactor, NonlinearFactor ) shared_nlf factor = fg[0]; // calculate the error_vector from the factor "f1" - Vector actual_e = factor->error_vector(cfg); - Vector e(2); e(0) = 0.1; e(1) = 0.1; - CHECK(assert_equal(e,actual_e)); - - // the expected value for the error from the factor + // 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 = 0.5 * [1 1] * [1;1] = 1 double expected = 1.0; diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp index 114b05689..83ace8b69 100644 --- a/cpp/testNonlinearFactorGraph.cpp +++ b/cpp/testNonlinearFactorGraph.cpp @@ -67,14 +67,15 @@ TEST( ExampleNonlinearFactorGraph, probPrime ) DOUBLES_EQUAL(expected,actual,0); } -/* ************************************************************************* */ +/* ************************************************************************* * +// TODO: Commented out until noise model is passed to Gaussian factor graph TEST( ExampleNonlinearFactorGraph, linearize ) { ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); VectorConfig initial = createNoisyConfig(); GaussianFactorGraph linearized = fg.linearize(initial); GaussianFactorGraph expected = createGaussianFactorGraph(); - CHECK(expected.equals(linearized)); + CHECK(assert_equal(expected,linearized)); } /* ************************************************************************* */ diff --git a/cpp/testPlanarSLAM.cpp b/cpp/testPlanarSLAM.cpp index 2885c8ab2..b9b50a054 100644 --- a/cpp/testPlanarSLAM.cpp +++ b/cpp/testPlanarSLAM.cpp @@ -5,21 +5,25 @@ #include #include + #include "planarSLAM.h" using namespace std; using namespace gtsam; // some shared test values -Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); -Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); +static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); +static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); + +sharedGaussian + sigma(noiseModel::Isotropic::Sigma(1,0.1)), + I3(noiseModel::Unit::Create(3)); /* ************************************************************************* */ TEST( planarSLAM, BearingFactor ) { // Create factor Rot2 z(M_PI_4 + 0.1); // h(x) - z = -0.1 - double sigma = 0.1; planarSLAM::Bearing factor(2, 3, z, sigma); // create config @@ -28,7 +32,7 @@ TEST( planarSLAM, BearingFactor ) c.insert(3, l3); // Check error - Vector actual = factor.error_vector(c); + Vector actual = factor.unwhitenedError(c); CHECK(assert_equal(Vector_(1,-0.1),actual)); } @@ -37,7 +41,6 @@ TEST( planarSLAM, RangeFactor ) { // Create factor double z(sqrt(2) - 0.22); // h(x) - z = 0.22 - double sigma = 0.1; planarSLAM::Range factor(2, 3, z, sigma); // create config @@ -46,7 +49,7 @@ TEST( planarSLAM, RangeFactor ) c.insert(3, l3); // Check error - Vector actual = factor.error_vector(c); + Vector actual = factor.unwhitenedError(c); CHECK(assert_equal(Vector_(1,0.22),actual)); } @@ -66,20 +69,18 @@ TEST( planarSLAM, constructor ) G.addPoseConstraint(2, x2); // make it feasible :-) // Add odometry - G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), eye(3)); + G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3); // Create bearing factor Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1 - double sigma1 = 0.1; - G.addBearing(2, 3, z1, sigma1); + G.addBearing(2, 3, z1, sigma); // Create range factor double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 - double sigma2 = 0.1; - G.addRange(2, 3, z2, sigma2); + 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); - CHECK(assert_equal(expected,G.error_vector(c))); + CHECK(assert_equal(expected,G.unwhitenedError(c))); } /* ************************************************************************* */ diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index da9e17a38..1ee558b77 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -16,11 +16,12 @@ using namespace gtsam; // Common measurement covariance static double sx=0.5, sy=0.5,st=0.1; -static Matrix covariance = Matrix_(3,3, - sx*sx, 0.0, 0.0, - 0.0, sy*sy, 0.0, - 0.0, 0.0, st*st - ); +static noiseModel::Gaussian::shared_ptr covariance( + noiseModel::Gaussian::Covariance(Matrix_(3, 3, + sx*sx, 0.0, 0.0, + 0.0, sy*sy, 0.0, + 0.0, 0.0, st*st + ))); /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) @@ -45,14 +46,14 @@ TEST( Pose2Factor, error ) delta.insert("x1", zero(3)); delta.insert("x2", zero(3)); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.error_vector(x0))); + CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); // Check error after increasing p2 VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0)); Pose2Config x1 = expmap(x0, plus); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.error_vector(x1))); + CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); } @@ -78,7 +79,7 @@ TEST( Pose2Factor, rhs ) Pose2 hx0 = between(p1,p2); CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); - CHECK(assert_equal(expected_b,-factor.error_vector(x0))); + CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); CHECK(assert_equal(expected_b,linear->get_b())); } @@ -86,7 +87,7 @@ TEST( Pose2Factor, rhs ) // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // Hence i.e., b = approximates z-h(x0) = error_vector(x0) Vector h(const Pose2& p1,const Pose2& p2) { - return factor.evaluateError(p1,p2); + return covariance->whiten(factor.evaluateError(p1,p2)); } /* ************************************************************************* */ @@ -100,21 +101,16 @@ TEST( Pose2Factor, linearize ) x0.insert(2,p2); // expected linearization - Matrix square_root_inverse_covariance = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 10.0 - ); - Matrix expectedH1 = square_root_inverse_covariance*Matrix_(3,3, + Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, 0.0,-1.0,-2.0, 1.0, 0.0,-2.0, 0.0, 0.0,-1.0 - ); - Matrix expectedH2 = square_root_inverse_covariance*Matrix_(3,3, + )); + Matrix expectedH2 = covariance->Whiten(Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 - ); + )); Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); // expected linear factor diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp index dd7801e6d..1e44a3116 100644 --- a/cpp/testPose2Prior.cpp +++ b/cpp/testPose2Prior.cpp @@ -16,7 +16,7 @@ using namespace gtsam; // Common measurement covariance static double sx=0.5, sy=0.5,st=0.1; -static GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(Vector_(3,sx,sy,st)); +static sharedGaussian sigmas = Diagonal::Sigmas(Vector_(3,sx,sy,st)); /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) @@ -28,7 +28,7 @@ TEST( Pose2Prior, error ) x0.insert(1, p1); // Create factor - Pose2Prior factor(1, p1, model); + Pose2Prior factor(1, p1, sigmas); // Actual linearization boost::shared_ptr linear = factor.linearize(x0); @@ -37,27 +37,27 @@ TEST( Pose2Prior, error ) VectorConfig delta; delta.insert("x1", zero(3)); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.error_vector(x0))); + CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); // Check error after increasing p2 VectorConfig plus = delta + VectorConfig("x1", Vector_(3, 0.1, 0.0, 0.0)); Pose2Config x1 = expmap(x0, plus); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.error_vector(x1))); + CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); } /* ************************************************************************* */ // common Pose2Prior for tests below static Pose2 prior(2,2,M_PI_2); -static Pose2Prior factor(1,prior, model); +static Pose2Prior factor(1,prior, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // Hence i.e., b = approximates z-h(x0) = error_vector(x0) Vector h(const Pose2& p1) { - return factor.evaluateError(p1); + return sigmas->whiten(factor.evaluateError(p1)); } /* ************************************************************************* */ diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index 951011743..f8f236a14 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -23,11 +23,12 @@ using namespace gtsam; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; -static Matrix covariance = Matrix_(3,3, - sx*sx, 0.0, 0.0, - 0.0, sy*sy, 0.0, - 0.0, 0.0, st*st - ); +static noiseModel::Gaussian::shared_ptr covariance( + noiseModel::Gaussian::Covariance(Matrix_(3, 3, + sx*sx, 0.0, 0.0, + 0.0, sy*sy, 0.0, + 0.0, 0.0, st*st + ))), I3(noiseModel::Unit::Create(3)); /* ************************************************************************* */ TEST( Pose2Graph, constructor ) @@ -162,10 +163,9 @@ TEST(Pose2Graph, optimizeCircle) { // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose2Graph, findMinimumSpanningTree) { Pose2Graph G, T, C; - Matrix cov = eye(3); - G.addConstraint(1, 2, Pose2(0.,0.,0.), cov); - G.addConstraint(1, 3, Pose2(0.,0.,0.), cov); - G.addConstraint(2, 3, Pose2(0.,0.,0.), cov); + G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); + G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); + G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); PredecessorMap tree = G.findMinimumSpanningTree(); @@ -178,10 +178,9 @@ TEST(Pose2Graph, findMinimumSpanningTree) { // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose2Graph, split) { Pose2Graph G, T, C; - Matrix cov = eye(3); - G.addConstraint(1, 2, Pose2(0.,0.,0.), cov); - G.addConstraint(1, 3, Pose2(0.,0.,0.), cov); - G.addConstraint(2, 3, Pose2(0.,0.,0.), cov); + G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); + G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); + G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); PredecessorMap tree; tree.insert(1,2); diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index f0ef81dc4..7c74057f3 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -23,8 +23,8 @@ TEST( Pose3Factor, error ) Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; // Create factor - Matrix measurement_covariance = eye(6); - Pose3Factor factor(1,2, z, measurement_covariance); + sharedGaussian I6(noiseModel::Unit::Create(6)); + Pose3Factor factor(1,2, z, I6); // Create config Pose3Config x; @@ -32,7 +32,7 @@ TEST( Pose3Factor, error ) x.insert(2,t2); // Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2)) - Vector actual = factor.error_vector(x); + Vector actual = factor.unwhitenedError(x); Vector expected = logmap(z,between(t1,t2)); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testPose3SLAM.cpp b/cpp/testPose3SLAM.cpp index 7a4271edc..5920574fc 100644 --- a/cpp/testPose3SLAM.cpp +++ b/cpp/testPose3SLAM.cpp @@ -13,6 +13,8 @@ using namespace boost::assign; #include +// TODO: DANGEROUS, create shared pointers +#define GTSAM_MAGIC_GAUSSIAN 6 #define GTSAM_MAGIC_KEY #include "pose3SLAM.h" diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 0784d9233..a79d4df60 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -12,6 +12,8 @@ #include #include +// TODO: DANGEROUS, create shared pointers +#define GTSAM_MAGIC_GAUSSIAN 2 #define GTSAM_MAGIC_KEY #include diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 7eebf21e5..e8cbc8edf 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -29,6 +29,8 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); + // typedefs typedef boost::shared_ptr shared_config; typedef NonlinearFactorGraph NLGraph; @@ -102,13 +104,11 @@ NLGraph linearMapWarpGraph() { // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); - double sigma1 = 0.1; - shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1")); + shared f1(new simulated2D::Measurement(z1, sigma, "x1", "l1")); // measurement from x2 to l2 Vector z2 = Vector_(2, -4.0, 0.0); - double sigma2 = 0.1; - shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2")); + shared f2(new simulated2D::Measurement(z2, sigma, "x2", "l2")); // equality constraint between l1 and l2 list keys; keys += "l1", "l2"; @@ -262,13 +262,11 @@ pair obstacleAvoidGraph() { // measurement from x1 to x2 Vector x1x2 = Vector_(2, 5.0, 0.0); - double sigma1 = 0.1; - shared f1(new simulated2D::Odometry(x1x2, sigma1, "x1", "x2")); + shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2")); // measurement from x2 to x3 Vector x2x3 = Vector_(2, 5.0, 0.0); - double sigma2 = 0.1; - shared f2(new simulated2D::Odometry(x2x3, sigma2, "x2", "x3")); + shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3")); // create a binary inequality constraint that forces the middle point away from // the obstacle @@ -394,13 +392,11 @@ pair obstacleAvoidGraphGeneral() { // measurement from x1 to x2 Vector x1x2 = Vector_(2, 5.0, 0.0); - double sigma1 = 0.1; - shared f1(new simulated2D::Odometry(x1x2, sigma1, "x1", "x2")); + shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2")); // measurement from x2 to x3 Vector x2x3 = Vector_(2, 5.0, 0.0); - double sigma2 = 0.1; - shared f2(new simulated2D::Odometry(x2x3, sigma2, "x2", "x3")); + shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3")); double radius = 1.0; diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index aec5658ea..2c5cc30b4 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -15,13 +15,17 @@ using namespace gtsam; using namespace gtsam::visualSLAM; // make cube -Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1), - x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1); +static Point3 + x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1), + x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1); // make a realistic calibration matrix -double fov = 60; // degrees -size_t w=640,h=480; -Cal3_S2 K(fov,w,h); +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2 K(fov,w,h); + +static sharedGaussian sigma(noiseModel::Unit::Create(1)); +static shared_ptrK sK(new Cal3_S2(K)); // make cameras @@ -30,17 +34,16 @@ TEST( ProjectionFactor, error ) { // Create the factor with a measurement that is 3 pixels off in x Point2 z(323.,240.); - double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); + factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); // For the following configuration, the factor predicts 320,240 Config config; Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); Point3 l1; config.insert(1, l1); // Point should project to Point2(320.,240.) - CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->error_vector(config))); + CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config))); // Which yields an error of 3^2/2 = 4.5 DOUBLES_EQUAL(4.5,factor->error(config),1e-9); @@ -76,13 +79,12 @@ TEST( ProjectionFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); + factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); boost::shared_ptr - factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); + factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 6c5b18789..88c88ae94 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -23,6 +23,7 @@ using namespace gtsam; using namespace gtsam::visualSLAM; using namespace boost; typedef NonlinearOptimizer Optimizer; +static sharedGaussian sigma(noiseModel::Unit::Create(1)); /* ************************************************************************* */ Point3 landmark1(-1.0,-1.0, 0.0); @@ -55,7 +56,6 @@ Graph testGraph() { Point2 z23( 125,-125); Point2 z24( 125, 125); - double sigma = 1; shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); Graph g; g.add(ProjectionFactor(z11, sigma, 1, 1, sK)); diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h index 1fc27ab75..7a1d48edf 100644 --- a/cpp/visualSLAM.h +++ b/cpp/visualSLAM.h @@ -62,8 +62,11 @@ namespace gtsam { namespace visualSLAM { * @param landmarkNumber is the index of the landmark * @param K the constant calibration */ - ProjectionFactor(const Point2& z, double sigma, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K) : - z_(z), K_(K), Base(sigma, j_pose, j_landmark) {} + ProjectionFactor(const Point2& z, + const sharedGaussian& model, PoseKey j_pose, + PointKey j_landmark, const shared_ptrK& K) : + z_(z), K_(K), Base(model, j_pose, j_landmark) { + } /** * print diff --git a/myconfigure b/myconfigure index 1ebdc636b..cf903c101 100755 --- a/myconfigure +++ b/myconfigure @@ -1,4 +1,4 @@ ./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ # for maximum performance on Intel Core2 platform: -#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -ftree-vectorizer-verbose=2 -march=nocona -mtune=generic -DNDEBUG" --disable-static \ No newline at end of file +#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -ftree-vectorizer-verbose=2 -march=core2 -DNDEBUG" --disable-static \ No newline at end of file