Values and TupleValues throw error if attempting to insert the same key twice, also improved error handling with specific exception types.

gtsam/nonlinear/Values-inl.h

@@ -57,7 +57,7 @@ namespace gtsam {
   template<class J>
   const typename J::Value& Values<J>::at(const J& j) const {
     const_iterator it = values_.find(j);
-    if (it == values_.end()) throw std::invalid_argument("Values::at() invalid j: " + (string)j);
+    if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
     else return it->second;
   }
 
@@ -79,7 +79,8 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class J>
   void Values<J>::insert(const J& name, const typename J::Value& val) {
-    values_.insert(make_pair(name, val));
+    if(!values_.insert(make_pair(name, val)).second)
+      throw KeyAlreadyExists<J>(name, val);
   }
 
   /* ************************************************************************* */
@@ -124,7 +125,8 @@ namespace gtsam {
   template<class J>
   void Values<J>::erase(const J& j, size_t& dim) {
     iterator it = values_.find(j);
-    if (it == values_.end()) throw std::invalid_argument("Values::erase() invalid j: " + (string)j);
+    if (it == values_.end())
+      throw KeyDoesNotExist<J>("erase", j);
     dim = it->second.dim();
     values_.erase(it);
   }
@@ -205,6 +207,25 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  template<class J>
+  const char* KeyDoesNotExist<J>::what() const throw() {
+    if(message_.empty())
+      message_ = 
+          "Attempting to " + std::string(operation_) + " the key \"" +
+          (std::string)key_ + "\", which does not exist in the Values.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  template<class J>
+  const char* KeyAlreadyExists<J>::what() const throw() {
+    if(message_.empty())
+      message_ = 
+          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
+    return message_.c_str();
+  }
+
 }
 
 

gtsam/nonlinear/Values.h

@@ -37,6 +37,10 @@ namespace gtsam { class VectorValues; class Ordering; }
 
 namespace gtsam {
 
+  // Forward declarations
+  template<class J> class KeyDoesNotExist;
+  template<class J> class KeyAlreadyExists;
+
 	/**
 	 * Manifold type values structure
 	 * Takes two template types
@@ -86,11 +90,12 @@ namespace gtsam {
     /** Test whether configs are identical in keys and values */
     bool equals(const Values& expected, double tol=1e-9) const;
 
-    /** Retrieve a variable by j, throws std::invalid_argument if not found */
+    /** Retrieve a variable by j, throws KeyDoesNotExist<J> if not found */
     const Value& at(const J& j) const;
 
-    /** operator[] syntax for get */
+    /** operator[] syntax for get, throws KeyDoesNotExist<J> if not found */
-    const Value& operator[](const J& j) const { return at(j); }
+    const Value& operator[](const J& j) const {
+      return at(j); }
 
     /** Check if a variable exists */
     bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
@@ -131,10 +136,10 @@ namespace gtsam {
 
     // imperative methods:
 
-    /** Add a variable with the given j - does not replace existing values */
+    /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
     void insert(const J& j, const Value& val);
 
-    /** Add a set of variables - does note replace existing values */
+    /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
     void insert(const Values& cfg);
 
     /** update the current available values without adding new ones */
@@ -143,11 +148,12 @@ namespace gtsam {
     /** single element change of existing element */
     void update(const J& j, const Value& val);
 
-    /** Remove a variable from the config */
+    /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(const J& j);
 
     /** Remove a variable from the config while returning the dimensionality of
-     * the removed element (normally not needed by user code).
+     * the removed element (normally not needed by user code), throws
+     * KeyDoesNotExist<J> if j is not present.
      */
     void erase(const J& j, size_t& dim);
 
@@ -229,6 +235,58 @@ namespace gtsam {
     }
   };
 
+
+/* ************************************************************************* */
+template<class J>
+class KeyAlreadyExists : public std::exception {
+protected:
+  const J key_; ///< The key that already existed
+  const typename J::Value value_; ///< The value attempted to be inserted
+
+private:
+  mutable std::string message_;
+
+public:
+  /// Construct with the key-value pair attemped to be added
+  KeyAlreadyExists(const J& key, const typename J::Value& value) throw() :
+    key_(key), value_(value) {}
+
+  virtual ~KeyAlreadyExists() throw() {}
+
+  /// The duplicate key that was attemped to be added
+  const J& key() const throw() { return key_; }
+
+  /// The value that was attempted to be added
+  const typename J::Value& value() const throw() { return value_; }
+
+  /// The message to be displayed to the user
+  virtual const char* what() const throw();
+};
+
+/* ************************************************************************* */
+template<class J>
+class KeyDoesNotExist : public std::exception {
+protected:
+  const char* operation_; ///< The operation that attempted to access the key
+  const J key_; ///< The key that does not exist
+
+private:
+  mutable std::string message_;
+
+public:
+  /// Construct with the key that does not exist in the values
+  KeyDoesNotExist(const char* operation, const J& key) throw() :
+    operation_(operation), key_(key) {}
+
+  virtual ~KeyDoesNotExist() throw() {}
+
+  /// The key that was attempted to be accessed that does not exist
+  const J& key() const throw() { return key_; }
+
+  /// The message to be displayed to the user
+  virtual const char* what() const throw();
+};
+
 } // \namespace gtsam
 
 #include <gtsam/nonlinear/Values-inl.h>

gtsam/nonlinear/tests/testValues.cpp

@@ -69,7 +69,7 @@ TEST( TestValues, equals_nan )
 }
 
 /* ************************************************************************* */
-TEST( TestValues, insert_config )
+TEST( TestValues, insert_good )
 {
   TestValues cfg1, cfg2, expected;
   Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
@@ -78,17 +78,31 @@ TEST( TestValues, insert_config )
   Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
+  cfg2.insert(key3, v4);
+
+  cfg1.insert(cfg2);
+
+  expected.insert(key1, v1);
+  expected.insert(key2, v2);
+  expected.insert(key3, v4);
+
+  CHECK(assert_equal(cfg1, expected));
+}
+
+/* ************************************************************************* */
+TEST( TestValues, insert_bad )
+{
+  TestValues cfg1, cfg2;
+  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  cfg1.insert(key1, v1);
+  cfg1.insert(key2, v2);
   cfg2.insert(key2, v3);
   cfg2.insert(key3, v4);
 
-  cfg1.insert(cfg2);
+  CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
-
-  expected.insert(key1, v1);
-  expected.insert(key2, v2);
-  expected.insert(key2, v3);
-  expected.insert(key3, v4);
-
-  CHECK(assert_equal(cfg1, expected));
 }
 
 /* ************************************************************************* */

tests/testTupleValues.cpp

@@ -121,11 +121,7 @@ TEST( TupleValues, insert_duplicate )
   values1.insert(2, x2); // 6
   values1.insert(1, l1); // 8
   values1.insert(2, l2); // 10
-  values1.insert(2, l1); // still 10 !!!!
+  CHECK_EXCEPTION(values1.insert(2, l1), KeyAlreadyExists<PointKey>); // still 10 !!!!
-
-  CHECK(assert_equal(l2, values1[PointKey(2)]));
-  LONGS_EQUAL(4,values1.size());
-  LONGS_EQUAL(10,values1.dim());
 }
 
 /* ************************************************************************* */
@@ -161,8 +157,8 @@ TEST(TupleValues, at)
   EXPECT(assert_equal(l1, values1[PointKey(1)]));
   EXPECT(assert_equal(l2, values1[PointKey(2)]));
 
-  CHECK_EXCEPTION(values1[PoseKey(3)], std::invalid_argument);
+  CHECK_EXCEPTION(values1[PoseKey(3)], KeyDoesNotExist<PoseKey>);
-  CHECK_EXCEPTION(values1[PointKey(3)], std::invalid_argument);
+  CHECK_EXCEPTION(values1[PointKey(3)], KeyDoesNotExist<PointKey>);
 }
 
 /* ************************************************************************* */