Added at() to Values to get a Value base class reference

gtsam/nonlinear/Values.cpp

@@ -106,6 +106,17 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  const Value& Values::at(Key j) const {
+    // Find the item
+    KeyValueMap::const_iterator item = values_.find(j);
+
+    // Throw exception if it does not exist
+    if(item == values_.end())
+      throw ValuesKeyDoesNotExist("retrieve", j);
+    return *item->second;
+  }
+
   /* ************************************************************************* */
   void Values::insert(Key j, const Value& val) {
   	Key key = j; // Non-const duplicate to deal with non-const insert argument

gtsam/nonlinear/Values.h

@@ -255,6 +255,13 @@ namespace gtsam {
     template<typename ValueType>
     const ValueType& at(Key j) const;
 
+    /** Retrieve a variable by key \c j.  This version returns a reference
+     * to the base Value class, and needs to be casted before use.
+     * @param j Retrieve the value associated with this key
+     * @return A const reference to the stored value
+     */
+    const Value& at(Key j) const;
+
 #if 0
     /** Retrieve a variable by key \c j.  This non-templated version returns a
      * special ValueAutomaticCasting object that may be assigned to the proper