Added a NonlinearConstraint and testNonlinearConstraint. There is currently an abstract base class for constraints and a partially implemented unary constraint.

.cproject

@@ -521,10 +521,10 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
-<target name="testIncremental.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments/>
-<buildTarget>testIncremental.run</buildTarget>
+<buildTarget>testNonlinearConstraint.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>

cpp/Makefile.am

@@ -115,9 +115,12 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
 sources += NonlinearFactor.cpp 
 sources += NonlinearEquality.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP
+sources += NonlinearConstraint.h
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la
+testNonlinearConstraint_SOURCES  = $(example) testNonlinearConstraint.cpp
+testNonlinearConstraint_LDADD    = libgtsam.la
 testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
 testNonlinearFactorGraph_LDADD   = libgtsam.la
 testNonlinearOptimizer_SOURCES   = $(example) testNonlinearOptimizer.cpp

cpp/NonlinearConstraint.h

@@ -0,0 +1,224 @@
+/*
+ * @file NonlinearConstraint.h
+ * @brief Implements nonlinear constraints that can be linearized and
+ * inserted into an existing nonlinear graph and solved via SQP
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <map>
+#include "NonlinearFactor.h"
+
+namespace gtsam {
+
+/**
+ * Base class for nonlinear constraints
+ * This allows for both equality and inequality constraints,
+ * where equality constraints are active all the time (even slightly
+ * nonzero constraint functions will still be active - inequality
+ * constraints should be sure to force to actual zero)
+ *
+ * The measurement z in the underlying NonlinearFactor is the
+ * set of Lagrange multipliers.
+ */
+template <class Config>
+class NonlinearConstraint : public NonlinearFactor<Config> {
+
+protected:
+	/** key for the lagrange multipliers */
+	std::string lagrange_key_;
+
+	/** number of lagrange multipliers */
+	size_t p_;
+
+public:
+	/** Constructor - sets the cost function and the lagrange multipliers
+	 * @param lagrange_key is the label for the associated lagrange multipliers
+	 * @param dim_lagrange is the number of associated constraints
+	 */
+	NonlinearConstraint(const std::string& lagrange_key, size_t dim_lagrange) :
+		NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
+		lagrange_key_(lagrange_key), p_(dim_lagrange) {}
+
+	/** returns the key used for the Lagrange multipliers */
+	std::string& lagrangeKey() const { return lagrange_key_; }
+
+	/** returns the number of lagrange multipliers */
+	size_t nrConstraints() const { return p_; }
+
+	/** Print */
+	virtual void print(const std::string& s = "") const =0;
+
+	/** Check if two factors are equal */
+	virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
+
+	/** error function - returns the result of the constraint function */
+	virtual inline Vector error_vector(const Config& c) const=0;
+
+	/**
+	 * Linearize using a real Config and a VectorConfig of Lagrange multipliers
+	 * Returns the two separate Gaussian factors to solve
+	 * @param config is the real Config of the real variables
+	 * @param lagrange is the VectorConfig of lagrange multipliers
+	 * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
+	 */
+	virtual std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
+	linearize(const Config& config, const VectorConfig& lagrange) const=0;
+
+	/**
+	 * linearize with only Config, which is not currently implemented
+	 * This will be implemented later for other constrained optimization
+	 * algorithms
+	 */
+	virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
+		throw std::invalid_argument("No current constraint linearization for a single Config!");
+	}
+};
+
+
+/**
+ * A unary constraint with arbitrary cost and gradient functions
+ */
+template <class Config>
+class NonlinearConstraint1 :  NonlinearConstraint<Config> {
+
+private:
+	/** calculates the constraint function of the current config
+	 * If the value is zero, the constraint is not active
+	 * @param config is a configuration of all the variables
+	 * @param key is the id for the selected variable
+	 * @return the cost for each of p constraints, arranged in a vector
+	 */
+	Vector (*g_)(const Config& config, const std::string& key);
+
+	/**
+	 * Calculates the gradient of the constraint function
+	 * returns a pxn matrix
+	 * @param config to use for linearization
+	 * @param key of selected variable
+	 * @return the jacobian of the constraint in terms of key
+	 */
+	Matrix (*gradG_) (const Config& config, const std::string& key);
+
+	/** key for the constrained variable */
+	std::string key_;
+
+public:
+
+	/**
+	 * Basic constructor
+	 * @param key is the identifier for the variable constrained
+	 * @param gradG gives the gradient of the constraint function
+	 * @param g is the constraint function
+	 * @param dim_constraint is the size of the constraint (p)
+	 * @param lagrange_key is the identifier for the lagrange multiplier
+	 */
+	NonlinearConstraint1(
+			const std::string& key,
+			Matrix (*gradG)(const Config& config, const std::string& key),
+			Vector (*g)(const Config& config, const std::string& key),
+			size_t dim_constraint,
+			const std::string& lagrange_key="") :
+				NonlinearConstraint<Config>(lagrange_key, dim_constraint),
+				g_(g), gradG_(gradG), key_(key) {
+		// set a good lagrange key here - should do something smart to find a unique one
+		if (lagrange_key == "")
+			this->lagrange_key_ = "L_" + key;
+	}
+
+	/** Print */
+	void print(const std::string& s = "") const {
+		//FIXME: dummy implementation
+	}
+
+	/** Check if two factors are equal */
+	bool equals(const Factor<Config>& f, double tol=1e-9) const {
+		//FIXME: dummy implementation
+		return false;
+	}
+
+	/** error function - returns the result of the constraint function */
+	inline Vector error_vector(const Config& c) const {
+		return g_(c, key_);
+	}
+
+	/**
+	 * Linearize using a real Config and a VectorConfig of Lagrange multipliers
+	 * Returns the two separate Gaussian factors to solve
+	 * @param config is the real Config of the real variables
+	 * @param lagrange is the VectorConfig of lagrange multipliers
+	 * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
+	 */
+	std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
+	linearize(const Config& config, const VectorConfig& lagrange) const {
+		//FIXME: dummy implementation
+		GaussianFactor::shared_ptr factor(new GaussianFactor);
+		GaussianFactor::shared_ptr constraint(new GaussianFactor);
+		return std::make_pair(factor, constraint);
+	}
+
+};
+
+
+
+//template <class Config>
+//class NonlinearConstraint2 : public NonlinearConstraint<Config> {
+//
+//private:
+//	/**
+//	 * Calculates the gradient of the constraint function
+//	 * returns a pxn matrix for x1
+//	 * @param config
+//	 */
+//	Matrix (*gradG1_) (const Config& config);
+//
+//	/**
+//	 * Calculates the gradient of the constraint function
+//	 * returns a pxn matrix for x2
+//	 * @param config
+//	 */
+//	Matrix (*gradG2_) (const Config& config);
+//
+//	/** keys for the constrained variables */
+//	std::string key1_;
+//	std::string key2_;
+//
+//public:
+//
+//	/**
+//	 * Basic constructor
+//	 * @param key1 is the first variable
+//	 * @param gradG1 gives the gradient for the first variable
+//	 * @param key2 is the first variable
+//	 * @param gradG2 gives the gradient for the first variable
+//	 * @param g is the constraint function
+//	 * @param lambdas is vector of size p with Lagrange multipliers
+//	 */
+//	NonlinearConstraint2(
+//			const std::string& key1,
+//			Matrix (*gradG1)(const Config& config),
+//			const std::string& key2,
+//			Matrix (*gradG2)(const Config& config),
+//			Vector (*g)(const Config& config),
+//			const Vector& lambdas) :
+//				NonlinearConstraint<Config>(lambdas, g),
+//				gradG1_(gradG1), key1_(key1),
+//				gradG2_(gradG2), key2_(key2) {}
+//
+//	/** Print */
+//	void print(const std::string& s = "") const {}
+//
+//	/** Check if two factors are equal */
+//	bool equals(const Factor<Config>& f, double tol=1e-9) const {
+//		return true;
+//	}
+//
+//	/** Linearize a non-linearFactor2 to get a linearFactor2 */
+//	boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
+//		boost::shared_ptr<GaussianFactor> ret;
+//		return ret;
+//	}
+//};
+
+}

cpp/testNonlinearConstraint.cpp

@@ -0,0 +1,88 @@
+/*
+ * @file testNonlinearConstraint.cpp
+ * @brief Tests for nonlinear constraints handled via SQP
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <VectorConfig.h>
+#include <NonlinearConstraint.h>
+
+using namespace gtsam;
+
+// define some functions to use
+// these examples use scalar variables and a single constraint
+
+/* ************************************************************************* */
+// unary functions
+/* ************************************************************************* */
+namespace test1 {
+/** p = 1, gradG(x) = 2x */
+Matrix grad_g(const VectorConfig& config, const std::string& key) {
+	double x = config[key](0);
+	return Matrix_(1,1, 2*x);
+}
+
+/** p = 1, g(x) = x^2-5 = 0 */
+Vector g_func(const VectorConfig& config, const std::string& key) {
+	double x = config[key](0);
+	return Vector_(1, x*x-5);
+}
+} // \namespace test1
+
+/* ************************************************************************* */
+TEST( NonlinearConstraint, unary_construction ) {
+	// construct a constraint on x
+	// the lagrange multipliers will be expected on L_x1
+	// and there is only one multiplier
+	size_t p = 1;
+	NonlinearConstraint1<VectorConfig> c1("x", *test1::grad_g, *test1::g_func, p, "L_x1");
+
+	// get a configuration to use for finding the error
+	VectorConfig config;
+	config.insert("x", Vector_(1, 1.0));
+
+	// calculate the error
+	Vector actual = c1.error_vector(config);
+	Vector expected = Vector_(1.0, -4.0);
+	CHECK(assert_equal(actual, expected, 1e-5));
+}
+
+///* ************************************************************************* */
+//TEST( NonlinearConstraint, unary_linearize ) {
+//	// constuct a constraint
+//	Vector lambda = Vector_(1, 1.0);
+//	NonlinearConstraint1<VectorConfig> c1("x", *grad_g1, *g1_func, lambda);
+//
+//	// get a configuration to use for linearization
+//	VectorConfig config;
+//	config.insert("x", Vector_(1, 1.0));
+//
+//	// determine the gradient of the function
+//	Matrix
+//
+//}
+
+///* ************************************************************************* */
+//// binary functions
+//Matrix grad1_g2(const VectorConfig& config) {
+//	return eye(1);
+//}
+//
+//Matrix grad2_g2(const VectorConfig& config) {
+//	return eye(1);
+//}
+//
+//Vector g2_func(const VectorConfig& config) {
+//	return zero(1);
+//}
+//
+///* ************************************************************************* */
+//TEST( NonlinearConstraint, binary_construction ) {
+//	Vector lambda = Vector_(1, 1.0);
+//	NonlinearConstraint2<VectorConfig> c1("x", *grad1_g2, "y", *grad2_g2, *g2_func, lambda);
+//}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

cpp/testSQP.cpp

@@ -198,7 +198,7 @@ TEST (SQP, problem1_sqp ) {
 				new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
 								   "y", lam*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
 								   "lam", eye(1),     // dlam term
-								   Vector_(1.0, 0.0),             // rhs is zero
+								   Vector_(1, 0.0),             // rhs is zero
 								   1.0));                         // arbitrary sigma
 
 		// create the actual constraint
@@ -318,6 +318,69 @@ TEST (SQP, two_pose_truth ) {
 	CHECK(assert_equal(expected, *actual, 1e-5));
 }
 
+// Test of binary nonlinear equality constraint disabled until nonlinear constraints work
+
+///**
+// *  Version that actually uses nonlinear equality constraints
+// *  to to perform optimization.  Same as above, but no
+// *  equality constraint on x2, and two landmarks that
+// *  should be the same.
+// */
+//TEST (SQP, two_pose ) {
+//	// position (1, 1) constraint for x1
+//	VectorConfig feas;
+//	feas.insert("x1", Vector_(2, 1.0, 1.0));
+//
+//	// constant constraint on x1
+//	shared_eq ef1(new NonlinearEquality<VectorConfig>("x1", feas, 2, *vector_compare));
+//
+//	// measurement from x1 to l1
+//	Vector z1 = Vector_(2, 0.0, 5.0);
+//	double sigma1 = 0.1;
+//	shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
+//
+//	// measurement from x2 to l2
+//	Vector z2 = Vector_(2, -4.0, 0.0);
+//	double sigma2 = 0.1;
+//	shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
+//
+//	// equality constraint between l1 and l2
+//	boost::shared_ptr<NonlinearConstraint<VectorConfig> > c1(
+//			new NonlinearConstraint<VectorConfig>(
+//					"l1", "l2",        // specify nodes constrained
+//					*g_function,       // evaluate the cost
+//					*gradG_function)); // construct the gradient of g(x)
+//
+//	// construct the graph
+//	NLGraph graph;
+//	graph.push_back(ef1);
+//	graph.push_back(c1);
+//	graph.push_back(f1);
+//	graph.push_back(f2);
+//
+//	// create an initial estimate
+//	boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set
+//	initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
+//	//initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error
+//
+//	// optimize the graph
+//	Ordering ordering;
+//	ordering += "x1", "x2", "l1";
+//	Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
+//
+//	// display solution
+//	double relativeThreshold = 1e-5;
+//	double absoluteThreshold = 1e-5;
+//	Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold);
+//	boost::shared_ptr<const VectorConfig> actual = act_opt.config();
+//	//actual->print("Configuration after optimization");
+//
+//	// verify
+//	VectorConfig expected(feas);
+//	expected.insert("l1", Vector_(2, 1.0, 6.0));
+//	CHECK(assert_equal(expected, *actual, 1e-5));
+//}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */