diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 99b8f2ae7..988e88b19 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -33,7 +33,9 @@ class NonlinearConstraint : public NonlinearFactor { protected: typedef NonlinearConstraint This; typedef NonlinearFactor Base; - double mu_; // gain for quadratic merit function + + double mu_; /// gain for quadratic merit function + size_t dim_; /// dimension of the constraint public: @@ -42,7 +44,7 @@ public: * @param mu is the gain used at error evaluation (forced to be positive) */ NonlinearConstraint(size_t dim, double mu = 1000.0): - Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {} + Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)), dim_(dim) {} /** returns the gain mu */ double mu() const { return mu_; } @@ -50,6 +52,9 @@ public: /** Print */ virtual void print(const std::string& s = "") const=0; + /** dimension of the constraint (number of rows) */ + size_t dim() const { return dim_; } + /** Check if two factors are equal */ virtual bool equals(const Factor& f, double tol=1e-9) const { const This* p = dynamic_cast (&f); @@ -70,10 +75,12 @@ public: /** * active set check, defines what type of constraint this is - * By default, the constraint is always active, so it is an equality constraint + * + * In an inequality/bounding constraint, this active() returns true + * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const Config& c) const { return true; } + virtual bool active(const Config& c) const=0; /** * Linearizes around a given config @@ -124,8 +131,11 @@ public: return Base::equals(*p, tol) && (key_ == p->key_); } - /** error function g(x) */ + /** error function g(x), switched depending on whether the constraint is active */ inline Vector unwhitenedError(const Config& x) const { + if (!active(x)) { + return zero(this->dim()); + } const Key& j = key_; const X& xj = x[j]; return evaluateError(xj); @@ -145,11 +155,29 @@ public: return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, grad, g, model)); } - /** g(x) with optional derivative */ + /** g(x) with optional derivative - does not depend on active */ virtual Vector evaluateError(const X& x, boost::optional H = boost::none) const = 0; }; +/** + * Unary Equality constraint - simply forces the value of active() to true + */ +template +class NonlinearEqualityConstraint1 : public NonlinearConstraint1 { + +protected: + typedef NonlinearEqualityConstraint1 This; + typedef NonlinearConstraint1 Base; + +public: + NonlinearEqualityConstraint1(const Key& key, size_t dim, double mu = 1000.0) + : Base(key, dim, mu) {} + + /** Always active, so fixed value for active() */ + virtual bool active(const Config& c) const { return true; } +}; + /** * A binary constraint with arbitrary cost and jacobian functions */ @@ -194,8 +222,11 @@ public: return Base::equals(*p, tol) && (key1_ == p->key1_) && (key2_ == p->key2_); } - /** error function g(x) */ + /** error function g(x), switched depending on whether the constraint is active */ inline Vector unwhitenedError(const Config& x) const { + if (!active(x)) { + return zero(this->dim()); + } const Key1& j1 = key1_; const Key2& j2 = key2_; const X1& xj1 = x[j1]; @@ -217,20 +248,64 @@ public: return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, g, model)); } - /** g(x) with optional derivative2 */ + /** g(x) with optional derivative2 - does not depend on active */ virtual Vector evaluateError(const X1& x1, const X2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const = 0; }; +/** + * Binary Equality constraint - simply forces the value of active() to true + */ +template +class NonlinearEqualityConstraint2 : public NonlinearConstraint2 { + +protected: + typedef NonlinearEqualityConstraint2 This; + typedef NonlinearConstraint2 Base; + +public: + NonlinearEqualityConstraint2(const Key1& key1, const Key2& key2, size_t dim, double mu = 1000.0) + : Base(key1, key2, dim, mu) {} + + /** Always active, so fixed value for active() */ + virtual bool active(const Config& c) const { return true; } +}; + +/** + * Simple unary equality constraint - fixes a value for a variable + */ +template +class NonlinearEquality1 : public NonlinearEqualityConstraint1 { +protected: + typedef NonlinearEqualityConstraint1 Base; + + X value_; /// fixed value for variable + +public: + + typedef boost::shared_ptr > shared_ptr; + + NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) + : Base(key1, X::dim(), mu), value_(value) {} + + /** g(x) with optional derivative */ + Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { + const size_t p = X::dim(); + if (H1) *H1 = eye(p); + return logmap(value_, x1); + } +}; + + /** * Simple binary equality constraint - this constraint forces two factors to * be the same. This constraint requires the underlying type to a Lie type */ template -class NonlinearEquality2 : public NonlinearConstraint2 { +class NonlinearEquality2 : public NonlinearEqualityConstraint2 { protected: - typedef NonlinearConstraint2 Base; + typedef NonlinearEqualityConstraint2 Base; public: @@ -250,4 +325,35 @@ 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/nonlinear/NonlinearFactorGraph-inl.h b/nonlinear/NonlinearFactorGraph-inl.h index b34580ac0..34bb3d282 100644 --- a/nonlinear/NonlinearFactorGraph-inl.h +++ b/nonlinear/NonlinearFactorGraph-inl.h @@ -51,7 +51,7 @@ namespace gtsam { // linearize all factors BOOST_FOREACH(const sharedFactor& factor, this->factors_) { boost::shared_ptr lf = factor->linearize(config); - linearFG->push_back(lf); + if (lf) linearFG->push_back(lf); } return linearFG; diff --git a/slam/simulated2DConstraints.h b/slam/simulated2DConstraints.h index 7c589dd7e..e1ed73fc6 100644 --- a/slam/simulated2DConstraints.h +++ b/slam/simulated2DConstraints.h @@ -17,55 +17,56 @@ namespace gtsam { namespace simulated2D { - /** - * Unary constraint encoding a hard equality on a Point - */ - template - struct GenericUnaryEqualityConstraint: public NonlinearConstraint1 { - typedef NonlinearConstraint1 Base; - typedef boost::shared_ptr > shared_ptr; + namespace equality_constraints { - Point2 z_; + /** Typedefs for regular use */ + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef BetweenConstraint OdoEqualityConstraint; - GenericUnaryEqualityConstraint(const Point2& z, const Key& key, double mu = 1000.0) : - Base(key, 2, mu), z_(z) { - } + /** Equality between variables */ + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; - Vector evaluateError(const Point2& x, boost::optional H = - boost::none) const { - return (prior(x, H) - z_).vector(); - } + } // \namespace equality_constraints - }; + namespace inequality_constraints { - /** - * Binary constraint simulating "odometry" between two Poses - */ - template - struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2 { - typedef NonlinearConstraint2 Base; - typedef boost::shared_ptr > shared_ptr; - Point2 z_; + /** + * Unary inequality constraint forcing a coordinate to be greater than a fixed value (c) + */ + template + struct ScalarInequalityConstraint1: public NonlinearConstraint1 { + typedef NonlinearConstraint1 Base; + typedef boost::shared_ptr > shared_ptr; - GenericOdoHardEqualityConstraint( - const Point2& z, const Key& i1, const Key& i2, double mu = 1000.0) : - Base (i1, i2, 2, mu), z_(z) { - } + double c_; /// min value of the selected coordinate - Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { - return (odo(x1, x2, H1, H2) - z_).vector(); - } + ScalarInequalityConstraint1(const Key& key, double c, double mu = 1000.0) : + Base(key, 1, mu), c_(c) { + } - }; + /** 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 + } - /** Typedefs for regular use */ - typedef GenericUnaryEqualityConstraint UnaryEqualityConstraint; - typedef GenericOdoHardEqualityConstraint OdoEqualityConstraint; + Vector evaluateError(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_); + } - /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; + }; - } // namespace simulated2D -} // namespace gtsam + /** typedefs for use with simulated2D systems */ + typedef ScalarInequalityConstraint1 PoseXInequality; + typedef ScalarInequalityConstraint1 PoseYInequality; + + } // \namespace inequality_constraints + + } // \namespace simulated2D +} // \namespace gtsam diff --git a/tests/Makefile.am b/tests/Makefile.am index 6210bd1f1..57f66a265 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -11,7 +11,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig # check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged -check_PROGRAMS += testNonlinearEqualityConstraint +check_PROGRAMS += testNonlinearEqualityConstraint testNonlinearInequalityConstraint if USE_LDL check_PROGRAMS += testConstraintOptimizer diff --git a/tests/testNonlinearConstraint.cpp b/tests/testNonlinearConstraint.cpp index 9b9a06ae5..f94deacf5 100644 --- a/tests/testNonlinearConstraint.cpp +++ b/tests/testNonlinearConstraint.cpp @@ -49,158 +49,158 @@ using namespace boost::assign; //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. -///* ********************************************************************* */ +/* ********************************************************************* */ +// 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; // -////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()))); -////} +//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/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp index 9348b2631..3a1fe96a6 100644 --- a/tests/testNonlinearEqualityConstraint.cpp +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -10,6 +10,8 @@ #include #include +namespace eq2D = gtsam::simulated2D::equality_constraints; + using namespace std; using namespace gtsam; @@ -28,7 +30,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 pt(1.0, 2.0); simulated2D::PoseKey key(1); double mu = 1000.0; - simulated2D::UnaryEqualityConstraint constraint(pt, key, mu); + eq2D::UnaryEqualityConstraint constraint(pt, key, mu); simulated2D::Config config1; config1.insert(key, pt); @@ -51,7 +53,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); simulated2D::PoseKey key(1); double mu = 1000.0; - simulated2D::UnaryEqualityConstraint constraint(pt, key, mu); + eq2D::UnaryEqualityConstraint constraint(pt, key, mu); simulated2D::Config config1; config1.insert(key, pt); @@ -74,8 +76,8 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { Point2 truth_pt(1.0, 2.0); simulated2D::PoseKey key(1); double mu = 1000.0; - simulated2D::UnaryEqualityConstraint::shared_ptr constraint( - new simulated2D::UnaryEqualityConstraint(truth_pt, key, mu)); + eq2D::UnaryEqualityConstraint::shared_ptr constraint( + new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); Point2 badPt(100.0, -200.0); simulated2D::Prior::shared_ptr factor( @@ -99,7 +101,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); simulated2D::PoseKey key1(1), key2(2); double mu = 1000.0; - simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); + eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); simulated2D::Config config1; config1.insert(key1, x1); @@ -125,7 +127,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); simulated2D::PoseKey key1(1), key2(2); double mu = 1000.0; - simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); + eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); simulated2D::Config config1; config1.insert(key1, x1); @@ -155,8 +157,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { simulated2D::PoseKey key1(1), key2(2); // hard prior on x1 - simulated2D::UnaryEqualityConstraint::shared_ptr constraint1( - new simulated2D::UnaryEqualityConstraint(truth_pt1, key1)); + eq2D::UnaryEqualityConstraint::shared_ptr constraint1( + new eq2D::UnaryEqualityConstraint(truth_pt1, key1)); // soft prior on x2 Point2 badPt(100.0, -200.0); @@ -164,8 +166,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { new simulated2D::Prior(badPt, soft_model, key2)); // odometry constraint - simulated2D::OdoEqualityConstraint::shared_ptr constraint2( - new simulated2D::OdoEqualityConstraint( + eq2D::OdoEqualityConstraint::shared_ptr constraint2( + new eq2D::OdoEqualityConstraint( gtsam::between(truth_pt1, truth_pt2), key1, key2)); shared_graph graph(new Graph()); @@ -198,8 +200,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { simulated2D::PointKey l1(1), l2(2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); - graph->add(simulated2D::UnaryEqualityConstraint(pt_x1, x1)); - graph->add(simulated2D::UnaryEqualityConstraint(pt_x2, x2)); + graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); + graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); Point2 z1(0.0, 5.0); SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); @@ -208,7 +210,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Point2 z2(-4.0, 0.0); graph->add(simulated2D::Measurement(z2, sigma, x2,l2)); - graph->add(simulated2D::PointEqualityConstraint(l1, l2)); + graph->add(eq2D::PointEqualityConstraint(l1, l2)); shared_config initialEstimate(new simulated2D::Config()); initialEstimate->insert(x1, pt_x1); @@ -237,7 +239,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // constant constraint on x1 Point2 pose1(1.0, 1.0); - graph->add(simulated2D::UnaryEqualityConstraint(pose1, x1)); + graph->add(eq2D::UnaryEqualityConstraint(pose1, x1)); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1); @@ -250,7 +252,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { graph->add(simulated2D::Measurement(z2, sigma, x2, l2)); // equality constraint between l1 and l2 - graph->add(simulated2D::PointEqualityConstraint(l1, l2)); + graph->add(eq2D::PointEqualityConstraint(l1, l2)); // create an initial estimate shared_config initialEstimate(new simulated2D::Config()); diff --git a/tests/testNonlinearInequalityConstraint.cpp b/tests/testNonlinearInequalityConstraint.cpp new file mode 100644 index 000000000..9ec1b9d4c --- /dev/null +++ b/tests/testNonlinearInequalityConstraint.cpp @@ -0,0 +1,112 @@ +/** + * @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); } +/* ************************************************************************* */ + +