diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 6327dad2b..cbb06cf4b 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -86,7 +86,7 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); // construct the constraint - GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, g, 0.0)); + GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, 0.0)); return std::make_pair(factor, constraint); } @@ -163,7 +163,7 @@ NonlinearConstraint2::linearize(const Config& config, const VectorConfig this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); // construct the constraint - GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, g, 0.0)); + GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1*g, 0.0)); return std::make_pair(factor, constraint); } diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index f39be7398..5b1e34cbe 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -96,7 +96,7 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { if (verbose) fg.print("Before Optimization"); // optimize linear graph to get full delta config - VectorConfig delta = fg.optimize(full_ordering_.subtract(rem)).scale(-1.0); + VectorConfig delta = fg.optimize(full_ordering_.subtract(rem)); if (verbose) delta.print("Delta Config"); diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index fc1624bc0..150f2f49f 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -66,7 +66,7 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { // verify GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0); - GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1,-4.0), 0.0); + GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); CHECK(assert_equal(*actFactor, expFactor)); CHECK(assert_equal(*actConstraint, expConstraint)); } @@ -159,7 +159,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { "L_xy", eye(1), zero(1), 1.0); GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), "y", Matrix_(1,1, -1.0), - Vector_(1,-6.0), 0.0); + Vector_(1, 6.0), 0.0); CHECK(assert_equal(*actFactor, expFactor)); CHECK(assert_equal(*actConstraint, expConstraint)); } @@ -248,7 +248,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { // verify GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0); - GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1,-4.0), 0.0); + GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); CHECK(assert_equal(*actFactor2, expFactor)); CHECK(assert_equal(*actConstraint2, expConstraint)); } diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index a2034b64d..9e9149b7f 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -223,11 +223,11 @@ TEST (SQP, problem1_sqp ) { // solve Ordering ord; ord += "x", "y", "lam"; - VectorConfig delta = fg.optimize(ord).scale(-1.0); // flip sign + VectorConfig delta = fg.optimize(ord); if (verbose) delta.print("Delta"); // update initial estimate - VectorConfig newState = state.exmap(delta); + VectorConfig newState = state.exmap(delta.scale(-1.0)); // set the state to the updated state state = newState; @@ -434,7 +434,7 @@ TEST (SQP, two_pose ) { ordering += "x1", "x2", "l1", "l2", "L_l1l2", "L_x1"; // optimize linear graph to get full delta config - VectorConfig delta = fg.optimize(ordering).scale(-1.0); + VectorConfig delta = fg.optimize(ordering); if (verbose) delta.print("Delta Config"); // update both state variables diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index de11fc8ef..2a0418b2d 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -15,10 +15,12 @@ #include "NonlinearEquality.h" #include "VectorConfig.h" #include "Ordering.h" +#include "NonlinearOptimizer.h" #include "SQPOptimizer.h" // implementations #include "NonlinearConstraint-inl.h" +#include "NonlinearOptimizer-inl.h" #include "SQPOptimizer-inl.h" using namespace std; @@ -157,7 +159,7 @@ TEST ( SQPOptimizer, map_warp_initLam ) { expected.insert("l1", Vector_(2, 1.0, 6.0)); expected.insert("l2", Vector_(2, 1.0, 6.0)); expected.insert("x2", Vector_(2, 5.0, 6.0)); - CHECK(assert_equal(actual, expected)); + CHECK(assert_equal(expected, actual)); } /* ********************************************************************* */ @@ -191,7 +193,7 @@ TEST ( SQPOptimizer, map_warp ) { expected.insert("l1", Vector_(2, 1.0, 6.0)); expected.insert("l2", Vector_(2, 1.0, 6.0)); expected.insert("x2", Vector_(2, 5.0, 6.0)); - CHECK(assert_equal(actual, expected)); + CHECK(assert_equal(expected, actual)); } /* ********************************************************************* */ @@ -235,13 +237,16 @@ Vector g_func(const VectorConfig& config, const list& keys) { /** gradient at pose */ Matrix grad_g1(const VectorConfig& config, const list& keys) { - Matrix grad; - return grad; + Vector x2 = config[keys.front()], obs = config[keys.back()]; + Vector grad = 2.0*(x2-obs); + return Matrix_(1,2, grad(0), grad(1)); } /** gradient at obstacle */ Matrix grad_g2(const VectorConfig& config, const list& keys) { - return -1*eye(1); + Vector x2 = config[keys.front()], obs = config[keys.back()]; + Vector grad = -2.0*(x2-obs); + return Matrix_(1,2, grad(0), grad(1)); } } @@ -276,13 +281,59 @@ pair obstacleAvoidGraph() { graph.push_back(e1); graph.push_back(e2); graph.push_back(e3); - graph.push_back(c1); + //graph.push_back(c1); graph.push_back(f1); graph.push_back(f2); return make_pair(graph, feasible); } +/* ********************************************************************* */ +TEST ( SQPOptimizer, trajectory_shortening ) { + // fix start, end 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")); + + // construct the graph + NLGraph graph; + graph.push_back(e1); + graph.push_back(e2); + graph.push_back(e3); + graph.push_back(f1); + graph.push_back(f2); + + // optimize + typedef NonlinearOptimizer Optimizer; + //typedef SQPOptimizer Optimizer; + Ordering ordering; + ordering += "x1", "x2", "x3", "obs"; + shared_config config(new VectorConfig(feasible)); + config->insert("x2", Vector_(2, 5.0, 10.0)); + Optimizer optimizer(graph, ordering, config); + + // perform iteration + Optimizer afterOneIteration = optimizer.iterate(); + + // print for evaluation +// config->print("Initial config"); +// afterOneIteration.config()->print("Config after optimization"); +} + /* ********************************************************************* */ TEST ( SQPOptimizer, inequality_inactive ) { // create the graph @@ -291,7 +342,7 @@ TEST ( SQPOptimizer, inequality_inactive ) { // create the rest of the config shared_config init(new VectorConfig(feasible)); - init->insert("x2", Vector_(2, 5.0, 0.6)); + init->insert("x2", Vector_(2, 5.0, 100.0)); // create an ordering Ordering ord; @@ -301,18 +352,21 @@ TEST ( SQPOptimizer, inequality_inactive ) { Optimizer optimizer(graph, ord, init); // perform optimization - // FIXME: avoidance constraint not correctly implemented - //Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); + Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); + + // print for evaluation +// init->print("Before SQP iteration"); +// afterOneIteration.config()->print("After SQP iteration"); } /* ********************************************************************* */ -TEST ( SQPOptimizer, inequality_active ) { - // create the graph - NLGraph graph; VectorConfig feasible; - boost::tie(graph, feasible) = obstacleAvoidGraph(); - -} +//TEST ( SQPOptimizer, inequality_active ) { +// // create the graph +// NLGraph graph; VectorConfig feasible; +// boost::tie(graph, feasible) = obstacleAvoidGraph(); +// +//} /* ************************************************************************* */