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

Disabling SQP tests while transitioning config types and SQP implementation

release/4.3a0
Alex Cunningham 2010-02-04 16:07:20 +00:00
parent 843f11678b
commit 01bbd3cf8d
2 changed files with 1167 additions and 1181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1678
cpp/testSQP.cpp
View File

File diff suppressed because it is too large Load Diff

670
cpp/testSQPOptimizer.cpp
View File

@ -17,11 +17,11 @@
#include "NonlinearEquality.h" #include "NonlinearEquality.h"
#include "VectorConfig.h" #include "VectorConfig.h"
#include "Ordering.h" #include "Ordering.h"
#include "SQPOptimizer.h" //#include "SQPOptimizer.h"
// implementations // implementations
#include "NonlinearConstraint-inl.h" #include "NonlinearConstraint-inl.h"
#include "SQPOptimizer-inl.h" //#include "SQPOptimizer-inl.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -31,349 +31,335 @@ using namespace simulated2D;
static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
// typedefs //// typedefs
typedef simulated2D::Config Config2D; //typedef simulated2D::Config Config2D;
typedef boost::shared_ptr<Config2D> shared_config; //typedef boost::shared_ptr<Config2D> shared_config;
typedef NonlinearFactorGraph<Config2D> NLGraph; //typedef NonlinearFactorGraph<Config2D> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared; //typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
//
//namespace map_warp_example {
//typedef NonlinearConstraint1<
// Config2D, simulated2D::PoseKey, Point2> NLC1;
//typedef NonlinearConstraint2<
// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
//} // \namespace map_warp_example
//
///* ********************************************************************* */
//// Example that moves two separate maps into the same frame of reference
//// Note that this is a linear example, so it should converge in one step
///* ********************************************************************* */
//
//namespace sqp_LinearMapWarp2 {
//// binary constraint between landmarks
///** g(x) = x-y = 0 */
//Vector g_func(const Config2D& config, const PointKey& key1, const PointKey& key2) {
// Point2 p = config[key1]-config[key2];
// return Vector_(2, p.x(), p.y());
//}
//
///** jacobian at l1 */
//Matrix jac_g1(const Config2D& config) {
// return eye(2);
//}
//
///** jacobian at l2 */
//Matrix jac_g2(const Config2D& config) {
// return -1*eye(2);
//}
//} // \namespace sqp_LinearMapWarp2
//
//namespace sqp_LinearMapWarp1 {
//// Unary Constraint on x1
///** g(x) = x -[1;1] = 0 */
//Vector g_func(const Config2D& config, const PoseKey& key) {
// Point2 p = config[key]-Point2(1.0, 1.0);
// return Vector_(2, p.x(), p.y());
//}
//
///** jacobian at x1 */
//Matrix jac_g(const Config2D& config) {
// return eye(2);
//}
//} // \namespace sqp_LinearMapWarp12
//
////typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
//
///**
// * Creates the graph with each robot seeing the landmark, and it is
// * known that it is the same landmark
// */
//NLGraph linearMapWarpGraph() {
// using namespace map_warp_example;
// // keys
// PoseKey x1(1), x2(2);
// PointKey l1(1), l2(2);
//
// // constant constraint on x1
// list<Symbol> keyx; keyx += "x1";
// LagrangeKey L1(1);
// shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
// x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
// 2, L1));
//
// // measurement from x1 to l1
// Point2 z1(0.0, 5.0);
// shared f1(new simulated2D::Measurement(z1, sigma, 1,1));
//
// // measurement from x2 to l2
// Point2 z2(-4.0, 0.0);
// shared f2(new simulated2D::Measurement(z2, sigma, 2,2));
//
// // equality constraint between l1 and l2
// LagrangeKey L12(12);
// list<Symbol> keys; keys += "l1", "l2";
// shared_ptr<NLC2> c2 (new NLC2(
// boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
// l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
// l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
// 2, L12));
//
// // construct the graph
// NLGraph graph;
// graph.push_back(c1);
// graph.push_back(c2);
// graph.push_back(f1);
// graph.push_back(f2);
//
// return graph;
//}
namespace map_warp_example { ///* ********************************************************************* */
typedef NonlinearConstraint1< //TEST ( SQPOptimizer, map_warp_initLam ) {
Config2D, simulated2D::PoseKey, Point2> NLC1; // bool verbose = false;
typedef NonlinearConstraint2< // // get a graph
Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; // NLGraph graph = linearMapWarpGraph();
} // \namespace map_warp_example //
// // keys
// PoseKey x1(1), x2(2);
// PointKey l1(1), l2(2);
// LagrangeKey L1(1), L12(12);
//
// // create an initial estimate
// shared_config initialEstimate(new Config2D);
// initialEstimate->insert(x1, Point2(1.0, 1.0));
// initialEstimate->insert(l1, Point2(1.0, 6.0));
// initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
// initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
// initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
// initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
//
// // create an ordering
// Ordering ordering;
// ordering += "x1", "x2", "l1", "l2", "L12", "L1";
//
// // create an optimizer
// Optimizer optimizer(graph, ordering, initialEstimate);
// if (verbose) optimizer.print("Initialized Optimizer");
//
// // perform an iteration of optimization
// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
//
// // get the config back out and verify
// Config2D actual = *(oneIteration.config());
// Config2D expected;
// expected.insert(x1, Point2(1.0, 1.0));
// expected.insert(l1, Point2(1.0, 6.0));
// expected.insert(l2, Point2(1.0, 6.0));
// expected.insert(x2, Point2(5.0, 6.0));
// CHECK(assert_equal(expected, actual));
//}
/* ********************************************************************* */ ///* ********************************************************************* */
TEST ( SQPOptimizer, basic ) { //TEST ( SQPOptimizer, map_warp ) {
// create a basic optimizer // bool verbose = false;
NLGraph graph; // // get a graph
Ordering ordering; // NLGraph graph = linearMapWarpGraph();
shared_config config(new Config2D); // if (verbose) graph.print("Initial map warp graph");
//
SQPOptimizer<NLGraph, Config2D> optimizer(graph, ordering, config); // // keys
// PoseKey x1(1), x2(2);
// verify components // PointKey l1(1), l2(2);
CHECK(assert_equal(graph, *(optimizer.graph()))); //
CHECK(assert_equal(ordering, *(optimizer.ordering()))); // // create an initial estimate
CHECK(assert_equal(*config, *(optimizer.config()))); // shared_config initialEstimate(new Config2D);
} // initialEstimate->insert(x1, Point2(1.0, 1.0));
// initialEstimate->insert(l1, Point2(.0, 6.0));
/* ********************************************************************* */ // initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
// Example that moves two separate maps into the same frame of reference // initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin
// Note that this is a linear example, so it should converge in one step //
/* ********************************************************************* */ // // create an ordering
// Ordering ordering;
namespace sqp_LinearMapWarp2 { // ordering += "x1", "x2", "l1", "l2";
// binary constraint between landmarks //
/** g(x) = x-y = 0 */ // // create an optimizer
Vector g_func(const Config2D& config, const PointKey& key1, const PointKey& key2) { // Optimizer optimizer(graph, ordering, initialEstimate);
Point2 p = config[key1]-config[key2]; //
return Vector_(2, p.x(), p.y()); // // perform an iteration of optimization
} // Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
//
/** jacobian at l1 */ // // get the config back out and verify
Matrix jac_g1(const Config2D& config) { // Config2D actual = *(oneIteration.config());
return eye(2); // Config2D expected;
} // expected.insert(x1, Point2(1.0, 1.0));
// expected.insert(l1, Point2(1.0, 6.0));
/** jacobian at l2 */ // expected.insert(l2, Point2(1.0, 6.0));
Matrix jac_g2(const Config2D& config) { // expected.insert(x2, Point2(5.0, 6.0));
return -1*eye(2); // CHECK(assert_equal(expected, actual));
} //}
} // \namespace sqp_LinearMapWarp2 //
///* ********************************************************************* */
namespace sqp_LinearMapWarp1 { //// This is an obstacle avoidance demo, where there is a trajectory of
// Unary Constraint on x1 //// three points, where there is a circular obstacle in the middle. There
/** g(x) = x -[1;1] = 0 */ //// is a binary inequality constraint connecting the obstacle to the
Vector g_func(const Config2D& config, const PoseKey& key) { //// states, which enforces a minimum distance.
Point2 p = config[key]-Point2(1.0, 1.0); ///* ********************************************************************* */
return Vector_(2, p.x(), p.y()); //
} //typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
//typedef shared_ptr<AvoidConstraint> shared_a;
/** jacobian at x1 */ //typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseConstraint;
Matrix jac_g(const Config2D& config) { //typedef shared_ptr<PoseConstraint> shared_pc;
return eye(2); //typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
} //typedef shared_ptr<ObstacleConstraint> shared_oc;
} // \namespace sqp_LinearMapWarp12 //
//
typedef SQPOptimizer<NLGraph, Config2D> Optimizer; //namespace sqp_avoid1 {
//// avoidance radius
/** //double radius = 1.0;
* Creates the graph with each robot seeing the landmark, and it is //
* known that it is the same landmark //// binary avoidance constraint
*/ ///** g(x) = ||x2-obs||^2 - radius^2 > 0 */
NLGraph linearMapWarpGraph() { //Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
using namespace map_warp_example; // double dist2 = config[x].dist(config[obs]);
// keys // double thresh = radius*radius;
PoseKey x1(1), x2(2); // return Vector_(1, dist2-thresh);
PointKey l1(1), l2(2); //}
//
// constant constraint on x1 ///** jacobian at pose */
list<Symbol> keyx; keyx += "x1"; //Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), // Point2 p = config[x]-config[obs];
x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), // return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
2, "L1")); //}
//
// measurement from x1 to l1 ///** jacobian at obstacle */
Point2 z1(0.0, 5.0); //Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
shared f1(new simulated2D::Measurement(z1, sigma, 1,1)); // Point2 p = config[x]-config[obs];
// return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
// measurement from x2 to l2 //}
Point2 z2(-4.0, 0.0); //}
shared f2(new simulated2D::Measurement(z2, sigma, 2,2)); //
//pair<NLGraph, Config2D> obstacleAvoidGraph() {
// equality constraint between l1 and l2 // // Keys
list<Symbol> keys; keys += "l1", "l2"; // PoseKey x1(1), x2(2), x3(3);
shared_ptr<NLC2> c2 (new NLC2( // PointKey l1(1);
boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), // LagrangeKey L20(20);
l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), //
l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), // // Constrained Points
2, "L12")); // Point2 pt_x1,
// pt_x3(10.0, 0.0),
// construct the graph // pt_l1(5.0, -0.5);
NLGraph graph; //
graph.push_back(c1); // shared_pc e1(new PoseConstraint(x1, pt_x1));
graph.push_back(c2); // shared_pc e2(new PoseConstraint(x3, pt_x3));
graph.push_back(f1); // shared_oc e3(new ObstacleConstraint(l1, pt_l1));
graph.push_back(f2); //
// // measurement from x1 to x2
return graph; // Point2 x1x2(5.0, 0.0);
} // shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
//
/* ********************************************************************* */ // // measurement from x2 to x3
TEST ( SQPOptimizer, map_warp_initLam ) { // Point2 x2x3(5.0, 0.0);
bool verbose = false; // shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
// get a graph //
NLGraph graph = linearMapWarpGraph(); // // create a binary inequality constraint that forces the middle point away from
// // the obstacle
// keys // shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1),
PoseKey x1(1), x2(2); // x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1),
PointKey l1(1), l2(2); // l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1),
// 1, L20, false));
// create an initial estimate //
shared_config initialEstimate(new Config2D); // // construct the graph
initialEstimate->insert(x1, Point2(1.0, 1.0)); // NLGraph graph;
initialEstimate->insert(l1, Point2(1.0, 6.0)); // graph.push_back(e1);
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame // graph.push_back(e2);
initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin // graph.push_back(e3);
// graph.push_back(c1);
// create an initial estimate for the lagrange multiplier // graph.push_back(f1);
shared_ptr<VectorConfig> initLagrange(new VectorConfig); // graph.push_back(f2);
initLagrange->insert("L12", Vector_(2, 1.0, 1.0)); //
initLagrange->insert("L1", Vector_(2, 1.0, 1.0)); // // make a config of the fixed values, for convenience
// Config2D config;
// create an ordering // config.insert(x1, pt_x1);
Ordering ordering; // config.insert(x3, pt_x3);
ordering += "x1", "x2", "l1", "l2", "L12", "L1"; // config.insert(l1, pt_l1);
//
// create an optimizer // return make_pair(graph, config);
Optimizer optimizer(graph, ordering, initialEstimate, initLagrange); //}
if (verbose) optimizer.print("Initialized Optimizer"); //
///* ********************************************************************* */
// perform an iteration of optimization //TEST ( SQPOptimizer, inequality_avoid ) {
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); // // create the graph
// NLGraph graph; Config2D feasible;
// get the config back out and verify // boost::tie(graph, feasible) = obstacleAvoidGraph();
Config2D actual = *(oneIteration.config()); //
Config2D expected; // // create the rest of the config
expected.insert(x1, Point2(1.0, 1.0)); // shared_ptr<Config2D> init(new Config2D(feasible));
expected.insert(l1, Point2(1.0, 6.0)); // PoseKey x2(2);
expected.insert(l2, Point2(1.0, 6.0)); // init->insert(x2, Point2(5.0, 100.0));
expected.insert(x2, Point2(5.0, 6.0)); //
CHECK(assert_equal(expected, actual)); // // create an ordering
} // Ordering ord;
// ord += "x1", "x2", "x3", "l1";
/* ********************************************************************* */ //
TEST ( SQPOptimizer, map_warp ) { // // create an optimizer
bool verbose = false; // Optimizer optimizer(graph, ord, init);
// get a graph //
NLGraph graph = linearMapWarpGraph(); // // perform an iteration of optimization
if (verbose) graph.print("Initial map warp graph"); // // NOTE: the constraint will be inactive in the first iteration,
// // so it will violate the constraint after one iteration
// keys // Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
PoseKey x1(1), x2(2); //
PointKey l1(1), l2(2); // Config2D exp1(feasible);
// exp1.insert(x2, Point2(5.0, 0.0));
// create an initial estimate // CHECK(assert_equal(exp1, *(afterOneIteration.config())));
shared_config initialEstimate(new Config2D); //
initialEstimate->insert(x1, Point2(1.0, 1.0)); // // the second iteration will activate the constraint and force the
initialEstimate->insert(l1, Point2(.0, 6.0)); // // config to a viable configuration.
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame // Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin //
// Config2D exp2(feasible);
// create an ordering // exp2.insert(x2, Point2(5.0, 0.5));
Ordering ordering; // CHECK(assert_equal(exp2, *(after2ndIteration.config())));
ordering += "x1", "x2", "l1", "l2"; //}
//
// create an optimizer ///* ********************************************************************* */
Optimizer optimizer(graph, ordering, initialEstimate); //TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// // create the graph
// perform an iteration of optimization // NLGraph graph; Config2D feasible;
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); // boost::tie(graph, feasible) = obstacleAvoidGraph();
//
// get the config back out and verify // // create the rest of the config
Config2D actual = *(oneIteration.config()); // shared_ptr<Config2D> init(new Config2D(feasible));
Config2D expected; // PoseKey x2(2);
expected.insert(x1, Point2(1.0, 1.0)); // init->insert(x2, Point2(5.0, 100.0));
expected.insert(l1, Point2(1.0, 6.0)); //
expected.insert(l2, Point2(1.0, 6.0)); // // create an ordering
expected.insert(x2, Point2(5.0, 6.0)); // Ordering ord;
CHECK(assert_equal(expected, actual)); // ord += "x1", "x2", "x3", "l1";
} //
// // create an optimizer
/* ********************************************************************* */ // Optimizer optimizer(graph, ord, init);
// This is an obstacle avoidance demo, where there is a trajectory of //
// three points, where there is a circular obstacle in the middle. There // double relThresh = 1e-5; // minimum change in error between iterations
// is a binary inequality constraint connecting the obstacle to the // double absThresh = 1e-5; // minimum error necessary to converge
// states, which enforces a minimum distance. // double constraintThresh = 1e-9; // minimum constraint error to be feasible
/* ********************************************************************* */ // Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
//
typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint; // // verify
typedef shared_ptr<AvoidConstraint> shared_a; // Config2D exp2(feasible);
typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseConstraint; // exp2.insert(x2, Point2(5.0, 0.5));
typedef shared_ptr<PoseConstraint> shared_pc; // CHECK(assert_equal(exp2, *(final.config())));
typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint; //}
typedef shared_ptr<ObstacleConstraint> shared_oc;
namespace sqp_avoid1 {
// avoidance radius
double radius = 1.0;
// binary avoidance constraint
/** g(x) = ||x2-obs||^2 - radius^2 > 0 */
Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
double dist2 = config[x].dist(config[obs]);
double thresh = radius*radius;
return Vector_(1, dist2-thresh);
}
/** jacobian at pose */
Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
Point2 p = config[x]-config[obs];
return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
}
/** jacobian at obstacle */
Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
Point2 p = config[x]-config[obs];
return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
}
}
pair<NLGraph, Config2D> obstacleAvoidGraph() {
// Keys
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1);
// Constrained Points
Point2 pt_x1,
pt_x3(10.0, 0.0),
pt_l1(5.0, -0.5);
shared_pc e1(new PoseConstraint(x1, pt_x1));
shared_pc e2(new PoseConstraint(x3, pt_x3));
shared_oc e3(new ObstacleConstraint(l1, pt_l1));
// measurement from x1 to x2
Point2 x1x2(5.0, 0.0);
shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
// measurement from x2 to x3
Point2 x2x3(5.0, 0.0);
shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
// create a binary inequality constraint that forces the middle point away from
// the obstacle
shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1),
x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1),
l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1),
1, "L20", false));
// construct the graph
NLGraph graph;
graph.push_back(e1);
graph.push_back(e2);
graph.push_back(e3);
graph.push_back(c1);
graph.push_back(f1);
graph.push_back(f2);
// make a config of the fixed values, for convenience
Config2D config;
config.insert(x1, pt_x1);
config.insert(x3, pt_x3);
config.insert(l1, pt_l1);
return make_pair(graph, config);
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_avoid ) {
// create the graph
NLGraph graph; Config2D feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph();
// create the rest of the config
shared_ptr<Config2D> init(new Config2D(feasible));
PoseKey x2(2);
init->insert(x2, Point2(5.0, 100.0));
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "l1";
// create an optimizer
Optimizer optimizer(graph, ord, init);
// perform an iteration of optimization
// NOTE: the constraint will be inactive in the first iteration,
// so it will violate the constraint after one iteration
Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
Config2D exp1(feasible);
exp1.insert(x2, Point2(5.0, 0.0));
CHECK(assert_equal(exp1, *(afterOneIteration.config())));
// the second iteration will activate the constraint and force the
// config to a viable configuration.
Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
Config2D exp2(feasible);
exp2.insert(x2, Point2(5.0, 0.5));
CHECK(assert_equal(exp2, *(after2ndIteration.config())));
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// create the graph
NLGraph graph; Config2D feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph();
// create the rest of the config
shared_ptr<Config2D> init(new Config2D(feasible));
PoseKey x2(2);
init->insert(x2, Point2(5.0, 100.0));
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "l1";
// create an optimizer
Optimizer optimizer(graph, ord, init);
double relThresh = 1e-5; // minimum change in error between iterations
double absThresh = 1e-5; // minimum error necessary to converge
double constraintThresh = 1e-9; // minimum constraint error to be feasible
Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
// verify
Config2D exp2(feasible);
exp2.insert(x2, Point2(5.0, 0.5));
CHECK(assert_equal(exp2, *(final.config())));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }