ConstrainedLinearFactorGraphs now handles multiple constraints on a node properly.

smallExample was changed to include two of the examples used in testConstrainedLinearFactorGraph
ConditionalGaussian was changed to make solve() virtual, as this is necessary for ConstrainedConditionalGaussian

cpp/ConditionalGaussian.h

@@ -115,7 +115,7 @@ namespace gtsam {
      * @param x configuration in which the parents values (y,z,...) are known
      * @return solution x = R \ (d - Sy - Tz - ...)
      */
-    Vector solve(const FGConfig& x) const;
+    virtual Vector solve(const FGConfig& x) const;
 
     /**
      * adds a parent

cpp/ConstrainedLinearFactorGraph.cpp

@@ -13,10 +13,12 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
 ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph() {
 
 }
 
+/* ************************************************************************* */
 ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph(
 		const LinearFactorGraph& lfg) {
 	BOOST_FOREACH(LinearFactor::shared_ptr s, lfg)
@@ -24,14 +26,17 @@ ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph(
 	}
 }
 
+/* ************************************************************************* */
 ConstrainedLinearFactorGraph::~ConstrainedLinearFactorGraph() {
 }
 
+/* ************************************************************************* */
 void ConstrainedLinearFactorGraph::push_back_constraint(LinearConstraint::shared_ptr factor)
 {
 	constraints_.push_back(factor);
 }
 
+/* ************************************************************************* */
 bool ConstrainedLinearFactorGraph::is_constrained(const std::string& key) const
 {
 	BOOST_FOREACH(LinearConstraint::shared_ptr e, constraints_)
@@ -41,6 +46,7 @@ bool ConstrainedLinearFactorGraph::is_constrained(const std::string& key) const
 	return false;
 }
 
+/* ************************************************************************* */
 bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double tol) const
 {
 	const ConstrainedLinearFactorGraph* p = (const ConstrainedLinearFactorGraph *) &fg;
@@ -61,6 +67,7 @@ bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double to
 	return LinearFactorGraph::equals(fg, tol);
 }
 
+/* ************************************************************************* */
 ChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering) {
 	ChordalBayesNet::shared_ptr cbn (new ChordalBayesNet());
 
@@ -82,10 +89,19 @@ ChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Orderi
 	return cbn;
 }
 
+/* ************************************************************************* */
 ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::eliminate_constraint(const string& key)
 {
-	// extract the constraint - in-place remove from graph
+	// extract all adjacent constraints
-	LinearConstraint::shared_ptr constraint = extract_constraint(key);
+	vector<LinearConstraint::shared_ptr> constraint_separator = find_constraints_and_remove(key);
+	if (constraint_separator.size() == 0)
+		throw invalid_argument("No constraints on node " + key);
+
+	// split out a constraint to apply
+	LinearConstraint::shared_ptr constraint = pick_constraint(constraint_separator);
+
+	// perform change of variables on the remaining constraints and reinsert
+	update_constraints(key, constraint_separator, constraint);
 
 	// perform elimination on the constraint itself to get the constrained conditional gaussian
 	ConstrainedConditionalGaussian::shared_ptr ccg = constraint->eliminate(key);
@@ -93,11 +109,18 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
 	// perform a change of variables on the linear factors in the separator
 	LinearFactorSet separator = find_factors_and_remove(key);
 	BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) {
-		// reconstruct with a mutable factor
+		// store the block matrices
-		boost::shared_ptr<MutableLinearFactor> new_factor(new MutableLinearFactor);
+		map<string, Matrix> blocks;
+
+		// get the A1 term and reconstruct new_factor without the eliminated block
+		Matrix A1 = factor->get_A(key);
+		list<string> factor_keys = factor->keys();
+		BOOST_FOREACH(string cur_key, factor_keys) {
+			if (cur_key != key)
+				blocks.insert(make_pair(cur_key, factor->get_A(cur_key)));
+		}
 
 		// get T = A1*inv(C1) term
-		Matrix A1 = factor->get_A(key);
 		Matrix invC1 = inverse(constraint->get_A(key));
 		Matrix T = A1*invC1;
 
@@ -107,15 +130,26 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
 			Matrix Ci = constraint->get_A(cur_key);
 			if (cur_key != key && !factor->involves(cur_key)) {
 				Matrix Ai = T*Ci;
-				new_factor->insert(cur_key, -1 *Ai);
+				blocks.insert(make_pair(cur_key, -1 *Ai));
 			} else if (cur_key != key) {
 				Matrix Ai = factor->get_A(cur_key) - T*Ci;
-				new_factor->insert(cur_key, Ai);
+				blocks.insert(make_pair(cur_key, Ai));
 			}
 		}
 
+		// construct the updated factor
+		boost::shared_ptr<MutableLinearFactor> new_factor(new MutableLinearFactor);
+		string cur_key; Matrix M;
+		FOREACH_PAIR(cur_key, M, blocks) {
+			new_factor->insert(cur_key, M);
+		}
+
 		// update RHS of updated factor
-		Vector new_b = factor->get_b() - T*constraint->get_b();
+		Vector new_b(A1.size1());
+		if (factor->get_b().size() == 0)
+			new_b = -1 * (T * constraint->get_b());
+		else
+			new_b = factor->get_b() - T * constraint->get_b();
 		new_factor->set_b(new_b);
 
 		// insert the new factor into the graph
@@ -125,32 +159,87 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina
 	return ccg;
 }
 
-LinearConstraint::shared_ptr ConstrainedLinearFactorGraph::extract_constraint(const string& key)
+/* ************************************************************************* */
-{
+LinearConstraint::shared_ptr ConstrainedLinearFactorGraph::pick_constraint(
-	LinearConstraint::shared_ptr ret;
+		const std::vector<LinearConstraint::shared_ptr>& constraints) const {
-	bool found_key = false;
+	if (constraints.size() == 0)
-	vector<LinearConstraint::shared_ptr> new_vec;
+		throw invalid_argument("Constraint set is empty!");
-	BOOST_FOREACH(LinearConstraint::shared_ptr constraint, constraints_)
+	return constraints[0];
-	{
-		if (constraint->involves(key)) {
-			ret = constraint;
-			found_key = true;
-		}
-		else
-			new_vec.push_back(constraint);
-	}
-	constraints_ = new_vec;
-	if (!found_key)
-		throw invalid_argument("No constraint connected to node: " + key);
-	return ret;
 }
 
+/* ************************************************************************* */
+void ConstrainedLinearFactorGraph::update_constraints(const std::string& key,
+		const std::vector<LinearConstraint::shared_ptr>& separator,
+		const LinearConstraint::shared_ptr& primary) {
+	// Implements update for each constraint, where primary is
+	//    C1*x1 = d - C2*x2 - ...
+	// and each constraint is
+	//    A1*x1 + A2*x2 + ... = b;
+
+	// extract components from primary
+	Matrix invC1 = inverse(primary->get_A(key));
+	Vector d = primary->get_b();
+
+	// perform transform on each constraint
+	// constraint c is the one being updated
+	BOOST_FOREACH(LinearConstraint::shared_ptr updatee, separator) {
+		if (!updatee->equals(*primary)) {
+			// build transform matrix
+			Matrix A1 = updatee->get_A(key);
+			Matrix T = A1 * invC1;
+
+			// copy existing nodes into new factor without the eliminated variable
+			list<string> updatee_keys = updatee->keys(key);
+			map<string, Matrix> blocks;
+			BOOST_FOREACH(string cur_key, updatee_keys) {
+				blocks[cur_key] = updatee->get_A(cur_key);
+			}
+
+			// loop over all nodes in separator of constraint
+			list<string> primary_keys = primary->keys(key); // keys that are not key
+			BOOST_FOREACH(string cur_key, primary_keys) {
+				Matrix Ci = primary->get_A(cur_key);
+				if (cur_key != key && !updatee->involves(cur_key)) {
+					Matrix Ai = T*Ci;
+					blocks[cur_key] = -1 * Ai;
+				} else if (cur_key != key) {
+					Matrix Ai = updatee->get_A(cur_key) - T*Ci;
+					blocks[cur_key] = Ai;
+				}
+			}
+			Vector rhs = updatee->get_b() - T * d;
+			LinearConstraint::shared_ptr new_constraint(new LinearConstraint(blocks, rhs));
+
+			// reinsert into graph
+			push_back_constraint(new_constraint);
+		}
+	}
+}
+
+/* ************************************************************************* */
 FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) {
 	ChordalBayesNet::shared_ptr cbn = eliminate(ordering);
 	boost::shared_ptr<FGConfig> newConfig = cbn->optimize();
 	return *newConfig;
 }
 
+/* ************************************************************************* */
+std::vector<LinearConstraint::shared_ptr>
+ConstrainedLinearFactorGraph::find_constraints_and_remove(const string& key)
+{
+	vector<LinearConstraint::shared_ptr> found, uninvolved;
+	BOOST_FOREACH(LinearConstraint::shared_ptr constraint, constraints_) {
+		if (constraint->involves(key)) {
+			found.push_back(constraint);
+		} else {
+			uninvolved.push_back(constraint);
+		}
+	}
+	constraints_ = uninvolved;
+	return found;
+}
+
+/* ************************************************************************* */
 void ConstrainedLinearFactorGraph::print(const std::string& s) const
 {
 	cout << "ConstrainedFactorGraph: " << s << endl;
@@ -164,9 +253,11 @@ void ConstrainedLinearFactorGraph::print(const std::string& s) const
 	}
 }
 
+/* ************************************************************************* */
 Ordering ConstrainedLinearFactorGraph::getOrdering() const
 {
 	Ordering ord = LinearFactorGraph::getOrdering();
+	cout << "ConstrainedLinearFactorGraph::getOrdering() - Not Implemented!" << endl;
 	//	BOOST_FOREACH(LinearConstraint::shared_ptr e, constraints_)
 	//		ord.push_back(e->get_key());
 	return ord;

cpp/ConstrainedLinearFactorGraph.h

@@ -43,7 +43,11 @@ public:
 	 */
 	void push_back_constraint(LinearConstraint::shared_ptr constraint);
 
-	// Additional STL-like functions for Equality Factors
+	/**
+	 * STL-like indexing into the constraint vector
+	 * @param i index of the target constraint
+	 * @return the constraint to be returned
+	 */
 	LinearConstraint::shared_ptr constraint_at(const size_t i) const {return constraints_.at(i);}
 
     /** return the iterator pointing to the first equality factor */
@@ -78,11 +82,31 @@ public:
      */
     ChordalBayesNet::shared_ptr eliminate(const Ordering& ordering);
 
+    /**
+     * Picks one of the contraints in a set of constraints to eliminate
+     * Currently just picks the first one - should probably be optimized
+     * @param constraints is a set of constraints of which one will be eliminated
+     * @return one of the constraints to use
+     */
+	LinearConstraint::shared_ptr pick_constraint(
+			const std::vector<LinearConstraint::shared_ptr>& constraints) const;
+
+	/**
+	 * Eliminates the specified constraint for the selected variable,
+	 * and then performs a change of variables on all the constraints in the separator
+	 * and reinserts them into the graph.
+	 * @param key is the identifier for the node to eliminate
+	 * @param separator is the set of constraints to transform and reinsert
+	 * @param constraint is the primary constraint to eliminate
+	 */
+	void update_constraints(const std::string& key,
+			const std::vector<LinearConstraint::shared_ptr>& separator,
+			const LinearConstraint::shared_ptr& constraint);
+
     /**
      * Eliminates a node with a constraint on it
      * Other factors have a change of variables performed via Schur complement to remove the
      * eliminated node.
-     * FIXME: currently will not handle multiple constraints on the same node
      */
     ConstrainedConditionalGaussian::shared_ptr eliminate_constraint(const std::string& key);
 
@@ -103,10 +127,12 @@ public:
     void print(const std::string& s="") const;
 
     /**
-     * Finds a matching equality constraint by key - assumed present
+     * Pulls out all constraints attached to a particular node
-     * Performs in-place removal of the constraint
+     * Note: this removes the constraints in place
+     * @param key of the node to pull constraints on
+     * @return a set of constraints
      */
-    LinearConstraint::shared_ptr extract_constraint(const std::string& key);
+    std::vector<LinearConstraint::shared_ptr> find_constraints_and_remove(const std::string& key);
 
     /**
      * This function returns the best ordering for this linear factor

cpp/LinearConstraint.cpp

@@ -1,18 +1,20 @@
-/*
+/**
- * LinearConstraint.cpp
+ * @file LinearConstraint.cpp
- *
+ * @author Alex Cunningham
- *  Created on: Aug 10, 2009
- *      Author: alexgc
  */
 
 #include <iostream>
 #include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
 #include "LinearConstraint.h"
 #include "Matrix.h"
 
 namespace gtsam {
 using namespace std;
 
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
 LinearConstraint::LinearConstraint() {
 }
 
@@ -55,7 +57,12 @@ ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std
 }
 
 void LinearConstraint::print(const string& s) const {
-	cout << s << ": " << dump() << endl;
+	cout << s << ": " << endl;
+	string key; Matrix A;
+	FOREACH_PAIR(key, A, As) {
+		gtsam::print(A, key);
+	}
+	gtsam::print(b, "rhs= ");
 }
 
 bool LinearConstraint::equals(const LinearConstraint& f, double tol) const {
@@ -73,7 +80,7 @@ bool LinearConstraint::equals(const LinearConstraint& f, double tol) const {
 		if (it == f.As.end()) return false;
 		Matrix f_mat = it->second;
 		// check matrix
-		if (!(f_mat == p.second)) return false;
+		if (!equal_with_abs_tol(f_mat,p.second,tol)) return false;
 	}
 	return true;
 }
@@ -108,4 +115,31 @@ bool assert_equal(const LinearConstraint& actual,
 	return ret;
 }
 
+LinearConstraint::shared_ptr combineConstraints(
+		const set<LinearConstraint::shared_ptr>& constraints) {
+	map<string, Matrix> blocks;
+	Vector rhs;
+	BOOST_FOREACH(LinearConstraint::shared_ptr c, constraints) {
+		string key; Matrix A;
+		FOREACH_PAIR( key, A, c->As) {
+			if (blocks.find(key) == blocks.end())
+				blocks[key] = A;
+			else {
+				Matrix Aold = blocks[key];
+				if (A.size1() != Aold.size1() || A.size2() != Aold.size2())
+					throw invalid_argument("Dimension mismatch");
+				blocks[key] = A + Aold;
+			}
+		}
+
+		// assemble rhs
+		if (rhs.size() == 0)
+			rhs = c->get_b();
+		else
+			rhs = rhs + c->get_b();
+	}
+	LinearConstraint::shared_ptr result(new LinearConstraint(blocks, rhs));
+	return result;
+}
+
 }

cpp/LinearConstraint.h

@@ -1,14 +1,14 @@
-/*
+/**
- * LinearConstraint.h
+ * @file LinearConstraint.h
- *
+ * @brief Class that implements linear equality constraints
- *  Created on: Aug 10, 2009
+ * @author Alex Cunningham
- *      Author: alexgc
  */
 
 #ifndef EQUALITYFACTOR_H_
 #define EQUALITYFACTOR_H_
 
 #include <list>
+#include <set>
 #include "Matrix.h"
 #include "ConstrainedConditionalGaussian.h"
 
@@ -60,12 +60,14 @@ public:
 	 * @param matrices is the full map of A matrices
 	 * @param b is the RHS vector
 	 */
-	LinearConstraint(const std::map<std::string, Matrix>& matrices, const Vector& b);
+	LinearConstraint(const std::map<std::string, Matrix>& matrices,
+			const Vector& b);
 
 	/**
 	 * Default Destructor
 	 */
-	~LinearConstraint() {}
+	~LinearConstraint() {
+	}
 
 	/**
 	 * Eliminates the constraint
@@ -75,26 +77,29 @@ public:
 	 * @return a constrained conditional gaussian for the variable that is a
 	 * function of its parents
 	 */
-	ConstrainedConditionalGaussian::shared_ptr eliminate(const std::string& key);
+	ConstrainedConditionalGaussian::shared_ptr
+	eliminate(const std::string& key);
 
-    /**
+	/**
-     * print
+	 * print
-     * @param s optional string naming the factor
+	 * @param s optional string naming the factor
-     */
+	 */
-    void print(const std::string& s="") const;
+	void print(const std::string& s = "") const;
 
-    /**
+	/**
-     * equality up to tolerance
+	 * equality up to tolerance
-     */
+	 */
-    bool equals(const LinearConstraint& f, double tol=1e-9) const;
+	bool equals(const LinearConstraint& f, double tol = 1e-9) const;
 
-    /**
+	/**
-     * returns a version of the factor as a string
+	 * returns a version of the factor as a string
-     */
+	 */
-    std::string dump() const;
+	std::string dump() const;
 
 	/** get a copy of b */
-	const Vector& get_b() const {	return b;	}
+	const Vector& get_b() const {
+		return b;
+	}
 
 	/** check if the constraint is connected to a particular node */
 	bool involves(const std::string& key) const;
@@ -115,16 +120,27 @@ public:
 	 * @param key is a key to leave out of the final set
 	 * @return a list of the keys for nodes connected to the constraint
 	 */
-	std::list<std::string> keys(const std::string& key="") const;
+	std::list<std::string> keys(const std::string& key = "") const;
 
-    /**
+	/**
-     * @return the number of nodes the constraint connects
+	 * @return the number of nodes the constraint connects
-     */
+	 */
-    std::size_t size() const {return As.size();}
+	std::size_t size() const {
+		return As.size();
+	}
+
+	// friends
+	friend LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
 };
 
+/**
+ * Combines constraints into one constraint
+ */
+LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
+
 /** assert equals for testing - prints when not equal */
-bool assert_equal(const LinearConstraint& actual, const LinearConstraint& expected, double tol=1e-9);
+bool assert_equal(const LinearConstraint& actual,
+		const LinearConstraint& expected, double tol = 1e-9);
 
 }
 

cpp/smallExample.cpp

@@ -17,8 +17,7 @@ using namespace std;
 #include "Ordering.h"
 #include "Matrix.h"
 #include "NonlinearFactor.h"
-#include "LinearConstraint.h"
+#include "ConstrainedLinearFactorGraph.h"
-#include "ConstrainedConditionalGaussian.h"
 #include "smallExample.h"
 #include "Point2Prior.h"
 #include "Simulated2DOdometry.h"
@@ -61,51 +60,12 @@ boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph(
 	return nlfg;
 }
 
+/* ************************************************************************* */
 ExampleNonlinearFactorGraph createNonlinearFactorGraph() {
 	return *sharedNonlinearFactorGraph();
 }
 
 /* ************************************************************************* */
-//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph()
-//{
-//	ConstrainedLinearFactorGraph graph;
-//
-//	// add an equality factor
-//	Vector v1(2); v1(0)=1.;v1(1)=2.;
-//	LinearConstraint::shared_ptr f1(new LinearConstraint(v1, "x0"));
-//	graph.push_back_eq(f1);
-//
-//	// add a normal linear factor
-//	Matrix A21 = -1 * eye(2);
-//
-//	Matrix A22 = eye(2);
-//
-//	Vector b(2);
-//	b(0) = 2 ; b(1) = 3;
-//
-//	double sigma = 0.1;
-//	LinearFactor::shared_ptr f2(new LinearFactor("x0", A21/sigma,  "x1", A22/sigma, b/sigma));
-//	graph.push_back(f2);
-//	return graph;
-//}
-
-/* ************************************************************************* */
-//	ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
-//		ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> graph;
-//		FGConfig c = createConstrainedConfig();
-//
-//		// equality constraint for initial pose
-//		LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0"));
-//		graph.push_back_eq(f1);
-//
-//		// odometry between x0 and x1
-//		double sigma = 0.1;
-//		shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1"));
-//		graph.push_back(f2); // TODO
-//		return graph;
-//	}
-
-	/* ************************************************************************* */
 FGConfig createConfig()
 {
     Vector v_x1(2); v_x1(0) = 0.;  v_x1(1) = 0.;
@@ -131,52 +91,11 @@ boost::shared_ptr<const FGConfig> sharedNoisyConfig()
     return c;
 }
 
+/* ************************************************************************* */
 FGConfig createNoisyConfig() {
 	return *sharedNoisyConfig();
 }
 
-/* ************************************************************************* */
-//FGConfig createConstrainedConfig()
-//{
-//	FGConfig config;
-//
-//	Vector x0(2); x0(0)=1.0; x0(1)=2.0;
-//	config.insert("x0", x0);
-//
-//	Vector x1(2); x1(0)=3.0; x1(1)=5.0;
-//	config.insert("x1", x1);
-//
-//	return config;
-//}
-
-/* ************************************************************************* */
-//FGConfig createConstrainedLinConfig()
-//{
-//	FGConfig config;
-//
-//	Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
-//	config.insert("x0", x0);
-//
-//	Vector x1(2); x1(0)=2.3; x1(1)=5.3;
-//	config.insert("x1", x1);
-//
-//	return config;
-//}
-
-/* ************************************************************************* */
-//FGConfig createConstrainedCorrectDelta()
-//{
-//	FGConfig config;
-//
-//	Vector x0(2); x0(0)=0.; x0(1)=0.;
-//	config.insert("x0", x0);
-//
-//	Vector x1(2); x1(0)= 0.7; x1(1)= -0.3;
-//	config.insert("x1", x1);
-//
-//	return config;
-//}
-
 /* ************************************************************************* */
 FGConfig createCorrectDelta() {
   Vector v_x1(2); v_x1(0) = -0.1;  v_x1(1) = -0.1;
@@ -289,26 +208,6 @@ ChordalBayesNet createSmallChordalBayesNet()
   return cbn;
 }
 
-/* ************************************************************************* */
-//ConstrainedChordalBayesNet createConstrainedChordalBayesNet()
-//{
-//	ConstrainedChordalBayesNet cbn;
-//	FGConfig c = createConstrainedConfig();
-//
-//	// add regular conditional gaussian - no parent
-//	Matrix R = eye(2);
-//	Vector d = c["x1"];
-//	double sigma = 0.1;
-//	ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma));
-//	cbn.insert("x1", f1);
-//
-//	// add a delta function to the cbn
-//	ConstrainedConditionalGaussian::shared_ptr f2(new ConstrainedConditionalGaussian); //(c["x0"], "x0"));
-//	cbn.insert_df("x0", f2);
-//
-//	return cbn;
-//}
-
 /* ************************************************************************* */
 // Some nonlinear functions to optimize
 /* ************************************************************************* */
@@ -340,5 +239,175 @@ ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
 }
 
 /* ************************************************************************* */
+ConstrainedLinearFactorGraph createSingleConstraintGraph() {
+	// create unary factor
+	double sigma = 0.1;
+	Matrix Ax = eye(2) / sigma;
+	Vector b1(2);
+	b1(0) = 1.0;
+	b1(1) = -1.0;
+	LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1 / sigma));
+
+	// create binary constraint factor
+	Matrix Ax1(2, 2);
+	Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
+	Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
+	Matrix Ay1 = eye(2) * 10;
+	Vector b2 = Vector_(2, 1.0, 2.0);
+	LinearConstraint::shared_ptr f2(
+			new LinearConstraint("x", Ax1, "y", Ay1, b2));
+
+	// construct the graph
+	ConstrainedLinearFactorGraph fg;
+	fg.push_back(f1);
+	fg.push_back_constraint(f2);
+
+	return fg;
+}
+
+/* ************************************************************************* */
+ConstrainedLinearFactorGraph createMultiConstraintGraph() {
+	// unary factor 1
+	double sigma = 0.1;
+	Matrix A = eye(2) / sigma;
+	Vector b = Vector_(2, -2.0, 2.0)/sigma;
+	LinearFactor::shared_ptr lf1(new LinearFactor("x", A, b));
+
+	// constraint 1
+	Matrix A11(2,2);
+	A11(0,0) = 1.0 ; A11(0,1) = 2.0;
+	A11(1,0) = 2.0 ; A11(1,1) = 1.0;
+
+	Matrix A12(2,2);
+	A12(0,0) = 10.0 ; A12(0,1) = 0.0;
+	A12(1,0) = 0.0 ; A12(1,1) = 10.0;
+
+	Vector b1(2);
+	b1(0) = 1.0; b1(1) = 2.0;
+	LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
+
+	// constraint 2
+	Matrix A21(2,2);
+	A21(0,0) =  3.0 ; A21(0,1) =  4.0;
+	A21(1,0) = -1.0 ; A21(1,1) = -2.0;
+
+	Matrix A22(2,2);
+	A22(0,0) = 1.0 ; A22(0,1) = 1.0;
+	A22(1,0) = 1.0 ; A22(1,1) = 2.0;
+
+	Vector b2(2);
+	b2(0) = 3.0; b2(1) = 4.0;
+	LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
+
+	// construct the graph
+	ConstrainedLinearFactorGraph fg;
+	fg.push_back(lf1);
+	fg.push_back_constraint(lc1);
+	fg.push_back_constraint(lc2);
+
+	return fg;
+}
+
+/* ************************************************************************* */
+//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph()
+//{
+//	ConstrainedLinearFactorGraph graph;
+//
+//	// add an equality factor
+//	Vector v1(2); v1(0)=1.;v1(1)=2.;
+//	LinearConstraint::shared_ptr f1(new LinearConstraint(v1, "x0"));
+//	graph.push_back_eq(f1);
+//
+//	// add a normal linear factor
+//	Matrix A21 = -1 * eye(2);
+//
+//	Matrix A22 = eye(2);
+//
+//	Vector b(2);
+//	b(0) = 2 ; b(1) = 3;
+//
+//	double sigma = 0.1;
+//	LinearFactor::shared_ptr f2(new LinearFactor("x0", A21/sigma,  "x1", A22/sigma, b/sigma));
+//	graph.push_back(f2);
+//	return graph;
+//}
+
+/* ************************************************************************* */
+//	ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
+//		ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> graph;
+//		FGConfig c = createConstrainedConfig();
+//
+//		// equality constraint for initial pose
+//		LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0"));
+//		graph.push_back_eq(f1);
+//
+//		// odometry between x0 and x1
+//		double sigma = 0.1;
+//		shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1"));
+//		graph.push_back(f2); // TODO
+//		return graph;
+//	}
+
+/* ************************************************************************* */
+//FGConfig createConstrainedConfig()
+//{
+//	FGConfig config;
+//
+//	Vector x0(2); x0(0)=1.0; x0(1)=2.0;
+//	config.insert("x0", x0);
+//
+//	Vector x1(2); x1(0)=3.0; x1(1)=5.0;
+//	config.insert("x1", x1);
+//
+//	return config;
+//}
+
+/* ************************************************************************* */
+//FGConfig createConstrainedLinConfig()
+//{
+//	FGConfig config;
+//
+//	Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
+//	config.insert("x0", x0);
+//
+//	Vector x1(2); x1(0)=2.3; x1(1)=5.3;
+//	config.insert("x1", x1);
+//
+//	return config;
+//}
+
+/* ************************************************************************* */
+//FGConfig createConstrainedCorrectDelta()
+//{
+//	FGConfig config;
+//
+//	Vector x0(2); x0(0)=0.; x0(1)=0.;
+//	config.insert("x0", x0);
+//
+//	Vector x1(2); x1(0)= 0.7; x1(1)= -0.3;
+//	config.insert("x1", x1);
+//
+//	return config;
+//}
+
+/* ************************************************************************* */
+//ConstrainedChordalBayesNet createConstrainedChordalBayesNet()
+//{
+//	ConstrainedChordalBayesNet cbn;
+//	FGConfig c = createConstrainedConfig();
+//
+//	// add regular conditional gaussian - no parent
+//	Matrix R = eye(2);
+//	Vector d = c["x1"];
+//	double sigma = 0.1;
+//	ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma));
+//	cbn.insert("x1", f1);
+//
+//	// add a delta function to the cbn
+//	ConstrainedConditionalGaussian::shared_ptr f2(new ConstrainedConditionalGaussian); //(c["x0"], "x0"));
+//	cbn.insert_df("x0", f2);
+//
+//	return cbn;
+//}
 
 } // namespace gtsam

cpp/smallExample.h

@@ -12,7 +12,7 @@
 
 #include <boost/shared_ptr.hpp>
 //#include "ConstrainedNonlinearFactorGraph.h" // will be added back once design is solidified
-//#include "ConstrainedLinearFactorGraph.h"
+#include "ConstrainedLinearFactorGraph.h"
 #include "NonlinearFactorGraph.h"
 #include "ChordalBayesNet.h"
 #include "LinearFactorGraph.h"
@@ -30,29 +30,12 @@ namespace gtsam {
 	boost::shared_ptr<const ExampleNonlinearFactorGraph > sharedNonlinearFactorGraph();
 	ExampleNonlinearFactorGraph createNonlinearFactorGraph();
 
-	/**
-	 * Create small example constrained factor graph
-	 */
-	//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph();
-
-	/**
-	 * Create small example constrained nonlinear factor graph
-	 */
-//	ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig>
-//		createConstrainedNonlinearFactorGraph();
-
 	/**
 	 * Create configuration to go with it
 	 * The ground truth configuration for the example above
 	 */
 	FGConfig createConfig();
 
-	/**
-	 * Create configuration for constrained example
-	 * This is the ground truth version
-	 */
-	//FGConfig createConstrainedConfig();
-
 	/**
 	 * create a noisy configuration for a nonlinear factor graph
 	 */
@@ -86,6 +69,32 @@ namespace gtsam {
 	boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
 	ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
 
+	/* ******************************************************* */
+	// Constrained Examples
+	/* ******************************************************* */
+
+	/**
+	 * Creates a simple constrained graph with one linear factor and
+	 * one binary constraint.
+	 */
+	ConstrainedLinearFactorGraph createSingleConstraintGraph();
+
+	/**
+	 * Creates a constrained graph with a linear factor and two
+	 * binary constraints that share a node
+	 */
+	ConstrainedLinearFactorGraph createMultiConstraintGraph();
+
+	/**
+	 * These are the old examples from the EqualityFactor/DeltaFunction
+	 * They should be updated for use at some point, but are disabled for now
+	 */
+	/**
+	 * Create configuration for constrained example
+	 * This is the ground truth version
+	 */
+	//FGConfig createConstrainedConfig();
+
 	/**
 	 * Create a noisy configuration for linearization
 	 */
@@ -95,4 +104,15 @@ namespace gtsam {
 	 * Create the correct delta configuration
 	 */
 	//FGConfig createConstrainedCorrectDelta();
+
+	/**
+	 * Create small example constrained factor graph
+	 */
+	//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph();
+
+	/**
+	 * Create small example constrained nonlinear factor graph
+	 */
+//	ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig>
+//		createConstrainedNonlinearFactorGraph();
 }

cpp/testConstrainedLinearFactorGraph.cpp

@@ -15,27 +15,8 @@ using namespace std;
 /* ************************************************************************* */
 TEST( ConstrainedLinearFactorGraph, elimination1 )
 {
-	// create unary factor
+	// get the graph
-	double sigma = 0.1;
+	ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
-	Matrix Ax = eye(2) / sigma;
-	Vector b1(2);
-	b1(0) = 1.0;
-	b1(1) = -1.0;
-	LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1 / sigma));
-
-	// create binary constraint factor
-	Matrix Ax1(2, 2);
-	Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
-	Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
-	Matrix Ay1 = eye(2) * 10;
-	Vector b2 = Vector_(2, 1.0, 2.0);
-	LinearConstraint::shared_ptr f2(
-			new LinearConstraint("x", Ax1, "y", Ay1, b2));
-
-	// construct the graph
-	ConstrainedLinearFactorGraph fg;
-	fg.push_back(f1);
-	fg.push_back_constraint(f2);
 
 	// verify construction of the graph
 	CHECK(fg.size() == 2);
@@ -48,6 +29,11 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
 	//verify changes and output
 	CHECK(fg.size() == 1);
 	CHECK(cbn->size() == 1);
+	Matrix Ax1(2, 2);
+	Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
+	Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
+	Matrix Ay1 = eye(2) * 10;
+	Vector b2 = Vector_(2, 1.0, 2.0);
 	ConstrainedConditionalGaussian expectedCCG1(b2, Ax1, "y", Ay1);
 	CHECK(expectedCCG1.equals(*(cbn->get("x"))));
 	Matrix Ap(2,2);
@@ -74,27 +60,8 @@ TEST( ConstrainedLinearFactorGraph, elimination1 )
 /* ************************************************************************* */
 TEST( ConstrainedLinearFactorGraph, optimize )
 {
-	// create unary factor
+	// create graph
-	double sigma = 0.1;
+	ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
-	Matrix Ax = eye(2) / sigma;
-	Vector b1(2);
-	b1(0) = 1.0;
-	b1(1) = -1.0;
-	LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1 / sigma));
-
-	// create binary constraint factor
-	Matrix Ax1(2, 2);
-	Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
-	Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
-	Matrix Ay1 = eye(2) * 10;
-	Vector b2 = Vector_(2, 1.0, 2.0);
-	LinearConstraint::shared_ptr f2(
-			new LinearConstraint("x", Ax1, "y", Ay1, b2));
-
-	// construct the graph
-	ConstrainedLinearFactorGraph fg;
-	fg.push_back(f1);
-	fg.push_back_constraint(f2);
 
 	// perform optimization
 	Ordering ord;
@@ -111,6 +78,27 @@ TEST( ConstrainedLinearFactorGraph, optimize )
 	CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
 }
 
+/* ************************************************************************* */
+TEST( ConstrainedLinearFactorGraph, optimize2 )
+{
+	// create graph
+	ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
+
+	// perform optimization
+	Ordering ord;
+	ord.push_back("x");
+	ord.push_back("y");
+	FGConfig actual = fg.optimize(ord);
+
+	FGConfig expected;
+	expected.insert("x", Vector_(2, 1.0, -1.0));
+	expected.insert("y", Vector_(2, 0.2,  0.1));
+
+	CHECK(expected.size() == actual.size());
+	CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); // Fails here: gets x = (-3, 1)
+	CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
+}
+
 /* ************************************************************************* */
 TEST( ConstrainedLinearFactorGraph, is_constrained )
 {
@@ -133,6 +121,163 @@ TEST( ConstrainedLinearFactorGraph, is_constrained )
 	CHECK(!fg.is_constrained("w"));
 }
 
+/* ************************************************************************* */
+TEST( ConstrainedLinearFactorGraph, get_constraint_separator )
+{
+	ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
+	ConstrainedLinearFactorGraph fg2 = createMultiConstraintGraph();
+	LinearConstraint::shared_ptr lc1 = fg1.constraint_at(0);
+	LinearConstraint::shared_ptr lc2 = fg1.constraint_at(1);
+
+	vector<LinearConstraint::shared_ptr> actual1 = fg1.find_constraints_and_remove("y");
+	CHECK(fg1.size() == 2);
+	CHECK(actual1.size() == 1);
+	CHECK((*actual1.begin())->equals(*lc1));
+
+	vector<LinearConstraint::shared_ptr> actual2 = fg2.find_constraints_and_remove("x");
+	CHECK(fg2.size() == 1);
+	CHECK(actual2.size() == 2);
+	CHECK((*actual1.begin())->equals(*lc1));
+	LinearConstraint::shared_ptr act = *(++actual2.begin());
+	CHECK(act->equals(*lc2));
+}
+
+/* ************************************************************************* */
+TEST( ConstrainedLinearFactorGraph, update_constraints )
+{
+	// create a graph
+	ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
+
+	// process constraints - picking first constraint on x
+	vector<LinearConstraint::shared_ptr> constraints = fg1.find_constraints_and_remove("x");
+	CHECK(constraints.size() == 2);
+	CHECK(fg1.size() == 1); // both constraints removed
+	LinearConstraint::shared_ptr primary = constraints[0];
+	LinearConstraint::shared_ptr updatee = constraints[1];
+	fg1.update_constraints("x", constraints, primary);
+	CHECK(fg1.size() == 2); // induced constraint added back
+
+	// expected induced constraint
+	Matrix Ar(2,2);
+	Ar(0, 0) = -16.6666; Ar(0, 1) = -6.6666;
+	Ar(1, 0) = 10.0;     Ar(1, 1) = 0.0;
+	Matrix A22(2,2);
+	A22(0,0) = 1.0 ; A22(0,1) = 1.0;
+	A22(1,0) = 1.0 ; A22(1,1) = 2.0;
+	Vector br = Vector_(2, 0.0, 5.0);
+	LinearConstraint::shared_ptr exp(new LinearConstraint("y", Ar, "z", A22, br));
+
+	// evaluate
+	CHECK(assert_equal(*(fg1.constraint_at(0)), *exp, 1e-4));
+}
+
+/* ************************************************************************* */
+TEST( ConstrainedLinearFactorGraph, find_constraints_and_remove )
+{
+	// constraint 1
+	Matrix A11(2,2);
+	A11(0,0) = 1.0 ; A11(0,1) = 2.0;
+	A11(1,0) = 2.0 ; A11(1,1) = 1.0;
+
+	Matrix A12(2,2);
+	A12(0,0) = 10.0 ; A12(0,1) = 0.0;
+	A12(1,0) = 0.0 ; A12(1,1) = 10.0;
+
+	Vector b1(2);
+	b1(0) = 1.0; b1(1) = 2.0;
+	LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
+
+	// constraint 2
+	Matrix A21(2,2);
+	A21(0,0) =  3.0 ; A21(0,1) =  4.0;
+	A21(1,0) = -1.0 ; A21(1,1) = -2.0;
+
+	Matrix A22(2,2);
+	A22(0,0) = 1.0 ; A22(0,1) = 1.0;
+	A22(1,0) = 1.0 ; A22(1,1) = 2.0;
+
+	Vector b2(2);
+	b2(0) = 3.0; b2(1) = 4.0;
+	LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
+
+	// construct the graph
+	ConstrainedLinearFactorGraph fg1;
+	fg1.push_back_constraint(lc1);
+	fg1.push_back_constraint(lc2);
+
+	// constraints on x
+	vector<LinearConstraint::shared_ptr> expected1, actual1;
+	expected1.push_back(lc1);
+	expected1.push_back(lc2);
+	actual1 = fg1.find_constraints_and_remove("x");
+	CHECK(fg1.size() == 0);
+	CHECK(expected1.size() == actual1.size());
+	vector<LinearConstraint::shared_ptr>::const_iterator exp1, act1;
+	for(exp1=expected1.begin(), act1=actual1.begin(); act1 != actual1.end(); ++act1, ++exp1) {
+		CHECK((*exp1)->equals(**act1));
+	}
+}
+
+/* ************************************************************************* */
+TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
+{
+	ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
+
+	// eliminate the constraint
+	ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x");
+	CHECK(cg1->size() == 1);
+	CHECK(fg.size() == 2);
+
+	// eliminate the induced constraint
+	ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y");
+	CHECK(fg.size() == 1);
+	CHECK(cg2->size() == 1);
+
+	// eliminate the linear factor
+	ConditionalGaussian::shared_ptr cg3 = fg.eliminate_one("z");
+	CHECK(fg.size() == 0);
+	CHECK(cg3->size() == 0);
+
+	// solve piecewise
+	FGConfig actual;
+	Vector act_z = cg3->solve(actual);
+	actual.insert("z", act_z);
+	CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4));
+	Vector act_y = cg2->solve(actual);
+	actual.insert("y", act_y);
+	CHECK(assert_equal(act_y, Vector_(2, -0.1, 0.4), 1e-4));
+	Vector act_x = cg1->solve(actual);
+	CHECK(assert_equal(act_x, Vector_(2, -2.0, 2.0), 1e-4));
+
+}
+
+/* ************************************************************************* */
+TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
+{
+	ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
+	// solve the graph
+	Ordering ord;
+	ord.push_back("x");
+	ord.push_back("y");
+	ord.push_back("z");
+
+	FGConfig actual = fg.optimize(ord);
+
+	// verify
+	FGConfig expected;
+	expected.insert("x", Vector_(2, -2.0, 2.0));
+	expected.insert("y", Vector_(2, -0.1, 0.4));
+	expected.insert("z", Vector_(2, -4.0, 5.0));
+	CHECK(expected.size() == actual.size());
+	CHECK(assert_equal(expected["x"], actual["x"], 1e-4));
+	CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
+	CHECK(assert_equal(expected["z"], actual["z"], 1e-4));
+}
+
+/* ************************************************************************* */
+//  OLD TESTS - should be ported into the new structure when possible
+/* ************************************************************************* */
+
 /* ************************************************************************* */
 //TEST( ConstrainedLinearFactorGraph, basic )
 //{

cpp/testLinearConstraint.cpp

@@ -127,13 +127,16 @@ TEST ( LinearConstraint, equals )
 
 	Vector b(2);
 	b(0) = 2 ; b(1) = -1;
+	Vector b2 = Vector_(2, 0.0, 0.0);
 
 	LinearConstraint lc1("x1", A1,  "x2", A2, b);
 	LinearConstraint lc2("x1", A1,  "x2", A2, b);
+	LinearConstraint lc3("x1", A1,  "x2", A2, b2);
 
 	// check for basic equality
 	CHECK(lc1.equals(lc2));
 	CHECK(lc2.equals(lc1));
+	CHECK(!lc1.equals(lc3));
 }
 
 /* ************************************************************************* */
@@ -240,6 +243,57 @@ TEST ( LinearConstraint, keys )
 	CHECK(actual == expected);
 }
 
+/* ************************************************************************* */
+TEST ( LinearConstraint, combine )
+{
+	// constraint 1
+	Matrix A11(2,2);
+	A11(0,0) = 1.0 ; A11(0,1) = 2.0;
+	A11(1,0) = 2.0 ; A11(1,1) = 1.0;
+
+	Matrix A12(2,2);
+	A12(0,0) = 10.0 ; A12(0,1) = 0.0;
+	A12(1,0) = 0.0 ; A12(1,1) = 10.0;
+
+	Vector b1(2);
+	b1(0) = 1.0; b1(1) = 2.0;
+	LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
+
+	// constraint 2
+	Matrix A21(2,2);
+	A21(0,0) =  3.0 ; A21(0,1) =  4.0;
+	A21(1,0) = -1.0 ; A21(1,1) = -2.0;
+
+	Matrix A22(2,2);
+	A22(0,0) = 1.0 ; A22(0,1) = 1.0;
+	A22(1,0) = 1.0 ; A22(1,1) = 2.0;
+
+	Vector b2(2);
+	b2(0) = 3.0; b2(1) = 4.0;
+	LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
+
+	// combine
+	set<LinearConstraint::shared_ptr> constraints;
+	constraints.insert(lc1);
+	constraints.insert(lc2);
+	LinearConstraint::shared_ptr actual = combineConstraints(constraints);
+
+	// expected
+	Matrix A1 = A11 + A21;
+	Matrix A2 = A12;
+	Matrix A3 = A22;
+	Vector b = b1 + b2;
+	map<string, Matrix> blocks;
+	blocks.insert(make_pair("x", A1));
+	blocks.insert(make_pair("y", A2));
+	blocks.insert(make_pair("z", A3));
+	LinearConstraint expected(blocks, b);
+
+	// verify
+	CHECK(actual->equals(expected));
+
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */