From 3e2924a5bf93ad532fb509f40040476eb1cd9135 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 6 Mar 2010 22:51:03 +0000 Subject: [PATCH] Uncommented some testSQP tests, which do work --- cpp/testSQP.cpp | 406 ++++++++++++++++++++++++------------------------ 1 file changed, 203 insertions(+), 203 deletions(-) diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index a50843662..9ff0e738b 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -755,119 +755,119 @@ TEST (SQP, stereo_sqp ) { // create optimizer VOptimizer optimizer(graph, truthConfig, solver); -// // optimize -// VOptimizer afterOneIteration = optimizer.iterate(); -// -// // check if correct -// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); + // optimize + VOptimizer afterOneIteration = optimizer.iterate(); + + // check if correct + CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); } -///* ********************************************************************* -// * SQP version of the above stereo example, -// * with noise in the initial estimate -// */ -//TEST (SQP, stereo_sqp_noisy ) { -// bool verbose = false; -// -// // get a graph -// boost::shared_ptr graph = stereoExampleGraph(); -// -// // create initial data -// Rot3 faceDownY(Matrix_(3,3, -// 1.0, 0.0, 0.0, -// 0.0, 0.0, 1.0, -// 0.0, 1.0, 0.0)); -// Pose3 pose1(faceDownY, Point3()); // origin, left camera -// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left -// Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away -// Point3 landmark2(1.5, 5.0, 0.0); -// -// // noisy config -// boost::shared_ptr initConfig(new VConfig); -// initConfig->insert(Pose3Key(1), pose1); -// initConfig->insert(Pose3Key(2), pose2); -// initConfig->insert(Point3Key(1), landmark1); -// initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place -// initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); -// -// // create ordering -// shared_ptr ord(new Ordering()); -// *ord += "x1", "x2", "l1", "l2", "L12"; -// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); -// -// // create optimizer -// VOptimizer optimizer(graph, initConfig, solver); -// -// // optimize -// VOptimizer *pointer = new VOptimizer(optimizer); -// for (int i=0;i<1;i++) { -// VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); -// delete pointer; -// pointer = newOptimizer; -// } -// VOptimizer::shared_config actual = pointer->config(); -// delete(pointer); -// -// // get the truth config -// boost::shared_ptr truthConfig = stereoExampleTruthConfig(); -// truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0)); -// -// // check if correct -// CHECK(assert_equal(*truthConfig,*actual, 1e-5)); -//} -// -//static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); -// -//// typedefs -////typedef simulated2D::Config Config2D; -////typedef boost::shared_ptr shared_config; -////typedef NonlinearFactorGraph NLGraph; -////typedef boost::shared_ptr > 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 simulated2D::PointKey& key1, const simulated2D::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 simulated2D::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 +/* ********************************************************************* + * SQP version of the above stereo example, + * with noise in the initial estimate + */ +TEST (SQP, stereo_sqp_noisy ) { + bool verbose = false; + + // get a graph + boost::shared_ptr graph = stereoExampleGraph(); + + // create initial data + Rot3 faceDownY(Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, 1.0, 0.0)); + Pose3 pose1(faceDownY, Point3()); // origin, left camera + Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left + Point3 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away + Point3 landmark2(1.5, 5.0, 0.0); + + // noisy config + boost::shared_ptr initConfig(new VConfig); + initConfig->insert(Pose3Key(1), pose1); + initConfig->insert(Pose3Key(2), pose2); + initConfig->insert(Point3Key(1), landmark1); + initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place + initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); + + // create ordering + shared_ptr ord(new Ordering()); + *ord += "x1", "x2", "l1", "l2", "L12"; + VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); + + // create optimizer + VOptimizer optimizer(graph, initConfig, solver); + + // optimize + VOptimizer *pointer = new VOptimizer(optimizer); + for (int i=0;i<1;i++) { + VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); + delete pointer; + pointer = newOptimizer; + } + VOptimizer::shared_config actual = pointer->config(); + delete(pointer); + + // get the truth config + boost::shared_ptr truthConfig = stereoExampleTruthConfig(); + truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0)); + + // check if correct + CHECK(assert_equal(*truthConfig,*actual, 1e-5)); +} + +static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); + +// typedefs +//typedef simulated2D::Config Config2D; +//typedef boost::shared_ptr shared_config; +//typedef NonlinearFactorGraph NLGraph; +//typedef boost::shared_ptr > 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 simulated2D::PointKey& key1, const simulated2D::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 simulated2D::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 Optimizer; @@ -875,98 +875,98 @@ TEST (SQP, stereo_sqp ) { * Creates the graph with each robot seeing the landmark, and it is * known that it is the same landmark */ -//boost::shared_ptr linearMapWarpGraph() { -// using namespace map_warp_example; -// // keys -// simulated2D::PoseKey x1(1), x2(2); -// simulated2D::PointKey l1(1), l2(2); -// -// // constant constraint on x1 -// LagrangeKey L1(1); -// shared_ptr 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::GenericMeasurement(z1, sigma, x1,l1)); -// -// // measurement from x2 to l2 -// Point2 z2(-4.0, 0.0); -// shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); -// -// // equality constraint between l1 and l2 -// LagrangeKey L12(12); -// shared_ptr 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 -// boost::shared_ptr graph(new Graph2D()); -// graph->push_back(c1); -// graph->push_back(c2); -// graph->push_back(f1); -// graph->push_back(f2); -// -// return graph; -//} -// -///* ********************************************************************* */ -//TEST ( SQPOptimizer, map_warp_initLam ) { -// bool verbose = false; -// // get a graph -// boost::shared_ptr graph = linearMapWarpGraph(); -// -// // keys -// simulated2D::PoseKey x1(1), x2(2); -// simulated2D::PointKey l1(1), l2(2); -// LagrangeKey L1(1), L12(12); -// -// // create an initial estimate -// shared_ptr 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 -// shared_ptr ordering(new Ordering()); -// *ordering += "x1", "x2", "l1", "l2", "L12", "L1"; -// -// // create an optimizer -// Optimizer::shared_solver solver(new Optimizer::solver(ordering)); -// Optimizer optimizer(graph, initialEstimate, solver); -// -// // 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)); -// expected.insert(L1, Vector_(2, 1.0, 1.0)); -// expected.insert(L12, Vector_(2, 6.0, 7.0)); -// CHECK(assert_equal(expected, actual)); -//} +boost::shared_ptr linearMapWarpGraph() { + using namespace map_warp_example; + // keys + simulated2D::PoseKey x1(1), x2(2); + simulated2D::PointKey l1(1), l2(2); + + // constant constraint on x1 + LagrangeKey L1(1); + shared_ptr 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::GenericMeasurement(z1, sigma, x1,l1)); + + // measurement from x2 to l2 + Point2 z2(-4.0, 0.0); + shared f2(new simulated2D::GenericMeasurement(z2, sigma, x2,l2)); + + // equality constraint between l1 and l2 + LagrangeKey L12(12); + shared_ptr 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 + boost::shared_ptr graph(new Graph2D()); + graph->push_back(c1); + graph->push_back(c2); + graph->push_back(f1); + graph->push_back(f2); + + return graph; +} + +/* ********************************************************************* */ +TEST ( SQPOptimizer, map_warp_initLam ) { + bool verbose = false; + // get a graph + boost::shared_ptr graph = linearMapWarpGraph(); + + // keys + simulated2D::PoseKey x1(1), x2(2); + simulated2D::PointKey l1(1), l2(2); + LagrangeKey L1(1), L12(12); + + // create an initial estimate + shared_ptr 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 + shared_ptr ordering(new Ordering()); + *ordering += "x1", "x2", "l1", "l2", "L12", "L1"; + + // create an optimizer + Optimizer::shared_solver solver(new Optimizer::solver(ordering)); + Optimizer optimizer(graph, initialEstimate, solver); + + // 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)); + expected.insert(L1, Vector_(2, 1.0, 1.0)); + expected.insert(L12, Vector_(2, 6.0, 7.0)); + CHECK(assert_equal(expected, actual)); +} + +/* ********************************************************************* */ +// This is an obstacle avoidance demo, where there is a trajectory of +// three points, where there is a circular obstacle in the middle. There +// is a binary inequality constraint connecting the obstacle to the +// states, which enforces a minimum distance. +/* ********************************************************************* */ -///* ********************************************************************* */ -//// This is an obstacle avoidance demo, where there is a trajectory of -//// three points, where there is a circular obstacle in the middle. There -//// is a binary inequality constraint connecting the obstacle to the -//// states, which enforces a minimum distance. -///* ********************************************************************* */ -// //typedef NonlinearConstraint2 AvoidConstraint; //typedef shared_ptr shared_a; -//typedef NonlinearEquality PoseConstraint; -//typedef shared_ptr shared_pc; +//typedef NonlinearEquality PoseNLConstraint; +//typedef shared_ptr shared_pc; //typedef NonlinearEquality ObstacleConstraint; //typedef shared_ptr shared_oc; // @@ -1007,8 +1007,8 @@ TEST (SQP, stereo_sqp ) { // 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_pc e1(new PoseNLConstraint(x1, pt_x1)); +// shared_pc e2(new PoseNLConstraint(x3, pt_x3)); // shared_oc e3(new ObstacleConstraint(l1, pt_l1)); // // // measurement from x1 to x2 @@ -1079,7 +1079,7 @@ TEST (SQP, stereo_sqp ) { // exp2.insert(x2, Point2(5.0, 0.5)); // CHECK(assert_equal(exp2, *(after2ndIteration.config()))); //} -// + ///* ********************************************************************* */ //TEST ( SQPOptimizer, inequality_avoid_iterative ) { // // create the graph