GaussianFactor::transposeMultiplyAdd

cpp/GaussianFactor.cpp

@@ -210,6 +210,18 @@ VectorConfig GaussianFactor::operator^(const Vector& e) const {
 	return x;
 }
 
+/* ************************************************************************* */
+void GaussianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
+		VectorConfig& x) const {
+	Vector E = alpha * model_->whiten(e);
+	// Just iterate over all A matrices and insert Ai^e into VectorConfig
+	FOREACH_PAIR(j, Aj, As_)
+	{
+		Vector& Xj = x.getReference(*j);
+		gtsam::transposeMultiplyAdd(*Aj, E, Xj);
+	}
+}
+
 /* ************************************************************************* */  
 pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight) const {
 

cpp/GaussianFactor.h

@@ -197,16 +197,15 @@ public:
 	void tally_separator(const Symbol& key,
 			std::set<Symbol>& separator) const;
 
-	/**
+	/** Return A*x */
-	 * Return A*x
-	 */
 	Vector operator*(const VectorConfig& x) const;
 
-	/**
+	/** Return A'*e */
-	 * Return A^x
-	 */
 	VectorConfig operator^(const Vector& e) const;
 
+	/** x += A'*e */
+	void transposeMultiplyAdd(double alpha, const Vector& e, VectorConfig& x) const;
+
 	/**
 	 * Return (dense) matrix associated with factor
 	 * @param ordering of variables needed for matrix column order

cpp/GaussianFactorGraph.cpp

@@ -91,9 +91,9 @@ VectorConfig GaussianFactorGraph::operator^(const Errors& e) const {
 void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
 		VectorConfig& x) const {
 	// For each factor add the gradient contribution
-	Errors::const_iterator it = e.begin();
+	Errors::const_iterator ei = e.begin();
 	BOOST_FOREACH(sharedFactor Ai,factors_)
-		x += (*Ai)^(alpha*(*(it++)));
+		Ai->transposeMultiplyAdd(alpha,*(ei++),x);
 }
 
 /* ************************************************************************* */

cpp/iterative.h

@@ -46,24 +46,24 @@ namespace gtsam {
 			return A_ ^ (A_ * x - b_);
 		}
 
-		/** Apply operator A_ */
+		/** Apply operator A */
 		inline Vector operator*(const Vector& x) const {
 			return A_ * x;
 		}
 
-		/** Apply operator A_ in place*/
+		/** Apply operator A in place */
 		inline void multiplyInPlace(const Vector& x, Vector& e) const {
 			e = A_ * x;
 		}
 
-		/** Apply operator A_^T */
+		/** Apply operator A'*e */
 		inline Vector operator^(const Vector& e) const {
 			return A_ ^ e;
 		}
 
-		/** x += alpha* A_^T */
+		/** x += alpha* A'*e */
 		inline void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
-			x += alpha * A_ ^ e;
+			gtsam::transposeMultiplyAdd(A_,alpha*e,x);
 		}
 
 		/**

cpp/testGaussianFactor.cpp

@@ -68,6 +68,14 @@ TEST( GaussianFactor, operators )
 	expectedX.insert("x1",Vector_(2,-2000.,-4000.));
 	expectedX.insert("x2",Vector_(2, 2000., 4000.));
 	CHECK(assert_equal(expectedX,lf^e));
+
+	// test transposeMultiplyAdd
+	VectorConfig x;
+	x.insert("x1",Vector_(2, 1.,2.));
+	x.insert("x2",Vector_(2, 3.,4.));
+	VectorConfig expectedX2 = x + 0.1 * (lf^e);
+	lf.transposeMultiplyAdd(0.1,e,x);
+	CHECK(assert_equal(expectedX2,x));
 }
 
 /* ************************************************************************* */