diff --git a/cpp/BayesNetPreconditioner.cpp b/cpp/BayesNetPreconditioner.cpp index 2fb4ab44f..07a8e1590 100644 --- a/cpp/BayesNetPreconditioner.cpp +++ b/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 { diff --git a/cpp/BayesNetPreconditioner.h b/cpp/BayesNetPreconditioner.h index 176118633..ae78ea3fe 100644 --- a/cpp/BayesNetPreconditioner.h +++ b/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; }; diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 744edc00a..806016865 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/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; diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 0f70fe19b..7850a5d54 100644 --- a/cpp/GaussianFactorGraph.h +++ b/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; diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp index 2ee73f096..bb4b89582 100644 --- a/cpp/SubgraphPreconditioner.cpp +++ b/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 { diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index d8ea213ab..555ec162f 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/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 */ diff --git a/cpp/iterative-inl.h b/cpp/iterative-inl.h index d993c9b19..0dd17b4b8 100644 --- a/cpp/iterative-inl.h +++ b/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; - if (dotg0 < epsilon_abs) return x; - double threshold = epsilon * epsilon * dotg0; + double gamma0 = dot(g, g), gamma_old = gamma0; + if (gamma0 < epsilon_abs) return x; + 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; - if (dotg < threshold) break; + << ", dotg = " << gamma << endl; + if (gamma < threshold) break; // calculate new search direction if (steepest) d = g; else { - double beta = dotg / prev_dotg; - prev_dotg = dotg; + double beta = gamma / gamma_old; + 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; } diff --git a/cpp/iterative.h b/cpp/iterative.h index 6f7cb0719..8f87b8e9c 100644 --- a/cpp/iterative.h +++ b/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;