From 4f9a60d41c2677773e17ee843e01b1303c6ecf62 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 9 Aug 2010 21:59:29 +0000 Subject: [PATCH] Nonlinear bounding constraints are now working for simple examples. Also, removed extraneous constraint test files. --- nonlinear/NonlinearConstraint.h | 31 --- slam/BetweenConstraint.h | 44 ++++ slam/BoundingConstraint.h | 105 ++++++++ slam/Makefile.am | 3 + slam/simulated2DConstraints.h | 85 ++++-- tests/Makefile.am | 3 +- tests/testBoundingConstraint.cpp | 271 ++++++++++++++++++++ tests/testNonlinearConstraint.cpp | 207 --------------- tests/testNonlinearInequalityConstraint.cpp | 112 -------- 9 files changed, 492 insertions(+), 369 deletions(-) create mode 100644 slam/BetweenConstraint.h create mode 100644 slam/BoundingConstraint.h create mode 100644 tests/testBoundingConstraint.cpp delete mode 100644 tests/testNonlinearConstraint.cpp delete mode 100644 tests/testNonlinearInequalityConstraint.cpp diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 988e88b19..2bc65922a 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -325,35 +325,4 @@ public: } }; -/** - * Binary between constraint - forces between to a given value - * This constraint requires the underlying type to a Lie type - */ -template -class BetweenConstraint : public NonlinearEqualityConstraint2 { -protected: - typedef NonlinearEqualityConstraint2 Base; - - X measured_; /// fixed between value - -public: - - typedef boost::shared_ptr > shared_ptr; - - BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::dim(), mu), measured_(measured) {} - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - X hx = between(x1, x2, H1, H2); - return logmap(measured_, hx); - } - - inline const X measured() const { - return measured_; - } -}; - } diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h new file mode 100644 index 000000000..d009c520c --- /dev/null +++ b/slam/BetweenConstraint.h @@ -0,0 +1,44 @@ +/** + * @file BetweenConstraint.h + * @brief Implements a constraint to force a between + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + + /** + * Binary between constraint - forces between to a given value + * This constraint requires the underlying type to a Lie type + */ + template + class BetweenConstraint : public NonlinearEqualityConstraint2 { + protected: + typedef NonlinearEqualityConstraint2 Base; + + X measured_; /// fixed between value + + public: + + typedef boost::shared_ptr > shared_ptr; + + BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) + : Base(key1, key2, X::dim(), mu), measured_(measured) {} + + /** g(x) with optional derivative2 */ + Vector evaluateError(const X& x1, const X& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + X hx = between(x1, x2, H1, H2); + return logmap(measured_, hx); + } + + inline const X measured() const { + return measured_; + } + }; + +} // \namespace gtsam diff --git a/slam/BoundingConstraint.h b/slam/BoundingConstraint.h new file mode 100644 index 000000000..ab1dc25d7 --- /dev/null +++ b/slam/BoundingConstraint.h @@ -0,0 +1,105 @@ +/** + * @file BoundingConstraint.h + * @brief Provides partially implemented constraints to implement bounds + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Unary inequality constraint forcing a scalar to be + * greater/less than a fixed threshold. The function + * will need to have its value function implemented to return + * a scalar for comparison. + */ + template + struct BoundingConstraint1: public NonlinearConstraint1 { + typedef NonlinearConstraint1 Base; + typedef boost::shared_ptr > shared_ptr; + + double threshold_; + bool isGreaterThan_; /// flag for greater/less than + + BoundingConstraint1(const Key& key, double threshold, + bool isGreaterThan, double mu = 1000.0) : + Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) { + } + + inline double threshold() const { return threshold_; } + inline bool isGreaterThan() const { return isGreaterThan_; } + + /** + * function producing a scalar value to compare to the threshold + * Must have optional argument for derivative with 1xN matrix, where + * N = X::dim() + */ + virtual double value(const X& x, boost::optional H = + boost::none) const = 0; + + /** active when constraint *NOT* met */ + bool active(const Cfg& c) const { + // note: still active at equality to avoid zigzagging + double x = value(c[this->key_]); + return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + } + + Vector evaluateError(const X& x, boost::optional H = + boost::none) const { + Matrix D; + double error = value(x, D) - threshold_; + if (H) *H = (isGreaterThan_) ? D : -1.0 * D; + return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error); + } + }; + + /** + * Binary scalar inequality constraint, with a similar value() function + * to implement for specific systems + */ + template + struct BoundingConstraint2: public NonlinearConstraint2 { + typedef NonlinearConstraint2 Base; + typedef boost::shared_ptr > shared_ptr; + + double threshold_; + bool isGreaterThan_; /// flag for greater/less than + + BoundingConstraint2(const Key1& key1, const Key2& key2, double threshold, + bool isGreaterThan, double mu = 1000.0) + : Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {} + + inline double threshold() const { return threshold_; } + inline bool isGreaterThan() const { return isGreaterThan_; } + + /** + * function producing a scalar value to compare to the threshold + * Must have optional argument for derivatives) + */ + virtual double value(const X1& x1, const X2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const = 0; + + /** active when constraint *NOT* met */ + bool active(const Cfg& c) const { + // note: still active at equality to avoid zigzagging + double x = value(c[this->key1_], c[this->key2_]); + return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; + } + + Vector evaluateError(const X1& x1, const X2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Matrix D1, D2; + double error = value(x1, x2, D1, D2) - threshold_; + if (H1) *H1 = (isGreaterThan_) ? D1 : -1.0 * D1; + if (H2) *H2 = (isGreaterThan_) ? D2 : -1.0 * D2; + return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error); + } + }; + +} // \namespace gtsam diff --git a/slam/Makefile.am b/slam/Makefile.am index 5e7c2f1c7..fd65d3505 100644 --- a/slam/Makefile.am +++ b/slam/Makefile.am @@ -27,6 +27,9 @@ check_PROGRAMS += tests/testSimulated3D # Pose SLAM headers headers += BetweenFactor.h PriorFactor.h +# General constraints +headers += BetweenConstraint.h BoundingConstraint.h + # 2D Pose SLAM sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp check_PROGRAMS += tests/testPose2Factor tests/testPose2Config tests/testPose2SLAM tests/testPose2Prior diff --git a/slam/simulated2DConstraints.h b/slam/simulated2DConstraints.h index e1ed73fc6..2ab247981 100644 --- a/slam/simulated2DConstraints.h +++ b/slam/simulated2DConstraints.h @@ -8,7 +8,11 @@ #pragma once +#include // TODO: remove + #include +#include +#include #include // \namespace @@ -21,6 +25,7 @@ namespace gtsam { /** Typedefs for regular use */ typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; typedef BetweenConstraint OdoEqualityConstraint; /** Equality between variables */ @@ -32,39 +37,85 @@ namespace gtsam { namespace inequality_constraints { /** - * Unary inequality constraint forcing a coordinate to be greater than a fixed value (c) + * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) + * Demo implementation: should be made more general using BoundingConstraint */ template - struct ScalarInequalityConstraint1: public NonlinearConstraint1 { - typedef NonlinearConstraint1 Base; - typedef boost::shared_ptr > shared_ptr; + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; + typedef boost::shared_ptr > shared_ptr; - double c_; /// min value of the selected coordinate - - ScalarInequalityConstraint1(const Key& key, double c, double mu = 1000.0) : - Base(key, 1, mu), c_(c) { + ScalarCoordConstraint1(const Key& key, double c, + bool isGreaterThan, double mu = 1000.0) : + Base(key, c, isGreaterThan, mu) { } - /** active when constraint not met */ - virtual bool active(const Cfg& c) const { - return c[this->key_].vector()(Idx) <= c_; // greater than or equals to avoid zigzagging - } + inline unsigned int index() const { return Idx; } - Vector evaluateError(const Point2& x, boost::optional H = + /** extracts a single value from the point */ + virtual double value(const Point2& x, boost::optional H = boost::none) const { if (H) { Matrix D = zeros(1, 2); D(0, Idx) = 1.0; *H = D; } - return Vector_(1, x.vector()(Idx) - c_); + return x.vector()(Idx); } - }; /** typedefs for use with simulated2D systems */ - typedef ScalarInequalityConstraint1 PoseXInequality; - typedef ScalarInequalityConstraint1 PoseYInequality; + typedef ScalarCoordConstraint1 PoseXInequality; + typedef ScalarCoordConstraint1 PoseYInequality; + + double range(const Point2& a, const Point2& b) { return a.dist(b); } + + /** + * Binary inequality constraint forcing the range between points + * to be less than or equal to a bound + */ + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; + + MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0) + : Base(key1, key2, range_bound, false, mu) {} + + /** extracts a single scalar value with derivatives */ + virtual double value(const Point2& x1, const Point2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); + return x1.dist(x2); + } + }; + + typedef MaxDistanceConstraint PoseMaxDistConstraint; + + /** + * Binary inequality constraint forcing a minimum range + * NOTE: this is not a convex function! Be careful with initialization. + */ + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; + + MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0) + : Base(key1, key2, range_bound, true, mu) {} + + /** extracts a single scalar value with derivatives */ + virtual double value(const Point2& x1, const Point2& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5); + if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5); + return x1.dist(x2); + } + }; + + typedef MinDistanceConstraint LandmarkAvoid; + } // \namespace inequality_constraints diff --git a/tests/Makefile.am b/tests/Makefile.am index 57f66a265..996627194 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -10,8 +10,7 @@ check_PROGRAMS += testInference testIterative testGaussianJunctionTree check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig -# check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged -check_PROGRAMS += testNonlinearEqualityConstraint testNonlinearInequalityConstraint +check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint if USE_LDL check_PROGRAMS += testConstraintOptimizer diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp new file mode 100644 index 000000000..158004786 --- /dev/null +++ b/tests/testBoundingConstraint.cpp @@ -0,0 +1,271 @@ +/** + * @file testBoundingConstraint.cpp + * @brief test of nonlinear inequality constraints on scalar bounds + * @author Alex Cunningham + */ + +#include + +#include +#include +#include + +namespace iq2D = gtsam::simulated2D::inequality_constraints; +using namespace std; +using namespace gtsam; + +static const double tol = 1e-5; + +SharedDiagonal soft_model2 = noiseModel::Unit::Create(2); +SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1); +SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); + +typedef NonlinearFactorGraph Graph; +typedef boost::shared_ptr shared_graph; +typedef boost::shared_ptr shared_config; +typedef NonlinearOptimizer Optimizer; + +// some simple inequality constraints +simulated2D::PoseKey key(1); +double mu = 10.0; +// greater than +iq2D::PoseXInequality constraint1(key, 1.0, true, mu); +iq2D::PoseYInequality constraint2(key, 2.0, true, mu); + +// less than +iq2D::PoseXInequality constraint3(key, 1.0, false, mu); +iq2D::PoseYInequality constraint4(key, 2.0, false, mu); + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_basics_inactive1 ) { + Point2 pt1(2.0, 3.0); + simulated2D::Config config; + config.insert(key, pt1); + EXPECT(!constraint1.active(config)); + EXPECT(!constraint2.active(config)); + EXPECT_DOUBLES_EQUAL(1.0, constraint1.threshold(), tol); + EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol); + EXPECT(constraint1.isGreaterThan()); + EXPECT(constraint2.isGreaterThan()); + EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); + EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); + EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol)); + EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); + EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_basics_inactive2 ) { + Point2 pt2(-2.0, -3.0); + simulated2D::Config config; + config.insert(key, pt2); + EXPECT(!constraint3.active(config)); + EXPECT(!constraint4.active(config)); + EXPECT_DOUBLES_EQUAL(1.0, constraint3.threshold(), tol); + EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol); + EXPECT(!constraint3.isGreaterThan()); + EXPECT(!constraint4.isGreaterThan()); + EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol)); + EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol)); + EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol)); + EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); + EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_basics_active1 ) { + Point2 pt2(-2.0, -3.0); + simulated2D::Config config; + config.insert(key, pt2); + EXPECT(constraint1.active(config)); + EXPECT(constraint2.active(config)); + EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol)); + EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol)); + EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol); + EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_basics_active2 ) { + Point2 pt1(2.0, 3.0); + simulated2D::Config config; + config.insert(key, pt1); + EXPECT(constraint3.active(config)); + EXPECT(constraint4.active(config)); + EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol)); + EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol); + EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_linearization_inactive) { + Point2 pt1(2.0, 3.0); + simulated2D::Config config1; + config1.insert(key, pt1); + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1); + EXPECT(!actual1); + EXPECT(!actual2); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_linearization_active) { + Point2 pt2(-2.0, -3.0); + simulated2D::Config config2; + config2.insert(key, pt2); + GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); + GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); + GaussianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); + GaussianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); + EXPECT(assert_equal(expected1, *actual1, tol)); + EXPECT(assert_equal(expected2, *actual2, tol)); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_simple_optimization1) { + // create a single-node graph with a soft and hard constraint to + // ensure that the hard constraint overrides the soft constraint + Point2 goal_pt(1.0, 2.0); + Point2 start_pt(0.0, 1.0); + + shared_graph graph(new Graph()); + simulated2D::PoseKey x1(1); + graph->add(iq2D::PoseXInequality(x1, 1.0, true)); + graph->add(iq2D::PoseYInequality(x1, 2.0, true)); + graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); + + shared_config initConfig(new simulated2D::Config()); + initConfig->insert(x1, start_pt); + + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig, Optimizer::SILENT); + simulated2D::Config expected; + expected.insert(x1, goal_pt); + CHECK(assert_equal(expected, *actual, tol)); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, unary_simple_optimization2) { + // create a single-node graph with a soft and hard constraint to + // ensure that the hard constraint overrides the soft constraint + Point2 goal_pt(1.0, 2.0); + Point2 start_pt(2.0, 3.0); + + shared_graph graph(new Graph()); + simulated2D::PoseKey x1(1); + graph->add(iq2D::PoseXInequality(x1, 1.0, false)); + graph->add(iq2D::PoseYInequality(x1, 2.0, false)); + graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); + + shared_config initConfig(new simulated2D::Config()); + initConfig->insert(x1, start_pt); + + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig, Optimizer::SILENT); + simulated2D::Config expected; + expected.insert(x1, goal_pt); + CHECK(assert_equal(expected, *actual, tol)); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, MaxDistance_basics) { + simulated2D::PoseKey key1(1), key2(2); + Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); + iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); + EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); + EXPECT(!rangeBound.isGreaterThan()); + EXPECT(rangeBound.dim() == 1); + + EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1))); + EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); + EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); + EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); + + simulated2D::Config config1; + config1.insert(key1, pt1); + config1.insert(key2, pt1); + EXPECT(!rangeBound.active(config1)); + EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(!rangeBound.linearize(config1)); + EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); + + config1.update(key2, pt2); + EXPECT(!rangeBound.active(config1)); + EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(!rangeBound.linearize(config1)); + EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); + + config1.update(key2, pt3); + EXPECT(rangeBound.active(config1)); + EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); + + config1.update(key2, pt4); + EXPECT(rangeBound.active(config1)); + EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1))); + EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, MaxDistance_simple_optimization) { + + Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); + simulated2D::PoseKey x1(1), x2(2); + + Graph graph; + graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); + graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2)); + graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0)); + + simulated2D::Config initial_state; + initial_state.insert(x1, pt1); + initial_state.insert(x2, pt2_init); + + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initial_state); + + simulated2D::Config expected; + expected.insert(x1, pt1); + expected.insert(x2, pt2_goal); + + EXPECT(assert_equal(expected, *actual, tol)); +} + +/* ************************************************************************* */ +TEST( testBoundingConstraint, avoid_demo) { + + simulated2D::PoseKey x1(1), x2(2), x3(3); + simulated2D::PointKey l1(1); + double radius = 1.0; + Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); + Point2 odo(2.0, 0.0); + + Graph graph; + graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1)); + graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2)); + graph.add(iq2D::LandmarkAvoid(x2, l1, radius)); + graph.add(simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1)); + graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3)); + graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3)); + + simulated2D::Config init, expected; + init.insert(x1, x1_pt); + init.insert(x3, x3_pt); + init.insert(l1, l1_pt); + expected = init; + init.insert(x2, x2_init); + expected.insert(x2, x2_goal); + + Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init); + + EXPECT(assert_equal(expected, *actual, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + diff --git a/tests/testNonlinearConstraint.cpp b/tests/testNonlinearConstraint.cpp deleted file mode 100644 index f94deacf5..000000000 --- a/tests/testNonlinearConstraint.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/* - * @file testNonlinearConstraint.cpp - * @brief demos of constrained optimization using existing gtsam components - * @author Alex Cunningham - */ - -#include -#include -#include // for operator += -#include // for insert -#include -#include - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include -#include -#include - -// templated implementations -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost; -using namespace boost::assign; - -//// Models to use -//SharedDiagonal probModel1 = sharedSigma(1,1.0); -//SharedDiagonal probModel2 = sharedSigma(2,1.0); -//SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); -// -//// trick from some reading group -//#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -// -///* ********************************************************************* */ -//// full components -//typedef simulated2D::Config Config2D; -//typedef NonlinearFactorGraph Graph2D; -//typedef NonlinearEquality NLE; -//typedef boost::shared_ptr > shared; -//typedef NonlinearOptimizer Optimizer; - -/* ********************************************************************* */ -// 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 PoseNLConstraint; -//typedef shared_ptr shared_pc; -//typedef NonlinearEquality ObstacleConstraint; -//typedef shared_ptr shared_oc; -// -// -//namespace constrained_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 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 -// 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(constrained_avoid1::g_func, _1, x2, l1), -// x2, boost::bind(constrained_avoid1::jac_g1, _1, x2, l1), -// l1,boost::bind(constrained_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/tests/testNonlinearInequalityConstraint.cpp b/tests/testNonlinearInequalityConstraint.cpp deleted file mode 100644 index 9ec1b9d4c..000000000 --- a/tests/testNonlinearInequalityConstraint.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/** - * @file testNonlinearInequalityConstraint.cpp - * @author Alex Cunningham - */ - -#include - -#include -#include -#include - -namespace iq2D = gtsam::simulated2D::inequality_constraints; -using namespace std; -using namespace gtsam; - -static const double tol = 1e-5; - -SharedDiagonal soft_model2 = noiseModel::Unit::Create(2); -SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); - -typedef NonlinearFactorGraph Graph; -typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_config; -typedef NonlinearOptimizer Optimizer; - -// some simple inequality constraints -simulated2D::PoseKey key(1); -double mu = 10.0; -iq2D::PoseXInequality constraint1(key, 1.0, mu); -iq2D::PoseYInequality constraint2(key, 2.0, mu); - -/* ************************************************************************* */ -TEST( testNonlinearInequalityConstraint, unary_basics_inactive ) { - Point2 pt1(2.0, 3.0); - simulated2D::Config config1; - config1.insert(key, pt1); - EXPECT(!constraint1.active(config1)); - EXPECT(!constraint2.active(config1)); - EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); - EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); - EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config1), tol)); - EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config1), tol); - EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config1), tol); -} - -/* ************************************************************************* */ -TEST( testNonlinearInequalityConstraint, unary_basics_active ) { - Point2 pt2(-2.0, -3.0); - simulated2D::Config config2; - config2.insert(key, pt2); - EXPECT(constraint1.active(config2)); - EXPECT(constraint2.active(config2)); - EXPECT(assert_equal(repeat(1, -3.0), constraint1.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1, -5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1, -3.0), constraint1.unwhitenedError(config2), tol)); - EXPECT(assert_equal(repeat(1, -5.0), constraint2.unwhitenedError(config2), tol)); - EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config2), tol); - EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config2), tol); -} - -/* ************************************************************************* */ -TEST( testNonlinearInequalityConstraint, unary_linearization_inactive) { - Point2 pt1(2.0, 3.0); - simulated2D::Config config1; - config1.insert(key, pt1); - GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1); - GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1); - EXPECT(!actual1); - EXPECT(!actual2); -} - -/* ************************************************************************* */ -TEST( testNonlinearInequalityConstraint, unary_linearization_active) { - Point2 pt2(-2.0, -3.0); - simulated2D::Config config2; - config2.insert(key, pt2); - GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); - GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - GaussianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); - GaussianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); - EXPECT(assert_equal(expected1, *actual1, tol)); - EXPECT(assert_equal(expected2, *actual2, tol)); -} - -/* ************************************************************************* */ -TEST( testNonlinearInequalityConstraint, unary_simple_optimization) { - // create a single-node graph with a soft and hard constraint to - // ensure that the hard constraint overrides the soft constraint - Point2 goal_pt(1.0, 2.0); - Point2 start_pt(0.0, 1.0); - - shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); - graph->add(iq2D::PoseXInequality(x1, 1.0)); - graph->add(iq2D::PoseYInequality(x1, 2.0)); - graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - - shared_config initConfig(new simulated2D::Config()); - initConfig->insert(x1, start_pt); - - Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig, Optimizer::SILENT); - simulated2D::Config expected; - expected.insert(x1, goal_pt); - CHECK(assert_equal(expected, *actual, tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - -