From f88438bab498fc1942117ce9e895f6a35721e2ec Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 6 Feb 2010 14:48:46 +0000 Subject: [PATCH] Removed SQP optimizer and moved remaining SQP optimizer tests into testSQP. All equality constraints should be fully functional now. Inequality constraints still to come. --- .cproject | 70 ++++---- cpp/Makefile.am | 5 +- cpp/SQPOptimizer-inl.h | 177 ------------------- cpp/SQPOptimizer.h | 115 ------------ cpp/testSQP.cpp | 293 +++++++++++++++++++++++++++++++ cpp/testSQPOptimizer.cpp | 366 --------------------------------------- 6 files changed, 325 insertions(+), 701 deletions(-) delete mode 100644 cpp/SQPOptimizer-inl.h delete mode 100644 cpp/SQPOptimizer.h delete mode 100644 cpp/testSQPOptimizer.cpp diff --git a/.cproject b/.cproject index af582b925..b6946d98e 100644 --- a/.cproject +++ b/.cproject @@ -16,35 +16,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -538,14 +538,6 @@ true true - -make --j2 -testSQPOptimizer.run -true -true -true - make -j2 @@ -822,6 +814,6 @@ - - + + diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 260ef51fd..1a82c6ff9 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -139,12 +139,9 @@ testNonlinearEquality_SOURCES = testNonlinearEquality.cpp testNonlinearEquality_LDADD = libgtsam.la # SQP -headers += SQPOptimizer.h SQPOptimizer-inl.h -check_PROGRAMS += testSQP testSQPOptimizer +check_PROGRAMS += testSQP testSQP_SOURCES = $(example) testSQP.cpp testSQP_LDADD = libgtsam.la -testSQPOptimizer_SOURCES = testSQPOptimizer.cpp -testSQPOptimizer_LDADD = libgtsam.la # geometry headers += Lie.h Lie-inl.h diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h deleted file mode 100644 index de5d7b570..000000000 --- a/cpp/SQPOptimizer-inl.h +++ /dev/null @@ -1,177 +0,0 @@ -/* - * @file SQPOptimizer-inl.h - * @brief Implementation of the SQP Optimizer - * @author Alex Cunningham - */ - -#pragma once - -#include -#include // for operator += -#include // for insert -#include "GaussianFactorGraph.h" -#include "NonlinearFactorGraph.h" -#include "SQPOptimizer.h" - -// implementations -#include "NonlinearConstraint-inl.h" -#include "NonlinearFactorGraph-inl.h" - -using namespace std; -using namespace boost::assign; - -namespace gtsam { - -/* **************************************************************** */ -template -double constraintError(const G& graph, const C& config) { - // local typedefs - typedef typename G::const_iterator const_iterator; - typedef NonlinearConstraint NLConstraint; - typedef boost::shared_ptr shared_c; - - // accumulate error - double error = 0; - - // find the constraints - for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) { - const shared_c constraint = boost::shared_dynamic_cast(*factor); - if (constraint != NULL) { - Vector e = constraint->unwhitenedError(config); - error += inner_prod(trans(e),e); - } - } - return error; -} - -/* **************************************************************** */ -template -SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, - shared_config config) -: graph_(&graph), ordering_(&ordering), full_ordering_(ordering), - config_(config), lagrange_config_(new VectorConfig), error_(graph.error(*config)), - constraint_error_(constraintError(graph, *config)) -{ - // local typedefs - typedef typename G::const_iterator const_iterator; - typedef NonlinearConstraint NLConstraint; - typedef boost::shared_ptr shared_c; - - // find the constraints - for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) { - const shared_c constraint = boost::shared_dynamic_cast(*factor); - if (constraint != NULL) { - size_t p = constraint->nrConstraints(); - // update ordering - string key = constraint->lagrangeKey(); - full_ordering_ += key; - // initialize lagrange multipliers - lagrange_config_->insert(key, ones(p)); - } - } -} - -/* **************************************************************** */ -template -SQPOptimizer::SQPOptimizer(const G& graph, const Ordering& ordering, - shared_config config, shared_vconfig lagrange) -: graph_(&graph), ordering_(&ordering), full_ordering_(ordering), - config_(config), lagrange_config_(lagrange), error_(graph.error(*config)), - constraint_error_(constraintError(graph, *config)) -{ -} - -/* **************************************************************** */ -template -SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { - bool verbose = v == SQPOptimizer::FULL; - - // local typedefs - typedef typename G::const_iterator const_iterator; - typedef NonlinearConstraint NLConstraint; - typedef boost::shared_ptr shared_c; - - // linearize the graph - GaussianFactorGraph fg; - - // prepare an ordering of lagrange multipliers to remove - Ordering keysToRemove; - - // iterate over all factors and linearize - for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) { - const shared_c constraint = boost::shared_dynamic_cast(*factor); - if (constraint == NULL) { - // if a regular factor, linearize using the default linearization - GaussianFactor::shared_ptr f = (*factor)->linearize(*config_); - if (verbose) f->print("Regular Factor"); - fg.push_back(f); - } else if (constraint->active(*config_)) { - // if a constraint, linearize using the constraint method (2 configs) - GaussianFactor::shared_ptr f, c; - boost::tie(f,c) = constraint->linearize(*config_, *lagrange_config_); - if (verbose) f->print("Constrained Factor"); - if (verbose) c->print("Constraint"); - fg.push_back(f); - fg.push_back(c); - } else { - if (verbose) constraint->print("Skipping..."); - keysToRemove += constraint->lagrangeKey(); - } - } - if (verbose) fg.print("Before Optimization"); - - // optimize linear graph to get full delta config - VectorConfig delta = fg.optimize(full_ordering_.subtract(keysToRemove)); - - if (verbose) delta.print("Delta Config"); - - // update both state variables - shared_config newConfig(new C(expmap(*config_, delta))); - shared_vconfig newLambdas(new VectorConfig(expmap(*lagrange_config_, delta))); - - // construct a new optimizer - return SQPOptimizer(*graph_, full_ordering_, newConfig, newLambdas); -} - -/* **************************************************************** */ -template -SQPOptimizer SQPOptimizer::iterateSolve(double relThresh, double absThresh, - double constraintThresh, size_t maxIterations, Verbosity v) const { - bool verbose = v == SQPOptimizer::FULL; - - // do an iteration - SQPOptimizer next = iterate(v); - - // if converged or out of iterations, return result - if (maxIterations == 1 || - next.checkConvergence(relThresh, absThresh, constraintThresh, - error_, constraint_error_)) - return next; - else // otherwise, recurse with a lower maxIterations - return next.iterateSolve(relThresh, absThresh, constraintThresh, - maxIterations-1, v); -} - -/* **************************************************************** */ -template -bool SQPOptimizer::checkConvergence(double relThresh, double absThresh, - double constraintThresh, double full_error, double constraint_error) const { - // if error sufficiently low, then the system has converged - if (error_ < absThresh && constraint_error_ < constraintThresh) - return true; - - // TODO: determine other cases - return false; -} - -/* **************************************************************** */ -template -void SQPOptimizer::print(const std::string& s) { - graph_->print("Nonlinear Graph"); - ordering_->print("Initial Ordering"); - full_ordering_.print("Ordering including all Lagrange Multipliers"); - config_->print("Real Config"); - lagrange_config_->print("Lagrange Multiplier Config"); -} - -} diff --git a/cpp/SQPOptimizer.h b/cpp/SQPOptimizer.h deleted file mode 100644 index 6f1f65b8e..000000000 --- a/cpp/SQPOptimizer.h +++ /dev/null @@ -1,115 +0,0 @@ -/** - * @file SQPOptimizer.h - * @brief Interface for a generic SQP-based nonlinear optimization engine - * @author Alex Cunningham - */ - -#pragma once - -#include "Ordering.h" -#include "VectorConfig.h" - -namespace gtsam { - -/** - * This class is an engine for performing SQP-based optimization - * It stores a graph, a config, and needs a specific ordering, and - * then will perform optimization iterations in a functional way. - */ -template -class SQPOptimizer { - -public: - // verbosity level - typedef enum { - SILENT, - FULL - } Verbosity; - - // useful for storing configurations - typedef boost::shared_ptr shared_config; - typedef boost::shared_ptr shared_vconfig; - -private: - // keep const references to the graph and initial ordering - const FactorGraph* graph_; - const Ordering* ordering_; - - // keep configurations - shared_config config_; - shared_vconfig lagrange_config_; - - // keep a configuration that has been updated to include the lagrange multipliers - Ordering full_ordering_; - - - // keep a set of errors for the overall system and just the constraints - double error_; - double constraint_error_; - -public: - /** - * Standard external constructor - * @param graph is the nonlinear graph to optimize - * @param ordering is the elimination ordering to use - * @param config is the initial configuration for the real variables - */ - SQPOptimizer(const FactorGraph& graph, const Ordering& ordering, - shared_config config); - - /** - * Constructor that includes a lagrange initialization. Primarily - * for internal iterations, but if the user has an idea of what a good - * set of lagrange multipliers is, they can specify them, assuming that - * the naming convention is the same as the internal system. - * @param graph is the nonlinear graph to optimize - * @param ordering is the elimination ordering to use - * @param config is the initial configuration for the real variables - * @param lagrange is the configuration of lagrange multipliers - */ - SQPOptimizer(const FactorGraph& graph, const Ordering& ordering, - shared_config config, shared_vconfig lagrange); - - /// Access functions - const FactorGraph* graph() const { return graph_; } - const Ordering* ordering() const { return ordering_; } - shared_config config() const { return config_; } - shared_vconfig configLagrange() const { return lagrange_config_; } - double error() const { return error_; } - - /** - * Primary optimization iteration, updates the configs - * @return a new optimization object with updated values - */ - SQPOptimizer iterate(Verbosity verbosity=SILENT) const; - - /** - * Iterates recursively until converence occurs - * @param relThresh minimum change in error between iterations - * @param absThresh minimum error necessary to converge - * @param constraintThresh minimum constraint error to be feasible - * @param maxIterations is the maximum number of iterations - * @param verbosity controls output print statements - * @return a new optimization object with final values - */ - SQPOptimizer - iterateSolve(double relThresh, double absThresh, double constraintThresh, - size_t maxIterations = 10, Verbosity verbosity=SILENT) const; - - /** - * Checks whether convergence has occurred, and returns true if - * the solution will not get better, based on the previous error conditions. - * @param full_error is the error all the factors and constraints - * @param constraint_error is the error of just the constraints - * @param relThresh is the relative threshold between - * @return true if the problem has converged - */ - bool checkConvergence(double relThresh, double absThresh, - double constraintThresh, double full_error, double constraint_error) const; - - /** Standard print function with optional name */ - void print(const std::string& s); -}; - -} - diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 3e1f5e546..3204cc01f 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -816,6 +816,299 @@ TEST (SQP, stereo_sqp_noisy ) { 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; + +/** + * 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)); +} + +///* ********************************************************************* */ +//// 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 ObstacleConstraint; +//typedef shared_ptr 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 obstacleAvoidGraph() { +// // Keys +// PoseKey x1(1), x2(2), x3(3); +// PointKey l1(1); +// LagrangeKey L20(20); +// +// // 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 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 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); } /* ************************************************************************* */ diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp deleted file mode 100644 index 9e6edc17c..000000000 --- a/cpp/testSQPOptimizer.cpp +++ /dev/null @@ -1,366 +0,0 @@ -/* - * @file testSQPOptimizer.cpp - * @brief tests the optimization algorithm for nonlinear graphs with nonlinear constraints - * @author Alex Cunningham - */ - -#include -#include // for operator += -#include // for insert -#include - -#define GTSAM_MAGIC_KEY - -#include -#include "NonlinearFactorGraph.h" -#include "NonlinearConstraint.h" -#include "NonlinearEquality.h" -#include "VectorConfig.h" -#include "Ordering.h" -//#include "SQPOptimizer.h" - -// implementations -#include "NonlinearConstraint-inl.h" -//#include "SQPOptimizer-inl.h" - -using namespace std; -using namespace gtsam; -using namespace boost; -using namespace boost::assign; -using namespace simulated2D; - -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 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 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 keyx; keyx += "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::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 keys; keys += "l1", "l2"; -// 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 -// NLGraph graph; -// 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 -// NLGraph graph = linearMapWarpGraph(); -// -// // 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, map_warp ) { -// bool verbose = false; -// // get a graph -// NLGraph graph = linearMapWarpGraph(); -// if (verbose) graph.print("Initial map warp graph"); -// -// // keys -// PoseKey x1(1), x2(2); -// PointKey l1(1), l2(2); -// -// // create an initial estimate -// 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 -// initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin -// -// // create an ordering -// Ordering ordering; -// ordering += "x1", "x2", "l1", "l2"; -// -// // create an optimizer -// Optimizer optimizer(graph, ordering, initialEstimate); -// -// // 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)); -//} -// -///* ********************************************************************* */ -//// 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 ObstacleConstraint; -//typedef shared_ptr 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 obstacleAvoidGraph() { -// // Keys -// PoseKey x1(1), x2(2), x3(3); -// PointKey l1(1); -// LagrangeKey L20(20); -// -// // 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 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 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); } -/* ************************************************************************* */