diff --git a/.cproject b/.cproject index 5ffad9b6a..33cca43b3 100644 --- a/.cproject +++ b/.cproject @@ -302,7 +302,6 @@ make - all true true @@ -310,7 +309,6 @@ make - clean true true @@ -318,7 +316,6 @@ make - check true true @@ -326,7 +323,6 @@ make - testGaussianConditional.run true true @@ -334,7 +330,6 @@ make - testGaussianFactor.run true true @@ -342,7 +337,6 @@ make - timeGaussianFactor.run true true @@ -350,7 +344,6 @@ make - timeVectorConfig.run true true @@ -358,7 +351,6 @@ make - testVectorBTree.run true true @@ -366,7 +358,6 @@ make - testVectorMap.run true true @@ -374,7 +365,6 @@ make - testNoiseModel.run true true @@ -382,7 +372,6 @@ make - testBayesNetPreconditioner.run true true @@ -390,13 +379,12 @@ make - testErrors.run true false true - + make check @@ -404,8 +392,16 @@ true true + + make + check + true + true + true + make + check true true @@ -413,6 +409,7 @@ make + clean true true @@ -420,6 +417,7 @@ make + testBTree.run true true @@ -427,6 +425,7 @@ make + testDSF.run true true @@ -434,6 +433,7 @@ make + testDSFVector.run true true @@ -441,6 +441,7 @@ make + testMatrix.run true true @@ -448,6 +449,7 @@ make + testSPQRUtil.run true true @@ -455,6 +457,7 @@ make + testVector.run true true @@ -462,6 +465,7 @@ make + timeMatrix.run true true @@ -469,6 +473,7 @@ make + all true true @@ -476,7 +481,6 @@ make - all true true @@ -484,7 +488,6 @@ make - clean true true @@ -500,7 +503,6 @@ make - testBayesTree.run true false @@ -508,7 +510,6 @@ make - testBinaryBayesNet.run true false @@ -516,7 +517,6 @@ make - testFactorGraph.run true true @@ -524,7 +524,6 @@ make - testISAM.run true true @@ -532,7 +531,6 @@ make - testJunctionTree.run true true @@ -540,7 +538,6 @@ make - testKey.run true true @@ -548,7 +545,6 @@ make - testOrdering.run true true @@ -556,7 +552,6 @@ make - testSymbolicBayesNet.run true false @@ -564,7 +559,6 @@ make - testSymbolicFactor.run true false @@ -572,7 +566,6 @@ make - testSymbolicFactorGraph.run true false @@ -580,7 +573,6 @@ make - timeSymbolMaps.run true true @@ -588,7 +580,6 @@ make - check true true @@ -596,7 +587,6 @@ make - testClusterTree.run true true @@ -604,7 +594,6 @@ make - testJunctionTree.run true true @@ -612,7 +601,6 @@ make - testEliminationTree.run true true @@ -620,6 +608,7 @@ make + check true true @@ -627,6 +616,7 @@ make + testGaussianFactorGraph.run true true @@ -634,6 +624,7 @@ make + testGaussianISAM.run true true @@ -641,6 +632,7 @@ make + testGaussianISAM2.run true true @@ -648,6 +640,7 @@ make + testGraph.run true false @@ -655,6 +648,7 @@ make + testIterative.run true true @@ -662,6 +656,7 @@ make + testNonlinearEquality.run true true @@ -669,6 +664,7 @@ make + testNonlinearFactor.run true true @@ -676,6 +672,7 @@ make + testNonlinearFactorGraph.run true true @@ -683,6 +680,7 @@ make + testNonlinearOptimizer.run true true @@ -690,6 +688,7 @@ make + testSQP.run true true @@ -697,6 +696,7 @@ make + testSubgraphPreconditioner.run true true @@ -704,6 +704,7 @@ make + testTupleConfig.run true true @@ -711,6 +712,7 @@ make + timeGaussianFactorGraph.run true true @@ -718,6 +720,7 @@ make + testBayesNetPreconditioner.run true true @@ -725,6 +728,7 @@ make + testConstraintOptimizer.run true true @@ -732,6 +736,7 @@ make + testInference.run true false @@ -739,6 +744,7 @@ make + testGaussianBayesNet.run true false @@ -746,6 +752,7 @@ make + testGaussianFactor.run true false @@ -753,6 +760,7 @@ make + testJunctionTree.run true false @@ -760,6 +768,7 @@ make + testSymbolicBayesNet.run true false @@ -767,6 +776,7 @@ make + testSymbolicFactorGraph.run true false @@ -774,7 +784,6 @@ make - clean true true @@ -782,7 +791,6 @@ make - all true true @@ -790,7 +798,6 @@ make - testNonlinearConstraint.run true true @@ -798,7 +805,6 @@ make - testLieConfig.run true true @@ -806,7 +812,6 @@ make - testConstraintOptimizer.run true true @@ -814,7 +819,6 @@ make - install true true @@ -822,7 +826,6 @@ make - clean true true @@ -830,6 +833,7 @@ make + all true true @@ -837,6 +841,7 @@ make + clean true true @@ -844,6 +849,7 @@ make + all true true @@ -851,6 +857,7 @@ make + clean true true @@ -858,6 +865,7 @@ make + all true true @@ -865,6 +873,7 @@ make + clean true true @@ -872,7 +881,6 @@ make - all true true @@ -880,7 +888,6 @@ make - clean true true @@ -888,6 +895,7 @@ make + clean all true true @@ -895,6 +903,7 @@ make + all true true @@ -902,6 +911,7 @@ make + check true true @@ -909,6 +919,7 @@ make + clean true true @@ -916,6 +927,7 @@ make + testPlanarSLAM.run true true @@ -923,6 +935,7 @@ make + testPose2Config.run true true @@ -930,6 +943,7 @@ make + testPose2Factor.run true true @@ -937,6 +951,7 @@ make + testPose2Prior.run true true @@ -944,6 +959,7 @@ make + testPose2SLAM.run true true @@ -951,6 +967,7 @@ make + testPose3Config.run true true @@ -958,6 +975,7 @@ make + testPose3SLAM.run true true @@ -965,6 +983,7 @@ make + testSimulated2DOriented.run true false @@ -972,6 +991,7 @@ make + testVSLAMConfig.run true true @@ -979,6 +999,7 @@ make + testVSLAMFactor.run true true @@ -986,6 +1007,7 @@ make + testVSLAMGraph.run true true @@ -993,6 +1015,7 @@ make + testPose3Factor.run true true @@ -1000,6 +1023,7 @@ make + testSimulated2D.run true false @@ -1007,11 +1031,36 @@ make + testSimulated3D.run true false true + + make + + check + true + true + true + + + make + + check + true + true + true + + + make + + check + true + true + true + make -j2 @@ -1030,96 +1079,11 @@ make - clean true true true - - make - testRot3.run - true - true - true - - - make - testRot2.run - true - true - true - - - make - testPose3.run - true - true - true - - - make - timeRot3.run - true - true - true - - - make - testPose2.run - true - true - true - - - make - testCal3_S2.run - true - true - true - - - make - testSimpleCamera.run - true - true - true - - - make - testHomography2.run - true - true - true - - - make - testCalibratedCamera.run - true - true - true - - - make - check - true - true - true - - - make - clean - true - true - true - - - make - testPoint2.run - true - true - true - make -j2 @@ -1146,6 +1110,7 @@ make + all true true @@ -1153,12 +1118,85 @@ make + dist true true true - + + make + + testRot3.run + true + true + true + + + make + + testRot2.run + true + true + true + + + make + + testPose3.run + true + true + true + + + make + + timeRot3.run + true + true + true + + + make + + testPose2.run + true + true + true + + + make + + testCal3_S2.run + true + true + true + + + make + + testSimpleCamera.run + true + true + true + + + make + + testHomography2.run + true + true + true + + + make + + testCalibratedCamera.run + true + true + true + + make check @@ -1166,15 +1204,7 @@ true true - - make - - check - true - true - true - - + make clean @@ -1182,9 +1212,37 @@ true true - + make + testPoint2.run + true + true + true + + + make + check + true + true + true + + + make + check + true + true + true + + + make + clean + true + true + true + + + make install true true @@ -1192,7 +1250,6 @@ make - all true true diff --git a/nonlinear/NonlinearConstraint-inl.h b/nonlinear/NonlinearConstraint-inl.h index c480e60c3..3fe0f9e90 100644 --- a/nonlinear/NonlinearConstraint-inl.h +++ b/nonlinear/NonlinearConstraint-inl.h @@ -22,18 +22,17 @@ namespace gtsam { /* ************************************************************************* */ template -NonlinearConstraint::NonlinearConstraint(const LagrangeKey& lagrange_key, - size_t dim_lagrange, - bool isEquality) : - NonlinearFactor(noiseModel::Unit::Create(2*dim_lagrange)), - lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) { - this->keys_.push_back(lagrange_key_); +NonlinearConstraint::NonlinearConstraint(size_t dim, double mu) : + NonlinearFactor(noiseModel::Constrained::All(dim)), + mu_(fabs(mu)) +{ } /* ************************************************************************* */ template -bool NonlinearConstraint::active(const Config& config) const { - return !(!isEquality_ && greaterThanOrEqual(unwhitenedError(config), zero(p_))); +double NonlinearConstraint::error(const Config& c) const { + const Vector error_vector = unwhitenedError(c); + return mu_ * inner_prod(error_vector, error_vector); } /* ************************************************************************* */ @@ -45,10 +44,8 @@ NonlinearConstraint1::NonlinearConstraint1( Vector (*g)(const Config& config), const Key& key, Matrix (*gradG)(const Config& config), - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality) : - NonlinearConstraint(lagrange_key, dim_constraint, isEquality), + size_t dim, double mu) : + NonlinearConstraint(dim, mu), G_(boost::bind(gradG, _1)), g_(boost::bind(g, _1)), key_(key) { this->keys_.push_back(key); @@ -60,10 +57,8 @@ NonlinearConstraint1::NonlinearConstraint1( boost::function g, const Key& key, boost::function gradG, - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality) : - NonlinearConstraint(lagrange_key, dim_constraint, isEquality), + size_t dim, double mu) : + NonlinearConstraint(dim, mu), G_(gradG), g_(g), key_(key) { this->keys_.push_back(key); @@ -72,13 +67,9 @@ NonlinearConstraint1::NonlinearConstraint1( /* ************************************************************************* */ template void NonlinearConstraint1::print(const std::string& s) const { - std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->p_ << "\n" - << " Key : " << (std::string) this->key_ << "\n" - << " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n"; - if (this->isEquality_) - std::cout << " Equality Factor" << std::endl; - else - std::cout << " Inequality Factor" << std::endl; + std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->dim() + << " mu: " << this->mu_ << "\n" + << " Key : " << (std::string) this->key_ << "\n"; } /* ************************************************************************* */ @@ -87,44 +78,18 @@ bool NonlinearConstraint1::equals(const Factor& f, doubl const NonlinearConstraint1* p = dynamic_cast*> (&f); if (p == NULL) return false; if (!(key_ == p->key_)) return false; - if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false; - if (this->isEquality_ != p->isEquality_) return false; - return this->p_ == p->p_; + if (fabs(this->mu_ - p->mu_ ) > tol) return false; + return this->dim() == p->dim(); } /* ************************************************************************* */ template GaussianFactor::shared_ptr NonlinearConstraint1::linearize(const Config& config) const { - const size_t p = this->p_; - - // extract lagrange multiplier - Vector lambda = config[this->lagrange_key_]; - - // find the error - Vector g = g_(config); - - // construct the gradient + Vector g = -1.0 * g_(config); Matrix grad = G_(config); - - // construct combined factor - Matrix Ax = zeros(grad.size1()*2, grad.size2()); - insertSub(Ax, vector_scale(lambda, grad), 0, 0); - insertSub(Ax, grad, grad.size1(), 0); - - Matrix AL = eye(p*2, p); - - Vector rhs = zero(p*2); - subInsert(rhs, -1*g, p); - - // construct a mixed constraint model - Vector sigmas = zero(p*2); - subInsert(sigmas, ones(p), 0); - SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas); - - GaussianFactor::shared_ptr factor(new - GaussianFactor(key_, Ax, this->lagrange_key_, AL, rhs, mixedConstraint)); - + SharedDiagonal model = noiseModel::Constrained::All(this->dim()); + GaussianFactor::shared_ptr factor(new GaussianFactor(this->key_, grad, g, model)); return factor; } @@ -140,12 +105,10 @@ NonlinearConstraint2::NonlinearConstraint2( Matrix (*G1)(const Config& config), const Key2& key2, Matrix (*G2)(const Config& config), - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality) : - NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)), - key1_(key1), key2_(key2) + size_t dim, double mu) + : NonlinearConstraint(dim, mu), + G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)), + key1_(key1), key2_(key2) { this->keys_.push_back(key1); this->keys_.push_back(key2); @@ -159,12 +122,10 @@ NonlinearConstraint2::NonlinearConstraint2( boost::function G1, const Key2& key2, boost::function G2, - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality) : - NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - G1_(G1), G2_(G2), g_(g), - key1_(key1), key2_(key2) + size_t dim, double mu) + : NonlinearConstraint(dim, mu), + G1_(G1), G2_(G2), g_(g), + key1_(key1), key2_(key2) { this->keys_.push_back(key1); this->keys_.push_back(key2); @@ -173,14 +134,10 @@ NonlinearConstraint2::NonlinearConstraint2( /* ************************************************************************* */ template void NonlinearConstraint2::print(const std::string& s) const { - std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->p_ << "\n" + std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->dim() + << " mu: " << this->mu_ << "\n" << " Key1 : " << (std::string) this->key1_ << "\n" - << " Key2 : " << (std::string) this->key2_ << "\n" - << " Lagrange Key: " << (std::string) this->lagrange_key_ << "\n"; - if (this->isEquality_) - std::cout << " Equality Factor" << std::endl; - else - std::cout << " Inequality Factor" << std::endl; + << " Key2 : " << (std::string) this->key2_ << "\n"; } /* ************************************************************************* */ @@ -190,48 +147,19 @@ bool NonlinearConstraint2::equals(const Factorkey1_)) return false; if (!(key2_ == p->key2_)) return false; - if (!(this->lagrange_key_.equals(p->lagrange_key_))) return false; - if (this->isEquality_ != p->isEquality_) return false; - return this->p_ == p->p_; + if (fabs(this->mu_ - p->mu_ ) > tol) return false; + return this->dim() == p->dim(); } /* ************************************************************************* */ template GaussianFactor::shared_ptr NonlinearConstraint2::linearize(const Config& config) const { - const size_t p = this->p_; - // extract lagrange multiplier - Vector lambda = config[this->lagrange_key_]; - - // find the error - Vector g = g_(config); - - // construct the gradients + Vector g = -1.0 * g_(config); Matrix grad1 = G1_(config); Matrix grad2 = G2_(config); - - // create matrices - Matrix Ax1 = zeros(grad1.size1()*2, grad1.size2()), - Ax2 = zeros(grad2.size1()*2, grad2.size2()), - AL = eye(p*2, p); - - // insert matrix components - insertSub(Ax1, vector_scale(lambda, grad1), 0, 0); - insertSub(Ax1, grad1, grad1.size1(), 0); - - insertSub(Ax2, vector_scale(lambda, grad2), 0, 0); - insertSub(Ax2, grad2, grad2.size1(), 0); - - Vector rhs = zero(p*2); - subInsert(rhs, -1*g, p); - - // construct a mixed constraint model - Vector sigmas = zero(p*2); - subInsert(sigmas, ones(p), 0); - SharedDiagonal mixedConstraint = noiseModel::Constrained::MixedSigmas(sigmas); - - GaussianFactor::shared_ptr factor(new - GaussianFactor(key1_, Ax1, key2_, Ax2, this->lagrange_key_, AL, rhs, mixedConstraint)); + SharedDiagonal model = noiseModel::Constrained::All(this->dim()); + GaussianFactor::shared_ptr factor(new GaussianFactor(this->key1_, grad1, this->key2_, grad2, g, model)); return factor; } diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index b2e807c73..e9fc1f89f 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -1,7 +1,7 @@ /* * @file NonlinearConstraint.h - * @brief Implements nonlinear constraints that can be linearized and - * inserted into an existing nonlinear graph and solved via SQP + * @brief Implements nonlinear constraints that can be linearized using + * direct linearization and solving through a quadratic merit function * @author Alex Cunningham */ @@ -13,9 +13,6 @@ namespace gtsam { -/** Typedef for Lagrange key type - must be present in factors and config */ -typedef TypedSymbol LagrangeKey; - /** * Base class for nonlinear constraints * This allows for both equality and inequality constraints, @@ -23,68 +20,43 @@ typedef TypedSymbol LagrangeKey; * nonzero constraint functions will still be active - inequality * constraints should be sure to force to actual zero) * - * The measurement z in the underlying NonlinearFactor is the - * set of Lagrange multipliers. + * NOTE: inequality constraints removed for now * - * Note on NoiseModel: - * The nonlinear constraint actually uses a Unit noisemodel so that - * it is possible to have a finite error value when the constraint is - * not fulfilled. Using a constrained noisemodel will immediately cause - * infinite error and break optimization. + * 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: - /** key for the lagrange multipliers */ - LagrangeKey lagrange_key_; - - /** number of lagrange multipliers */ - size_t p_; - - /** type of constraint */ - bool isEquality_; + double mu_; // gain for quadratic merit function public: /** Constructor - sets the cost function and the lagrange multipliers - * @param lagrange_key is the label for the associated lagrange multipliers - * @param dim_lagrange is the number of associated constraints - * @param isEquality is true if the constraint is an equality constraint + * @param dim is the dimension of the factor + * @param mu is the gain used at error evaluation (forced to be positive) */ - NonlinearConstraint(const LagrangeKey& lagrange_key, - size_t dim_lagrange, - bool isEquality=true); + NonlinearConstraint(size_t dim, double mu = 1000.0); - /** returns the key used for the Lagrange multipliers */ - LagrangeKey lagrangeKey() const { return lagrange_key_; } - - /** returns the number of lagrange multipliers */ - size_t nrConstraints() const { return p_; } - - /** returns the type of constraint */ - bool isEquality() const { return isEquality_; } + /** returns the gain mu */ + double mu() const { return mu_; } /** Print */ - virtual void print(const std::string& s = "") const =0; + virtual void print(const std::string& s = "") const=0; /** Check if two factors are equal */ virtual bool equals(const Factor& f, double tol=1e-9) const=0; - /** error function - returns the result of the constraint function */ - virtual Vector unwhitenedError(const Config& c) const=0; + /** error function - returns the quadratic merit function */ + virtual double error(const Config& c) const; /** - * Determines whether the constraint is active given a particular configuration - * @param config is the input to the g(x) function - * @return true if constraint needs to be linearized - */ - bool active(const Config& config) const; - - /** - * Real linearize, given a config that includes Lagrange multipliers - * @param config is the configuration (with lagrange multipliers) + * Linearizes around a given config + * @param config is the configuration * @return a combined linear factor containing both the constraint and the constraint factor */ virtual boost::shared_ptr linearize(const Config& c) const=0; @@ -128,34 +100,30 @@ public: * @param key is the identifier for the variable constrained * @param G gives the jacobian of the constraint function * @param g is the constraint function - * @param dim_constraint is the size of the constraint (p) - * @param lagrange_key is the identifier for the lagrange multiplier - * @param isEquality is true if the constraint is an equality constraint + * @param dim is the size of the constraint (p) + * @param mu is the gain for the factor */ NonlinearConstraint1( Vector (*g)(const Config& config), const Key& key, Matrix (*G)(const Config& config), - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality=true); + size_t dim, + double mu = 1000.0); /** * Basic constructor with boost function pointers * @param key is the identifier for the variable constrained * @param G gives the jacobian of the constraint function * @param g is the constraint function as a boost function pointer - * @param dim_constraint is the size of the constraint (p) - * @param lagrange_key is the identifier for the lagrange multiplier - * @param isEquality is true if the constraint is an equality constraint + * @param dim is the size of the constraint (p) + * @param mu is the gain for the factor */ NonlinearConstraint1( boost::function g, const Key& key, boost::function G, - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality=true); + size_t dim, + double mu = 1000.0); /** Print */ void print(const std::string& s = "") const; @@ -166,9 +134,7 @@ public: /** Error function */ virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); } - /** - * Linearize from config - must have Lagrange multipliers - */ + /** Linearize from config */ virtual boost::shared_ptr linearize(const Config& c) const; }; @@ -209,9 +175,8 @@ public: * @param key is the identifier for the variable constrained * @param G gives the jacobian of the constraint function * @param g is the constraint function - * @param dim_constraint is the size of the constraint (p) - * @param lagrange_key is the identifier for the lagrange multiplier - * @param isEquality is true if the constraint is an equality constraint + * @param dim is the size of the constraint (p) + * @param mu is the gain for the factor */ NonlinearConstraint2( Vector (*g)(const Config& config), @@ -219,9 +184,8 @@ public: Matrix (*G1)(const Config& config), const Key2& key2, Matrix (*G2)(const Config& config), - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality=true); + size_t dim, + double mu = 1000.0); /** * Basic constructor with direct function objects @@ -229,9 +193,8 @@ public: * @param key is the identifier for the variable constrained * @param G gives the jacobian of the constraint function * @param g is the constraint function - * @param dim_constraint is the size of the constraint (p) - * @param lagrange_key is the identifier for the lagrange multiplier - * @param isEquality is true if the constraint is an equality constraint + * @param dim is the size of the constraint (p) + * @param mu is the gain for the factor */ NonlinearConstraint2( boost::function g, @@ -239,9 +202,8 @@ public: boost::function G1, const Key2& key2, boost::function G2, - size_t dim_constraint, - const LagrangeKey& lagrange_key, - bool isEquality=true); + size_t dim, + double mu = 1000.0); /** Print */ void print(const std::string& s = "") const; @@ -252,9 +214,7 @@ public: /** Error function */ virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); } - /** - * Linearize from config - must have Lagrange multipliers - */ + /** Linearize from config */ virtual boost::shared_ptr linearize(const Config& c) const; }; diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index 53b4b142f..a79ef33c6 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -86,6 +86,11 @@ namespace gtsam { return keys_; } + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const { + return noiseModel_->dim(); + } + /* return the begin iterator of keys */ std::list::const_iterator begin() const { return keys_.begin(); } diff --git a/nonlinear/NonlinearOptimizer.cpp b/nonlinear/NonlinearOptimizer.cpp index 119e524a1..95814f13e 100644 --- a/nonlinear/NonlinearOptimizer.cpp +++ b/nonlinear/NonlinearOptimizer.cpp @@ -5,8 +5,6 @@ * Created on: Jul 17, 2010 */ -#pragma once - #include "NonlinearOptimizer.h" namespace gtsam { diff --git a/nonlinear/tests/testNonlinearConstraint.cpp b/nonlinear/tests/testNonlinearConstraint.cpp index 50ecc94ba..fb65a7e82 100644 --- a/nonlinear/tests/testNonlinearConstraint.cpp +++ b/nonlinear/tests/testNonlinearConstraint.cpp @@ -21,8 +21,7 @@ using namespace gtsam; using namespace boost::assign; typedef TypedSymbol Key; -typedef TupleConfig2< LieConfig, - LieConfig > VecConfig; +typedef LieConfig VecConfig; typedef NonlinearConstraint1 NLC1; typedef NonlinearConstraint2 NLC2; @@ -48,15 +47,12 @@ namespace test1 { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_construction ) { // construct a constraint on x - // the lagrange multipliers will be expected on L_x1 - // and there is only one multiplier size_t p = 1; Key x1(1); list keys; keys += x1; - LagrangeKey L1(1); NLC1 c1(boost::bind(test1::g, _1, keys), x1, boost::bind(test1::G, _1, keys), - p, L1); + p, 10.0); // get a configuration to use for finding the error VecConfig config; @@ -68,7 +64,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { CHECK(assert_equal(actualVec, expectedVec, 1e-5)); double actError = c1.error(config); - double expError = 8.0; + double expError = 160.0; DOUBLES_EQUAL(expError, actError, 1e-5); } @@ -77,27 +73,22 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { size_t p = 1; Key x1(1); list keys; keys += x1; - LagrangeKey L1(1); NLC1 c1(boost::bind(test1::g, _1, keys), x1, boost::bind(test1::G, _1, keys), - p, L1); + p); - // get a configuration to use for linearization (with lagrange multipliers) + // get a configuration to use for linearization VecConfig realconfig; realconfig.insert(x1, Vector_(1, 1.0)); - realconfig.insert(L1, Vector_(1, 3.0)); // linearize the system GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig); - // verify - probabilistic component goes on top - Vector sigmas = Vector_(2, 1.0, 0.0); - SharedDiagonal mixedModel = noiseModel::Constrained::MixedSigmas(sigmas); - // stack the matrices to combine - Matrix Ax1 = Matrix_(2,1, 6.0, 2.0), - AL1 = Matrix_(2,1, 1.0, 0.0); - Vector rhs = Vector_(2, 0.0, 4.0); - GaussianFactor expectedFactor(x1, Ax1, L1, AL1, rhs, mixedModel); + // verify + SharedDiagonal model = noiseModel::Constrained::All(p); + Matrix Ax1 = Matrix_(p,1, 2.0); + Vector rhs = Vector_(p, 4.0); + GaussianFactor expectedFactor(x1, Ax1, rhs, model); CHECK(assert_equal(*linfactor, expectedFactor)); } @@ -106,12 +97,11 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { TEST( NonlinearConstraint1, unary_scalar_equal ) { Key x(0), y(1); list keys1, keys2; keys1 += x; keys2 += y; - LagrangeKey L1(1); NLC1 - c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1, true), - c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, L1), - c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, L1), - c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, L1); + c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1), + c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1), + c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2), + c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -147,17 +137,14 @@ namespace test2 { /* ************************************************************************* */ TEST( NonlinearConstraint2, binary_scalar_construction ) { // construct a constraint on x and y - // the lagrange multipliers will be expected on L_xy - // and there is only one multiplier size_t p = 1; Key x0(0), x1(1); list keys; keys += x0, x1; - LagrangeKey L1(1); NLC2 c1( boost::bind(test2::g, _1, keys), x0, boost::bind(test2::G1, _1, keys), x1, boost::bind(test2::G1, _1, keys), - p, L1); + p); // get a configuration to use for finding the error VecConfig config; @@ -176,30 +163,26 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { size_t p = 1; Key x0(0), x1(1); list keys; keys += x0, x1; - LagrangeKey L1(1); NLC2 c1( boost::bind(test2::g, _1, keys), x0, boost::bind(test2::G1, _1, keys), x1, boost::bind(test2::G2, _1, keys), - p, L1); + p); // get a configuration to use for finding the error VecConfig realconfig; realconfig.insert(x0, Vector_(1, 1.0)); realconfig.insert(x1, Vector_(1, 2.0)); - realconfig.insert(L1, Vector_(1, 3.0)); // linearize the system GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig); - // verify - probabilistic component goes on top - Matrix Ax0 = Matrix_(2,1, 6.0, 2.0), - Ax1 = Matrix_(2,1,-3.0,-1.0), - AL = Matrix_(2,1, 1.0, 0.0); - Vector rhs = Vector_(2, 0.0, 6.0), - sigmas = Vector_(2, 1.0, 0.0); - SharedDiagonal expModel = noiseModel::Constrained::MixedSigmas(sigmas); - GaussianFactor expFactor(x0,Ax0, x1, Ax1,L1, AL, rhs, expModel); + // verify + Matrix Ax0 = Matrix_(1,1, 2.0), + Ax1 = Matrix_(1,1,-1.0); + Vector rhs = Vector_(1, 6.0); + SharedDiagonal expModel = noiseModel::Constrained::All(p); + GaussianFactor expFactor(x0,Ax0, x1, Ax1,rhs, expModel); CHECK(assert_equal(expFactor, *actualFactor)); } @@ -208,12 +191,11 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) { list keys1, keys2, keys3; Key x0(0), x1(1), x2(2); keys1 += x0, x1; keys2 += x1, x0; keys3 += x0; - LagrangeKey L1(1); NLC2 - c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1), - c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, L1), - c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, L1), - c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, L1); + c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1), + c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1), + c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1), + c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -222,127 +204,120 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) { } /* ************************************************************************* */ -// Inequality tests +// Inequality tests - DISABLED /* ************************************************************************* */ -namespace inequality1 { - - /** p = 1, g(x) x^2 - 5 > 0 */ - Vector g(const VecConfig& config, const Key& key) { - double x = config[key](0); - double g = x * x - 5; - return Vector_(1, g); // return the actual cost - } - - /** p = 1, jacobianG(x) = 2*x */ - Matrix G(const VecConfig& config, const Key& key) { - double x = config[key](0); - return Matrix_(1, 1, 2 * x); - } - -} // \namespace inequality1 - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_inequality ) { - size_t p = 1; - Key x0(0); - LagrangeKey L1(1); - NLC1 c1(boost::bind(inequality1::g, _1, x0), - x0, boost::bind(inequality1::G, _1, x0), - p, L1, - false); // inequality constraint - - // get configurations to use for evaluation - VecConfig config1, config2; - config1.insert(x0, Vector_(1, 10.0)); // should be inactive - config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error - - // check error - CHECK(!c1.active(config1)); - Vector actualError2 = c1.unwhitenedError(config2); - CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); - CHECK(c1.active(config2)); -} - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_inequality_linearize ) { - size_t p = 1; - Key x0(0); - LagrangeKey L1(1); - NLC1 c1(boost::bind(inequality1::g, _1, x0), - x0, boost::bind(inequality1::G, _1, x0), - p, L1, - false); // inequality constraint - - // get configurations to use for linearization - VecConfig config1, config2; - config1.insert(x0, Vector_(1, 10.0)); // should have zero error - config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error - config1.insert(L1, Vector_(1, 3.0)); - config2.insert(L1, Vector_(1, 3.0)); - - // linearize for inactive constraint - GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1); - - // check if the factor is active - CHECK(!c1.active(config1)); - - // linearize for active constraint - GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2); - CHECK(c1.active(config2)); - - // verify - Vector sigmas = Vector_(2, 1.0, 0.0); - SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas); - GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0), - L1, Matrix_(2,1, 1.0, 0.0), - Vector_(2, 0.0, 4.0), model); - - CHECK(assert_equal(*actualFactor2, expectedFactor)); -} +//namespace inequality1 { +// +// /** p = 1, g(x) x^2 - 5 > 0 */ +// Vector g(const VecConfig& config, const Key& key) { +// double x = config[key](0); +// double g = x * x - 5; +// return Vector_(1, g); // return the actual cost +// } +// +// /** p = 1, jacobianG(x) = 2*x */ +// Matrix G(const VecConfig& config, const Key& key) { +// double x = config[key](0); +// return Matrix_(1, 1, 2 * x); +// } +// +//} // \namespace inequality1 +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint1, unary_inequality ) { +// size_t p = 1; +// Key x0(0); +// NLC1 c1(boost::bind(inequality1::g, _1, x0), +// x0, boost::bind(inequality1::G, _1, x0), +// p, false); // inequality constraint +// +// // get configurations to use for evaluation +// VecConfig config1, config2; +// config1.insert(x0, Vector_(1, 10.0)); // should be inactive +// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error +// +// // check error +// CHECK(!c1.active(config1)); +// Vector actualError2 = c1.unwhitenedError(config2); +// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); +// CHECK(c1.active(config2)); +//} +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint1, unary_inequality_linearize ) { +// size_t p = 1; +// Key x0(0); +// NLC1 c1(boost::bind(inequality1::g, _1, x0), +// x0, boost::bind(inequality1::G, _1, x0), +// p, false); // inequality constraint +// +// // get configurations to use for linearization +// VecConfig config1, config2; +// config1.insert(x0, Vector_(1, 10.0)); // should have zero error +// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error +// +// // linearize for inactive constraint +// GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1); +// +// // check if the factor is active +// CHECK(!c1.active(config1)); +// +// // linearize for active constraint +// GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2); +// CHECK(c1.active(config2)); +// +// // verify +// Vector sigmas = Vector_(2, 1.0, 0.0); +// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas); +// GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0), +// L1, Matrix_(2,1, 1.0, 0.0), +// Vector_(2, 0.0, 4.0), model); +// +// CHECK(assert_equal(*actualFactor2, expectedFactor)); +//} /* ************************************************************************* */ // Binding arbitrary functions /* ************************************************************************* */ -namespace binding1 { - - /** p = 1, g(x) x^2 - r > 0 */ - Vector g(double r, const VecConfig& config, const Key& key) { - double x = config[key](0); - double g = x * x - r; - return Vector_(1, g); // return the actual cost - } - - /** p = 1, jacobianG(x) = 2*x */ - Matrix G(double coeff, const VecConfig& config, - const Key& key) { - double x = config[key](0); - return Matrix_(1, 1, coeff * x); - } - -} // \namespace binding1 - -/* ************************************************************************* */ -TEST( NonlinearConstraint1, unary_binding ) { - size_t p = 1; - double coeff = 2; - double radius = 5; - Key x0(0); LagrangeKey L1(1); - NLC1 c1(boost::bind(binding1::g, radius, _1, x0), - x0, boost::bind(binding1::G, coeff, _1, x0), - p, L1, - false); // inequality constraint - - // get configurations to use for evaluation - VecConfig config1, config2; - config1.insert(x0, Vector_(1, 10.0)); // should have zero error - config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error - - // check error - CHECK(!c1.active(config1)); - Vector actualError2 = c1.unwhitenedError(config2); - CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); - CHECK(c1.active(config2)); -} +//namespace binding1 { +// +// /** p = 1, g(x) x^2 - r > 0 */ +// Vector g(double r, const VecConfig& config, const Key& key) { +// double x = config[key](0); +// double g = x * x - r; +// return Vector_(1, g); // return the actual cost +// } +// +// /** p = 1, jacobianG(x) = 2*x */ +// Matrix G(double coeff, const VecConfig& config, +// const Key& key) { +// double x = config[key](0); +// return Matrix_(1, 1, coeff * x); +// } +// +//} // \namespace binding1 +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint1, unary_binding ) { +// size_t p = 1; +// double coeff = 2; +// double radius = 5; +// Key x0(0); +// NLC1 c1(boost::bind(binding1::g, radius, _1, x0), +// x0, boost::bind(binding1::G, coeff, _1, x0), +// p, false); // inequality constraint +// +// // get configurations to use for evaluation +// VecConfig config1, config2; +// config1.insert(x0, Vector_(1, 10.0)); // should have zero error +// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error +// +// // check error +// CHECK(!c1.active(config1)); +// Vector actualError2 = c1.unwhitenedError(config2); +// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9))); +// CHECK(c1.active(config2)); +//} /* ************************************************************************* */ namespace binding2 { @@ -370,17 +345,15 @@ namespace binding2 { /* ************************************************************************* */ TEST( NonlinearConstraint2, binary_binding ) { // construct a constraint on x and y - // the lagrange multipliers will be expected on L_xy - // and there is only one multiplier size_t p = 1; double a = 2.0; double b = 1.0; double r = 5.0; - Key x0(0), x1(1); LagrangeKey L1(1); + Key x0(0), x1(1); NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1), x0, boost::bind(binding2::G1, a, _1, x0), x1, boost::bind(binding2::G2, b, _1), - p, L1); + p); // get a configuration to use for finding the error VecConfig config; @@ -393,6 +366,8 @@ TEST( NonlinearConstraint2, binary_binding ) { CHECK(assert_equal(actual, expected, 1e-5)); } + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/Makefile.am b/tests/Makefile.am index ca1c564c7..72ba74bb4 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -8,7 +8,7 @@ check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGrap check_PROGRAMS += testGaussianISAM testGraph check_PROGRAMS += testInference testIterative testGaussianJunctionTree check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph -check_PROGRAMS += testNonlinearOptimizer testSQP testSubgraphPreconditioner +check_PROGRAMS += testNonlinearOptimizer testNonlinearConstraint testSubgraphPreconditioner check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig if USE_LDL diff --git a/tests/testConstraintOptimizer.cpp b/tests/testConstraintOptimizer.cpp index 7f93585d6..caa8a2116 100644 --- a/tests/testConstraintOptimizer.cpp +++ b/tests/testConstraintOptimizer.cpp @@ -102,6 +102,217 @@ TEST( matrix, unconstrained_fg_ata ) { // CHECK(assert_equal(expected,x, 1e-4)); //} +SharedDiagonal probModel1 = sharedSigma(1,1.0); +SharedDiagonal probModel2 = sharedSigma(2,1.0); +SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); + +/* ********************************************************************* + * This example uses a nonlinear objective function and + * nonlinear equality constraint. The formulation is actually + * the Cholesky form that creates the full Hessian explicitly, + * which should really be avoided with our QR-based machinery. + * + * Note: the update equation used here has a fixed step size + * and gain that is rather arbitrarily chosen, and as such, + * will take a silly number of iterations. + */ +TEST (SQP, problem1_cholesky ) { + bool verbose = false; + // use a nonlinear function of f(x) = x^2+y^2 + // nonlinear equality constraint: g(x) = x^2-5-y=0 + // Lagrangian: f(x) + \lambda*g(x) + + // Symbols + Symbol x1("x1"), y1("y1"), L1("L1"); + + // state structure: [x y \lambda] + VectorConfig init, state; + init.insert(x1, Vector_(1, 1.0)); + init.insert(y1, Vector_(1, 1.0)); + init.insert(L1, Vector_(1, 1.0)); + state = init; + + if (verbose) init.print("Initial State"); + + // loop until convergence + int maxIt = 10; + for (int i = 0; i ||Ax-b||^2 + * where: + * h(x) simply returns the inputs + * z zeros(2) + * A identity + * b linearization point + */ + Matrix A = eye(2); + Vector b = Vector_(2, x, y); + GaussianFactor::shared_ptr f1( + new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1) + y1, sub(A, 0,2, 1,2), // A(:,2) + b, // rhs of f(x) + probModel2)); // arbitrary sigma + + /** create the constraint-linear factor + * Provides a mechanism to use variable gain to force the constraint + * \lambda*gradG*dx + d\lambda = zero + * formulated in matrix form as: + * [\lambda*gradG eye(1)] [dx; d\lambda] = zero + */ + Matrix gradG = Matrix_(1, 2,2*x, -1.0); + GaussianFactor::shared_ptr f2( + new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) + y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) + L1, eye(1), // dlambda term + Vector_(1, 0.0), // rhs is zero + probModel1)); // arbitrary sigma + + // create the actual constraint + // [gradG] [x; y] - g = 0 + Vector g = Vector_(1,x*x-y-5); + GaussianFactor::shared_ptr c1( + new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG + y1, sub(gradG, 0,1, 1,2), // slice second part of gradG + g, // value of constraint function + constraintModel1)); // force to constraint + + // construct graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); + fg.push_back(c1); + if (verbose) fg.print("Graph"); + + // solve + Ordering ord; + ord += x1, y1, L1; + VectorConfig delta = fg.optimize(ord); + if (verbose) delta.print("Delta"); + + // update initial estimate + VectorConfig newState = expmap(state, delta.scale(-1.0)); + + // set the state to the updated state + state = newState; + + if (verbose) state.print("Updated State"); + } + + // verify that it converges to the nearest optimal point + VectorConfig expected; + expected.insert(x1, Vector_(1, 2.12)); + expected.insert(y1, Vector_(1, -0.5)); + CHECK(assert_equal(state[x1], expected[x1], 1e-2)); + CHECK(assert_equal(state[y1], expected[y1], 1e-2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testSQP.cpp b/tests/testNonlinearConstraint.cpp similarity index 94% rename from tests/testSQP.cpp rename to tests/testNonlinearConstraint.cpp index dc4bb7ba8..944bc4faa 100644 --- a/tests/testSQP.cpp +++ b/tests/testNonlinearConstraint.cpp @@ -41,223 +41,9 @@ SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -/* ********************************************************************* - * This example uses a nonlinear objective function and - * nonlinear equality constraint. The formulation is actually - * the Cholesky form that creates the full Hessian explicitly, - * which should really be avoided with our QR-based machinery. - * - * Note: the update equation used here has a fixed step size - * and gain that is rather arbitrarily chosen, and as such, - * will take a silly number of iterations. - */ -TEST (SQP, problem1_cholesky ) { - bool verbose = false; - // use a nonlinear function of f(x) = x^2+y^2 - // nonlinear equality constraint: g(x) = x^2-5-y=0 - // Lagrangian: f(x) + \lambda*g(x) - - // Symbols - Symbol x1("x1"), y1("y1"), L1("L1"); - - // state structure: [x y \lambda] - VectorConfig init, state; - init.insert(x1, Vector_(1, 1.0)); - init.insert(y1, Vector_(1, 1.0)); - init.insert(L1, Vector_(1, 1.0)); - state = init; - - if (verbose) init.print("Initial State"); - - // loop until convergence - int maxIt = 10; - for (int i = 0; i ||Ax-b||^2 - * where: - * h(x) simply returns the inputs - * z zeros(2) - * A identity - * b linearization point - */ - Matrix A = eye(2); - Vector b = Vector_(2, x, y); - GaussianFactor::shared_ptr f1( - new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1) - y1, sub(A, 0,2, 1,2), // A(:,2) - b, // rhs of f(x) - probModel2)); // arbitrary sigma - - /** create the constraint-linear factor - * Provides a mechanism to use variable gain to force the constraint - * \lambda*gradG*dx + d\lambda = zero - * formulated in matrix form as: - * [\lambda*gradG eye(1)] [dx; d\lambda] = zero - */ - Matrix gradG = Matrix_(1, 2,2*x, -1.0); - GaussianFactor::shared_ptr f2( - new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) - y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) - L1, eye(1), // dlambda term - Vector_(1, 0.0), // rhs is zero - probModel1)); // arbitrary sigma - - // create the actual constraint - // [gradG] [x; y] - g = 0 - Vector g = Vector_(1,x*x-y-5); - GaussianFactor::shared_ptr c1( - new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG - y1, sub(gradG, 0,1, 1,2), // slice second part of gradG - g, // value of constraint function - constraintModel1)); // force to constraint - - // construct graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - fg.push_back(c1); - if (verbose) fg.print("Graph"); - - // solve - Ordering ord; - ord += x1, y1, L1; - VectorConfig delta = fg.optimize(ord); - if (verbose) delta.print("Delta"); - - // update initial estimate - VectorConfig newState = expmap(state, delta.scale(-1.0)); - - // set the state to the updated state - state = newState; - - if (verbose) state.print("Updated State"); - } - - // verify that it converges to the nearest optimal point - VectorConfig expected; - expected.insert(x1, Vector_(1, 2.12)); - expected.insert(y1, Vector_(1, -0.5)); - CHECK(assert_equal(state[x1], expected[x1], 1e-2)); - CHECK(assert_equal(state[y1], expected[y1], 1e-2)); -} - /* ********************************************************************* */ - -// Basic configs -typedef LieConfig LagrangeConfig; - // full components -typedef TupleConfig3, - LieConfig, - LieConfig > Config2D; -//typedef TupleConfig > Config2D; +typedef simulated2D::Config Config2D; typedef NonlinearFactorGraph Graph2D; typedef NonlinearEquality NLE; typedef boost::shared_ptr > shared; @@ -268,7 +54,7 @@ typedef NonlinearOptimizer Optimizer; * with two poses seeing one landmark, with each pose * constrained to a particular value */ -TEST (SQP, two_pose_truth ) { +TEST (NonlinearConstraint, two_pose_truth ) { bool verbose = false; // create a graph @@ -379,7 +165,7 @@ typedef NonlinearConstraint2< * should be the same. Note that this is a linear system, * so it will converge in one step. */ -TEST (SQP, two_pose ) { +TEST (NonlinearConstraint, two_pose ) { bool verbose = false; // create the graph @@ -407,13 +193,12 @@ TEST (SQP, two_pose ) { graph->push_back(f2); // equality constraint between l1 and l2 - LagrangeKey L1(1); list keys2; keys2 += l1, l2; boost::shared_ptr c2(new NLC2( boost::bind(sqp_test1::g, _1, keys2), l1, boost::bind(sqp_test1::G1, _1, keys2), l2, boost::bind(sqp_test1::G2, _1, keys2), - 2, L1)); + 2)); graph->push_back(c2); if (verbose) graph->print("Initial nonlinear graph with constraints"); @@ -424,7 +209,6 @@ TEST (SQP, two_pose ) { initialEstimate->insert(x2, Point2()); initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); // connect the landmarks // create state config variables and initialize them Config2D state(*initialEstimate); @@ -436,7 +220,7 @@ TEST (SQP, two_pose ) { // create an ordering Ordering ordering; - ordering += "x1", "x2", "l1", "l2", "L1"; + ordering += "x1", "x2", "l1", "l2"; // optimize linear graph to get full delta config GaussianBayesNet cbn = fg->eliminate(ordering); @@ -455,7 +239,6 @@ TEST (SQP, two_pose ) { expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); expected.insert(x2, Point2(5.0, 6.0)); - expected.insert(L1, Vector_(2, 6.0, 7.0)); CHECK(assert_equal(expected, state, 1e-5)); } @@ -474,9 +257,10 @@ using namespace boost; // typedefs for visual SLAM example typedef TypedSymbol Pose3Key; typedef TypedSymbol Point3Key; -typedef TupleConfig3, - LieConfig, - LieConfig > VConfig; +//typedef TupleConfig3, +// LieConfig, +// LieConfig > VConfig; +typedef visualSLAM::Config VConfig; typedef NonlinearFactorGraph VGraph; typedef boost::shared_ptr > shared_vf; typedef NonlinearOptimizer VOptimizer; @@ -487,7 +271,7 @@ typedef NonlinearEquality Pose3Constraint; /** * Ground truth for a visual SLAM example with stereo vision */ -TEST (SQP, stereo_truth ) { +TEST (NonlinearConstraint, stereo_truth ) { bool verbose = false; // create initial estimates @@ -555,7 +339,7 @@ TEST (SQP, stereo_truth ) { * Ground truth for a visual SLAM example with stereo vision * with some noise injected into the initial config */ -TEST (SQP, stereo_truth_noisy ) { +TEST (NonlinearConstraint, stereo_truth_noisy ) { bool verbose = false; // setting to determine how far away the noisy landmark is, @@ -696,12 +480,11 @@ boost::shared_ptr stereoExampleGraph() { // as the previous examples visualSLAM::PointKey l1(1), l2(2); list keys; keys += l1, l2; - LagrangeKey L12(12); shared_ptr c2( new VNLC2(boost::bind(sqp_stereo::g, _1, keys), l1, boost::bind(sqp_stereo::G1, _1, keys), l2, boost::bind(sqp_stereo::G2, _1, keys), - 3, L12)); + 3)); graph->push_back(c2); return graph; @@ -736,7 +519,7 @@ boost::shared_ptr stereoExampleTruthConfig() { * SQP version of the above stereo example, * with the initial case as the ground truth */ -TEST (SQP, stereo_sqp ) { +TEST (NonlinearConstraint, stereo_sqp ) { bool verbose = false; // get a graph @@ -745,11 +528,10 @@ TEST (SQP, stereo_sqp ) { // get the truth config boost::shared_ptr truthConfig = stereoExampleTruthConfig(); - truthConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); // create ordering shared_ptr ord(new Ordering()); - *ord += "x1", "x2", "l1", "l2", "L12"; + *ord += "x1", "x2", "l1", "l2"; VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); // create optimizer @@ -766,7 +548,7 @@ TEST (SQP, stereo_sqp ) { * SQP version of the above stereo example, * with noise in the initial estimate */ -TEST (SQP, stereo_sqp_noisy ) { +TEST (NonlinearConstraint, stereo_sqp_noisy ) { // get a graph boost::shared_ptr graph = stereoExampleGraph(); @@ -787,11 +569,10 @@ TEST (SQP, stereo_sqp_noisy ) { initConfig->insert(Pose3Key(2), pose2); initConfig->insert(Point3Key(1), landmark1); initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place - initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); // create ordering shared_ptr ord(new Ordering()); - *ord += "x1", "x2", "l1", "l2", "L12"; + *ord += "x1", "x2", "l1", "l2"; VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); // create optimizer @@ -809,7 +590,6 @@ TEST (SQP, stereo_sqp_noisy ) { // get the truth config boost::shared_ptr truthConfig = stereoExampleTruthConfig(); - truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0)); // check if correct CHECK(assert_equal(*truthConfig,*actual, 1e-5)); @@ -817,17 +597,9 @@ TEST (SQP, stereo_sqp_noisy ) { static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); -// typedefs -//typedef simulated2D::Config Config2D; -//typedef boost::shared_ptr shared_config; -//typedef NonlinearFactorGraph NLGraph; -//typedef boost::shared_ptr > shared; - namespace map_warp_example { typedef NonlinearConstraint1< Config2D, simulated2D::PoseKey, Point2> NLC1; -//typedef NonlinearConstraint2< -// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; } // \namespace map_warp_example /* ********************************************************************* */ @@ -881,10 +653,9 @@ boost::shared_ptr linearMapWarpGraph() { simulated2D::PointKey l1(1), l2(2); // constant constraint on x1 - LagrangeKey L1(1); shared_ptr c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), - 2, L1)); + 2)); // measurement from x1 to l1 Point2 z1(0.0, 5.0); @@ -895,12 +666,11 @@ boost::shared_ptr linearMapWarpGraph() { shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); // equality constraint between l1 and l2 - LagrangeKey L12(12); shared_ptr c2 (new NLC2( boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), - 2, L12)); + 2)); // construct the graph boost::shared_ptr graph(new Graph2D()); @@ -920,7 +690,6 @@ TEST ( SQPOptimizer, map_warp_initLam ) { // keys simulated2D::PoseKey x1(1), x2(2); simulated2D::PointKey l1(1), l2(2); - LagrangeKey L1(1), L12(12); // create an initial estimate shared_ptr initialEstimate(new Config2D); @@ -928,12 +697,10 @@ TEST ( SQPOptimizer, map_warp_initLam ) { initialEstimate->insert(l1, Point2(1.0, 6.0)); initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin - initialEstimate->insert(L12, Vector_(2, 1.0, 1.0)); - initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); // create an ordering shared_ptr ordering(new Ordering()); - *ordering += "x1", "x2", "l1", "l2", "L12", "L1"; + *ordering += "x1", "x2", "l1", "l2"; // create an optimizer Optimizer::shared_solver solver(new Optimizer::solver(ordering)); @@ -949,8 +716,6 @@ TEST ( SQPOptimizer, map_warp_initLam ) { expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); expected.insert(x2, Point2(5.0, 6.0)); - expected.insert(L1, Vector_(2, 1.0, 1.0)); - expected.insert(L12, Vector_(2, 6.0, 7.0)); CHECK(assert_equal(expected, actual)); } @@ -1107,6 +872,215 @@ TEST ( SQPOptimizer, map_warp_initLam ) { // CHECK(assert_equal(exp2, *(final.config()))); //} +/* ********************************************************************* + * Example from SQP testing: + * + * This example uses a nonlinear objective function and + * nonlinear equality constraint. The formulation is actually + * the Cholesky form that creates the full Hessian explicitly, + * which should really be avoided with our QR-based machinery. + * + * Note: the update equation used here has a fixed step size + * and gain that is rather arbitrarily chosen, and as such, + * will take a silly number of iterations. + */ +TEST (NonlinearConstraint, problem1_cholesky ) { + bool verbose = false; + // use a nonlinear function of f(x) = x^2+y^2 + // nonlinear equality constraint: g(x) = x^2-5-y=0 + // Lagrangian: f(x) + \lambda*g(x) + + // Symbols + Symbol x1("x1"), y1("y1"), L1("L1"); + + // state structure: [x y \lambda] + VectorConfig init, state; + init.insert(x1, Vector_(1, 1.0)); + init.insert(y1, Vector_(1, 1.0)); + init.insert(L1, Vector_(1, 1.0)); + state = init; + + if (verbose) init.print("Initial State"); + + // loop until convergence + int maxIt = 10; + for (int i = 0; i ||Ax-b||^2 + * where: + * h(x) simply returns the inputs + * z zeros(2) + * A identity + * b linearization point + */ + Matrix A = eye(2); + Vector b = Vector_(2, x, y); + GaussianFactor::shared_ptr f1( + new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1) + y1, sub(A, 0,2, 1,2), // A(:,2) + b, // rhs of f(x) + probModel2)); // arbitrary sigma + + /** create the constraint-linear factor + * Provides a mechanism to use variable gain to force the constraint + * \lambda*gradG*dx + d\lambda = zero + * formulated in matrix form as: + * [\lambda*gradG eye(1)] [dx; d\lambda] = zero + */ + Matrix gradG = Matrix_(1, 2,2*x, -1.0); + GaussianFactor::shared_ptr f2( + new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) + y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) + L1, eye(1), // dlambda term + Vector_(1, 0.0), // rhs is zero + probModel1)); // arbitrary sigma + + // create the actual constraint + // [gradG] [x; y] - g = 0 + Vector g = Vector_(1,x*x-y-5); + GaussianFactor::shared_ptr c1( + new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG + y1, sub(gradG, 0,1, 1,2), // slice second part of gradG + g, // value of constraint function + constraintModel1)); // force to constraint + + // construct graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); + fg.push_back(c1); + if (verbose) fg.print("Graph"); + + // solve + Ordering ord; + ord += x1, y1, L1; + VectorConfig delta = fg.optimize(ord); + if (verbose) delta.print("Delta"); + + // update initial estimate + VectorConfig newState = expmap(state, delta.scale(-1.0)); + + // set the state to the updated state + state = newState; + + if (verbose) state.print("Updated State"); + } + + // verify that it converges to the nearest optimal point + VectorConfig expected; + expected.insert(x1, Vector_(1, 2.12)); + expected.insert(y1, Vector_(1, -0.5)); + CHECK(assert_equal(state[x1], expected[x1], 1e-2)); + CHECK(assert_equal(state[y1], expected[y1], 1e-2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */