From 9b4ff5e09973fcd257a5235dfa2ca80abac7d13f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2010 23:59:29 +0000 Subject: [PATCH] transposeMultiplyAdd provied BLAS-style call for iterative speed --- cpp/BayesNetPreconditioner.cpp | 8 ++++++++ cpp/BayesNetPreconditioner.h | 3 +++ cpp/GaussianFactorGraph.cpp | 10 ++++++++++ cpp/GaussianFactorGraph.h | 5 ++++- cpp/SubgraphPreconditioner.cpp | 33 +++++++++++++++++++++++++++++---- cpp/SubgraphPreconditioner.h | 3 +++ cpp/iterative-inl.h | 3 ++- cpp/iterative.h | 5 +++++ 8 files changed, 64 insertions(+), 6 deletions(-) diff --git a/cpp/BayesNetPreconditioner.cpp b/cpp/BayesNetPreconditioner.cpp index 07a8e1590..d3d3a006c 100644 --- a/cpp/BayesNetPreconditioner.cpp +++ b/cpp/BayesNetPreconditioner.cpp @@ -60,6 +60,14 @@ namespace gtsam { } /* ************************************************************************* */ + // y += alpha*inv(R')*A'*e + void BayesNetPreconditioner::transposeMultiplyAdd(double alpha, + const Errors& e, VectorConfig& y) const { + VectorConfig x = Ab_ ^ e; // x = A'*e2 + y += alpha * gtsam::backSubstituteTranspose(Rd_, x); // TODO avoid temp + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/BayesNetPreconditioner.h b/cpp/BayesNetPreconditioner.h index ae78ea3fe..9c7f96a82 100644 --- a/cpp/BayesNetPreconditioner.h +++ b/cpp/BayesNetPreconditioner.h @@ -58,6 +58,9 @@ namespace gtsam { /** Apply operator A' */ VectorConfig operator^(const Errors& e) const; + + /** BLAS level 2 equivalent y += alpha*inv(R')*A'*e */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const; }; } // namespace gtsam diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 806016865..e60fc0415 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -86,6 +86,16 @@ VectorConfig GaussianFactorGraph::operator^(const Errors& e) const { return x; } +/* ************************************************************************* */ +// x += alpha*A'*e +void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorConfig& x) const { + // For each factor add the gradient contribution + Errors::const_iterator it = e.begin(); + BOOST_FOREACH(sharedFactor Ai,factors_) + x += (*Ai)^(alpha*(*(it++))); +} + /* ************************************************************************* */ VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const { const GaussianFactorGraph& A = *this; diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 7850a5d54..e1c5eface 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -91,9 +91,12 @@ namespace gtsam { /* In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorConfig& x, const Errors::iterator& e) const; - /** return A^x */ + /** return A^e */ VectorConfig operator^(const Errors& e) const; + /** x += alpha*A'*e */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& x) const; + /** * Calculate Gradient of A^(A*x-b) for a given config * @param x: VectorConfig specifying where to calculate gradient diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp index bb4b89582..6b8556c4f 100644 --- a/cpp/SubgraphPreconditioner.cpp +++ b/cpp/SubgraphPreconditioner.cpp @@ -92,14 +92,14 @@ namespace gtsam { // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 VectorConfig SubgraphPreconditioner::operator^(const Errors& e) const { - VectorConfig y1; + VectorConfig y; // Use BayesNet order to remove y contributions in order Errors::const_iterator it = e.begin(); BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { const Symbol& j = cg->key(); const Vector& ej = *(it++); - y1.insert(j,ej); + y.insert(j,ej); } // create e2 with what's left of e @@ -109,9 +109,34 @@ namespace gtsam { // get A2 part, VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2 - VectorConfig y2 = gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; + y += gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; - return y1 + y2; + return y; + } + + /* ************************************************************************* */ + // y += alpha*A'*e + // TODO avoid code duplication + void SubgraphPreconditioner::transposeMultiplyAdd + (double alpha, const Errors& e, VectorConfig& y) const { + + // Use BayesNet order to remove y contributions in order + Errors::const_iterator it = e.begin(); + BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) { + const Symbol& j = cg->key(); + const Vector& ej = *(it++); + Vector& yj = y.getReference(j); + axpy(alpha,ej,yj); + } + + // create e2 with what's left of e + Errors e2; + while (it != e.end()) + e2.push_back(*(it++)); + + // get A2 part, + VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2 + y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x; } /* ************************************************************************* */ diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index 555ec162f..02603edba 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -70,6 +70,9 @@ namespace gtsam { /** Apply operator A' */ VectorConfig operator^(const Errors& e) const; + /** y += alpha*A'*e */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const; + /** print the object */ void print(const std::string& s = "SubgraphPreconditioner") const; }; diff --git a/cpp/iterative-inl.h b/cpp/iterative-inl.h index 0dd17b4b8..79de39eb1 100644 --- a/cpp/iterative-inl.h +++ b/cpp/iterative-inl.h @@ -54,7 +54,8 @@ namespace gtsam { if (k%reset==0) g = Ab.gradient(x); else - axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) + // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) + Ab.transposeMultiplyAdd(alpha, Ad, g); // check for convergence double gamma = dot(g, g); diff --git a/cpp/iterative.h b/cpp/iterative.h index 8f87b8e9c..a835fdff3 100644 --- a/cpp/iterative.h +++ b/cpp/iterative.h @@ -61,6 +61,11 @@ namespace gtsam { return A_ ^ e; } + /** x += alpha* A_^T */ + inline void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const { + x += alpha * A_ ^ e; + } + /** * Print with optional string */