diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 57c7e02ca..d6339a245 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -48,9 +48,9 @@ bool NonlinearConstraint::active(const Config& config) const { template NonlinearConstraint1::NonlinearConstraint1( + Vector (*g)(const Config& config, const std::list& keys), const std::string& key, Matrix (*gradG)(const Config& config, const std::list& keys), - Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : @@ -67,9 +67,9 @@ NonlinearConstraint1::NonlinearConstraint1( /* ************************************************************************* */ template NonlinearConstraint1::NonlinearConstraint1( + boost::function& keys)> g, const std::string& key, boost::function& keys)> gradG, - boost::function& keys)> g, size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : @@ -138,11 +138,11 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig /* ************************************************************************* */ template NonlinearConstraint2::NonlinearConstraint2( + Vector (*g)(const Config& config, const std::list& keys), const std::string& key1, Matrix (*gradG1)(const Config& config, const std::list& keys), const std::string& key2, Matrix (*gradG2)(const Config& config, const std::list& keys), - Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : @@ -161,11 +161,11 @@ NonlinearConstraint2::NonlinearConstraint2( /* ************************************************************************* */ template NonlinearConstraint2::NonlinearConstraint2( + boost::function& keys)> g, const std::string& key1, boost::function& keys)> gradG1, const std::string& key2, boost::function& keys)> gradG2, - boost::function& keys)> g, size_t dim_constraint, const std::string& lagrange_key, bool isEquality) : diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index f08cf90f4..40db0a987 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -148,9 +148,9 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint1( + Vector (*g)(const Config& config, const std::list& keys), const std::string& key, Matrix (*gradG)(const Config& config, const std::list& keys), - Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -165,9 +165,9 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint1( + boost::function& keys)> g, const std::string& key, boost::function& keys)> gradG, - boost::function& keys)> g, size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -224,11 +224,11 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint2( + Vector (*g)(const Config& config, const std::list& keys), const std::string& key1, Matrix (*gradG1)(const Config& config, const std::list& keys), const std::string& key2, Matrix (*gradG2)(const Config& config, const std::list& keys), - Vector (*g)(const Config& config, const std::list& keys), size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); @@ -244,11 +244,11 @@ public: * @param isEquality is true if the constraint is an equality constraint */ NonlinearConstraint2( + boost::function& keys)> g, const std::string& key1, boost::function& keys)> gradG1, const std::string& key2, boost::function& keys)> gradG2, - boost::function& keys)> g, size_t dim_constraint, const std::string& lagrange_key="", bool isEquality=true); diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index c84035310..3eff4303c 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -38,7 +38,7 @@ 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("x", *test1::G, *test1::g, p, "L_x1"); + NonlinearConstraint1 c1(*test1::g, "x", *test1::G, p, "L_x1"); // get a configuration to use for finding the error VectorConfig config; @@ -53,7 +53,7 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_linearize ) { size_t p = 1; - NonlinearConstraint1 c1("x", *test1::G, *test1::g, p, "L_x1"); + NonlinearConstraint1 c1(*test1::g, "x", *test1::G, p, "L_x1"); // get a configuration to use for linearization VectorConfig realconfig; @@ -77,10 +77,10 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_equal ) { NonlinearConstraint1 - c1("x", *test1::G, *test1::g, 1, "L_x1", true), - c2("x", *test1::G, *test1::g, 1, "L_x1"), - c3("x", *test1::G, *test1::g, 2, "L_x1"), - c4("y", *test1::G, *test1::g, 1, "L_x1"); + 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"); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -121,9 +121,10 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { // and there is only one multiplier size_t p = 1; NonlinearConstraint2 c1( + *test2::g, "x", *test2::G1, "y", *test2::G2, - *test2::g, p, "L_xy"); + p, "L_xy"); // get a configuration to use for finding the error VectorConfig config; @@ -141,9 +142,10 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { // create a constraint size_t p = 1; NonlinearConstraint2 c1( + *test2::g, "x", *test2::G1, "y", *test2::G2, - *test2::g, p, "L_xy"); + p, "L_xy"); // get a configuration to use for finding the error VectorConfig realconfig; @@ -172,10 +174,10 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { /* ************************************************************************* */ TEST( NonlinearConstraint2, binary_scalar_equal ) { NonlinearConstraint2 - c1("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"), - c2("x", *test2::G1, "y", *test2::G2,*test2::g, 1, "L_xy"), - c3("y", *test2::G1, "x", *test2::G2,*test2::g, 1, "L_xy"), - c4("x", *test2::G1, "z", *test2::G2,*test2::g, 3, "L_xy"); + 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"); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -206,8 +208,9 @@ namespace inequality1 { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_inequality ) { size_t p = 1; - NonlinearConstraint1 c1("x", *inequality1::G, - *inequality1::g, p, "L_x1", + NonlinearConstraint1 c1(*inequality1::g, + "x", *inequality1::G, + p, "L_x1", false); // inequality constraint // get configurations to use for evaluation @@ -225,8 +228,9 @@ TEST( NonlinearConstraint1, unary_inequality ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_inequality_linearize ) { size_t p = 1; - NonlinearConstraint1 c1("x", *inequality1::G, - *inequality1::g, p, "L_x", + NonlinearConstraint1 c1(*inequality1::g, + "x", *inequality1::G, + p, "L_x", false); // inequality constraint // get configurations to use for linearization @@ -283,9 +287,9 @@ TEST( NonlinearConstraint1, unary_binding ) { size_t p = 1; double coeff = 2; double radius = 5; - NonlinearConstraint1 c1("x", - boost::bind(binding1::G, coeff, _1, _2), + NonlinearConstraint1 c1( boost::bind(binding1::g, radius, _1, _2), + "x", boost::bind(binding1::G, coeff, _1, _2), p, "L_x1", false); // inequality constraint @@ -335,9 +339,10 @@ TEST( NonlinearConstraint2, binary_binding ) { double b = 1.0; double r = 5.0; 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, _2), p, "L_xy"); + p, "L_xy"); // get a configuration to use for finding the error VectorConfig config; diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 2c0ff254f..533fb9113 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -367,8 +367,9 @@ TEST (SQP, two_pose ) { // constant constraint on x1 boost::shared_ptr > c1( new NonlinearConstraint1( + *sqp_test2::g, "x1", *sqp_test2::G, - *sqp_test2::g, 2, "L_x1")); + 2, "L_x1")); // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); @@ -383,9 +384,10 @@ TEST (SQP, two_pose ) { // equality constraint between l1 and l2 boost::shared_ptr > c2( new NonlinearConstraint2( + *sqp_test1::g, "l1", *sqp_test1::G1, "l2", *sqp_test1::G2, - *sqp_test1::g, 2, "L_l1l2")); + 2, "L_l1l2")); // construct the graph NLGraph graph; @@ -675,9 +677,10 @@ VSLAMGraph stereoExampleGraph() { // as the previous examples boost::shared_ptr > c2( new NonlinearConstraint2( + *sqp_stereo::g, "l1", *sqp_stereo::G1, "l2", *sqp_stereo::G2, - *sqp_stereo::g, 3, "L_l1l2")); + 3, "L_l1l2")); graph.push_back(c2); return graph; diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index e0b600884..a2a3c93da 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -94,8 +94,9 @@ NLGraph linearMapWarpGraph() { // constant constraint on x1 boost::shared_ptr > c1( new NonlinearConstraint1( + *sqp_LinearMapWarp1::g_func, "x1", *sqp_LinearMapWarp1::grad_g, - *sqp_LinearMapWarp1::g_func, 2, "L_x1")); + 2, "L_x1")); // measurement from x1 to l1 Vector z1 = Vector_(2, 0.0, 5.0); @@ -110,9 +111,10 @@ NLGraph linearMapWarpGraph() { // equality constraint between l1 and l2 boost::shared_ptr > c2( new NonlinearConstraint2( + *sqp_LinearMapWarp2::g_func, "l1", *sqp_LinearMapWarp2::grad_g1, "l2", *sqp_LinearMapWarp2::grad_g2, - *sqp_LinearMapWarp2::g_func, 2, "L_l1l2")); + 2, "L_l1l2")); // construct the graph NLGraph graph; @@ -270,9 +272,10 @@ pair obstacleAvoidGraph() { // 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", false)); + shared_NLC2 c1(new NLC2(*sqp_avoid1::g_func, + "x2", *sqp_avoid1::grad_g1, + "obs", *sqp_avoid1::grad_g2, + 1, "L_x2obs", false)); // construct the graph NLGraph graph; @@ -400,9 +403,10 @@ pair obstacleAvoidGraphGeneral() { // create a binary inequality constraint that forces the middle point away from // the obstacle - shared_NLC2 c1(new NLC2("x2", boost::bind(sqp_avoid2::grad_g1, _1, _2), - "obs", boost::bind(sqp_avoid2::grad_g2, _1, _2), - boost::bind(sqp_avoid2::g_func, radius, _1, _2), 1, "L_x2obs", false)); + 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), + 1, "L_x2obs", false)); // construct the graph NLGraph graph;