diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 9087b3c90..53ea94d6e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -149,6 +149,20 @@ void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) c Ab2_.multiplyInPlace(x, ei); // use iterator version } +/* ************************************************************************* */ +// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 +VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { + + Errors::const_iterator it = e.begin(); + VectorValues y = zero(); + for(auto& key_value: y) { + key_value.second = *it; + ++it; + } + transposeMultiplyAdd2(1.0, it, e.end(), y); + return y; +} + /* ************************************************************************* */ // y += alpha*A'*e void SubgraphPreconditioner::transposeMultiplyAdd diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 9e005dd53..558a80bd8 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -125,6 +125,9 @@ namespace gtsam { /** Apply operator A in place: needs e allocated already */ void multiplyInPlace(const VectorValues& y, Errors& e) const; + /** Apply operator A' */ + VectorValues operator^(const Errors& e) const; + /** * Add A'*e to y * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index ff3d73801..9bd1831e9 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -59,6 +59,11 @@ namespace gtsam { /** Access b vector */ const Vector& b() const { return b_; } + /** Apply operator A'*e */ + Vector operator^(const Vector& e) const { + return A_.transpose() * e; + } + /** * Print with optional string */