Implemented linearization and equality for unary NonlinearConstraints. Current tests use a scalar example. Split out implementation into a separate implementation file.

cpp/Makefile.am

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

cpp/NonlinearConstraint-inl.h

@@ -0,0 +1,60 @@
+/*
+ * @file NonlinearConstraint-inl.h
+ * @brief Implementation for NonlinearConstraints
+ * @author alexgc
+ */
+
+#pragma once
+
+#include <iostream>
+#include "NonlinearConstraint.h"
+
+namespace gtsam {
+
+/**
+ * Implementations of unary nonlinear constraints
+ */
+
+template <class Config>
+void NonlinearConstraint1<Config>::print(const std::string& s) const {
+	std::cout << "NonlinearConstraint [" << s << "]:\n"
+			<< "  Variable:   " << key_ << "\n"
+			<< "  p:          " << this->p_ << "\n"
+			<< "  lambda key: " << this->lagrange_key_ << std::endl;
+}
+
+template <class Config>
+bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) const {
+	const NonlinearConstraint1<Config>* p = dynamic_cast<const NonlinearConstraint1<Config>*> (&f);
+	if (p == NULL) return false;
+	if (key_ != p->key_) return false;
+	if (this->lagrange_key_ != p->lagrange_key_) return false;
+	if (g_ != p->g_) return false;
+	if (gradG_ != p->gradG_) return false;
+	return this->p_ == p->p_;
+}
+
+template <class Config>
+std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
+NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
+	// extract lagrange multiplier
+	Vector lambda = lagrange[this->lagrange_key_];
+
+	// find the error
+	Vector g = g_(config, key_);
+
+	// construct the gradient
+	Matrix grad = gradG_(config, key_);
+
+	// construct probabilistic factor
+	Matrix A1 = vector_scale(grad, lambda);
+	GaussianFactor::shared_ptr factor(new
+			GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
+
+	// construct the constraint
+	GaussianFactor::shared_ptr constraint(new GaussianFactor("x", grad, g, 0.0));
+
+	return std::make_pair(factor, constraint);
+}
+
+}

cpp/NonlinearConstraint.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include <map>
+#include <iostream>
 #include "NonlinearFactor.h"
 
 namespace gtsam {
@@ -81,7 +82,7 @@ public:
  * A unary constraint with arbitrary cost and gradient functions
  */
 template <class Config>
-class NonlinearConstraint1 :  NonlinearConstraint<Config> {
+class NonlinearConstraint1 : public NonlinearConstraint<Config> {
 
 private:
 	/** calculates the constraint function of the current config
@@ -122,21 +123,17 @@ public:
 			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
+		// set a good lagrange key here
+		// TODO: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 {
+	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 {
+	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 {
@@ -151,13 +148,7 @@ public:
 	 * @return a pair GaussianFactor (probabilistic) and GaussianFactor (constraint)
 	 */
 	std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
-	linearize(const Config& config, const VectorConfig& lagrange) const {
+	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);
-	}
-
 };
 
 

cpp/testNonlinearConstraint.cpp

@@ -7,14 +7,12 @@
 #include <CppUnitLite/TestHarness.h>
 #include <VectorConfig.h>
 #include <NonlinearConstraint.h>
+#include <NonlinearConstraint-inl.h>
 
 using namespace gtsam;
 
-// define some functions to use
-// these examples use scalar variables and a single constraint
-
 /* ************************************************************************* */
-// unary functions
+// unary functions with scalar variables
 /* ************************************************************************* */
 namespace test1 {
 /** p = 1, gradG(x) = 2x */
@@ -31,7 +29,7 @@ Vector g_func(const VectorConfig& config, const std::string& key) {
 } // \namespace test1
 
 /* ************************************************************************* */
-TEST( NonlinearConstraint, unary_construction ) {
+TEST( NonlinearConstraint, unary_scalar_construction ) {
 	// construct a constraint on x
 	// the lagrange multipliers will be expected on L_x1
 	// and there is only one multiplier
@@ -48,40 +46,46 @@ TEST( NonlinearConstraint, unary_construction ) {
 	CHECK(assert_equal(actual, expected, 1e-5));
 }
 
-///* ************************************************************************* */
+/* ************************************************************************* */
-//TEST( NonlinearConstraint, unary_linearize ) {
+TEST( NonlinearConstraint, unary_scalar_linearize ) {
-//	// constuct a constraint
+	// construct a constraint on x
-//	Vector lambda = Vector_(1, 1.0);
+	// the lagrange multipliers will be expected on L_x1
-//	NonlinearConstraint1<VectorConfig> c1("x", *grad_g1, *g1_func, lambda);
+	// and there is only one multiplier
-//
+	size_t p = 1;
-//	// get a configuration to use for linearization
+	NonlinearConstraint1<VectorConfig> c1("x", *test1::grad_g, *test1::g_func, p, "L_x1");
-//	VectorConfig config;
-//	config.insert("x", Vector_(1, 1.0));
-//
-//	// determine the gradient of the function
-//	Matrix
-//
-//}
 
-///* ************************************************************************* */
+	// get a configuration to use for linearization
-//// binary functions
+	VectorConfig realconfig;
-//Matrix grad1_g2(const VectorConfig& config) {
+	realconfig.insert("x", Vector_(1, 1.0));
-//	return eye(1);
+
-//}
+	// get a configuration of Lagrange multipliers
-//
+	VectorConfig lagrangeConfig;
-//Matrix grad2_g2(const VectorConfig& config) {
+	lagrangeConfig.insert("L_x1", Vector_(1, 3.0));
-//	return eye(1);
+
-//}
+	// linearize the system
-//
+	GaussianFactor::shared_ptr actFactor, actConstraint;
-//Vector g2_func(const VectorConfig& config) {
+	boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig);
-//	return zero(1);
+
-//}
+	// verify
-//
+	GaussianFactor expFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0);
-///* ************************************************************************* */
+	GaussianFactor expConstraint("x", Matrix_(1,1, 2.0), Vector_(1,-4.0), 0.0);
-//TEST( NonlinearConstraint, binary_construction ) {
+	CHECK(assert_equal(*actFactor, expFactor));
-//	Vector lambda = Vector_(1, 1.0);
+	CHECK(assert_equal(*actConstraint, expConstraint));
-//	NonlinearConstraint2<VectorConfig> c1("x", *grad1_g2, "y", *grad2_g2, *g2_func, lambda);
+}
-//}
+
+/* ************************************************************************* */
+TEST( NonlinearConstraint, unary_scalar_equal ) {
+	NonlinearConstraint1<VectorConfig>
+		c1("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
+		c2("x", *test1::grad_g, *test1::g_func, 1, "L_x1"),
+		c3("x", *test1::grad_g, *test1::g_func, 2, "L_x1"),
+		c4("y", *test1::grad_g, *test1::g_func, 1, "L_x1");
+
+	CHECK(assert_equal(c1, c2));
+	CHECK(assert_equal(c2, c1));
+	CHECK(!c1.equals(c3));
+	CHECK(!c1.equals(c4));
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

cpp/testSQP.cpp

@@ -192,7 +192,7 @@ TEST (SQP, problem1_sqp ) {
 		 * to zero
 		 * lam*gradG*dx + dlam + lam
 		 * formulated in matrix form as:
-		 * [lam*gradG eye(1)] [dx; dlam] = lam
+		 * [lam*gradG eye(1)] [dx; dlam] = zero
 		 */
 		GaussianFactor::shared_ptr f2(
 				new GaussianFactor("x", lam*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)