Added to LinearFactor a matrix_augmented() function to get a single augmented matrix [A b]

Removed extra copying in LinearFactor::eliminate()

cpp/LinearFactor.cpp

@@ -183,6 +183,27 @@ pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
   return make_pair(A, t*b_);
 }
 
+/* ************************************************************************* */
+Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const {
+	// get pointers to the matrices
+	vector<const Matrix *> matrices;
+	BOOST_FOREACH(string j, ordering) {
+		const Matrix& Aj = get_A(j);
+		matrices.push_back(&Aj);
+	}
+
+	// load b into a matrix
+	Matrix B_mat(numberOfRows(), 1);
+	for (int i=0; i<b_.size(); ++i)
+		B_mat(i,0) = b_(i);
+	matrices.push_back(&B_mat);
+
+	// divide in sigma so error is indeed 0.5*|Ax-b|
+	Matrix t = diag(ediv(ones(sigmas_.size()),sigmas_));
+	Matrix A = t*collect(matrices);
+	return A;
+}
+
 /* ************************************************************************* */
 void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
 		const size_t pos) {
@@ -248,9 +269,9 @@ LinearFactor::eliminate(const string& key)
 		if (k != key) ordering += k;
 
 	// extract A, b from the combined linear factor (ensure that x is leading)
-	Matrix A; Vector b;
-	boost::tie(A, b) = matrix(ordering);
-	size_t m = A.size1(); size_t n = A.size2();
+	// use an augmented system [A b] to prevent copying
+	Matrix Rd = matrix_augmented(ordering);
+	size_t m = Rd.size1(); size_t n = Rd.size2()-1;
 
 	// get dimensions of the eliminated variable
 	size_t n1 = getDim(key);
@@ -260,17 +281,6 @@ LinearFactor::eliminate(const string& key)
 	if (maxRank<n1)
 		throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
 
-	// QR performed using an augmented matrix Rd =[A b]
-	// TODO: We should get rid of this copy
-	Matrix Rd(m, n+1);
-	// copy in A
-	for (int i=0; i<m; ++i)
-		for (int j=0; j<n; ++j)
-			Rd(i,j) = A(i,j);
-	// copy in b
-	for (int i=0; i<m; ++i)
-		Rd(i,n) = b(i);
-
 	// Do in-place QR to get R, d of the augmented system
 	if (verbose) ::print(Rd,"Rd before");
 	householder(Rd, maxRank);

cpp/LinearFactor.h

@@ -189,6 +189,13 @@ public:
 	 */
 	std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
 
+	/**
+	 * Return (dense) matrix associated with factor
+	 * The returned system is an augmented matrix: [A b]
+	 * @param ordering of variables needed for matrix column order
+	 */
+	Matrix matrix_augmented(const Ordering& ordering) const;
+
 	/* ************************************************************************* */
 	// MUTABLE functions. FD:on the path to being eradicated
 	/* ************************************************************************* */

cpp/Matrix.h

@@ -172,9 +172,17 @@ void householder(Matrix& A, size_t k);
 Vector backsubstitution(const Matrix& R, const Vector& b);
 
 /**
- * create a matrix by stacking other matrices 
+ * create a matrix by stacking other matrices
+ * Given a set of matrices: A1, A2, A3...
+ * @return combined matrix [A1; A2; A3]
  */
 Matrix stack(size_t nrMatrices, ...);
+
+/**
+ * create a matrix by concatenating
+ * Given a set of matrices: A1, A2, A3...
+ * @return combined matrix [A1 A2 A3]
+ */
 Matrix collect(std::vector<const Matrix *> matrices);
 Matrix collect(size_t nrMatrices, ...);
 

cpp/testLinearFactor.cpp

@@ -497,9 +497,9 @@ TEST( LinearFactor, matrix )
 	// get the factor "f2" from the factor graph
 	LinearFactor::shared_ptr lf = fg[1];
 
-  // render with a given ordering
-  Ordering ord;
-  ord += "x1","x2";
+	// render with a given ordering
+	Ordering ord;
+	ord += "x1","x2";
 
 	Matrix A; Vector b;
 	boost::tie(A,b) = lf->matrix(ord);
@@ -513,6 +513,29 @@ TEST( LinearFactor, matrix )
 	EQUALITY(b,b1);
 }
 
+/* ************************************************************************* */
+TEST( LinearFactor, matrix_aug )
+{
+	// create a small linear factor graph
+	LinearFactorGraph fg = createLinearFactorGraph();
+
+	// get the factor "f2" from the factor graph
+	LinearFactor::shared_ptr lf = fg[1];
+
+	// render with a given ordering
+	Ordering ord;
+	ord += "x1","x2";
+
+	Matrix Ab;
+	Ab = lf->matrix_augmented(ord);
+
+	Matrix Ab1 = Matrix_(2,5,
+			-10.0,  0.0, 10.0,  0.0, 2.0,
+			000.0,-10.0,  0.0, 10.0, -1.0 );
+
+	EQUALITY(Ab,Ab1);
+}
+
 /* ************************************************************************* */
 TEST( LinearFactor, size )
 {