diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 18f21ae82..0747c2d43 100644 --- a/cpp/GaussianFactor.cpp +++ b/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 GaussianFactor::matrix(const Ordering& ordering, bool weight) const { diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 669034916..2714ce1d4 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -197,16 +197,15 @@ public: void tally_separator(const Symbol& key, std::set& separator) const; - /** - * Return A*x - */ + /** Return A*x */ Vector operator*(const VectorConfig& x) const; - /** - * Return A^x - */ + /** Return A'*e */ 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 diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index e60fc0415..61fa680a8 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/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); } /* ************************************************************************* */ diff --git a/cpp/iterative.h b/cpp/iterative.h index a835fdff3..07394563d 100644 --- a/cpp/iterative.h +++ b/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); } /** diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index 19cd0c618..b596b6d77 100644 --- a/cpp/testGaussianFactor.cpp +++ b/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)); } /* ************************************************************************* */