diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp
index 070fa0f50..def369a13 100644
--- a/cpp/LinearFactor.cpp
+++ b/cpp/LinearFactor.cpp
@@ -165,7 +165,7 @@ void LinearFactor::tally_separator(const string& key, set<string>& separator) co
 }
 
 /* ************************************************************************* */  
-pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
+pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering, bool weight) const {
 
 	// get pointers to the matrices
 	vector<const Matrix *> matrices;
@@ -174,12 +174,17 @@ pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
 		matrices.push_back(&Aj);
 	}
 
-	// divide in sigma so error is indeed 0.5*|Ax-b|
-	Vector t = ediv(ones(sigmas_.size()),sigmas_);
-	Matrix A = vector_scale(collect(matrices), t);
+	// assemble
+	Matrix A = collect(matrices);
 	Vector b(b_);
-	for (int i=0; i<b_.size(); ++i)
-		b(i) *= t(i);
+
+	// divide in sigma so error is indeed 0.5*|Ax-b|
+	if (weight) {
+		Vector t = ediv(ones(sigmas_.size()),sigmas_);
+		A = vector_scale(A, t);
+		for (int i=0; i<b_.size(); ++i)
+			b(i) *= t(i);
+	}
 	return make_pair(A, b);
 }
 
@@ -303,38 +308,41 @@ LinearFactor::eliminate(const string& key)
 		if (k != key) ordering += k;
 
 	// extract A, b from the combined linear factor (ensure that x is leading)
-	// use an augmented system [A b] to prevent copying
-	Matrix Ab = matrix_augmented(ordering);
-	size_t m = Ab.size1(); size_t n = Ab.size2()-1;
+	Matrix A; Vector b;
+	boost::tie(A, b) = matrix(ordering, false);
+	size_t n = A.size2();
+
+	// Do in-place QR to get R, d of the augmented system
+	if (verbose) ::print(A,"A before");
+	if (verbose) ::print(b,"b before");
+	std::list<boost::tuple<Vector, double, double> > solution =
+							weighted_eliminate(A, b, sigmas_);
 
 	// get dimensions of the eliminated variable
 	size_t n1 = getDim(key);
 
 	// if m<n1, this factor cannot be eliminated
-	size_t maxRank = min(m,n);
+	size_t maxRank = solution.size();
 	if (maxRank<n1)
 		throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
 
-	// Do in-place QR to get R, d of the augmented system
-	if (verbose) ::print(Ab,"Rd before");
-	Matrix Rd; Vector sigmas;
-	boost::tie(Rd, sigmas) = weighted_eliminate(Ab, sigmas_);
-	if (verbose) ::print(Rd,"Rd after");
-
-	//NOTE: this actually normalizes the entire R matrix automatically,
-	//      and produces the precisions
-	//      This could probably be made more efficient, but is accurate
-
-	// extract RHS
-	Vector d(maxRank);
-	for (int i=0; i<maxRank; ++i)
-		d(i) = Rd(i,n);
+	// unpack the solutions
+	Matrix R(maxRank, n);
+	Vector r, d(maxRank), newSigmas(maxRank); double di, sigma;
+	Matrix::iterator2 Rit = R.begin2();
+	size_t i = 0;
+	BOOST_FOREACH(boost::tie(r, di, sigma), solution) {
+		copy(r.begin(), r.end(), Rit); // copy r vector
+		d(i) = di;                     // copy in rhs
+		newSigmas(i) = sigma;          // copy in new sigmas
+		Rit += n; i += 1;
+	}
 
 	// create base conditional Gaussian
 	ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key,
-			sub(d, 0, n1),         // form d vector
-			sub(Rd, 0, n1, 0, n1), // form R matrix
-			sub(sigmas, 0, n1)));  // get standard deviations
+			sub(d, 0, n1),            // form d vector
+			sub(R, 0, n1, 0, n1),     // form R matrix
+			sub(newSigmas, 0, n1)));  // get standard deviations
 
 	// extract the block matrices for parents in both CG and LF
 	LinearFactor::shared_ptr lf(new LinearFactor);
@@ -342,13 +350,13 @@ LinearFactor::eliminate(const string& key)
 	BOOST_FOREACH(string cur_key, ordering)
 		if (cur_key!=key) {
 			size_t dim = getDim(cur_key);
-			cg->add(cur_key, sub(Rd, 0, n1, j, j+dim));
-			lf->insert(cur_key, sub(Rd, n1, maxRank, j, j+dim));
+			cg->add(cur_key,    sub(R, 0, n1, j, j+dim));
+			lf->insert(cur_key, sub(R, n1, maxRank, j, j+dim));
 			j+=dim;
 		}
 
 	// Set sigmas
-	lf->sigmas_ = sub(sigmas,n1,maxRank);
+	lf->sigmas_ = sub(newSigmas,n1,maxRank);
 
 	// extract ds vector for the new b
 	lf->set_b(sub(d, n1, maxRank));
diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h
index 3837b4a15..18f932612 100644
--- a/cpp/LinearFactor.h
+++ b/cpp/LinearFactor.h
@@ -185,10 +185,10 @@ public:
 
 	/**
 	 * Return (dense) matrix associated with factor
-	 * NOTE: in this case, the standard deviations are baked into A and b
 	 * @param ordering of variables needed for matrix column order
+	 * @param set weight to true to bake in the weights
 	 */
-	std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
+	std::pair<Matrix, Vector> matrix(const Ordering& ordering, bool weight = true) const;
 
 	/**
 	 * Return (dense) matrix associated with factor
diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index ae7c0de06..8730df85a 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -7,6 +7,7 @@
 #include <stdarg.h>
 #include <string.h>
 #include <iomanip>
+#include <list>
 
 #include <boost/foreach.hpp>
 #include <boost/numeric/ublas/lu.hpp>
@@ -160,6 +161,18 @@ Vector column(const Matrix& A, size_t j) {
 	return a;
 }
 
+/* ************************************************************************* */
+Vector row(const Matrix& A, size_t i) {
+	if (i>=A.size1())
+		throw invalid_argument("Row index out of bounds!");
+
+	size_t n = A.size2();
+	Vector a(n);
+	for (int j=0; j<n; ++j)
+		a(j) = A(i,j);
+	return a;
+}
+
 /* ************************************************************************* */
 void print(const Matrix& A, const string &s) {
   size_t m = A.size1(), n = A.size2();
@@ -285,18 +298,16 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
 }
 
 /* ************************************************************************* */
-std::pair<Matrix, Vector> weighted_eliminate(Matrix& A, const Vector& sigmas) {
+list<boost::tuple<Vector, double, double> >
+weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
 	bool verbose = false;
 	// get sizes
 	size_t m = A.size1();
 	size_t n = A.size2();
 	size_t maxRank = min(m,n);
 
-	// create an R matrix to store the solution
-	Matrix R = eye(maxRank, n);
-
-	// create a sigma vector
-	Vector newSigmas(maxRank);
+	// create list
+	list<boost::tuple<Vector, double, double> > results;
 
 	// loop over the columns
 	for (int j=0; j<maxRank; ++j) {
@@ -308,26 +319,40 @@ std::pair<Matrix, Vector> weighted_eliminate(Matrix& A, const Vector& sigmas) {
 		Vector pseudo; double precision;
 		if (verbose) print(sigmas, "sigmas");
 		boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas);
+
+		// if precision is zero, rank(A) was less than maxRank, so return
+		if (precision < 1e-8) {
+			maxRank = j;
+			return results;
+		}
 		if (verbose) print(pseudo, "pseudo");
 
-		// create solution and copy into R
-		for (int j2=j; j2<n; ++j2) {
-			R(j,j2) = inner_prod(pseudo, column(A, j2));
+		// create solution and copy into r
+		Vector r = basis(n, j);
+		for (int j2=j+1; j2<n; ++j2)
+			r(j2) = inner_prod(pseudo, column(A, j2));
+
+		// create the rhs
+		double d = inner_prod(pseudo, b);
+		if (verbose) print(r, "updatedR");
+		if (verbose) cout << "d = " << d << endl;
+
+		// construct solution (r, d, sigma)
+		results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
+
+		// update A, b
+		for (int i=0;i<m;++i) { // update all rows
+			double ai = a(i);
+			b(i) -= ai*d;
+			for (int j2=j+1;j2<n;++j2) // limit to only columns in separator
+				A(i,j2) -= ai*r(j2);
 		}
-		if (verbose) print(R, "updatedR");
-
-		// update A
-		for (int i=0;i<m;++i) // update all rows
-			for (int j2=j+1;j2<n;++j2) { // limit to only columns in separator
-				A(i,j2) -= R(j,j2)*a(i);
-			}
 		if (verbose) print(A, "updatedA");
+		//if (verbose) print(sub(A, 0,m, j+1,n), "updatedA"); // bad index
 
-		// save precision information
-		newSigmas[j] = sqrt(1./precision);
 	}
 
-	return make_pair(R, newSigmas);
+	return results;
 }
 
 /* ************************************************************************* */
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index f55217c4e..8a882b56f 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -10,7 +10,9 @@
 
 #pragma once
 
+#include <list>
 #include <boost/numeric/ublas/matrix.hpp>
+#include <boost/tuple/tuple.hpp>
 #include "Vector.h"
 
 /**
@@ -118,6 +120,14 @@ Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);
  */
 Vector column(const Matrix& A, size_t j);
 
+/**
+ * extracts a row from a matrix
+ * @param matrix to extract row from
+ * @param index of the row
+ * @return the row in vector form
+ */
+Vector row(const Matrix& A, size_t j);
+
 // left multiply with scalar
 //inline Matrix operator*(double s, const Matrix& A) { return A*s;}
 
@@ -148,11 +158,13 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm);
 /**
  * Imperative algorithm for in-place full elimination with
  * weights and constraint handling
- * @param Ab is an augmented system to eliminate
+ * @param A is a matrix to eliminate
+ * @param b is the rhs
  * @param sigmas is a vector of the measurement standard deviation
- * @return a pair of R (normalized) and new sigmas
+ * @return list of r vectors, d  and sigma
  */
-std::pair<Matrix, Vector> weighted_eliminate(Matrix& Ab, const Vector& sigmas);
+std::list<boost::tuple<Vector, double, double> >
+weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
 
 /**
  * Householder tranformation, Householder vectors below diagonal
diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp
index 2c9418515..39396e1fc 100644
--- a/cpp/Vector.cpp
+++ b/cpp/Vector.cpp
@@ -93,6 +93,13 @@ namespace gtsam {
     return v;
   }
 
+  /* ************************************************************************* */
+  Vector delta(size_t n, size_t i, double value) {
+	  Vector v = zero(n);
+	  v(i) = value;
+	  return v;
+  }
+
   /* ************************************************************************* */
   void print(const Vector& v, const string& s) {
     size_t n = v.size();
@@ -182,45 +189,32 @@ namespace gtsam {
   }
   
   /* ************************************************************************* */
-  pair<Vector, double> weightedPseudoinverse(const Vector& v, const Vector& sigmas) {
-	  if (v.size() != sigmas.size())
+  pair<Vector, double> weightedPseudoinverse(const Vector& a, const Vector& sigmas) {
+	  size_t m = sigmas.size();
+	  if (a.size() != m)
 		  throw invalid_argument("V and precisions have different sizes!");
 
 	  // detect constraints and sanity-check
-	  int constraint_index = -1;
-	  for(int i=0; i<sigmas.size(); ++i) {
-		  if (sigmas[i] < 1e-9 && v[i] > 1e-9) {
-			  if (constraint_index != -1)
-				  throw invalid_argument("Multiple constraints on a single node!");
-			  else
-				  constraint_index = i;
-		  }
-	  }
+	  for(int i=0; i<m; ++i)
+		  if (sigmas[i] < 1e-9 && fabs(a[i]) > 1e-9)
+			  return make_pair(delta(m,i,1/a[i]), 1.0/0.0);
 
-	  // compute pseudoinverse
-	  if (constraint_index != -1) {
-		  // constrained case
-		  Vector sol = zero(sigmas.size());
-		  sol(constraint_index) = 1.0;
-		  return make_pair(sol, 1.0/0.0);
-	  } else {
-		  // normal case
-		  double normV = 0.;
-		  Vector precisions(sigmas.size());
-		  for(int i = 0; i<v.size(); i++) {
-			  if (sigmas[i] < 1e-5) {
-				  precisions[i] = 1./0.;
-			  } else {
-				  precisions[i] = 1./(sigmas[i]*sigmas[i]);
-				  normV += v[i]*v[i]*precisions[i];
-			  }
+	  // normal case
+	  double normV = 0.;
+	  Vector precisions(m);
+	  for(int i = 0; i<a.size(); i++) {
+		  if (sigmas[i] < 1e-5) {
+			  precisions[i] = 1./0.;
+		  } else {
+			  precisions[i] = 1./(sigmas[i]*sigmas[i]);
+			  normV += a[i]*a[i]*precisions[i];
 		  }
-		  Vector sol = zero(v.size());
-		  for(int i = 0; i<v.size(); i++)
-			  if (sigmas[i] > 1e-5)
-				  sol[i] = precisions[i]*v[i];
-		  return make_pair(sol/normV, normV);
 	  }
+	  Vector sol = zero(a.size());
+	  for(int i = 0; i<a.size(); i++)
+		  if (sigmas[i] > 1e-5)
+			  sol[i] = precisions[i]*a[i];
+	  return make_pair(sol/normV, normV);
   }
 
   /* ************************************************************************* */
diff --git a/cpp/Vector.h b/cpp/Vector.h
index be0f9a142..174f637db 100644
--- a/cpp/Vector.h
+++ b/cpp/Vector.h
@@ -41,6 +41,25 @@ Vector Vector_(size_t m, ...);
  */
 Vector repeat(size_t n, double value);
 
+/**
+ * Create basis vector of dimension n,
+ * with a constant in spot i
+ * @param n is the size of the vector
+ * @param index of the one
+ * @param value is the value to insert into the vector
+ * @return delta vector
+ */
+Vector delta(size_t n, size_t i, double value);
+
+/**
+ * Create basis vector of dimension n,
+ * with one in spot i
+ * @param n is the size of the vector
+ * @param index of the one
+ * @return basis vector
+ */
+inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }
+
 /**
  * Create zero vector
  * @param size
diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp
index f5ae30748..a86272535 100644
--- a/cpp/testLinearFactorGraph.cpp
+++ b/cpp/testLinearFactorGraph.cpp
@@ -606,37 +606,53 @@ TEST( LinearFactorGraph, constrained_simple )
 }
 
 // These tests require multiple constraints on a single node and will fail
-///* ************************************************************************* */
-//TEST( LinearFactorGraph, constrained_single )
-//{
-//	// get a graph with a constraint in it
-//	LinearFactorGraph fg = createSingleConstraintGraph();
-//
-//	// eliminate and solve
-//	Ordering ord;
-//	ord += "x", "y";
-//	VectorConfig actual = fg.optimize(ord);
-//
-//	// verify
-//	VectorConfig expected = createSingleConstraintConfig();
-//	CHECK(assert_equal(actual, expected));
-//}
-//
-///* ************************************************************************* */
-//TEST( LinearFactorGraph, constrained_multi )
-//{
-//	// get a graph with a constraint in it
-//	LinearFactorGraph fg = createMultiConstraintGraph();
-//
-//	// eliminate and solve
-//	Ordering ord;
-//	ord += "x", "y", "z";
-//	VectorConfig actual = fg.optimize(ord);
-//
-//	// verify
-//	VectorConfig expected = createMultiConstraintConfig();
-//	CHECK(assert_equal(actual, expected));
-//}
+/* ************************************************************************* */
+TEST( LinearFactorGraph, constrained_single )
+{
+	// get a graph with a constraint in it
+	LinearFactorGraph fg = createSingleConstraintGraph();
+
+	// eliminate and solve
+	Ordering ord;
+	ord += "x", "y";
+	VectorConfig actual = fg.optimize(ord);
+
+	// verify
+	VectorConfig expected = createSingleConstraintConfig();
+	CHECK(assert_equal(actual, expected));
+}
+
+/* ************************************************************************* */
+TEST( LinearFactorGraph, constrained_single2 )
+{
+	// get a graph with a constraint in it
+	LinearFactorGraph fg = createSingleConstraintGraph();
+
+	// eliminate and solve
+	Ordering ord;
+	ord += "y", "x";
+	VectorConfig actual = fg.optimize(ord);
+
+	// verify
+	VectorConfig expected = createSingleConstraintConfig();
+	CHECK(assert_equal(actual, expected));
+}
+
+/* ************************************************************************* */
+TEST( LinearFactorGraph, constrained_multi )
+{
+	// get a graph with a constraint in it
+	LinearFactorGraph fg = createMultiConstraintGraph();
+
+	// eliminate and solve
+	Ordering ord;
+	ord += "x", "y", "z";
+	VectorConfig actual = fg.optimize(ord);
+
+	// verify
+	VectorConfig expected = createMultiConstraintConfig();
+	CHECK(assert_equal(actual, expected));
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index 35bfda605..96677201b 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -8,6 +8,7 @@
 #include <iostream>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/tuple/tuple.hpp>
+#include <boost/foreach.hpp>
 #include "Matrix.h"
 
 using namespace std;
@@ -123,6 +124,27 @@ TEST( matrix, column )
 	CHECK(assert_equal(a3, exp3));
 }
 
+/* ************************************************************************* */
+TEST( matrix, row )
+{
+	Matrix A = Matrix_(4, 7,
+	   -1.,  0.,  1.,  0.,  0.,  0., -0.2,
+		0., -1.,  0.,  1.,  0.,  0.,  0.3,
+		1.,  0.,  0.,  0., -1.,  0.,  0.2,
+		0.,  1.,  0.,  0.,  0., -1., -0.1);
+	Vector a1 = row(A, 0);
+	Vector exp1 = Vector_(7, -1.,  0.,  1.,  0.,  0.,  0., -0.2);
+	CHECK(assert_equal(a1, exp1));
+
+	Vector a2 = row(A, 2);
+	Vector exp2 = Vector_(7, 1.,  0.,  0.,  0., -1.,  0.,  0.2);
+	CHECK(assert_equal(a2, exp2));
+
+	Vector a3 = row(A, 3);
+	Vector exp3 = Vector_(7, 0.,  1.,  0.,  0.,  0., -1., -0.1);
+	CHECK(assert_equal(a3, exp3));
+}
+
 /* ************************************************************************* */
 TEST( matrix, zeros )
 {
@@ -532,27 +554,44 @@ TEST( matrix, svd )
 /* ************************************************************************* */
 TEST( matrix, weighted_elimination )
 {
-	// create a matrix to eliminate - assume augmented
-	Matrix A = Matrix_(4, 7,
-		   -1.,  0.,  1.,  0.,  0.,  0., -0.2,
-		    0., -1.,  0.,  1.,  0.,  0.,  0.3,
-	        1.,  0.,  0.,  0., -1.,  0.,  0.2,
-	        0.,  1.,  0.,  0.,  0., -1., -0.1);
+	// create a matrix to eliminate
+	Matrix A = Matrix_(4, 6,
+		   -1.,  0.,  1.,  0.,  0.,  0.,
+		    0., -1.,  0.,  1.,  0.,  0.,
+	        1.,  0.,  0.,  0., -1.,  0.,
+	        0.,  1.,  0.,  0.,  0., -1.);
+	Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1);
 	Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
 
 	// perform elimination
-	Matrix actual; Vector newSigmas;
-	boost::tie(actual, newSigmas) = weighted_eliminate(A, sigmas);
+	std::list<boost::tuple<Vector, double, double> > solution =
+								weighted_eliminate(A, b, sigmas);
 
-	// verify
-	Matrix expected = Matrix_(4, 7,
-		    1.,  0.,-0.2,  0.,-0.8,  0.,  0.2,
-			0.,  1.,  0.,-0.2,  0.,-0.8,-0.14,
-			0.,  0.,  1.,  0., -1.,  0.,  0.0,
-			0.,  0.,  0.,  1.,  0., -1.,  0.2);
-	CHECK(assert_equal(actual,expected));
+	// 	expected values
+	Matrix expectedR = Matrix_(4, 6,
+			1.,  0.,-0.2,  0.,-0.8,  0.,
+			0.,  1.,  0.,-0.2,  0.,-0.8,
+			0.,  0.,  1.,  0., -1.,  0.,
+			0.,  0.,  0.,  1.,  0., -1.);
+	Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2);
+	Vector newSigmas  = Vector_(4,
+			0.0894427,
+			0.0894427,
+			0.223607,
+			0.223607);
+
+	// unpack and verify
+	Vector r; double di, sigma;
+	size_t i = 0;
+	BOOST_FOREACH(boost::tie(r, di, sigma), solution) {
+		CHECK(assert_equal(r, row(expectedR, i))); // verify r
+		DOUBLES_EQUAL(d(i), di, 1e-8);             // verify d
+		DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5);  // verify sigma
+		i += 1;
+	}
 }
 
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */