diff --git a/.cproject b/.cproject index 83a6da8a5..2f25f98b7 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + install true true @@ -307,6 +308,7 @@ make + check true true @@ -322,6 +324,7 @@ make + testSimpleCamera.run true true @@ -337,7 +340,6 @@ make - testVSLAMFactor.run true true @@ -345,6 +347,7 @@ make + testCalibratedCamera.run true true @@ -352,7 +355,6 @@ make - testGaussianConditional.run true true @@ -360,6 +362,7 @@ make + testPose2.run true true @@ -367,6 +370,7 @@ make + testRot3.run true true @@ -374,7 +378,6 @@ make - testNonlinearOptimizer.run true true @@ -382,6 +385,7 @@ make + testGaussianFactor.run true true @@ -389,6 +393,7 @@ make + testGaussianFactorGraph.run true true @@ -396,7 +401,6 @@ make - testNonlinearFactorGraph.run true true @@ -404,6 +408,7 @@ make + testPose3.run true true @@ -411,7 +416,6 @@ make - testVectorConfig.run true true @@ -419,7 +423,6 @@ make - testPoint2.run true true @@ -427,6 +430,7 @@ make + testNonlinearFactor.run true true @@ -434,6 +438,7 @@ make + timeGaussianFactor.run true true @@ -441,6 +446,7 @@ make + timeGaussianFactorGraph.run true true @@ -448,6 +454,7 @@ make + testGaussianBayesNet.run true true @@ -455,7 +462,6 @@ make - testBayesTree.run true false @@ -463,6 +469,7 @@ make + testSymbolicBayesNet.run true false @@ -470,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -478,6 +484,7 @@ make + testVector.run true true @@ -485,6 +492,7 @@ make + testMatrix.run true true @@ -492,7 +500,6 @@ make - testVSLAMGraph.run true true @@ -500,7 +507,6 @@ make - testNonlinearEquality.run true true @@ -508,6 +514,7 @@ make + testSQP.run true true @@ -515,7 +522,6 @@ make - testNonlinearConstraint.run true true @@ -523,6 +529,7 @@ make + testSQPOptimizer.run true true @@ -530,6 +537,7 @@ make + testVSLAMConfig.run true true @@ -537,6 +545,7 @@ make + testControlConfig.run true true @@ -544,6 +553,7 @@ make + testControlPoint.run true true @@ -551,15 +561,21 @@ make - testControlGraph.run true true true + +make + +testOrdering.run +true +true +true + make - install true true @@ -567,7 +583,6 @@ make - clean true true @@ -575,7 +590,6 @@ make - check true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f282a8d1c..ab1f474dc 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -64,13 +64,15 @@ example = smallExample.cpp headers += inference.h inference-inl.h headers += FactorGraph.h FactorGraph-inl.h headers += BayesNet.h BayesNet-inl.h BayesTree.h BayesTree-inl.h -check_PROGRAMS += testFactorgraph testBayesTree testInference +check_PROGRAMS += testFactorgraph testBayesTree testInference testOrdering testFactorgraph_SOURCES = testFactorgraph.cpp testBayesTree_SOURCES = $(example) testBayesTree.cpp testInference_SOURCES = $(example) testInference.cpp testFactorgraph_LDADD = libgtsam.la testBayesTree_LDADD = libgtsam.la testInference_LDADD = libgtsam.la +testOrdering_SOURCES = testOrdering.cpp +testOrdering_LDADD = libgtsam.la # Symbolic Inference headers += SymbolicConditional.h diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 88fb526ee..f7ef45f96 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -11,6 +11,15 @@ namespace gtsam { +/* ************************************************************************* */ +template +bool NonlinearConstraint::active(const Config& config) const { + if (!isEquality_ && zero(error_vector(config))) + return false; + else + return true; +} + /* ************************************************************************* */ // Implementations of unary nonlinear constraints /* ************************************************************************* */ @@ -21,8 +30,9 @@ NonlinearConstraint1::NonlinearConstraint1( Matrix (*gradG)(const Config& config, const std::string& key), Vector (*g)(const Config& config, const std::string& key), size_t dim_constraint, - const std::string& lagrange_key) : - NonlinearConstraint(lagrange_key, dim_constraint), + const std::string& lagrange_key, + bool isEquality) : + NonlinearConstraint(lagrange_key, dim_constraint, isEquality), g_(g), gradG_(gradG), key_(key) { // set a good lagrange key here // TODO:should do something smart to find a unique one @@ -36,7 +46,11 @@ void NonlinearConstraint1::print(const std::string& s) const { std::cout << "NonlinearConstraint1 [" << s << "]:\n" << " key: " << key_ << "\n" << " p: " << this->p_ << "\n" - << " lambda key: " << this->lagrange_key_ << std::endl; + << " lambda key: " << this->lagrange_key_ << "\n"; + if (this->isEquality_) + std::cout << " Equality Factor" << std::endl; + else + std::cout << " Inequality Factor" << std::endl; } /* ************************************************************************* */ @@ -48,6 +62,7 @@ bool NonlinearConstraint1::equals(const Factor& f, double tol) c if (this->lagrange_key_ != p->lagrange_key_) return false; if (g_ != p->g_) return false; if (gradG_ != p->gradG_) return false; + if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } @@ -61,13 +76,6 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig // find the error Vector g = g_(config, key_); - // determine if this is an active constraint - if (zero(g)) { - GaussianFactor::shared_ptr factor(new GaussianFactor(Vector_(0))); - GaussianFactor::shared_ptr constraint(new GaussianFactor(Vector_(0))); - return std::make_pair(factor, constraint); - } - // construct the gradient Matrix grad = gradG_(config, key_); @@ -95,8 +103,9 @@ NonlinearConstraint2::NonlinearConstraint2( Matrix (*gradG2)(const Config& config, const std::string& key), Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), size_t dim_constraint, - const std::string& lagrange_key) : - NonlinearConstraint(lagrange_key, dim_constraint), + const std::string& lagrange_key, + bool isEquality) : + NonlinearConstraint(lagrange_key, dim_constraint, isEquality), g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) { // set a good lagrange key here // TODO:should do something smart to find a unique one @@ -125,6 +134,7 @@ bool NonlinearConstraint2::equals(const Factor& f, double tol) c if (g_ != p->g_) return false; if (gradG1_ != p->gradG1_) return false; if (gradG2_ != p->gradG2_) return false; + if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index a0db1cbc0..98910bbd3 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -8,7 +8,6 @@ #pragma once #include -#include #include "NonlinearFactor.h" namespace gtsam { @@ -33,14 +32,20 @@ protected: /** number of lagrange multipliers */ size_t p_; + /** type of constraint */ + bool isEquality_; + 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 */ - NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange) : - NonlinearFactor(zero(dim_lagrange), 1.0), - lagrange_key_(lagrange_key), p_(dim_lagrange) {} + NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, + bool isEquality=true) + : NonlinearFactor(zero(dim_lagrange), 1.0), + lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {} /** returns the key used for the Lagrange multipliers */ std::string lagrangeKey() const { return lagrange_key_; } @@ -48,6 +53,9 @@ public: /** returns the number of lagrange multipliers */ size_t nrConstraints() const { return p_; } + /** returns the type of constraint */ + bool isEquality() const { return isEquality_; } + /** Print */ virtual void print(const std::string& s = "") const =0; @@ -55,7 +63,14 @@ public: virtual bool equals(const Factor& f, double tol=1e-9) const=0; /** error function - returns the result of the constraint function */ - virtual inline Vector error_vector(const Config& c) const=0; + virtual inline Vector error_vector(const Config& c) const { return zero(1); } + + /** + * 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; /** * Linearize using a real Config and a VectorConfig of Lagrange multipliers @@ -114,13 +129,15 @@ public: * @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 */ NonlinearConstraint1( const std::string& key, Matrix (*gradG)(const Config& config, const std::string& key), Vector (*g)(const Config& config, const std::string& key), size_t dim_constraint, - const std::string& lagrange_key=""); + const std::string& lagrange_key="", + bool isEquality=true); /** Print */ void print(const std::string& s = "") const; @@ -184,6 +201,7 @@ public: * @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 */ NonlinearConstraint2( const std::string& key1, @@ -192,7 +210,8 @@ public: Matrix (*gradG2)(const Config& config, const std::string& key), Vector (*g)(const Config& config, const std::string& key1, const std::string& key2), size_t dim_constraint, - const std::string& lagrange_key=""); + const std::string& lagrange_key="", + bool isEquality=true); /** Print */ void print(const std::string& s = "") const; diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 59326e8cf..964b3aef7 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -5,11 +5,22 @@ */ #include +#include // for operator += #include #include "Ordering.h" using namespace std; using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +Ordering Ordering::subtract(const Ordering& keys) const { + Ordering newOrdering = *this; + BOOST_FOREACH(string key, keys) { + newOrdering.remove(key); + } + return newOrdering; +} /* ************************************************************************* */ void Ordering::print(const string& s) const { diff --git a/cpp/Ordering.h b/cpp/Ordering.h index 80d5fc249..bf5ce35c0 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -38,6 +38,13 @@ namespace gtsam { std::list(strings_in) { } + /** + * Remove a set of keys from an ordering + * @param keys to remove + * @return a new ordering without the selected keys + */ + Ordering subtract(const Ordering& keys) const; + void print(const std::string& s = "Ordering") const; /** diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index 77bfc77dc..f39be7398 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -55,7 +55,6 @@ SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, : graph_(&graph), ordering_(&ordering), full_ordering_(ordering), config_(config), lagrange_config_(lagrange), error_(graph.error(*config)) { - } /* **************************************************************** */ @@ -71,6 +70,9 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { // linearize the graph GaussianFactorGraph fg; + // prepare an ordering of lagrange multipliers to remove + Ordering rem; + // iterate over all factors and linearize for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) { const shared_c constraint = boost::shared_dynamic_cast(*factor); @@ -79,7 +81,7 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { GaussianFactor::shared_ptr f = (*factor)->linearize(*config_); if (verbose) f->print("Regular Factor"); fg.push_back(f); - } else { + } else if (constraint->active(*config_)) { // if a constraint, linearize using the constraint method (2 configs) GaussianFactor::shared_ptr f, c; boost::tie(f,c) = constraint->linearize(*config_, *lagrange_config_); @@ -87,12 +89,14 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { if (verbose) c->print("Constraint"); fg.push_back(f); fg.push_back(c); + } else { + rem += constraint->lagrangeKey(); } } if (verbose) fg.print("Before Optimization"); // optimize linear graph to get full delta config - VectorConfig delta = fg.optimize(full_ordering_).scale(-1.0); + VectorConfig delta = fg.optimize(full_ordering_.subtract(rem)).scale(-1.0); if (verbose) delta.print("Delta Config"); @@ -104,4 +108,14 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { return SQPOptimizer(*graph_, full_ordering_, newConfig, newLamConfig); } +/* **************************************************************** */ +template +void SQPOptimizer::print(const std::string& s) { + graph_->print("Nonlinear Graph"); + ordering_->print("Initial Ordering"); + full_ordering_.print("Ordering including all Lagrange Multipliers"); + config_->print("Real Config"); + lagrange_config_->print("Lagrange Multiplier Config"); +} + } diff --git a/cpp/SQPOptimizer.h b/cpp/SQPOptimizer.h index 4cdbd4fbb..d94adfa7f 100644 --- a/cpp/SQPOptimizer.h +++ b/cpp/SQPOptimizer.h @@ -83,6 +83,8 @@ public: */ SQPOptimizer iterate(Verbosity verbosity=SILENT) const; + /** Standard print function with optional name */ + void print(const std::string& s); }; } diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 00148e519..28ece8eb5 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -1,4 +1,4 @@ -/* +/** * @file testNonlinearConstraint.cpp * @brief Tests for nonlinear constraints handled via SQP * @author Alex Cunningham @@ -73,7 +73,7 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_equal ) { NonlinearConstraint1 - c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1"), + c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1", true), c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"), c3("x", *test1::grad_g, *test1::g_func, 2, "L_x1"), c4("y", *test1::grad_g, *test1::g_func, 1, "L_x1"); @@ -202,7 +202,8 @@ Vector g_func(const VectorConfig& config, const std::string& key) { TEST( NonlinearConstraint1, unary_inequality ) { size_t p = 1; NonlinearConstraint1 c1("x", *inequality_unary::grad_g, - *inequality_unary::g_func, p, "L_x1"); + *inequality_unary::g_func, p, "L_x1", + false); // inequality constraint // get configurations to use for evaluation VectorConfig config1, config2; @@ -220,7 +221,8 @@ TEST( NonlinearConstraint1, unary_inequality ) { TEST( NonlinearConstraint1, unary_inequality_linearize ) { size_t p = 1; NonlinearConstraint1 c1("x", *inequality_unary::grad_g, - *inequality_unary::g_func, p, "L_x"); + *inequality_unary::g_func, p, "L_x", + false); // inequality constraint // get configurations to use for linearization VectorConfig config1, config2; @@ -235,13 +237,13 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { GaussianFactor::shared_ptr actFactor1, actConstraint1; boost::tie(actFactor1, actConstraint1) = c1.linearize(config1, lagrangeConfig); - // verify empty factors - CHECK(actFactor1->empty()); - CHECK(actConstraint1->empty()); + // check if the factor is active + CHECK(!c1.active(config1)); // linearize for active constraint GaussianFactor::shared_ptr actFactor2, actConstraint2; boost::tie(actFactor2, actConstraint2) = c1.linearize(config2, lagrangeConfig); + CHECK(c1.active(config2)); // verify GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0); diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp new file mode 100644 index 000000000..f95682773 --- /dev/null +++ b/cpp/testOrdering.cpp @@ -0,0 +1,33 @@ +/** + * @file testOrdering.cpp + * @author Alex Cunningham + */ + +#include // for operator += +#include +#include "Ordering.h" + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST ( Ordering, subtract ) { + Ordering init, delta; + init += "a", "b", "c", "d", "e"; + CHECK(assert_equal(init.subtract(delta), init)); + + delta += "b"; + Ordering expected1; + expected1 += "a", "c", "d", "e"; + CHECK(assert_equal(init.subtract(delta), expected1)); + + delta += "e"; + Ordering expected2; + expected2 += "a", "c", "d"; + CHECK(assert_equal(init.subtract(delta), expected2)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index d0636bb65..1f1ba78fe 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -8,9 +8,11 @@ #include // for operator += #include // for insert #include +#include #include #include "NonlinearFactorGraph.h" #include "NonlinearConstraint.h" +#include "NonlinearEquality.h" #include "VectorConfig.h" #include "Ordering.h" #include "SQPOptimizer.h" @@ -143,6 +145,7 @@ TEST ( SQPOptimizer, map_warp_initLam ) { // create an optimizer Optimizer optimizer(graph, ordering, initialEstimate, initLagrange); + if (verbose) optimizer.print("Initialized Optimizer"); // perform an iteration of optimization Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); @@ -159,8 +162,10 @@ TEST ( SQPOptimizer, map_warp_initLam ) { /* ********************************************************************* */ TEST ( SQPOptimizer, map_warp ) { + bool verbose = false; // get a graph NLGraph graph = linearMapWarpGraph(); + if (verbose) graph.print("Initial map warp graph"); // create an initial estimate shared_config initialEstimate(new VectorConfig); @@ -189,6 +194,127 @@ TEST ( SQPOptimizer, map_warp ) { CHECK(assert_equal(actual, expected)); } +/* ********************************************************************* */ +// This is an obstacle avoidance demo, where there is a trajectory of +// three points, where there is a circular obstacle in the middle. There +// is a binary inequality constraint connecting the obstacle to the +// states, which enforces a minimum distance. +/* ********************************************************************* */ + +bool vector_compare(const std::string& key, + const VectorConfig& feasible, + const VectorConfig& input) { + Vector feas, lin; + feas = feasible[key]; + lin = input[key]; + return equal_with_abs_tol(lin, feas, 1e-5); +} + +typedef NonlinearConstraint1 NLC1; +typedef boost::shared_ptr shared_NLC1; +typedef NonlinearConstraint2 NLC2; +typedef boost::shared_ptr shared_NLC2; +typedef NonlinearEquality NLE; +typedef boost::shared_ptr shared_NLE; + +namespace sqp_avoid1 { +// avoidance radius +double radius = 1.0; + +// binary avoidance constraint +/** g(x) = ||x2-obs||^2 > radius^2 */ +Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) { + Vector delta = config[key1]-config[key2]; + double dist2 = sum(emul(delta, delta)); + double thresh = radius*radius; + if (dist2 <= thresh) + return Vector_(1, dist2-thresh); + else + return zero(1); +} + +/** gradient at pose */ +Matrix grad_g1(const VectorConfig& config, const std::string& key) { + Matrix grad; + return grad; +} + +/** gradient at obstacle */ +Matrix grad_g2(const VectorConfig& config, const std::string& key) { + return -1*eye(1); +} +} + +pair obstacleAvoidGraph() { + // fix start, end, obstacle positions + VectorConfig feasible; + feasible.insert("x1", Vector_(2, 0.0, 0.0)); + feasible.insert("x3", Vector_(2, 10.0, 0.0)); + feasible.insert("obs", Vector_(2, 5.0, -0.5)); + shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare)); + shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare)); + shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare)); + + // measurement from x1 to x2 + Vector x1x2 = Vector_(2, 5.0, 0.0); + double sigma1 = 0.1; + shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2")); + + // measurement from x2 to x3 + Vector x2x3 = Vector_(2, 5.0, 0.0); + double sigma2 = 0.1; + shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3")); + + // create a binary inequality constraint that forces the middle point away from + // the obstacle + shared_NLC2 c1(new NLC2("x2", *sqp_avoid1::grad_g1, + "obs", *sqp_avoid1::grad_g2, + *sqp_avoid1::g_func, 1, "L_x2obs")); + + // construct the graph + NLGraph graph; + graph.push_back(e1); + graph.push_back(e2); + graph.push_back(e3); + graph.push_back(c1); + graph.push_back(f1); + graph.push_back(f2); + + return make_pair(graph, feasible); +} + +/* ********************************************************************* */ +TEST ( SQPOptimizer, inequality_inactive ) { + // create the graph + NLGraph graph; VectorConfig feasible; + boost::tie(graph, feasible) = obstacleAvoidGraph(); + + // create the rest of the config + shared_config init(new VectorConfig(feasible)); + init->insert("x2", Vector_(2, 5.0, 0.6)); + + // create an ordering + Ordering ord; + ord += "x1", "x2", "x3", "obs"; + + // create an optimizer + Optimizer optimizer(graph, ord, init); + + // perform optimization + // FIXME: avoidance constraint not correctly implemented + //Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); + +} + +/* ********************************************************************* */ +TEST ( SQPOptimizer, inequality_active ) { + // create the graph + NLGraph graph; VectorConfig feasible; + boost::tie(graph, feasible) = obstacleAvoidGraph(); + +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */