Added dim function and removed commented out code

gtsam/nonlinear/Values-inl.h

@@ -58,12 +58,6 @@ namespace gtsam {
     }
   };
 
-//  /* ************************************************************************* */
-//  template<class ValueType>
-//  ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) {
-//    lhs = rhs;
-//  }
-
   /* ************************************************************************* */
   template<typename ValueType>
   const ValueType& Values::at(const Symbol& j) const {

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);

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<size_t> 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