diff --git a/.cproject b/.cproject index 08625e0dd..2698ef3ac 100644 --- a/.cproject +++ b/.cproject @@ -521,10 +521,10 @@ true true - + make -testIncremental.run +testNonlinearConstraint.run true true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 847243aee..da712d7d9 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -115,9 +115,12 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h sources += NonlinearFactor.cpp sources += NonlinearEquality.h -check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP +sources += NonlinearConstraint.h +check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la +testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp +testNonlinearConstraint_LDADD = libgtsam.la testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp testNonlinearFactorGraph_LDADD = libgtsam.la testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h new file mode 100644 index 000000000..0325e96c7 --- /dev/null +++ b/cpp/NonlinearConstraint.h @@ -0,0 +1,224 @@ +/* + * @file NonlinearConstraint.h + * @brief Implements nonlinear constraints that can be linearized and + * inserted into an existing nonlinear graph and solved via SQP + * @author Alex Cunningham + */ + +#pragma once + +#include +#include "NonlinearFactor.h" + +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) + * + * The measurement z in the underlying NonlinearFactor is the + * set of Lagrange multipliers. + */ +template +class NonlinearConstraint : public NonlinearFactor { + +protected: + /** key for the lagrange multipliers */ + std::string lagrange_key_; + + /** number of lagrange multipliers */ + size_t p_; + +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 + */ + NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange) : + NonlinearFactor(zero(dim_lagrange), 1.0), + lagrange_key_(lagrange_key), p_(dim_lagrange) {} + + /** returns the key used for the Lagrange multipliers */ + std::string& lagrangeKey() const { return lagrange_key_; } + + /** returns the number of lagrange multipliers */ + size_t nrConstraints() const { return p_; } + + /** Print */ + 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 inline Vector error_vector(const Config& c) const=0; + + /** + * Linearize using a real Config and a VectorConfig of Lagrange multipliers + * Returns the two separate Gaussian factors to solve + * @param config is the real Config of the real variables + * @param lagrange is the VectorConfig of lagrange multipliers + * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint) + */ + virtual std::pair + linearize(const Config& config, const VectorConfig& lagrange) const=0; + + /** + * linearize with only Config, which is not currently implemented + * This will be implemented later for other constrained optimization + * algorithms + */ + virtual boost::shared_ptr linearize(const Config& c) const { + throw std::invalid_argument("No current constraint linearization for a single Config!"); + } +}; + + +/** + * A unary constraint with arbitrary cost and gradient functions + */ +template +class NonlinearConstraint1 : NonlinearConstraint { + +private: + /** calculates the constraint function of the current config + * If the value is zero, the constraint is not active + * @param config is a configuration of all the variables + * @param key is the id for the selected variable + * @return the cost for each of p constraints, arranged in a vector + */ + Vector (*g_)(const Config& config, const std::string& key); + + /** + * Calculates the gradient of the constraint function + * returns a pxn matrix + * @param config to use for linearization + * @param key of selected variable + * @return the jacobian of the constraint in terms of key + */ + Matrix (*gradG_) (const Config& config, const std::string& key); + + /** key for the constrained variable */ + std::string key_; + +public: + + /** + * Basic constructor + * @param key is the identifier for the variable constrained + * @param gradG gives the gradient 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 + */ + 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="") : + NonlinearConstraint(lagrange_key, dim_constraint), + g_(g), gradG_(gradG), key_(key) { + // set a good lagrange key here - should do something smart to find a unique one + if (lagrange_key == "") + this->lagrange_key_ = "L_" + key; + } + + /** Print */ + void print(const std::string& s = "") const { + //FIXME: dummy implementation + } + + /** Check if two factors are equal */ + bool equals(const Factor& f, double tol=1e-9) const { + //FIXME: dummy implementation + return false; + } + + /** error function - returns the result of the constraint function */ + inline Vector error_vector(const Config& c) const { + return g_(c, key_); + } + + /** + * Linearize using a real Config and a VectorConfig of Lagrange multipliers + * Returns the two separate Gaussian factors to solve + * @param config is the real Config of the real variables + * @param lagrange is the VectorConfig of lagrange multipliers + * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint) + */ + std::pair + linearize(const Config& config, const VectorConfig& lagrange) const { + //FIXME: dummy implementation + GaussianFactor::shared_ptr factor(new GaussianFactor); + GaussianFactor::shared_ptr constraint(new GaussianFactor); + return std::make_pair(factor, constraint); + } + +}; + + + +//template +//class NonlinearConstraint2 : public NonlinearConstraint { +// +//private: +// /** +// * Calculates the gradient of the constraint function +// * returns a pxn matrix for x1 +// * @param config +// */ +// Matrix (*gradG1_) (const Config& config); +// +// /** +// * Calculates the gradient of the constraint function +// * returns a pxn matrix for x2 +// * @param config +// */ +// Matrix (*gradG2_) (const Config& config); +// +// /** keys for the constrained variables */ +// std::string key1_; +// std::string key2_; +// +//public: +// +// /** +// * Basic constructor +// * @param key1 is the first variable +// * @param gradG1 gives the gradient for the first variable +// * @param key2 is the first variable +// * @param gradG2 gives the gradient for the first variable +// * @param g is the constraint function +// * @param lambdas is vector of size p with Lagrange multipliers +// */ +// NonlinearConstraint2( +// const std::string& key1, +// Matrix (*gradG1)(const Config& config), +// const std::string& key2, +// Matrix (*gradG2)(const Config& config), +// Vector (*g)(const Config& config), +// const Vector& lambdas) : +// NonlinearConstraint(lambdas, g), +// gradG1_(gradG1), key1_(key1), +// gradG2_(gradG2), key2_(key2) {} +// +// /** Print */ +// void print(const std::string& s = "") const {} +// +// /** Check if two factors are equal */ +// bool equals(const Factor& f, double tol=1e-9) const { +// return true; +// } +// +// /** Linearize a non-linearFactor2 to get a linearFactor2 */ +// boost::shared_ptr linearize(const Config& c) const { +// boost::shared_ptr ret; +// return ret; +// } +//}; + +} diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp new file mode 100644 index 000000000..bffbd7f55 --- /dev/null +++ b/cpp/testNonlinearConstraint.cpp @@ -0,0 +1,88 @@ +/* + * @file testNonlinearConstraint.cpp + * @brief Tests for nonlinear constraints handled via SQP + * @author Alex Cunningham + */ + +#include +#include +#include + +using namespace gtsam; + +// define some functions to use +// these examples use scalar variables and a single constraint + +/* ************************************************************************* */ +// unary functions +/* ************************************************************************* */ +namespace test1 { +/** p = 1, gradG(x) = 2x */ +Matrix grad_g(const VectorConfig& config, const std::string& key) { + double x = config[key](0); + return Matrix_(1,1, 2*x); +} + +/** p = 1, g(x) = x^2-5 = 0 */ +Vector g_func(const VectorConfig& config, const std::string& key) { + double x = config[key](0); + return Vector_(1, x*x-5); +} +} // \namespace test1 + +/* ************************************************************************* */ +TEST( NonlinearConstraint, unary_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; + NonlinearConstraint1 c1("x", *test1::grad_g, *test1::g_func, p, "L_x1"); + + // get a configuration to use for finding the error + VectorConfig config; + config.insert("x", Vector_(1, 1.0)); + + // calculate the error + Vector actual = c1.error_vector(config); + Vector expected = Vector_(1.0, -4.0); + CHECK(assert_equal(actual, expected, 1e-5)); +} + +///* ************************************************************************* */ +//TEST( NonlinearConstraint, unary_linearize ) { +// // constuct a constraint +// Vector lambda = Vector_(1, 1.0); +// NonlinearConstraint1 c1("x", *grad_g1, *g1_func, lambda); +// +// // get a configuration to use for linearization +// VectorConfig config; +// config.insert("x", Vector_(1, 1.0)); +// +// // determine the gradient of the function +// Matrix +// +//} + +///* ************************************************************************* */ +//// binary functions +//Matrix grad1_g2(const VectorConfig& config) { +// return eye(1); +//} +// +//Matrix grad2_g2(const VectorConfig& config) { +// return eye(1); +//} +// +//Vector g2_func(const VectorConfig& config) { +// return zero(1); +//} +// +///* ************************************************************************* */ +//TEST( NonlinearConstraint, binary_construction ) { +// Vector lambda = Vector_(1, 1.0); +// NonlinearConstraint2 c1("x", *grad1_g2, "y", *grad2_g2, *g2_func, lambda); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index c3ebe87c8..2156380e2 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -198,7 +198,7 @@ TEST (SQP, problem1_sqp ) { new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) "y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) "lam", eye(1), // dlam term - Vector_(1.0, 0.0), // rhs is zero + Vector_(1, 0.0), // rhs is zero 1.0)); // arbitrary sigma // create the actual constraint @@ -318,6 +318,69 @@ TEST (SQP, two_pose_truth ) { CHECK(assert_equal(expected, *actual, 1e-5)); } +// Test of binary nonlinear equality constraint disabled until nonlinear constraints work + +///** +// * Version that actually uses nonlinear equality constraints +// * to to perform optimization. Same as above, but no +// * equality constraint on x2, and two landmarks that +// * should be the same. +// */ +//TEST (SQP, two_pose ) { +// // position (1, 1) constraint for x1 +// VectorConfig feas; +// feas.insert("x1", Vector_(2, 1.0, 1.0)); +// +// // constant constraint on x1 +// shared_eq ef1(new NonlinearEquality("x1", feas, 2, *vector_compare)); +// +// // measurement from x1 to l1 +// Vector z1 = Vector_(2, 0.0, 5.0); +// double sigma1 = 0.1; +// shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1")); +// +// // measurement from x2 to l2 +// Vector z2 = Vector_(2, -4.0, 0.0); +// double sigma2 = 0.1; +// shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1")); +// +// // equality constraint between l1 and l2 +// boost::shared_ptr > c1( +// new NonlinearConstraint( +// "l1", "l2", // specify nodes constrained +// *g_function, // evaluate the cost +// *gradG_function)); // construct the gradient of g(x) +// +// // construct the graph +// NLGraph graph; +// graph.push_back(ef1); +// graph.push_back(c1); +// graph.push_back(f1); +// graph.push_back(f2); +// +// // create an initial estimate +// boost::shared_ptr initialEstimate(new VectorConfig(feas)); // must start with feasible set +// initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth +// //initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error +// +// // optimize the graph +// Ordering ordering; +// ordering += "x1", "x2", "l1"; +// Optimizer optimizer(graph, ordering, initialEstimate, 1e-5); +// +// // display solution +// double relativeThreshold = 1e-5; +// double absoluteThreshold = 1e-5; +// Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); +// boost::shared_ptr actual = act_opt.config(); +// //actual->print("Configuration after optimization"); +// +// // verify +// VectorConfig expected(feas); +// expected.insert("l1", Vector_(2, 1.0, 6.0)); +// CHECK(assert_equal(expected, *actual, 1e-5)); +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */