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

Changed initial SQP example to use exmap using the new scaling function for VectorConfig

release/4.3a0
Alex Cunningham 2009-11-20 05:13:32 +00:00
parent acfe742c29
commit 6aba2f1c1c
1 changed files with 45 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
cpp/testSQP.cpp
View File

@ -20,7 +20,8 @@
// templated implementations // templated implementations
#include <NonlinearFactorGraph-inl.h> #include <NonlinearFactorGraph-inl.h>
#include "NonlinearOptimizer-inl.h" #include <NonlinearConstraint-inl.h>
#include <NonlinearOptimizer-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -219,19 +220,11 @@ TEST (SQP, problem1_sqp ) {
// solve // solve
Ordering ord; Ordering ord;
ord += "x", "y", "lam"; ord += "x", "y", "lam";
VectorConfig delta = fg.optimize(ord); VectorConfig delta = fg.optimize(ord).scale(-1.0); // flip sign
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
// update initial estimate // update initial estimate
double gain = 1.0; VectorConfig newState = state.exmap(delta);
// Note:
// It appears that exmap uses adding rather than subtracting to
// update, which breaks the system.
//VectorConfig newState = state.exmap(delta);
VectorConfig newState;
newState.insert("x", state["x"]-gain*delta["x"]);
newState.insert("y", state["y"]-gain*delta["y"]);
newState.insert("lam", state["lam"]-gain*delta["lam"]);
// set the state to the updated state // set the state to the updated state
state = newState; state = newState;
@ -318,8 +311,25 @@ TEST (SQP, two_pose_truth ) {
CHECK(assert_equal(expected, *actual, 1e-5)); CHECK(assert_equal(expected, *actual, 1e-5));
} }
// Test of binary nonlinear equality constraint disabled until nonlinear constraints work namespace sqp_test1 {
/** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
return config[key1]-config[key2];
}
/** gradient at l1 */
Matrix grad_g1(const VectorConfig& config, const std::string& key) {
return eye(2);
}
/** gradient at l2 */
Matrix grad_g2(const VectorConfig& config, const std::string& key) {
return -1*eye(2);
}
} // \namespace sqp_test1
//typedef SQPOptimizer<NLGraph,VectorConfig> Optimizer;
//
///** ///**
// * Version that actually uses nonlinear equality constraints // * Version that actually uses nonlinear equality constraints
// * to to perform optimization. Same as above, but no // * to to perform optimization. Same as above, but no
@ -342,14 +352,14 @@ TEST (SQP, two_pose_truth ) {
// // measurement from x2 to l2 // // measurement from x2 to l2
// Vector z2 = Vector_(2, -4.0, 0.0); // Vector z2 = Vector_(2, -4.0, 0.0);
// double sigma2 = 0.1; // double sigma2 = 0.1;
// shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1")); // shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
// //
// // equality constraint between l1 and l2 // // equality constraint between l1 and l2
// boost::shared_ptr<NonlinearConstraint<VectorConfig> > c1( // boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c1(
// new NonlinearConstraint<VectorConfig>( // new NonlinearConstraint2<VectorConfig>(
// "l1", "l2", // specify nodes constrained // "l1", *sqp_test1::grad_g1,
// *g_function, // evaluate the cost // "l2", *sqp_test1::grad_g2,
// *gradG_function)); // construct the gradient of g(x) // *sqp_test1::g_func, 2, "L_l1l2"));
// //
// // construct the graph // // construct the graph
// NLGraph graph; // NLGraph graph;
@ -361,24 +371,25 @@ TEST (SQP, two_pose_truth ) {
// // create an initial estimate // // create an initial estimate
// boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set // boost::shared_ptr<VectorConfig> 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.0, 6.0)); // ground truth
// //initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error // initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame
// initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin
// //
// // optimize the graph // // optimize the graph
// Ordering ordering; //// Ordering ordering;
// ordering += "x1", "x2", "l1"; //// ordering += "x1", "x2", "l1";
// Optimizer optimizer(graph, ordering, initialEstimate, 1e-5); //// NewOptimizer optimizer(graph, ordering, initialEstimate, 1e-5);
// ////
// // display solution //// // display solution
// double relativeThreshold = 1e-5; //// double relativeThreshold = 1e-5;
// double absoluteThreshold = 1e-5; //// double absoluteThreshold = 1e-5;
// Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); //// Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold);
// boost::shared_ptr<const VectorConfig> actual = act_opt.config(); //// boost::shared_ptr<const VectorConfig> actual = act_opt.config();
// //actual->print("Configuration after optimization"); //// //actual->print("Configuration after optimization");
// ////
// // verify //// // verify
// VectorConfig expected(feas); //// VectorConfig expected(feas);
// expected.insert("l1", Vector_(2, 1.0, 6.0)); //// expected.insert("l1", Vector_(2, 1.0, 6.0));
// CHECK(assert_equal(expected, *actual, 1e-5)); //// CHECK(assert_equal(expected, *actual, 1e-5));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */