Added tests for nonlinear equality constraints, generalized equality constraints

nonlinear/NonlinearConstraint.h

@@ -33,7 +33,9 @@ class NonlinearConstraint : public NonlinearFactor<Config> {
 protected:
 	typedef NonlinearConstraint<Config> This;
 	typedef NonlinearFactor<Config> 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<Config>& f, double tol=1e-9) const {
 		const This* p = dynamic_cast<const This*> (&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<Matrix&> H =
 			boost::none) const = 0;
 };
 
+/**
+ * Unary Equality constraint - simply forces the value of active() to true
+ */
+template <class Config, class Key, class X>
+class NonlinearEqualityConstraint1 : public NonlinearConstraint1<Config, Key, X> {
+
+protected:
+	typedef NonlinearEqualityConstraint1<Config,Key,X> This;
+	typedef NonlinearConstraint1<Config,Key,X> 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<Matrix&> H1 = boost::none,
 			boost::optional<Matrix&> H2 = boost::none) const = 0;
 };
 
+/**
+ * Binary Equality constraint - simply forces the value of active() to true
+ */
+template <class Config, class Key1, class X1, class Key2, class X2>
+class NonlinearEqualityConstraint2 : public NonlinearConstraint2<Config, Key1, X1, Key2, X2> {
+
+protected:
+	typedef NonlinearEqualityConstraint2<Config,Key1,X1,Key2,X2> This;
+	typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> 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 Config, class Key, class X>
+class NonlinearEquality1 : public NonlinearEqualityConstraint1<Config, Key, X> {
+protected:
+	typedef NonlinearEqualityConstraint1<Config, Key, X> Base;
+
+	X value_; /// fixed value for variable
+
+public:
+
+	typedef boost::shared_ptr<NonlinearEquality1<Config, Key, X> > 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<Matrix&> 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 Config, class Key, class X>
-class NonlinearEquality2 : public NonlinearConstraint2<Config, Key, X, Key, X> {
+class NonlinearEquality2 : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> {
 protected:
-	typedef NonlinearConstraint2<Config, Key, X, Key, X> Base;
+	typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> 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 Config, class Key, class X>
+class BetweenConstraint : public NonlinearEqualityConstraint2<Config, Key, X, Key, X> {
+protected:
+	typedef NonlinearEqualityConstraint2<Config, Key, X, Key, X> Base;
+
+	X measured_; /// fixed between value
+
+public:
+
+	typedef boost::shared_ptr<BetweenConstraint<Config, Key, X> > 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<Matrix&> H1 = boost::none,
+			boost::optional<Matrix&> H2 = boost::none) const {
+		X hx = between(x1, x2, H1, H2);
+		return logmap(measured_, hx);
+	}
+
+	inline const X measured() const {
+		return measured_;
+	}
+};
+
 }

nonlinear/NonlinearFactorGraph-inl.h

@@ -51,7 +51,7 @@ namespace gtsam {
 		// linearize all factors
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
 			boost::shared_ptr<GaussianFactor> lf = factor->linearize(config);
-			linearFG->push_back(lf);
+			if (lf)	linearFG->push_back(lf);
 		}
 
 		return linearFG;

slam/simulated2DConstraints.h

@@ -17,55 +17,56 @@ namespace gtsam {
 
 	namespace simulated2D {
 
-		/**
+		namespace equality_constraints {
-		 * Unary constraint encoding a hard equality on a Point
-		 */
-		template<class Cfg = Config, class Key = PoseKey>
-		struct GenericUnaryEqualityConstraint: public NonlinearConstraint1<Cfg, Key, Point2> {
-			typedef NonlinearConstraint1<Cfg, Key, Point2> Base;
-			typedef boost::shared_ptr<GenericUnaryEqualityConstraint<Cfg, Key> > shared_ptr;
 
-			Point2 z_;
+			/** Typedefs for regular use */
+			typedef NonlinearEquality1<Config, PoseKey, Point2> UnaryEqualityConstraint;
+			typedef BetweenConstraint<Config, PoseKey, Point2> OdoEqualityConstraint;
 
-			GenericUnaryEqualityConstraint(const Point2& z, const Key& key, double mu = 1000.0) :
+			/** Equality between variables */
-				Base(key, 2, mu), z_(z) {
+			typedef NonlinearEquality2<Config, PoseKey, Point2> PoseEqualityConstraint;
-			}
+			typedef NonlinearEquality2<Config, PointKey, Point2> PointEqualityConstraint;
 
-			Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
+		} // \namespace equality_constraints
-					boost::none) const {
-				return (prior(x, H) - z_).vector();
-			}
 
-		};
+		namespace inequality_constraints {
 
-		/**
+			/**
-		 * Binary constraint simulating "odometry" between two Poses
+			 * Unary inequality constraint forcing a coordinate to be greater than a fixed value (c)
-		 */
+			 */
-		template<class Cfg = Config, class Key = PoseKey>
+			template<class Cfg, class Key, unsigned int Idx>
-		struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2<Cfg, Key, Point2, Key,	Point2> {
+			struct ScalarInequalityConstraint1: public NonlinearConstraint1<Cfg, Key, Point2> {
-			typedef NonlinearConstraint2<Cfg, Key, Point2, Key,	Point2> Base;
+				typedef NonlinearConstraint1<Cfg, Key, Point2> Base;
-			typedef boost::shared_ptr<GenericOdoHardEqualityConstraint<Cfg,Key> > shared_ptr;
+				typedef boost::shared_ptr<ScalarInequalityConstraint1<Cfg, Key, Idx> > shared_ptr;
-			Point2 z_;
 
-			GenericOdoHardEqualityConstraint(
+				double c_; 	   /// min value of the selected coordinate
-					const Point2& z, const Key& i1, const Key& i2, double mu = 1000.0) :
-				Base (i1, i2, 2, mu), z_(z) {
-			}
 
-			Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
+				ScalarInequalityConstraint1(const Key& key, double c, double mu = 1000.0) :
-					Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+					Base(key, 1, mu), c_(c) {
-				return (odo(x1, x2, H1, H2) - z_).vector();
+				}
-			}
 
-		};
+				/** 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 */
+				Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
-		typedef GenericUnaryEqualityConstraint<Config, PoseKey> UnaryEqualityConstraint;
+						boost::none) const {
-		typedef GenericOdoHardEqualityConstraint<Config, PoseKey> OdoEqualityConstraint;
+					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<Config, PoseKey, Point2> PoseEqualityConstraint;
-		typedef NonlinearEquality2<Config, PointKey, Point2> PointEqualityConstraint;
 
-	} // namespace simulated2D
+			/** typedefs for use with simulated2D systems */
-} // namespace gtsam
+			typedef ScalarInequalityConstraint1<Config, PoseKey, 0> PoseXInequality;
+			typedef ScalarInequalityConstraint1<Config, PoseKey, 1> PoseYInequality;
+
+		} // \namespace inequality_constraints
+
+	} // \namespace simulated2D
+} // \namespace gtsam

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 

tests/testNonlinearConstraint.cpp

@@ -49,158 +49,158 @@ using namespace boost::assign;
 //typedef boost::shared_ptr<simulated2D::GenericMeasurement<Config2D> > shared;
 //typedef NonlinearOptimizer<Graph2D, Config2D> Optimizer;
 
-///* ********************************************************************* */
+/* ********************************************************************* */
-//// This is an obstacle avoidance demo, where there is a trajectory of
+// This is an obstacle avoidance demo, where there is a trajectory of
-//// three points, where there is a circular obstacle in the middle.  There
+// three points, where there is a circular obstacle in the middle.  There
-//// is a binary inequality constraint connecting the obstacle to the
+// is a binary inequality constraint connecting the obstacle to the
-//// states, which enforces a minimum distance.
+// states, which enforces a minimum distance.
-///* ********************************************************************* */
+/* ********************************************************************* */
+
+//typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
+//typedef shared_ptr<AvoidConstraint> shared_a;
+//typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseNLConstraint;
+//typedef shared_ptr<PoseNLConstraint> shared_pc;
+//typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
+//typedef shared_ptr<ObstacleConstraint> shared_oc;
 //
-////typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
-////typedef shared_ptr<AvoidConstraint> shared_a;
-////typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseNLConstraint;
-////typedef shared_ptr<PoseNLConstraint> shared_pc;
-////typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
-////typedef shared_ptr<ObstacleConstraint> 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<NLGraph, Config2D> 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<Config2D> 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())));
-////}
 //
-/////* ********************************************************************* */
+//namespace constrained_avoid1 {
-////TEST ( SQPOptimizer, inequality_avoid_iterative ) {
+//// avoidance radius
-////	// create the graph
+//double radius = 1.0;
-////	NLGraph graph; Config2D feasible;
+//
-////	boost::tie(graph, feasible) = obstacleAvoidGraph();
+//// binary avoidance constraint
-////
+///** g(x) = ||x2-obs||^2 - radius^2 > 0 */
-////	// create the rest of the config
+//Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
-////	shared_ptr<Config2D> init(new Config2D(feasible));
+//	double dist2 = config[x].dist(config[obs]);
-////	PoseKey x2(2);
+//	double thresh = radius*radius;
-////	init->insert(x2, Point2(5.0, 100.0));
+//	return Vector_(1, dist2-thresh);
-////
+//}
-////	// create an ordering
+//
-////	Ordering ord;
+///** jacobian at pose */
-////	ord += "x1", "x2", "x3", "l1";
+//Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
-////
+//	Point2 p = config[x]-config[obs];
-////	// create an optimizer
+//	return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
-////	Optimizer optimizer(graph, ord, init);
+//}
-////
+//
-////	double relThresh = 1e-5; // minimum change in error between iterations
+///** jacobian at obstacle */
-////	double absThresh = 1e-5; // minimum error necessary to converge
+//Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
-////	double constraintThresh = 1e-9; // minimum constraint error to be feasible
+//	Point2 p = config[x]-config[obs];
-////	Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
+//	return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
-////
+//}
-////	// verify
+//}
-////	Config2D exp2(feasible);
+//
-////	exp2.insert(x2, Point2(5.0, 0.5));
+//pair<NLGraph, Config2D> obstacleAvoidGraph() {
-////	CHECK(assert_equal(exp2, *(final.config())));
+//	// 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<Config2D> 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<Config2D> 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); }

tests/testNonlinearEqualityConstraint.cpp

@@ -10,6 +10,8 @@
 #include <NonlinearFactorGraph-inl.h>
 #include <NonlinearOptimizer-inl.h>
 
+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(
+	eq2D::UnaryEqualityConstraint::shared_ptr constraint(
-			new simulated2D::UnaryEqualityConstraint(truth_pt, key, mu));
+			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(
+	eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
-			new simulated2D::UnaryEqualityConstraint(truth_pt1, key1));
+			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(
+	eq2D::OdoEqualityConstraint::shared_ptr constraint2(
-			new simulated2D::OdoEqualityConstraint(
+			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(eq2D::UnaryEqualityConstraint(pt_x1, x1));
-	graph->add(simulated2D::UnaryEqualityConstraint(pt_x2, x2));
+	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());

tests/testNonlinearInequalityConstraint.cpp

@@ -0,0 +1,112 @@
+/**
+ * @file testNonlinearInequalityConstraint.cpp
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <simulated2DConstraints.h>
+#include <NonlinearFactorGraph-inl.h>
+#include <NonlinearOptimizer-inl.h>
+
+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<simulated2D::Config> Graph;
+typedef boost::shared_ptr<Graph> shared_graph;
+typedef boost::shared_ptr<simulated2D::Config> shared_config;
+typedef NonlinearOptimizer<Graph, simulated2D::Config> 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); }
+/* ************************************************************************* */
+
+