diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 31d6c318a..7410ddade 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -58,12 +58,6 @@ namespace gtsam { } }; -// /* ************************************************************************* */ -// template -// ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) { -// lhs = rhs; -// } - /* ************************************************************************* */ template const ValueType& Values::at(const Symbol& j) const { diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 2d8b72120..44245c39c 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -175,6 +175,15 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + size_t Values::dim() const { + size_t result = 0; + BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + result += key_value.second.dim(); + } + return result; + } + /* ************************************************************************* */ Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { Ordering::shared_ptr ordering(new Ordering); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bad281a9a..e2a69d622 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -255,9 +255,12 @@ namespace gtsam { /** Remove all variables from the config */ void clear() { values_.clear(); } - /** Create an array of variable dimensions using the given ordering */ + /** Create an array of variable dimensions using the given ordering (\f$ O(n) \f$) */ std::vector dims(const Ordering& ordering) const; + /** Compute the total dimensionality of all values (\f$ O(n) \f$) */ + size_t dim() const; + /** * Generate a default ordering, simply in key sort order. To instead * create a fill-reducing ordering, use