diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index d6339a245..046b808e3 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -20,17 +20,17 @@ namespace gtsam { template NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, - Vector (*g)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config), bool isEquality) : NonlinearFactor(zero(dim_lagrange), 1.0), lagrange_key_(lagrange_key), p_(dim_lagrange), - isEquality_(isEquality), g_(boost::bind(g, _1, _2)) {} + isEquality_(isEquality), g_(boost::bind(g, _1)) {} /* ************************************************************************* */ template NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, - boost::function& keys)> g, + boost::function g, bool isEquality) : NonlinearFactor(zero(dim_lagrange), 1.0), lagrange_key_(lagrange_key), p_(dim_lagrange), @@ -48,14 +48,14 @@ bool NonlinearConstraint::active(const Config& config) const { template NonlinearConstraint1::NonlinearConstraint1( - Vector (*g)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config), const std::string& key, - Matrix (*gradG)(const Config& config, const std::list& keys), + Matrix (*gradG)(const Config& config), size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), - gradG_(boost::bind(gradG, _1, _2)), key_(key) + gradG_(boost::bind(gradG, _1)), key_(key) { // set a good lagrange key here // TODO:should do something smart to find a unique one @@ -67,9 +67,9 @@ NonlinearConstraint1::NonlinearConstraint1( /* ************************************************************************* */ template NonlinearConstraint1::NonlinearConstraint1( - boost::function& keys)> g, + boost::function g, const std::string& key, - boost::function& keys)> gradG, + boost::function gradG, size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : @@ -115,10 +115,10 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig Vector lambda = lagrange[this->lagrange_key_]; // find the error - Vector g = g_(config, this->keys()); + Vector g = g_(config); // construct the gradient - Matrix grad = gradG_(config, this->keys()); + Matrix grad = gradG_(config); // construct probabilistic factor Matrix A1 = vector_scale(lambda, grad); @@ -138,16 +138,16 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig /* ************************************************************************* */ template NonlinearConstraint2::NonlinearConstraint2( - Vector (*g)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config), const std::string& key1, - Matrix (*gradG1)(const Config& config, const std::list& keys), + Matrix (*gradG1)(const Config& config), const std::string& key2, - Matrix (*gradG2)(const Config& config, const std::list& keys), + Matrix (*gradG2)(const Config& config), size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, g, isEquality), - gradG1_(boost::bind(gradG1, _1, _2)), gradG2_(boost::bind(gradG2, _1, _2)), + gradG1_(boost::bind(gradG1, _1)), gradG2_(boost::bind(gradG2, _1)), key1_(key1), key2_(key2) { // set a good lagrange key here @@ -161,11 +161,11 @@ NonlinearConstraint2::NonlinearConstraint2( /* ************************************************************************* */ template NonlinearConstraint2::NonlinearConstraint2( - boost::function& keys)> g, + boost::function g, const std::string& key1, - boost::function& keys)> gradG1, + boost::function gradG1, const std::string& key2, - boost::function& keys)> gradG2, + boost::function gradG2, size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : @@ -211,11 +211,11 @@ std::pair NonlinearConst Vector lambda = lagrange[this->lagrange_key_]; // find the error - Vector g = g_(config, this->keys()); + Vector g = g_(config); // construct the gradients - Matrix grad1 = gradG1_(config, this->keys()); - Matrix grad2 = gradG2_(config, this->keys()); + Matrix grad1 = gradG1_(config); + Matrix grad2 = gradG2_(config); // construct probabilistic factor Matrix A1 = vector_scale(lambda, grad1); diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 40db0a987..65ca74502 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -40,10 +40,9 @@ protected: * If the value is zero, the constraint is not active * Use boost.bind to create the function object * @param config is a configuration of all the variables - * @param keys is the set of keys - assumed that the function knows how to use * @return the cost for each of p constraints, arranged in a vector */ - boost::function& keys)> g_; + boost::function g_; public: @@ -55,7 +54,7 @@ public: */ NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, - Vector (*g)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config), bool isEquality=true); /** Constructor - sets a more general cost function using boost::bind directly @@ -66,7 +65,7 @@ public: */ NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange, - boost::function& keys)> g, + boost::function g, bool isEquality=true); /** returns the key used for the Lagrange multipliers */ @@ -85,7 +84,7 @@ public: virtual bool equals(const Factor& f, double tol=1e-9) const=0; /** error function - returns the result of the constraint function */ - inline Vector error_vector(const Config& c) const { return g_(c, this->keys()); } + inline Vector error_vector(const Config& c) const { return g_(c); } /** * Determines whether the constraint is active given a particular configuration @@ -128,10 +127,9 @@ private: * returns a pxn matrix * Use boost.bind to create the function object * @param config to use for linearization - * @param key of selected variable * @return the jacobian of the constraint in terms of key */ - boost::function& keys)> gradG_; + boost::function gradG_; /** key for the constrained variable */ std::string key_; @@ -148,9 +146,9 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint1( - Vector (*g)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config), const std::string& key, - Matrix (*gradG)(const Config& config, const std::list& keys), + Matrix (*gradG)(const Config& config), size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -165,9 +163,9 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint1( - boost::function& keys)> g, + boost::function g, const std::string& key, - boost::function& keys)> gradG, + boost::function gradG, size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -202,11 +200,10 @@ private: * the first and second variables * 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 */ - boost::function& keys)> gradG1_; - boost::function& keys)> gradG2_; + boost::function gradG1_; + boost::function gradG2_; /** keys for the constrained variables */ std::string key1_; @@ -224,11 +221,11 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint2( - Vector (*g)(const Config& config, const std::list& keys), + Vector (*g)(const Config& config), const std::string& key1, - Matrix (*gradG1)(const Config& config, const std::list& keys), + Matrix (*gradG1)(const Config& config), const std::string& key2, - Matrix (*gradG2)(const Config& config, const std::list& keys), + Matrix (*gradG2)(const Config& config), size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -244,11 +241,11 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint2( - boost::function& keys)> g, + boost::function g, const std::string& key1, - boost::function& keys)> gradG1, + boost::function gradG1, const std::string& key2, - boost::function& keys)> gradG2, + boost::function gradG2, size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 3eff4303c..387b91924 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -4,14 +4,17 @@ * @author Alex Cunningham */ +#include #include #include +#include // for operator += #include #include #include using namespace std; using namespace gtsam; +using namespace boost::assign; /* ************************************************************************* */ // unary functions with scalar variables @@ -38,7 +41,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { // the lagrange multipliers will be expected on L_x1 // and there is only one multiplier size_t p = 1; - NonlinearConstraint1 c1(*test1::g, "x", *test1::G, p, "L_x1"); + list keys; keys += "x"; + NonlinearConstraint1 c1(boost::bind(test1::g, _1, keys), + "x", boost::bind(test1::G, _1, keys), + p, "L_x1"); // get a configuration to use for finding the error VectorConfig config; @@ -53,7 +59,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_linearize ) { size_t p = 1; - NonlinearConstraint1 c1(*test1::g, "x", *test1::G, p, "L_x1"); + list keys; keys += "x"; + NonlinearConstraint1 c1(boost::bind(test1::g, _1, keys), + "x", boost::bind(test1::G, _1, keys), + p, "L_x1"); // get a configuration to use for linearization VectorConfig realconfig; @@ -76,11 +85,12 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_equal ) { + list keys1, keys2; keys1 += "x"; keys2 += "y"; NonlinearConstraint1 - c1(*test1::g, "x", *test1::G, 1, "L_x1", true), - c2(*test1::g, "x", *test1::G, 1, "L_x1"), - c3(*test1::g, "x", *test1::G, 2, "L_x1"), - c4(*test1::g, "y", *test1::G, 1, "L_x1"); + c1(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1", true), + c2(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1"), + c3(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 2, "L_x1"), + c4(boost::bind(test1::g, _1, keys2), "y", boost::bind(test1::G, _1, keys2), 1, "L_x1"); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -120,10 +130,11 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { // the lagrange multipliers will be expected on L_xy // and there is only one multiplier size_t p = 1; + list keys; keys += "x", "y"; NonlinearConstraint2 c1( - *test2::g, - "x", *test2::G1, - "y", *test2::G2, + boost::bind(test2::g, _1, keys), + "x", boost::bind(test2::G1, _1, keys), + "y", boost::bind(test2::G1, _1, keys), p, "L_xy"); // get a configuration to use for finding the error @@ -141,10 +152,11 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { TEST( NonlinearConstraint2, binary_scalar_linearize ) { // create a constraint size_t p = 1; + list keys; keys += "x", "y"; NonlinearConstraint2 c1( - *test2::g, - "x", *test2::G1, - "y", *test2::G2, + boost::bind(test2::g, _1, keys), + "x", boost::bind(test2::G1, _1, keys), + "y", boost::bind(test2::G2, _1, keys), p, "L_xy"); // get a configuration to use for finding the error @@ -173,11 +185,13 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { /* ************************************************************************* */ TEST( NonlinearConstraint2, binary_scalar_equal ) { + list keys1, keys2, keys3; + keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z"; NonlinearConstraint2 - c1(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"), - c2(*test2::g, "x", *test2::G1, "y", *test2::G2, 1, "L_xy"), - c3(*test2::g, "y", *test2::G1, "x", *test2::G2, 1, "L_xy"), - c4(*test2::g, "x", *test2::G1, "z", *test2::G2, 3, "L_xy"); + c1(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"), + c2(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"), + c3(boost::bind(test2::g, _1, keys2), "y", boost::bind(test2::G1, _1, keys2), "x", boost::bind(test2::G2, _1, keys2), 1, "L_xy"), + c4(boost::bind(test2::g, _1, keys3), "x", boost::bind(test2::G1, _1, keys3), "z", boost::bind(test2::G2, _1, keys3), 3, "L_xy"); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -208,8 +222,9 @@ namespace inequality1 { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_inequality ) { size_t p = 1; - NonlinearConstraint1 c1(*inequality1::g, - "x", *inequality1::G, + list keys; keys += "x"; + NonlinearConstraint1 c1(boost::bind(inequality1::g, _1, keys), + "x", boost::bind(inequality1::G, _1, keys), p, "L_x1", false); // inequality constraint @@ -228,9 +243,10 @@ TEST( NonlinearConstraint1, unary_inequality ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_inequality_linearize ) { size_t p = 1; - NonlinearConstraint1 c1(*inequality1::g, - "x", *inequality1::G, - p, "L_x", + list keys; keys += "x"; + NonlinearConstraint1 c1(boost::bind(inequality1::g, _1, keys), + "x", boost::bind(inequality1::G, _1, keys), + p, "L_x1", false); // inequality constraint // get configurations to use for linearization @@ -240,13 +256,13 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { // get a configuration of Lagrange multipliers VectorConfig lagrangeConfig; - lagrangeConfig.insert("L_x", Vector_(1, 3.0)); + lagrangeConfig.insert("L_x1", Vector_(1, 3.0)); // linearize for inactive constraint GaussianFactor::shared_ptr actualFactor1, actualConstraint1; boost::tie(actualFactor1, actualConstraint1) = c1.linearize(config1, lagrangeConfig); - // check if the factualor is active + // check if the factor is active CHECK(!c1.active(config1)); // linearize for active constraint @@ -255,7 +271,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { CHECK(c1.active(config2)); // verify - GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0); + GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0); GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); CHECK(assert_equal(*actualFactor2, expectedFactor)); CHECK(assert_equal(*actualConstraint2, expectedConstraint)); @@ -287,9 +303,10 @@ TEST( NonlinearConstraint1, unary_binding ) { size_t p = 1; double coeff = 2; double radius = 5; + list keys; keys += "x"; NonlinearConstraint1 c1( - boost::bind(binding1::g, radius, _1, _2), - "x", boost::bind(binding1::G, coeff, _1, _2), + boost::bind(binding1::g, radius, _1, keys), + "x", boost::bind(binding1::G, coeff, _1, keys), p, "L_x1", false); // inequality constraint @@ -338,10 +355,11 @@ TEST( NonlinearConstraint2, binary_binding ) { double a = 2.0; double b = 1.0; double r = 5.0; + list keys; keys += "x", "y"; NonlinearConstraint2 c1( - boost::bind(binding2::g, r, _1, _2), - "x", boost::bind(binding2::G1, a, _1, _2), - "y", boost::bind(binding2::G2, b, _1, _2), + boost::bind(binding2::g, r, _1, keys), + "x", boost::bind(binding2::G1, a, _1, keys), + "y", boost::bind(binding2::G2, b, _1, keys), p, "L_xy"); // get a configuration to use for finding the error diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 533fb9113..9b8d3968a 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -365,10 +365,11 @@ TEST (SQP, two_pose ) { feas.insert("x1", Vector_(2, 1.0, 1.0)); // constant constraint on x1 + list keys1; keys1 += "x1"; boost::shared_ptr > c1( new NonlinearConstraint1( - *sqp_test2::g, - "x1", *sqp_test2::G, + boost::bind(sqp_test2::g, _1, keys1), + "x1", boost::bind(sqp_test2::G, _1, keys1), 2, "L_x1")); // measurement from x1 to l1 @@ -382,11 +383,12 @@ TEST (SQP, two_pose ) { shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2")); // equality constraint between l1 and l2 + list keys2; keys2 += "l1", "l2"; boost::shared_ptr > c2( new NonlinearConstraint2( - *sqp_test1::g, - "l1", *sqp_test1::G1, - "l2", *sqp_test1::G2, + boost::bind(sqp_test1::g, _1, keys2), + "l1", boost::bind(sqp_test1::G1, _1, keys2), + "l2", boost::bind(sqp_test1::G2, _1, keys2), 2, "L_l1l2")); // construct the graph @@ -675,11 +677,12 @@ VSLAMGraph stereoExampleGraph() { // create the binary equality constraint between the landmarks // NOTE: this is really just a linear constraint that is exactly the same // as the previous examples + list keys; keys += "l1", "l2"; boost::shared_ptr > c2( new NonlinearConstraint2( - *sqp_stereo::g, - "l1", *sqp_stereo::G1, - "l2", *sqp_stereo::G2, + boost::bind(sqp_stereo::g, _1, keys), + "l1", boost::bind(sqp_stereo::G1, _1, keys), + "l2", boost::bind(sqp_stereo::G2, _1, keys), 3, "L_l1l2")); graph.push_back(c2); diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index a2a3c93da..66cdd9367 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -92,10 +92,11 @@ typedef SQPOptimizer Optimizer; */ NLGraph linearMapWarpGraph() { // constant constraint on x1 + list keyx; keyx += "x1"; boost::shared_ptr > c1( new NonlinearConstraint1( - *sqp_LinearMapWarp1::g_func, - "x1", *sqp_LinearMapWarp1::grad_g, + boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx), + "x1", boost::bind(sqp_LinearMapWarp1::grad_g, _1, keyx), 2, "L_x1")); // measurement from x1 to l1 @@ -109,11 +110,12 @@ NLGraph linearMapWarpGraph() { shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2")); // equality constraint between l1 and l2 + list keys; keys += "l1", "l2"; boost::shared_ptr > c2( new NonlinearConstraint2( - *sqp_LinearMapWarp2::g_func, - "l1", *sqp_LinearMapWarp2::grad_g1, - "l2", *sqp_LinearMapWarp2::grad_g2, + boost::bind(sqp_LinearMapWarp2::g_func, _1, keys), + "l1", boost::bind(sqp_LinearMapWarp2::grad_g1, _1, keys), + "l2", boost::bind(sqp_LinearMapWarp2::grad_g2, _1, keys), 2, "L_l1l2")); // construct the graph @@ -272,9 +274,10 @@ pair obstacleAvoidGraph() { // create a binary inequality constraint that forces the middle point away from // the obstacle - shared_NLC2 c1(new NLC2(*sqp_avoid1::g_func, - "x2", *sqp_avoid1::grad_g1, - "obs", *sqp_avoid1::grad_g2, + list keys; keys += "x2", "obs"; + shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys), + "x2", boost::bind(sqp_avoid1::grad_g1, _1, keys), + "obs",boost::bind(sqp_avoid1::grad_g2, _1, keys), 1, "L_x2obs", false)); // construct the graph @@ -403,9 +406,10 @@ pair obstacleAvoidGraphGeneral() { // create a binary inequality constraint that forces the middle point away from // the obstacle - shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, _2), - "x2", boost::bind(sqp_avoid2::grad_g1, _1, _2), - "obs", boost::bind(sqp_avoid2::grad_g2, _1, _2), + list keys; keys += "x2", "obs"; + shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys), + "x2", boost::bind(sqp_avoid2::grad_g1, _1, keys), + "obs", boost::bind(sqp_avoid2::grad_g2, _1, keys), 1, "L_x2obs", false)); // construct the graph