multiplyInPlace shaves a few seconds off but is fairly dangerous - I feel ambivalent....

cpp/BayesNetPreconditioner.cpp

@@ -46,6 +46,12 @@ namespace gtsam {
 		return Ab_ * x(y);
 	}
 
+	/* ************************************************************************* */
+	// In-place version that overwrites e
+	void BayesNetPreconditioner::multiplyInPlace(const VectorConfig& y, Errors& e) const {
+		Ab_.multiplyInPlace(x(y),e); // TODO Avoid temporary x ?
+	}
+
 	/* ************************************************************************* */
 	// Apply operator inv(R')*A'*e
 	VectorConfig BayesNetPreconditioner::operator^(const Errors& e) const {

cpp/BayesNetPreconditioner.h

@@ -53,6 +53,9 @@ namespace gtsam {
 		/** Apply operator A */
 		Errors operator*(const VectorConfig& y) const;
 
+		/** In-place version that overwrites e */
+		void multiplyInPlace(const VectorConfig& y, Errors& e) const;
+
 		/** Apply operator A' */
 		VectorConfig operator^(const Errors& e) const;
 	};

cpp/GaussianFactorGraph.cpp

@@ -61,6 +61,21 @@ Errors GaussianFactorGraph::operator*(const VectorConfig& x) const {
 	return e;
 }
 
+/* ************************************************************************* */
+void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x, Errors& e) const {
+	multiplyInPlace(x,e.begin());
+}
+
+/* ************************************************************************* */
+void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
+		const Errors::iterator& e) const {
+	Errors::iterator ei = e;
+	BOOST_FOREACH(sharedFactor Ai,factors_) {
+		*ei = (*Ai)*x;
+		ei++;
+	}
+}
+
 /* ************************************************************************* */
 VectorConfig GaussianFactorGraph::operator^(const Errors& e) const {
 	VectorConfig x;

cpp/GaussianFactorGraph.h

@@ -85,6 +85,12 @@ namespace gtsam {
 		/** return A*x */
 		Errors operator*(const VectorConfig& x) const;
 
+		/* In-place version e <- A*x that overwrites e. */
+		void multiplyInPlace(const VectorConfig& x, Errors& e) const;
+
+		/* In-place version e <- A*x that takes an iterator. */
+		void multiplyInPlace(const VectorConfig& x, const Errors::iterator& e) const;
+
 		/** return A^x */
 		VectorConfig operator^(const Errors& e) const;
 

cpp/SubgraphPreconditioner.cpp

@@ -70,6 +70,24 @@ namespace gtsam {
 		return e;
 	}
 
+	/* ************************************************************************* */
+	// In-place version that overwrites e
+	void SubgraphPreconditioner::multiplyInPlace(const VectorConfig& y, Errors& e) const {
+
+		Errors::iterator ei = e.begin();
+
+		// Use BayesNet order to add y contributions in order
+		BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
+			const Symbol& j = cg->key();
+			*ei = y[j]; // append y
+			ei++;
+		}
+
+		// Add A2 contribution
+		VectorConfig x = gtsam::backSubstitute(*Rc1_, y); // x=inv(R1)*y
+		Ab2_->multiplyInPlace(x,ei); // use iterator version
+	}
+
 	/* ************************************************************************* */
 	// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
 	VectorConfig SubgraphPreconditioner::operator^(const Errors& e) const {

cpp/SubgraphPreconditioner.h

@@ -64,7 +64,10 @@ namespace gtsam {
 		/** Apply operator A */
 		Errors operator*(const VectorConfig& y) const;
 
-		/** Apply operator A' */
+		/** Apply operator A in place: needs e allocated already */
+		void multiplyInPlace(const VectorConfig& y, Errors& e) const;
+
+			/** Apply operator A' */
 		VectorConfig operator^(const Errors& e) const;
 
 		/** print the object */

cpp/iterative-inl.h

@@ -29,19 +29,21 @@ namespace gtsam {
 		// i.e., first step is in direction of negative gradient
 		V g = Ab.gradient(x);
 		V d = g; // instead of negating gradient, alpha will be negated
-		double dotg0 = dot(g, g), prev_dotg = dotg0;
+		double gamma0 = dot(g, g), gamma_old = gamma0;
-		if (dotg0 < epsilon_abs) return x;
+		if (gamma0 < epsilon_abs) return x;
-		double threshold = epsilon * epsilon * dotg0;
+		double threshold = epsilon * epsilon * gamma0;
 
 		if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
-				<< maxIterations << ", ||g0||^2 = " << dotg0 << ", threshold = "
+				<< maxIterations << ", ||g0||^2 = " << gamma0 << ", threshold = "
 				<< threshold << endl;
 
+		// Allocate and calculate A*d for first iteration
+		E Ad = Ab * d;
+
 		// loop maxIterations times
 		for (size_t k = 1;; k++) {
 
 			// calculate optimal step-size
-			E Ad = Ab * d;
 			double alpha = - dot(d, g) / dot(Ad, Ad);
 
 			// do step in new search direction
@@ -55,21 +57,24 @@ namespace gtsam {
 				axpy(alpha, Ab ^ Ad, g);  // g += alpha*(Ab^Ad)
 
 			// check for convergence
-			double dotg = dot(g, g);
+			double gamma = dot(g, g);
 			if (verbose) cout << "iteration " << k << ": alpha = " << alpha
-					<< ", dotg = " << dotg << endl;
+					<< ", dotg = " << gamma << endl;
-			if (dotg < threshold) break;
+			if (gamma < threshold) break;
 
 			// calculate new search direction
 			if (steepest)
 				d = g;
 			else {
-				double beta = dotg / prev_dotg;
+				double beta = gamma / gamma_old;
-				prev_dotg = dotg;
+				gamma_old = gamma;
 				// d = g + d*beta;
 				scal(beta,d);
 				axpy(1.0, g, d);
 			}
+
+			// In-place recalculation Ad <- A*d to avoid re-allocating Ad
+			Ab.multiplyInPlace(d,Ad);
 		}
 		return x;
 	}

cpp/iterative.h

@@ -51,6 +51,11 @@ namespace gtsam {
 			return A_ * x;
 		}
 
+		/** Apply operator A_ in place*/
+		inline void multiplyInPlace(const Vector& x, Vector& e) const {
+			e = A_ * x;
+		}
+
 		/** Apply operator A_^T */
 		inline Vector operator^(const Vector& e) const {
 			return A_ ^ e;