12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed sign issue on nonlinear constraints, so that that it is not necessary to flip the sign on the delta configs before using exmap.

release/4.3a0
Alex Cunningham 2009-11-28 21:00:09 +00:00
parent 0ff7e3a5d9
commit 395e4ae3f1
5 changed files with 78 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/NonlinearConstraint-inl.h
View File

@ -86,7 +86,7 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
// construct the constraint // 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); return std::make_pair(factor, constraint);
} }
@ -163,7 +163,7 @@ NonlinearConstraint2<Config>::linearize(const Config& config, const VectorConfig
this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0)); this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
// construct the constraint // 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); return std::make_pair(factor, constraint);
} }

2
cpp/SQPOptimizer-inl.h
View File

@ -96,7 +96,7 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
if (verbose) fg.print("Before Optimization"); if (verbose) fg.print("Before Optimization");
// optimize linear graph to get full delta config // 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"); if (verbose) delta.print("Delta Config");

6
cpp/testNonlinearConstraint.cpp
View File

@ -66,7 +66,7 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
// verify // verify
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0); 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(*actFactor, expFactor));
CHECK(assert_equal(*actConstraint, expConstraint)); CHECK(assert_equal(*actConstraint, expConstraint));
} }
@ -159,7 +159,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
"L_xy", eye(1), zero(1), 1.0); "L_xy", eye(1), zero(1), 1.0);
GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), GaussianFactor expConstraint("x", Matrix_(1,1, 2.0),
"y", Matrix_(1,1, -1.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(*actFactor, expFactor));
CHECK(assert_equal(*actConstraint, expConstraint)); CHECK(assert_equal(*actConstraint, expConstraint));
} }
@ -248,7 +248,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
// verify // verify
GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x", eye(1), zero(1), 1.0); 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(*actFactor2, expFactor));
CHECK(assert_equal(*actConstraint2, expConstraint)); CHECK(assert_equal(*actConstraint2, expConstraint));
} }

6
cpp/testSQP.cpp
View File

@ -223,11 +223,11 @@ TEST (SQP, problem1_sqp ) {
// solve // solve
Ordering ord; Ordering ord;
ord += "x", "y", "lam"; ord += "x", "y", "lam";
VectorConfig delta = fg.optimize(ord).scale(-1.0); // flip sign VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
// update initial estimate // update initial estimate
VectorConfig newState = state.exmap(delta); VectorConfig newState = state.exmap(delta.scale(-1.0));
// set the state to the updated state // set the state to the updated state
state = newState; state = newState;
@ -434,7 +434,7 @@ TEST (SQP, two_pose ) {
ordering += "x1", "x2", "l1", "l2", "L_l1l2", "L_x1"; ordering += "x1", "x2", "l1", "l2", "L_l1l2", "L_x1";
// optimize linear graph to get full delta config // 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"); if (verbose) delta.print("Delta Config");
// update both state variables // update both state variables

84
cpp/testSQPOptimizer.cpp
View File

@ -15,10 +15,12 @@
#include "NonlinearEquality.h" #include "NonlinearEquality.h"
#include "VectorConfig.h" #include "VectorConfig.h"
#include "Ordering.h" #include "Ordering.h"
#include "NonlinearOptimizer.h"
#include "SQPOptimizer.h" #include "SQPOptimizer.h"
// implementations // implementations
#include "NonlinearConstraint-inl.h" #include "NonlinearConstraint-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "SQPOptimizer-inl.h" #include "SQPOptimizer-inl.h"
using namespace std; using namespace std;
@ -157,7 +159,7 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
expected.insert("l1", Vector_(2, 1.0, 6.0)); expected.insert("l1", Vector_(2, 1.0, 6.0));
expected.insert("l2", Vector_(2, 1.0, 6.0)); expected.insert("l2", Vector_(2, 1.0, 6.0));
expected.insert("x2", Vector_(2, 5.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("l1", Vector_(2, 1.0, 6.0));
expected.insert("l2", Vector_(2, 1.0, 6.0)); expected.insert("l2", Vector_(2, 1.0, 6.0));
expected.insert("x2", Vector_(2, 5.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<string>& keys) {
/** gradient at pose */ /** gradient at pose */
Matrix grad_g1(const VectorConfig& config, const list<string>& keys) { Matrix grad_g1(const VectorConfig& config, const list<string>& keys) {
Matrix grad; Vector x2 = config[keys.front()], obs = config[keys.back()];
return grad; Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
} }
/** gradient at obstacle */ /** gradient at obstacle */
Matrix grad_g2(const VectorConfig& config, const list<string>& keys) { Matrix grad_g2(const VectorConfig& config, const list<string>& 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<NLGraph, VectorConfig> obstacleAvoidGraph() {
graph.push_back(e1); graph.push_back(e1);
graph.push_back(e2); graph.push_back(e2);
graph.push_back(e3); graph.push_back(e3);
graph.push_back(c1); //graph.push_back(c1);
graph.push_back(f1); graph.push_back(f1);
graph.push_back(f2); graph.push_back(f2);
return make_pair(graph, feasible); 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<NLGraph, VectorConfig> Optimizer;
//typedef SQPOptimizer<NLGraph, VectorConfig> 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 ) { TEST ( SQPOptimizer, inequality_inactive ) {
// create the graph // create the graph
@ -291,7 +342,7 @@ TEST ( SQPOptimizer, inequality_inactive ) {
// create the rest of the config // create the rest of the config
shared_config init(new VectorConfig(feasible)); 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 // create an ordering
Ordering ord; Ordering ord;
@ -301,18 +352,21 @@ TEST ( SQPOptimizer, inequality_inactive ) {
Optimizer optimizer(graph, ord, init); Optimizer optimizer(graph, ord, init);
// perform optimization // 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 ) { //TEST ( SQPOptimizer, inequality_active ) {
// create the graph // // create the graph
NLGraph graph; VectorConfig feasible; // NLGraph graph; VectorConfig feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph(); // boost::tie(graph, feasible) = obstacleAvoidGraph();
//
} //}
/* ************************************************************************* */ /* ************************************************************************* */