diff --git a/.cproject b/.cproject index 72285e057..bcb1b93d9 100644 --- a/.cproject +++ b/.cproject @@ -375,6 +375,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -401,7 +409,6 @@ make - tests/testBayesTree.run true false @@ -409,7 +416,6 @@ make - testBinaryBayesNet.run true false @@ -457,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -465,7 +470,6 @@ make - tests/testSymbolicFactor.run true false @@ -473,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -489,20 +492,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -529,6 +523,7 @@ make + testGraph.run true false @@ -600,6 +595,7 @@ make + testInference.run true false @@ -607,6 +603,7 @@ make + testGaussianFactor.run true false @@ -614,6 +611,7 @@ make + testJunctionTree.run true false @@ -621,6 +619,7 @@ make + testSymbolicBayesNet.run true false @@ -628,6 +627,7 @@ make + testSymbolicFactorGraph.run true false @@ -681,22 +681,6 @@ true true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -713,6 +697,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -737,15 +737,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -753,14 +745,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -801,7 +785,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -809,6 +801,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1025,6 +1025,22 @@ true true + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testNonlinearEqualityConstraint.run + true + true + true + make -j2 @@ -1123,7 +1139,6 @@ make - testErrors.run true false @@ -1531,6 +1546,7 @@ make + testSimulated2DOriented.run true false @@ -1570,6 +1586,7 @@ make + testSimulated2D.run true false @@ -1577,6 +1594,7 @@ make + testSimulated3D.run true false @@ -1824,6 +1842,7 @@ make + tests/testGaussianISAM2 true false @@ -1845,46 +1864,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -1981,6 +1960,62 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + make -j2 @@ -2021,22 +2056,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index cde1ba2ef..c20521c95 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -500,27 +500,43 @@ Matrix collect(size_t nrMatrices, ...) /* ************************************************************************* */ // row scaling, in-place -void vector_scale_inplace(const Vector& v, Matrix& A) { +void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { const size_t m = A.rows(); - for (size_t i=0; i::infinity(), 4.); + + Matrix actual = vector_scale(v, A, true); + + Matrix expected(3, 4); + expected(0, 0) = 2.; + expected(0, 1) = 2.; + expected(0, 2) = 2.; + expected(0, 3) = 2.; + expected(1, 0) = 1.; + expected(1, 1) = 1.; + expected(1, 2) = 1.; + expected(1, 3) = 1.; + expected(2, 0) = 4.; + expected(2, 1) = 4.; + expected(2, 2) = 4.; + expected(2, 3) = 4.; + + EXPECT(assert_equal(actual, expected)); +} + /* ************************************************************************* */ TEST( matrix, equal ) { diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index f8a0aebe0..44a2f1417 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -291,13 +291,35 @@ Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& /* ************************************************************************* */ void Constrained::print(const std::string& name) const { gtsam::print(sigmas_, name + ": constrained sigmas"); + gtsam::print(mu_, name + ": constrained mu"); } /* ************************************************************************* */ Vector Constrained::whiten(const Vector& v) const { // ediv_ does the right thing with the errors - // FIXME: solve this more effectively - return ediv_(v, sigmas_); +// return ediv_(v, sigmas_); + const Vector& a = v; + const Vector& b = sigmas_; + size_t n = a.size(); + assert (b.size()==a.size()); + Vector c(n); + for( size_t i = 0; i < n; i++ ) { + const double& ai = a(i), &bi = b(i); + c(i) = (bi==0.0) ? ai : ai/bi; + } + return c; +} + +/* ************************************************************************* */ +double Constrained::distance(const Vector& v) const { + Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements + for (size_t i=0; isigmas_(i) == 0.0) + sigmas(i) = 0.0; + return MixedSigmas(mu_, sigmas); } /* ************************************************************************* */ @@ -401,7 +432,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { } toc(4, "constrained_QR write back into Ab"); - return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions); + // Must include mus, as the defaults might be higher, resulting in non-convergence + return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 3943fbb75..7eb0116d1 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -60,7 +60,7 @@ namespace gtsam { /// Dimensionality inline size_t dim() const { return dim_;} - virtual void print(const std::string& name) const = 0; + virtual void print(const std::string& name = "") const = 0; virtual bool equals(const Base& expected, double tol=1e-9) const = 0; @@ -327,8 +327,11 @@ namespace gtsam { * 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. - * FIXME: the "otherwise return infinity" does not solve anything + * components that have zero sigma *and* zero error, unchanged otherwise. + * + * While a hard constraint may seem to be a case in which there is infinite error, + * we do not ever produce an error value of infinity to allow for constraints + * to actually be optimized rather than self-destructing if not initialized correctly. * * The distance function in this function provides an error model * for a penalty function with a scaling function, assuming a mask of @@ -387,8 +390,10 @@ namespace gtsam { /** * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero - * TODO: allow for mu */ + static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) { + return shared_ptr(new Constrained(mu, esqrt(variances))); + } static shared_ptr MixedVariances(const Vector& variances) { return shared_ptr(new Constrained(esqrt(variances))); } @@ -398,6 +403,9 @@ namespace gtsam { * precisions, some of which might be inf * TODO: allow for mu */ + static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) { + return MixedVariances(mu, reciprocal(precisions)); + } static shared_ptr MixedPrecisions(const Vector& precisions) { return MixedVariances(reciprocal(precisions)); } @@ -429,9 +437,8 @@ namespace gtsam { /// Calculates error vector with weights applied virtual Vector whiten(const Vector& v) const; - // Whitening Jacobians does not make sense for possibly constrained - // noise model and will throw an exception. - // FIXME: change to allow for use a normal linearization function + /// Whitening functions will perform partial whitening on rows + /// with a non-zero sigma. Other rows remain untouched. virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; @@ -446,6 +453,12 @@ namespace gtsam { */ virtual bool isConstrained() const {return true;} + /** + * Returns a Unit version of a constrained noisemodel in which + * constrained sigmas remain constrained and the rest are unit scaled + */ + shared_ptr unit() const; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 649e4a53f..393f62eab 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -187,7 +187,8 @@ TEST(NoiseModel, ConstrainedMixed ) Vector feasible = Vector_(3, 1.0, 0.0, 1.0), infeasible = Vector_(3, 1.0, 1.0, 1.0); Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma)); - EXPECT(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible))); + // NOTE: we catch constrained variables elsewhere, so whitening does nothing + EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible))); DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); @@ -201,7 +202,8 @@ TEST(NoiseModel, ConstrainedAll ) infeasible = Vector_(3, 1.0, 1.0, 1.0); Constrained::shared_ptr i = Constrained::All(3); - EXPECT(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible))); + // NOTE: we catch constrained variables elsewhere, so whitening does nothing + EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible))); EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible))); DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index 9478d808d..04b0ebe20 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -11,8 +11,7 @@ /* * @file NonlinearConstraint.h - * @brief Implements nonlinear constraints that can be linearized using - * direct linearization and solving through a quadratic merit function + * @brief Implements simple cases of constraints * @author Alex Cunningham */ @@ -22,566 +21,48 @@ namespace gtsam { -/** - * Base class for nonlinear constraints - * This allows for both equality and inequality constraints, - * where equality constraints are active all the time (even slightly - * nonzero constraint functions will still be active - inequality - * constraints should be sure to force to actual zero) - * - * Nonlinear constraints evaluate their error as a part of a quadratic - * error function: ||h(x)-z||^2 + mu * ||c(x)|| where mu is a gain - * on the constraint function that should be made high enough to be - * significant - */ -template -class NonlinearConstraint : public NonlinearFactor { - -protected: - typedef NonlinearConstraint This; - typedef NonlinearFactor Base; - typedef Factor BaseFactor; - - /** default constructor to allow for serialization */ - NonlinearConstraint() {} - - double mu_; /// gain for quadratic merit function - size_t dim_; /// dimension of the constraint - -public: - - /** 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) - */ - template - NonlinearConstraint(const boost::tuples::cons& keys, size_t dim, double mu = 1000.0) - : Base(keys), mu_(fabs(mu)), dim_(dim) { - } - - /** - * Constructor - * @param keys The variables involved in this factor - */ - template - NonlinearConstraint(ITERATOR beginKeys, ITERATOR endKeys, size_t dim, double mu = 1000.0) - : Base(beginKeys, endKeys), mu_(fabs(mu)), dim_(dim) { - } - - virtual ~NonlinearConstraint() {} - - /** returns the gain mu */ - double mu() const { return mu_; } - - /** get the dimension of the factor (number of rows on linearization) */ - virtual size_t dim() const { - return dim_; - } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << "NonlinearConstraint " << s << " 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 BaseFactor::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol) && dim_ == p->dim_; - } - - /** error function - returns the quadratic merit function */ - virtual double error(const VALUES& c) const { - if (active(c)) - return mu_ * unwhitenedError(c).squaredNorm(); - else - return 0.0; - } - - /** - * active set check, defines what type of constraint this is - * - * In an inequality/bounding constraint, this active() returns true - * when the constraint is *NOT* fulfilled. - * @return true if the constraint is active - */ - virtual bool active(const VALUES& c) const=0; - - /** - * Error function \f$ z-c(x) \f$. - * Override this method to finish implementing an N-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). - */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const = 0; - - /** - * Linearizes around a given config - * @param config is the values structure - * @return a combined linear factor containing both the constraint and the constraint factor - */ - virtual boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const { - if (!active(c)) - return boost::shared_ptr(); - - // Create the set of terms - Jacobians for each index - Vector b; - // Call evaluate error to get Jacobians and b vector - std::vector A(this->size()); - b = -unwhitenedError(c, A); - - std::vector > terms(this->size()); - // Fill in terms - for(size_t j=0; jsize(); ++j) { - terms[j].first = ordering[this->keys()[j]]; - terms[j].second.swap(A[j]); - } - - return GaussianFactor::shared_ptr( - new JacobianFactor(terms, b, noiseModel::Constrained::All(dim_))); - } - -private: - - /** 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(mu_); - ar & BOOST_SERIALIZATION_NVP(dim_); - } -}; - -/* ************************************************************************* */ -/** - * A unary constraint that defaults to an equality constraint - */ -template -class NonlinearConstraint1 : public NonlinearConstraint { - -public: - typedef typename KEY::Value X; - -protected: - typedef NonlinearConstraint1 This; - typedef NonlinearConstraint Base; - - /** default constructor to allow for serialization */ - NonlinearConstraint1() {} - - /** key for the constrained variable */ - KEY key_; - -public: - - /** - * Basic constructor - * @param key is the identifier for the variable constrained - * @param dim is the size of the constraint (p) - * @param mu is the gain for the factor - */ - NonlinearConstraint1(const KEY& key, size_t dim, double mu = 1000.0) - : Base(make_tuple(key), dim, mu), key_(key) { } - virtual ~NonlinearConstraint1() {} - - /** 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()); - } - } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearConstraint(" - << (std::string) this->key_ << ")," << - " mu = " << this->mu_ << - " dim = " << this->dim_ << std::endl; - } - - /** - * 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("NonlinearConstraint", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key_); - } -}; - -/* ************************************************************************* */ -/** - * Unary Equality constraint - simply forces the value of active() to true - */ -template -class NonlinearEqualityConstraint1 : public NonlinearConstraint1 { - -public: - typedef typename KEY::Value X; - -protected: - typedef NonlinearEqualityConstraint1 This; - typedef NonlinearConstraint1 Base; - - /** default constructor to allow for serialization */ - NonlinearEqualityConstraint1() {} - -public: - NonlinearEqualityConstraint1(const KEY& key, size_t dim, double mu = 1000.0) - : Base(key, dim, mu) {} - virtual ~NonlinearEqualityConstraint1() {} - - /** Always active, so fixed value for active() */ - virtual bool active(const VALUES& c) const { return true; } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearEqualityConstraint(" - << (std::string) this->key_ << ")," << - " mu = " << this->mu_ << - " dim = " << this->dim_ << std::endl; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearConstraint1", - boost::serialization::base_object(*this)); - } - -}; - -/* ************************************************************************* */ -/** - * A binary constraint with arbitrary cost and jacobian functions - */ -template -class NonlinearConstraint2 : public NonlinearConstraint { - -public: - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - -protected: - typedef NonlinearConstraint2 This; - typedef NonlinearConstraint Base; - - /** default constructor to allow for serialization */ - NonlinearConstraint2() {} - - /** keys for the constrained variables */ - KEY1 key1_; - KEY2 key2_; - -public: - - /** - * Basic constructor - * @param key1 is the identifier for the first variable constrained - * @param key2 is the identifier for the second variable constrained - * @param dim is the size of the constraint (p) - * @param mu is the gain for the factor - */ - NonlinearConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0) : - Base(make_tuple(key1, key2), dim, mu), key1_(key1), key2_(key2) { } - virtual ~NonlinearConstraint2() {} - - /** 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()); - } - } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearConstraint(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")," << - " mu = " << this->mu_ << - " dim = " << this->dim_ << std::endl; - } - - /** - * 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("NonlinearConstraint", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - } -}; - -/* ************************************************************************* */ -/** - * Binary Equality constraint - simply forces the value of active() to true - */ -template -class NonlinearEqualityConstraint2 : public NonlinearConstraint2 { - -public: - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - -protected: - typedef NonlinearEqualityConstraint2 This; - typedef NonlinearConstraint2 Base; - - /** default constructor to allow for serialization */ - NonlinearEqualityConstraint2() {} - -public: - NonlinearEqualityConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0) - : Base(key1, key2, dim, mu) {} - virtual ~NonlinearEqualityConstraint2() {} - - /** Always active, so fixed value for active() */ - virtual bool active(const VALUES& c) const { return true; } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearEqualityConstraint(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")," << - " mu = " << this->mu_ << - " dim = " << this->dim_ << std::endl; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearConstraint2", - boost::serialization::base_object(*this)); - } -}; - -/* ************************************************************************* */ -/** - * A ternary constraint - */ -template -class NonlinearConstraint3 : public NonlinearConstraint { - -public: - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - -protected: - typedef NonlinearConstraint3 This; - typedef NonlinearConstraint Base; - - /** default constructor to allow for serialization */ - NonlinearConstraint3() {} - - /** keys for the constrained variables */ - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - -public: - - /** - * Basic constructor - * @param key1 is the identifier for the first variable constrained - * @param key2 is the identifier for the second variable constrained - * @param key3 is the identifier for the second variable constrained - * @param dim is the size of the constraint (p) - * @param mu is the gain for the factor - */ - NonlinearConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3, - size_t dim, double mu = 1000.0) : - Base(make_tuple(key1, key2, key3), dim, mu), key1_(key1), key2_(key2), key3_(key3) { } - virtual ~NonlinearConstraint3() {} - - /** 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()); - } - } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearConstraint(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << ")," << - " mu = " << this->mu_ << - " dim = " << this->dim_ << std::endl; - } - - /** - * 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("NonlinearConstraint", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - } -}; - -/* ************************************************************************* */ -/** - * Ternary Equality constraint - simply forces the value of active() to true - */ -template -class NonlinearEqualityConstraint3 : public NonlinearConstraint3 { - -public: - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - -protected: - typedef NonlinearEqualityConstraint3 This; - typedef NonlinearConstraint3 Base; - - /** default constructor to allow for serialization */ - NonlinearEqualityConstraint3() {} - -public: - NonlinearEqualityConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3, - size_t dim, double mu = 1000.0) - : Base(key1, key2, key3, dim, mu) {} - virtual ~NonlinearEqualityConstraint3() {} - - /** Always active, so fixed value for active() */ - virtual bool active(const VALUES& c) const { return true; } - - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearEqualityConstraint(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << ")," << - " mu = " << this->mu_ << - " dim = " << this->dim_ << std::endl; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearConstraint3", - boost::serialization::base_object(*this)); - } -}; - /* ************************************************************************* */ /** * Simple unary equality constraint - fixes a value for a variable */ template -class NonlinearEquality1 : public NonlinearEqualityConstraint1 { +class NonlinearEquality1 : public NonlinearFactor1 { public: typedef typename KEY::Value X; protected: - typedef NonlinearEqualityConstraint1 Base; + typedef NonlinearFactor1 Base; /** default constructor to allow for serialization */ NonlinearEquality1() {} X value_; /// fixed value for variable + GTSAM_CONCEPT_MANIFOLD_TYPE(X); + GTSAM_CONCEPT_TESTABLE_TYPE(X); + public: typedef boost::shared_ptr > shared_ptr; NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) - : Base(key1, X::Dim(), mu), value_(value) {} + : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} + virtual ~NonlinearEquality1() {} /** g(x) with optional derivative */ - Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { - const size_t p = X::Dim(); - if (H1) *H1 = eye(p); + Vector evaluateError(const X& x1, boost::optional H = boost::none) const { + if (H) (*H) = eye(x1.dim()); + // manifold equivalent of h(x)-z -> log(z,h(x)) return value_.localCoordinates(x1); } /** Print */ virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearEquality(" - << (std::string) this->key_ << "),"<< - " mu = " << this->mu_ << - " dim = " << this->dim_ << "\n"; + std::cout << s << ": NonlinearEquality1(" + << (std::string) this->key_ << "),"<< "\n"; + this->noiseModel_->print(); value_.print("Value"); } @@ -591,7 +72,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearEqualityConstraint1", + ar & boost::serialization::make_nvp("NonlinearFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(value_); } @@ -600,15 +81,17 @@ private: /* ************************************************************************* */ /** * Simple binary equality constraint - this constraint forces two factors to - * be the same. This constraint requires the underlying type to a Lie type + * be the same. */ template -class NonlinearEquality2 : public NonlinearEqualityConstraint2 { +class NonlinearEquality2 : public NonlinearFactor2 { public: typedef typename KEY::Value X; protected: - typedef NonlinearEqualityConstraint2 Base; + typedef NonlinearFactor2 Base; + + GTSAM_CONCEPT_MANIFOLD_TYPE(X); /** default constructor to allow for serialization */ NonlinearEquality2() {} @@ -618,7 +101,7 @@ public: typedef boost::shared_ptr > shared_ptr; NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) - : Base(key1, key2, X::Dim(), mu) {} + : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} virtual ~NonlinearEquality2() {} /** g(x) with optional derivative2 */ @@ -637,7 +120,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2", + ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); } }; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 2b7ac2e7f..84fdb94a5 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -101,7 +101,8 @@ namespace gtsam { /** Check if two factors are equal */ bool equals(const NonlinearEquality& f, double tol = 1e-9) const { if (!Base::equals(f)) return false; - return compare_(feasible_, f.feasible_); + return feasible_.equals(f.feasible_, tol) && + fabs(error_gain_ - f.error_gain_) < tol; } /** actual error function calculation */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 68f0189e7..ad22d3e41 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -112,6 +112,18 @@ public: /** get the dimension of the factor (number of rows on linearization) */ virtual size_t dim() const = 0; + /** + * Checks whether a factor should be used based on a set of values. + * This is primarily used to implment inequality constraints that + * require a variable active set. For all others, the default implementation + * returning true solves this problem. + * + * In an inequality/bounding constraint, this active() returns true + * when the constraint is *NOT* fulfilled. + * @return true if the constraint is active + */ + virtual bool active(const VALUES& c) const { return true; } + /** linearize to a GaussianFactor */ virtual boost::shared_ptr linearize(const VALUES& c, const Ordering& ordering) const = 0; @@ -138,7 +150,7 @@ public: * 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. + * The noise model is typically Gaussian, but robust and constrained error models are also supported. */ template class NoiseModelFactor: public NonlinearFactor { @@ -237,7 +249,10 @@ public: * 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)); + if (active(c)) + return 0.5 * noiseModel_->distance(unwhitenedError(c)); + else + return 0.0; } /** @@ -246,6 +261,9 @@ public: * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { + // Only linearize if the factor is active + if (!active(x)) + return boost::shared_ptr(); // Create the set of terms - Jacobians for each index Vector b; @@ -253,11 +271,7 @@ public: 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); + this->noiseModel_->WhitenSystem(A,b); std::vector > terms(this->size()); // Fill in terms @@ -266,9 +280,12 @@ public: terms[j].second.swap(A[j]); } + // TODO pass unwhitened + noise model to Gaussian factor + noiseModel::Constrained::shared_ptr constrained = + boost::shared_dynamic_cast(this->noiseModel_); if(constrained) return GaussianFactor::shared_ptr( - new JacobianFactor(terms, b, constrained)); + new JacobianFactor(terms, b, constrained->unit())); else return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size()))); @@ -328,10 +345,16 @@ public: /** 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_]); + 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()); + } } /** Print */ @@ -406,10 +429,17 @@ public: /** 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_]); + 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()); + } } /** Print */ @@ -491,10 +521,14 @@ public: /** 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_]); + if(this->active(x)) { + 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_]); + } else { + return zero(this->dim()); + } } /** Print */ @@ -585,10 +619,14 @@ public: /** 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_]); + if(this->active(x)) { + 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_]); + } else { + return zero(this->dim()); + } } /** Print */ @@ -685,10 +723,14 @@ public: /** 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_]); + if(this->active(x)) { + 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_]); + } else { + return zero(this->dim()); + } } /** Print */ @@ -792,10 +834,14 @@ public: /** 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_]); + if(this->active(x)) { + 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_]); + } else { + return zero(this->dim()); + } } /** Print */ diff --git a/gtsam/slam/BetweenConstraint.h b/gtsam/slam/BetweenConstraint.h deleted file mode 100644 index e4085f0dc..000000000 --- a/gtsam/slam/BetweenConstraint.h +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file BetweenConstraint.h - * @brief Implements a constraint to force a between - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** - * Binary between constraint - forces between to a given value - * This constraint requires the underlying type to a Lie type - */ - template - class BetweenConstraint : public NonlinearEqualityConstraint2 { - public: - typedef typename KEY::Value X; - - protected: - typedef NonlinearEqualityConstraint2 Base; - - X measured_; /// fixed between value - - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(X) - - public: - - typedef boost::shared_ptr > shared_ptr; - - BetweenConstraint(const X& measured, const KEY& key1, const KEY& key2, double mu = 1000.0) - : Base(key1, key2, X::Dim(), mu), measured_(measured) {} - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - X hx = x1.between(x2, H1, H2); - return measured_.localCoordinates(hx); - } - - inline const X measured() const { - return measured_; - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; - -} // \namespace gtsam diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index c12b65d50..dee18ebc7 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -40,6 +40,7 @@ namespace gtsam { T measured_; /** The measurement */ /** concept check by type */ + GTSAM_CONCEPT_LIE_TYPE(T) GTSAM_CONCEPT_TESTABLE_TYPE(T) public: @@ -106,6 +107,33 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } - }; + }; // \class BetweenFactor + + /** + * Binary between constraint - forces between to a given value + * This constraint requires the underlying type to a Lie type + * + */ + template + class BetweenConstraint : public BetweenFactor { + public: + typedef boost::shared_ptr > shared_ptr; + + /** Syntactic sugar for constrained version */ + BetweenConstraint(const typename KEY::Value& measured, + const KEY& key1, const KEY& key2, double mu = 1000.0) + : BetweenFactor(key1, key2, measured, + noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("BetweenFactor", + boost::serialization::base_object >(*this)); + } + }; // \class BetweenConstraint } /// namespace gtsam diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 17129f962..289f0667c 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -29,9 +29,9 @@ namespace gtsam { * a scalar for comparison. */ template -struct BoundingConstraint1: public NonlinearConstraint1 { +struct BoundingConstraint1: public NonlinearFactor1 { typedef typename KEY::Value X; - typedef NonlinearConstraint1 Base; + typedef NonlinearFactor1 Base; typedef boost::shared_ptr > shared_ptr; double threshold_; @@ -39,7 +39,8 @@ struct BoundingConstraint1: public NonlinearConstraint1 { BoundingConstraint1(const KEY& key, double threshold, bool isGreaterThan, double mu = 1000.0) : - Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) { + Base(noiseModel::Constrained::All(1, mu), key), + threshold_(threshold), isGreaterThan_(isGreaterThan) { } virtual ~BoundingConstraint1() {} @@ -83,7 +84,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearConstraint1", + ar & boost::serialization::make_nvp("NonlinearFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); @@ -95,11 +96,11 @@ private: * to implement for specific systems */ template -struct BoundingConstraint2: public NonlinearConstraint2 { +struct BoundingConstraint2: public NonlinearFactor2 { typedef typename KEY1::Value X1; typedef typename KEY2::Value X2; - typedef NonlinearConstraint2 Base; + typedef NonlinearFactor2 Base; typedef boost::shared_ptr > shared_ptr; double threshold_; @@ -107,7 +108,8 @@ struct BoundingConstraint2: public NonlinearConstraint2 { BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold, bool isGreaterThan, double mu = 1000.0) - : Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {} + : Base(noiseModel::Constrained::All(1, mu), key1, key2), + threshold_(threshold), isGreaterThan_(isGreaterThan) {} virtual ~BoundingConstraint2() {} @@ -155,7 +157,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearConstraint2", + ar & boost::serialization::make_nvp("NonlinearFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 7ed3543f1..406431edd 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -40,7 +40,7 @@ headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h # Generic constraint headers -headers += BetweenConstraint.h BoundingConstraint.h +headers += BoundingConstraint.h # 2D Pose SLAM sources += pose2SLAM.cpp dataset.cpp diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 39e8e5452..04dbd0231 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 74caa32c1..8bad3cb2a 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -96,8 +96,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol)); EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol)); EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol)); - EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol); - EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol); + EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); + EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol); } /* ************************************************************************* */ @@ -111,8 +111,8 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol)); EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol)); EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol)); - EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol); - EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol); + EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); + EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } /* ************************************************************************* */ @@ -224,7 +224,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1))); - EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol); + EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp index d913aa0f6..f9966695b 100644 --- a/tests/testNonlinearEqualityConstraint.cpp +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -56,7 +56,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { EXPECT(constraint.active(config2)); EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol)); EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol)); - EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); + EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); } /* ************************************************************************* */ @@ -88,7 +88,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { // ensure that the hard constraint overrides the soft constraint Point2 truth_pt(1.0, 2.0); simulated2D::PoseKey key(1); - double mu = 1000.0; + double mu = 10.0; eq2D::UnaryEqualityConstraint::shared_ptr constraint( new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); @@ -103,10 +103,16 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { shared_values initValues(new simulated2D::Values()); initValues->insert(key, badPt); - Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); + // verify error values + EXPECT(constraint->active(*initValues)); + simulated2D::Values expected; expected.insert(key, truth_pt); - CHECK(assert_equal(expected, *actual, tol)); + EXPECT(constraint->active(expected)); + EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); + + Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); + EXPECT(assert_equal(expected, *actual, tol)); } /* ************************************************************************* */ @@ -132,7 +138,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { EXPECT(constraint.active(config2)); EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol)); - EXPECT_DOUBLES_EQUAL(2000.0, constraint.error(config2), tol); + EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 3ea0c10ba..01d2920ad 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -209,7 +209,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) // create expected Ordering ord(*config.orderingArbitrary()); Vector b = Vector_(2, 0., -3.); - JacobianFactor expected(ord["x1"], eye(2), b, constraint); + JacobianFactor expected(ord["x1"], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -229,8 +229,9 @@ TEST( NonlinearFactor, linearize_constraint2 ) // create expected Ordering ord(*config.orderingArbitrary()); - Vector b = Vector_(2, -3., -3.); - JacobianFactor expected(ord["x1"], -1*eye(2), ord["l1"], eye(2), b, constraint); + Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); + Vector b = Vector_(2, -15., -3.); + JacobianFactor expected(ord["x1"], -1*A, ord["l1"], A, b, constraint); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); }