diff --git a/linear/SubgraphPreconditioner.h b/linear/SubgraphPreconditioner.h index 1a51abed8..c6fe36fc1 100644 --- a/linear/SubgraphPreconditioner.h +++ b/linear/SubgraphPreconditioner.h @@ -8,7 +8,7 @@ #include #include -#include +#include namespace gtsam { @@ -56,7 +56,11 @@ namespace gtsam { VectorValues x(const VectorValues& y) const; /* A zero VectorValues with the structure of xbar */ - VectorValues zero() const { return VectorValues::zero(*xbar_);} + VectorValues zero() const { + VectorValues V(*xbar_) ; + V.makeZero(); + return V ; + } /* error, given y */ double error(const VectorValues& y) const; diff --git a/linear/VectorValues.h b/linear/VectorValues.h index e01a9c65c..beeabd57a 100644 --- a/linear/VectorValues.h +++ b/linear/VectorValues.h @@ -46,6 +46,7 @@ public: * slow reallocation of space at runtime. */ VectorValues() : varStarts_(1,0) {} + VectorValues(const VectorValues &V) : values_(V.values_), varStarts_(V.varStarts_) {} /** Construct from a container of variable dimensions (in variable order). */ template @@ -76,6 +77,10 @@ public: */ size_t dim() const { return varStarts_.back(); } + /* dot product */ + double dot(const VectorValues& V) const { return gtsam::dot(this->values_, V.values_) ; } + + /** Total dimensions capacity allocated */ size_t dimCapacity() const { return values_.size(); } @@ -163,6 +168,13 @@ public: protected: void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); } + + +public: + friend double dot(const VectorValues& V1, const VectorValues& V2) { return gtsam::dot(V1.values_, V2.values_) ; } + friend void scal(double alpha, VectorValues& x) { gtsam::scal(alpha, x.values_) ; } + friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { gtsam::axpy(alpha, x.values_, y.values_) ; } + }; diff --git a/linear/iterative-inl.h b/linear/iterative-inl.h index 7d06befa5..0abf33d1d 100644 --- a/linear/iterative-inl.h +++ b/linear/iterative-inl.h @@ -32,8 +32,8 @@ namespace gtsam { k = 0; verbose = verb; steepest = steep; - maxIterations = (maxIt > 0) ? maxIt : dim(x) * (steepest ? 10 : 1); - reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset + maxIterations = (maxIt > 0) ? maxIt : x.dim() * (steepest ? 10 : 1); + reset = (size_t) (sqrt(x.dim()) + 0.5); // when to reset // Start with g0 = A'*(A*x0-b), d0 = - g0 // i.e., first step is in direction of negative gradient @@ -41,7 +41,7 @@ namespace gtsam { d = g; // instead of negating gradient, alpha will be negated // init gamma and calculate threshold - gamma = dot(g, g); + gamma = dot(g,g) ; threshold = ::max(epsilon_abs, epsilon * epsilon * gamma); // Allocate and calculate A*d for first iteration