diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 35afddb3a..b037aad92 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -160,28 +160,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector VectorValues::vector(const FastVector& keys) const - { - // Count dimensions and collect pointers to avoid double lookups - DenseIndex totalDim = 0; - FastVector items(keys.size()); - for(size_t i = 0; i < keys.size(); ++i) { - items[i] = &at(keys[i]); - totalDim += items[i]->size(); - } - - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - for(const Vector *v: items) { - result.segment(pos, v->size()) = *v; - pos += v->size(); - } - - return result; - } - /* ************************************************************************* */ Vector VectorValues::vector(const Dims& keys) const { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index cb1e08f2d..2d8eb0ec0 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -244,7 +244,26 @@ namespace gtsam { Vector vector() const; /** Access a vector that is a subset of relevant keys. */ - Vector vector(const FastVector& keys) const; + template + Vector vector(const CONTAINER& keys) const { + DenseIndex totalDim = 0; + FastVector items; + items.reserve(keys.end() - keys.begin()); + for (Key key : keys) { + const Vector* v = &at(key); + totalDim += v->size(); + items.push_back(v); + } + + Vector result(totalDim); + DenseIndex pos = 0; + for (const Vector* v : items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; + } /** Access a vector that is a subset of relevant keys, dims version. */ Vector vector(const Dims& dims) const;