Basic implementation of a binary nonlinear constraint, with working linearization of a binary constraint.

cpp/NonlinearConstraint-inl.h

@@ -11,18 +11,20 @@
 
 namespace gtsam {
 
-/**
+/* ************************************************************************* */
- * Implementations of unary nonlinear constraints
+// Implementations of unary nonlinear constraints
- */
+/* ************************************************************************* */
 
+/* ************************************************************************* */
 template <class Config>
 void NonlinearConstraint1<Config>::print(const std::string& s) const {
-	std::cout << "NonlinearConstraint [" << s << "]:\n"
+	std::cout << "NonlinearConstraint1 [" << s << "]:\n"
-			<< "  Variable:   " << key_ << "\n"
+			<< "  key:        " << 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);
@@ -34,6 +36,7 @@ bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) c
 	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 {
@@ -52,7 +55,62 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
 			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));
+	GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, g, 0.0));
+
+	return std::make_pair(factor, constraint);
+}
+
+/* ************************************************************************* */
+// Implementations of binary nonlinear constraints
+/* ************************************************************************* */
+
+/* ************************************************************************* */
+template <class Config>
+void NonlinearConstraint2<Config>::print(const std::string& s) const {
+	std::cout << "NonlinearConstraint2 [" << s << "]:\n"
+			<< "  key1:       " << key1_ << "\n"
+			<< "  key2:       " << key2_ << "\n"
+			<< "  p:          " << this->p_ << "\n"
+			<< "  lambda key: " << this->lagrange_key_ << std::endl;
+}
+
+/* ************************************************************************* */
+template <class Config>
+bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) const {
+	const NonlinearConstraint2<Config>* p = dynamic_cast<const NonlinearConstraint2<Config>*> (&f);
+	if (p == NULL) return false;
+	if (key1_ != p->key1_) return false;
+	if (key2_ != p->key2_) return false;
+	if (this->lagrange_key_ != p->lagrange_key_) return false;
+	if (g_ != p->g_) return false;
+	if (gradG1_ != p->gradG1_) return false;
+	if (gradG2_ != p->gradG2_) return false;
+	return this->p_ == p->p_;
+}
+
+/* ************************************************************************* */
+template <class Config>
+std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
+NonlinearConstraint2<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, key1_, key2_);
+
+	// construct the gradients
+	Matrix grad1 = gradG1_(config, key1_);
+	Matrix grad2 = gradG2_(config, key2_);
+
+	// construct probabilistic factor
+	Matrix A1 = vector_scale(grad1, lambda);
+	Matrix A2 = vector_scale(grad2, lambda);
+	GaussianFactor::shared_ptr factor(new
+			GaussianFactor(key1_, A1, key2_, A2,
+					this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
+
+	// construct the constraint
+	GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, g, 0.0));
 
 	return std::make_pair(factor, constraint);
 }

cpp/NonlinearConstraint.h

@@ -151,65 +151,83 @@ public:
 	linearize(const Config& config, const VectorConfig& lagrange) const;
 };
 
+/**
+ * A binary constraint with arbitrary cost and gradient functions
+ */
+template <class Config>
+class NonlinearConstraint2 : public 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 key1 is the id for the first variable
+	 * @param key2 is the id for the second variable
+	 * @return the cost for each of p constraints, arranged in a vector
+	 */
+	Vector (*g_)(const Config& config, const std::string& key1, const std::string& key2);
 
-//template <class Config>
+	/**
-//class NonlinearConstraint2 : public NonlinearConstraint<Config> {
+	 * Calculates the gradients of the constraint function in terms of
-//
+	 * the first and second variables
-//private:
+	 * returns a pxn matrix
-//	/**
+	 * @param config to use for linearization
-//	 * Calculates the gradient of the constraint function
+	 * @param key of selected variable
-//	 * returns a pxn matrix for x1
+	 * @return the jacobian of the constraint in terms of key
-//	 * @param config
+	 */
-//	 */
+	Matrix (*gradG1_) (const Config& config, const std::string& key);
-//	Matrix (*gradG1_) (const Config& config);
+	Matrix (*gradG2_) (const Config& config, const std::string& key);
-//
+
-//	/**
+	/** keys for the constrained variables */
-//	 * Calculates the gradient of the constraint function
+	std::string key1_;
-//	 * returns a pxn matrix for x2
+	std::string key2_;
-//	 * @param config
+
-//	 */
+public:
-//	Matrix (*gradG2_) (const Config& config);
+
-//
+	/**
-//	/** keys for the constrained variables */
+	 * Basic constructor
-//	std::string key1_;
+	 * @param key is the identifier for the variable constrained
-//	std::string key2_;
+	 * @param gradG gives the gradient of the constraint function
-//
+	 * @param g is the constraint function
-//public:
+	 * @param dim_constraint is the size of the constraint (p)
-//
+	 * @param lagrange_key is the identifier for the lagrange multiplier
-//	/**
+	 */
-//	 * Basic constructor
+	NonlinearConstraint2(
-//	 * @param key1 is the first variable
+			const std::string& key1,
-//	 * @param gradG1 gives the gradient for the first variable
+			Matrix (*gradG1)(const Config& config, const std::string& key),
-//	 * @param key2 is the first variable
+			const std::string& key2,
-//	 * @param gradG2 gives the gradient for the first variable
+			Matrix (*gradG2)(const Config& config, const std::string& key),
-//	 * @param g is the constraint function
+			Vector (*g)(const Config& config, const std::string& key1, const std::string& key2),
-//	 * @param lambdas is vector of size p with Lagrange multipliers
+			size_t dim_constraint,
-//	 */
+			const std::string& lagrange_key="") :
-//	NonlinearConstraint2(
+				NonlinearConstraint<Config>(lagrange_key, dim_constraint),
-//			const std::string& key1,
+				g_(g), gradG1_(gradG1), gradG2_(gradG2), key1_(key1), key2_(key2) {
-//			Matrix (*gradG1)(const Config& config),
+		// set a good lagrange key here
-//			const std::string& key2,
+		// TODO:should do something smart to find a unique one
-//			Matrix (*gradG2)(const Config& config),
+		if (lagrange_key == "")
-//			Vector (*g)(const Config& config),
+			this->lagrange_key_ = "L_" + key1 + key2;
-//			const Vector& lambdas) :
+	}
-//				NonlinearConstraint<Config>(lambdas, g),
+
-//				gradG1_(gradG1), key1_(key1),
+	/** Print */
-//				gradG2_(gradG2), key2_(key2) {}
+	void print(const std::string& s = "") const;
-//
+
-//	/** Print */
+	/** Check if two factors are equal */
-//	void print(const std::string& s = "") const {}
+	bool equals(const Factor<Config>& f, double tol=1e-9) const;
-//
+
-//	/** Check if two factors are equal */
+	/** error function - returns the result of the constraint function */
-//	bool equals(const Factor<Config>& f, double tol=1e-9) const {
+	inline Vector error_vector(const Config& c) const {
-//		return true;
+		return g_(c, key1_, key2_);
-//	}
+	}
-//
+
-//	/** Linearize a non-linearFactor2 to get a linearFactor2 */
+	/**
-//	boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
+	 * Linearize using a real Config and a VectorConfig of Lagrange multipliers
-//		boost::shared_ptr<GaussianFactor> ret;
+	 * Returns the two separate Gaussian factors to solve
-//		return ret;
+	 * @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;
+};
 
 }

cpp/testNonlinearConstraint.cpp

@@ -29,7 +29,7 @@ Vector g_func(const VectorConfig& config, const std::string& key) {
 } // \namespace test1
 
 /* ************************************************************************* */
-TEST( NonlinearConstraint, unary_scalar_construction ) {
+TEST( NonlinearConstraint1, unary_scalar_construction ) {
 	// construct a constraint on x
 	// the lagrange multipliers will be expected on L_x1
 	// and there is only one multiplier
@@ -47,10 +47,7 @@ TEST( NonlinearConstraint, unary_scalar_construction ) {
 }
 
 /* ************************************************************************* */
-TEST( NonlinearConstraint, unary_scalar_linearize ) {
+TEST( NonlinearConstraint1, unary_scalar_linearize ) {
-	// 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");
 
@@ -74,7 +71,7 @@ TEST( NonlinearConstraint, unary_scalar_linearize ) {
 }
 
 /* ************************************************************************* */
-TEST( NonlinearConstraint, unary_scalar_equal ) {
+TEST( NonlinearConstraint1, 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"),
@@ -87,6 +84,99 @@ TEST( NonlinearConstraint, unary_scalar_equal ) {
 	CHECK(!c1.equals(c4));
 }
 
+/* ************************************************************************* */
+// binary functions with scalar variables
+/* ************************************************************************* */
+namespace test2 {
+/** gradient for x, gradG(x,y) in x: 2x*/
+Matrix grad_g1(const VectorConfig& config, const std::string& key) {
+	double x = config[key](0);
+	return Matrix_(1,1, 2.0*x);
+}
+
+/** gradient for y, gradG(x,y) in y: -1 */
+Matrix grad_g2(const VectorConfig& config, const std::string& key) {
+	double x = config[key](0);
+	return Matrix_(1,1, -1.0);
+}
+
+/** p = 1, g(x) = x^2-5 -y = 0 */
+Vector g_func(const VectorConfig& config, const std::string& key1, const std::string& key2) {
+	double x = config[key1](0);
+	double y = config[key2](0);
+	return Vector_(1, x*x-5.0-y);
+}
+} // \namespace test2
+
+/* ************************************************************************* */
+TEST( NonlinearConstraint2, binary_scalar_construction ) {
+	// construct a constraint on x and y
+	// the lagrange multipliers will be expected on L_xy
+	// and there is only one multiplier
+	size_t p = 1;
+	NonlinearConstraint2<VectorConfig> c1(
+			"x", *test2::grad_g1,
+			"y", *test2::grad_g2,
+			*test2::g_func, p, "L_xy");
+
+	// get a configuration to use for finding the error
+	VectorConfig config;
+	config.insert("x", Vector_(1, 1.0));
+	config.insert("y", Vector_(1, 2.0));
+
+	// calculate the error
+	Vector actual = c1.error_vector(config);
+	Vector expected = Vector_(1.0, -6.0);
+	CHECK(assert_equal(actual, expected, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST( NonlinearConstraint2, binary_scalar_linearize ) {
+	// create a constraint
+	size_t p = 1;
+	NonlinearConstraint2<VectorConfig> c1(
+			"x", *test2::grad_g1,
+			"y", *test2::grad_g2,
+			*test2::g_func, p, "L_xy");
+
+	// get a configuration to use for finding the error
+	VectorConfig realconfig;
+	realconfig.insert("x", Vector_(1, 1.0));
+	realconfig.insert("y", Vector_(1, 2.0));
+
+	// get a configuration of Lagrange multipliers
+	VectorConfig lagrangeConfig;
+	lagrangeConfig.insert("L_xy", Vector_(1, 3.0));
+
+	// linearize the system
+	GaussianFactor::shared_ptr actFactor, actConstraint;
+	boost::tie(actFactor, actConstraint) = c1.linearize(realconfig, lagrangeConfig);
+
+	// verify
+	GaussianFactor expFactor("x", Matrix_(1,1, 6.0),
+							 "y", Matrix_(1,1, -3.0),
+							 "L_xy", eye(1), zero(1), 1.0);
+	GaussianFactor expConstraint("x", Matrix_(1,1, 2.0),
+								 "y", Matrix_(1,1, -1.0),
+								 Vector_(1,-6.0), 0.0);
+	CHECK(assert_equal(*actFactor, expFactor));
+	CHECK(assert_equal(*actConstraint, expConstraint));
+}
+
+/* ************************************************************************* */
+TEST( NonlinearConstraint2, binary_scalar_equal ) {
+	NonlinearConstraint2<VectorConfig>
+		c1("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
+		c2("x", *test2::grad_g1, "y", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
+		c3("y", *test2::grad_g1, "x", *test2::grad_g2,*test2::g_func, 1, "L_xy"),
+		c4("x", *test2::grad_g1, "z", *test2::grad_g2,*test2::g_func, 3, "L_xy");
+
+	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); }
 /* ************************************************************************* */