[Symforce] Add a SetNew method that throws if key already exists

This is useful when setting up the initial values of a problem, the
function will throw if one accidentally set the same key twice, which is
usually a mistake.

The SetNew() method was implemented using the existing Set() function.
The only downside is that the value of the existing key will be overwritten
when the exception is thrown. The alternative would be to basically
re-implement SetInternal(), which I think is not worth it.

Topic: chaoqu_symforce_set_unique
GitOrigin-RevId: c3ff57ae0d5f2e3eae9951f63f94d9d3daf75f00

symforce/opt/values.h

@@ -81,6 +81,13 @@ class Values {
   template <typename Derived>
   std::enable_if_t<kIsEigenType<Derived>, bool> Set(const Key& key, const Derived& value);
 
+  /**
+   * Add a value by key, throws runtime_error if the key already exists.
+   * This is useful when setting up initial values for a problem.
+   */
+  template <typename T>
+  void SetNew(const Key& key, T&& value);
+
   /**
    * Update or add keys to this Values base on other Values of different structure.
    * index MUST be valid for other.

symforce/opt/values.tcc

@@ -3,6 +3,8 @@
  * This source code is under the Apache 2.0 license found in the LICENSE file.
  * ---------------------------------------------------------------------------- */
 
+#include <stdexcept>
+
 #include <fmt/format.h>
 #include <fmt/ostream.h>
 
@@ -56,6 +58,15 @@ std::enable_if_t<kIsEigenType<Derived>, bool> Values<Scalar>::Set(const Key& key
   return SetInternal<typename Derived::PlainMatrix>(key, value);
 }
 
+template <typename Scalar>
+template <typename T>
+void Values<Scalar>::SetNew(const Key& key, T&& value) {
+  const auto added = Set(key, std::forward<T>(value));
+  if (!added) {
+    throw std::runtime_error(fmt::format("Key {} already exists", key));
+  }
+}
+
 template <typename Scalar>
 template <typename T>
 std::enable_if_t<!kIsEigenType<T>> Values<Scalar>::Set(const index_entry_t& entry, const T& value) {

test/symforce_values_test.cc

@@ -396,3 +396,10 @@ TEST_CASE("Test Set with Eigen expressions", "[values]") {
   CHECK(values.At<Eigen::Vector3d>('b') == Eigen::Vector3d::Constant(2));
   CHECK(values.At<Eigen::Vector3d>('a') == Eigen::Vector3d::Zero());
 }
+
+TEST_CASE("Test SetNew", "[values]") {
+  sym::Valuesd values;
+  values.SetNew('a', Eigen::Vector3d::Zero());
+  CHECK(values.At<Eigen::Vector3d>('a') == Eigen::Vector3d::Zero());
+  CHECK_THROWS_AS(values.SetNew('a', Eigen::Vector3d::Zero()), std::runtime_error);
+}