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

Uncommented some testSQP tests, which do work

release/4.3a0
Alex Cunningham 2010-03-06 22:51:03 +00:00
parent 7c3dbf164e
commit 3e2924a5bf
1 changed files with 203 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

406
cpp/testSQP.cpp
View File

@ -755,119 +755,119 @@ TEST (SQP, stereo_sqp ) {
// create optimizer // create optimizer
VOptimizer optimizer(graph, truthConfig, solver); VOptimizer optimizer(graph, truthConfig, solver);
// // optimize // optimize
// VOptimizer afterOneIteration = optimizer.iterate(); VOptimizer afterOneIteration = optimizer.iterate();
//
// // check if correct // check if correct
// CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
} }
///* ********************************************************************* /* *********************************************************************
// * SQP version of the above stereo example, * SQP version of the above stereo example,
// * with noise in the initial estimate * with noise in the initial estimate
// */ */
//TEST (SQP, stereo_sqp_noisy ) { TEST (SQP, stereo_sqp_noisy ) {
// bool verbose = false; bool verbose = false;
//
// // get a graph // get a graph
// boost::shared_ptr<VGraph> graph = stereoExampleGraph(); boost::shared_ptr<VGraph> graph = stereoExampleGraph();
//
// // create initial data // create initial data
// Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
// 1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
// 0.0, 0.0, 1.0, 0.0, 0.0, 1.0,
// 0.0, 1.0, 0.0)); 0.0, 1.0, 0.0));
// Pose3 pose1(faceDownY, Point3()); // origin, left camera Pose3 pose1(faceDownY, Point3()); // origin, left camera
// Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left 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 landmark1(0.5, 5.0, 0.0); //centered between the cameras, 5 units away
// Point3 landmark2(1.5, 5.0, 0.0); Point3 landmark2(1.5, 5.0, 0.0);
//
// // noisy config // noisy config
// boost::shared_ptr<VConfig> initConfig(new VConfig); boost::shared_ptr<VConfig> initConfig(new VConfig);
// initConfig->insert(Pose3Key(1), pose1); initConfig->insert(Pose3Key(1), pose1);
// initConfig->insert(Pose3Key(2), pose2); initConfig->insert(Pose3Key(2), pose2);
// initConfig->insert(Point3Key(1), landmark1); initConfig->insert(Point3Key(1), landmark1);
// initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place initConfig->insert(Point3Key(2), landmark2); // create two landmarks in same place
// initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0)); initConfig->insert(LagrangeKey(12), Vector_(3, 1.0, 1.0, 1.0));
//
// // create ordering // create ordering
// shared_ptr<Ordering> ord(new Ordering()); shared_ptr<Ordering> ord(new Ordering());
// *ord += "x1", "x2", "l1", "l2", "L12"; *ord += "x1", "x2", "l1", "l2", "L12";
// VOptimizer::shared_solver solver(new VOptimizer::solver(ord)); VOptimizer::shared_solver solver(new VOptimizer::solver(ord));
//
// // create optimizer // create optimizer
// VOptimizer optimizer(graph, initConfig, solver); VOptimizer optimizer(graph, initConfig, solver);
//
// // optimize // optimize
// VOptimizer *pointer = new VOptimizer(optimizer); VOptimizer *pointer = new VOptimizer(optimizer);
// for (int i=0;i<1;i++) { for (int i=0;i<1;i++) {
// VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM()); VOptimizer* newOptimizer = new VOptimizer(pointer->iterateLM());
// delete pointer; delete pointer;
// pointer = newOptimizer; pointer = newOptimizer;
// } }
// VOptimizer::shared_config actual = pointer->config(); VOptimizer::shared_config actual = pointer->config();
// delete(pointer); delete(pointer);
//
// // get the truth config // get the truth config
// boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig(); boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
// truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0)); truthConfig->insert(LagrangeKey(12), Vector_(3, 0.0, 1.0, 1.0));
//
// // check if correct // check if correct
// CHECK(assert_equal(*truthConfig,*actual, 1e-5)); CHECK(assert_equal(*truthConfig,*actual, 1e-5));
//} }
//
//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 { namespace map_warp_example {
//typedef NonlinearConstraint1< typedef NonlinearConstraint1<
// Config2D, simulated2D::PoseKey, Point2> NLC1; Config2D, simulated2D::PoseKey, Point2> NLC1;
////typedef NonlinearConstraint2< //typedef NonlinearConstraint2<
//// Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; // Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
//} // \namespace map_warp_example } // \namespace map_warp_example
//
///* ********************************************************************* */ /* ********************************************************************* */
//// Example that moves two separate maps into the same frame of reference // 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 // Note that this is a linear example, so it should converge in one step
///* ********************************************************************* */ /* ********************************************************************* */
//
//namespace sqp_LinearMapWarp2 { namespace sqp_LinearMapWarp2 {
//// binary constraint between landmarks // binary constraint between landmarks
///** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
//Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) { Vector g_func(const Config2D& config, const simulated2D::PointKey& key1, const simulated2D::PointKey& key2) {
// Point2 p = config[key1]-config[key2]; Point2 p = config[key1]-config[key2];
// return Vector_(2, p.x(), p.y()); return Vector_(2, p.x(), p.y());
//} }
//
///** jacobian at l1 */ /** jacobian at l1 */
//Matrix jac_g1(const Config2D& config) { Matrix jac_g1(const Config2D& config) {
// return eye(2); return eye(2);
//} }
//
///** jacobian at l2 */ /** jacobian at l2 */
//Matrix jac_g2(const Config2D& config) { Matrix jac_g2(const Config2D& config) {
// return -1*eye(2); return -1*eye(2);
//} }
//} // \namespace sqp_LinearMapWarp2 } // \namespace sqp_LinearMapWarp2
//
//namespace sqp_LinearMapWarp1 { namespace sqp_LinearMapWarp1 {
//// Unary Constraint on x1 // Unary Constraint on x1
///** g(x) = x -[1;1] = 0 */ /** g(x) = x -[1;1] = 0 */
//Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) { Vector g_func(const Config2D& config, const simulated2D::PoseKey& key) {
// Point2 p = config[key]-Point2(1.0, 1.0); Point2 p = config[key]-Point2(1.0, 1.0);
// return Vector_(2, p.x(), p.y()); return Vector_(2, p.x(), p.y());
//} }
//
///** jacobian at x1 */ /** jacobian at x1 */
//Matrix jac_g(const Config2D& config) { Matrix jac_g(const Config2D& config) {
// return eye(2); return eye(2);
//} }
//} // \namespace sqp_LinearMapWarp12 } // \namespace sqp_LinearMapWarp12
//typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer; //typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
@ -875,98 +875,98 @@ TEST (SQP, stereo_sqp ) {
* Creates the graph with each robot seeing the landmark, and it is * Creates the graph with each robot seeing the landmark, and it is
* known that it is the same landmark * known that it is the same landmark
*/ */
//boost::shared_ptr<Graph2D> linearMapWarpGraph() { boost::shared_ptr<Graph2D> linearMapWarpGraph() {
// using namespace map_warp_example; using namespace map_warp_example;
// // keys // keys
// simulated2D::PoseKey x1(1), x2(2); simulated2D::PoseKey x1(1), x2(2);
// simulated2D::PointKey l1(1), l2(2); simulated2D::PointKey l1(1), l2(2);
//
// // constant constraint on x1 // constant constraint on x1
// LagrangeKey L1(1); LagrangeKey L1(1);
// shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
// x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
// 2, L1)); 2, L1));
//
// // measurement from x1 to l1 // measurement from x1 to l1
// Point2 z1(0.0, 5.0); Point2 z1(0.0, 5.0);
// shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1)); shared f1(new simulated2D::GenericMeasurement<Config2D>(z1, sigma, x1,l1));
//
// // measurement from x2 to l2 // measurement from x2 to l2
// Point2 z2(-4.0, 0.0); Point2 z2(-4.0, 0.0);
// shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2)); shared f2(new simulated2D::GenericMeasurement<Config2D>(z2, sigma, x2,l2));
//
// // equality constraint between l1 and l2 // equality constraint between l1 and l2
// LagrangeKey L12(12); LagrangeKey L12(12);
// shared_ptr<NLC2> c2 (new NLC2( shared_ptr<NLC2> c2 (new NLC2(
// boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
// l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
// l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
// 2, L12)); 2, L12));
//
// // construct the graph // construct the graph
// boost::shared_ptr<Graph2D> graph(new Graph2D()); boost::shared_ptr<Graph2D> graph(new Graph2D());
// graph->push_back(c1); graph->push_back(c1);
// graph->push_back(c2); graph->push_back(c2);
// graph->push_back(f1); graph->push_back(f1);
// graph->push_back(f2); graph->push_back(f2);
//
// return graph; return graph;
//} }
//
///* ********************************************************************* */ /* ********************************************************************* */
//TEST ( SQPOptimizer, map_warp_initLam ) { TEST ( SQPOptimizer, map_warp_initLam ) {
// bool verbose = false; bool verbose = false;
// // get a graph // get a graph
// boost::shared_ptr<Graph2D> graph = linearMapWarpGraph(); boost::shared_ptr<Graph2D> graph = linearMapWarpGraph();
//
// // keys // keys
// simulated2D::PoseKey x1(1), x2(2); simulated2D::PoseKey x1(1), x2(2);
// simulated2D::PointKey l1(1), l2(2); simulated2D::PointKey l1(1), l2(2);
// LagrangeKey L1(1), L12(12); LagrangeKey L1(1), L12(12);
//
// // create an initial estimate // create an initial estimate
// shared_ptr<Config2D> initialEstimate(new Config2D); shared_ptr<Config2D> initialEstimate(new Config2D);
// initialEstimate->insert(x1, Point2(1.0, 1.0)); initialEstimate->insert(x1, Point2(1.0, 1.0));
// initialEstimate->insert(l1, Point2(1.0, 6.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(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(x2, Point2(0.0, 0.0)); // other pose starts at origin
// initialEstimate->insert(L12, Vector_(2, 1.0, 1.0)); initialEstimate->insert(L12, Vector_(2, 1.0, 1.0));
// initialEstimate->insert(L1, Vector_(2, 1.0, 1.0)); initialEstimate->insert(L1, Vector_(2, 1.0, 1.0));
//
// // create an ordering // create an ordering
// shared_ptr<Ordering> ordering(new Ordering()); shared_ptr<Ordering> ordering(new Ordering());
// *ordering += "x1", "x2", "l1", "l2", "L12", "L1"; *ordering += "x1", "x2", "l1", "l2", "L12", "L1";
//
// // create an optimizer // create an optimizer
// Optimizer::shared_solver solver(new Optimizer::solver(ordering)); Optimizer::shared_solver solver(new Optimizer::solver(ordering));
// Optimizer optimizer(graph, initialEstimate, solver); Optimizer optimizer(graph, initialEstimate, solver);
//
// // perform an iteration of optimization // perform an iteration of optimization
// Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
//
// // get the config back out and verify // get the config back out and verify
// Config2D actual = *(oneIteration.config()); Config2D actual = *(oneIteration.config());
// Config2D expected; Config2D expected;
// expected.insert(x1, Point2(1.0, 1.0)); expected.insert(x1, Point2(1.0, 1.0));
// expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l1, Point2(1.0, 6.0));
// expected.insert(l2, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0));
// expected.insert(x2, Point2(5.0, 6.0)); expected.insert(x2, Point2(5.0, 6.0));
// expected.insert(L1, Vector_(2, 1.0, 1.0)); expected.insert(L1, Vector_(2, 1.0, 1.0));
// expected.insert(L12, Vector_(2, 6.0, 7.0)); expected.insert(L12, Vector_(2, 6.0, 7.0));
// CHECK(assert_equal(expected, actual)); 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<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint; //typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
//typedef shared_ptr<AvoidConstraint> shared_a; //typedef shared_ptr<AvoidConstraint> shared_a;
//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseConstraint; //typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseNLConstraint;
//typedef shared_ptr<PoseConstraint> shared_pc; //typedef shared_ptr<PoseNLConstraint> shared_pc;
//typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint; //typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
//typedef shared_ptr<ObstacleConstraint> shared_oc; //typedef shared_ptr<ObstacleConstraint> shared_oc;
// //
@ -1007,8 +1007,8 @@ TEST (SQP, stereo_sqp ) {
// pt_x3(10.0, 0.0), // pt_x3(10.0, 0.0),
// pt_l1(5.0, -0.5); // pt_l1(5.0, -0.5);
// //
// shared_pc e1(new PoseConstraint(x1, pt_x1)); // shared_pc e1(new PoseNLConstraint(x1, pt_x1));
// shared_pc e2(new PoseConstraint(x3, pt_x3)); // shared_pc e2(new PoseNLConstraint(x3, pt_x3));
// shared_oc e3(new ObstacleConstraint(l1, pt_l1)); // shared_oc e3(new ObstacleConstraint(l1, pt_l1));
// //
// // measurement from x1 to x2 // // measurement from x1 to x2
@ -1079,7 +1079,7 @@ TEST (SQP, stereo_sqp ) {
// exp2.insert(x2, Point2(5.0, 0.5)); // exp2.insert(x2, Point2(5.0, 0.5));
// CHECK(assert_equal(exp2, *(after2ndIteration.config()))); // CHECK(assert_equal(exp2, *(after2ndIteration.config())));
//} //}
//
///* ********************************************************************* */ ///* ********************************************************************* */
//TEST ( SQPOptimizer, inequality_avoid_iterative ) { //TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// // create the graph // // create the graph