Added evaluation of expressions when inserting in Values

gtsam/nonlinear/Values-inl.h

@@ -243,16 +243,50 @@ namespace gtsam {
     insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
   }
 
+  // partial specialization to insert an expression involving unary operators
+  template <typename UnaryOp, typename ValueType>
+  void Values::insert(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
+    insert(j, val.eval());
+  }
+
+  // partial specialization to insert an expression involving binary operators
+  template <typename BinaryOp, typename ValueType1, typename ValueType2>
+  void Values::insert(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
+    insert(j, val.eval());
+  }
+
   // update with templated value
   template <typename ValueType>
   void Values::update(Key j, const ValueType& val) {
     update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
   }
 
+  // partial specialization to update with an expression involving unary operators
+  template <typename UnaryOp, typename ValueType>
+  void Values::update(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
+      update(j, val.eval());
+  }
+
+  // partial specialization to update with an expression involving binary operators
+  template <typename BinaryOp, typename ValueType1, typename ValueType2>
+  void Values::update(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
+    update(j, val.eval());
+  }
+
   // insert_or_assign with templated value
   template <typename ValueType>
   void Values::insert_or_assign(Key j, const ValueType& val) {
     insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
   }
 
+  template <typename UnaryOp, typename ValueType>
+  void Values::insert_or_assign(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
+    insert_or_assign(j, val.eval());
+  }
+
+  template <typename BinaryOp, typename ValueType1, typename ValueType2>
+  void Values::insert_or_assign(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
+    insert_or_assign(j, val.eval());
+  }
+
 }

gtsam/nonlinear/Values.h

@@ -245,6 +245,31 @@ namespace gtsam {
     template <typename ValueType>
     void insert(Key j, const ValueType& val);
 
+    /** Partial specialization that allows passing a unary Eigen expression for val.
+      *
+      * A unary expression is an expression such as 2*a or -a, where a is a valid Vector or Matrix type.
+      * The typical usage is for types Point2 (i.e. Eigen::Vector2d) or Point3 (i.e. Eigen::Vector3d).
+      * For example, together with the partial specialization for binary operators, a user may call insert(j, 2*a + M*b - c),
+      * where M is an appropriately sized matrix (such as a rotation matrix).
+      * Thus, it isn't necessary to explicitly evaluate the Eigen expression, as in insert(j, (2*a + M*b - c).eval()),
+      * nor is it necessary to first assign the expression to a separate variable.
+      */
+    template <typename UnaryOp, typename ValueType>
+    void insert(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
+
+    /** Partial specialization that allows passing a binary Eigen expression for val.
+      *
+      * A binary expression is an expression such as a + b, where a and b are valid Vector or Matrix
+      * types of compatible size.
+      * The typical usage is for types Point2 (i.e. Eigen::Vector2d) or Point3 (i.e. Eigen::Vector3d).
+      * For example, together with the partial specialization for binary operators, a user may call insert(j, 2*a + M*b - c),
+      * where M is an appropriately sized matrix (such as a rotation matrix).
+      * Thus, it isn't necessary to explicitly evaluate the Eigen expression, as in insert(j, (2*a + M*b - c).eval()),
+      * nor is it necessary to first assign the expression to a separate variable.
+      */
+    template <typename BinaryOp, typename ValueType1, typename ValueType2>
+    void insert(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val);
+
     /// version for double
     void insertDouble(Key j, double c) { insert<double>(j,c); }
 
@@ -258,6 +283,18 @@ namespace gtsam {
     template <typename T>
     void update(Key j, const T& val);
 
+    /** Partial specialization that allows passing a unary Eigen expression for val,
+      * similar to the partial specialization for insert.
+      */
+    template <typename UnaryOp, typename ValueType>
+    void update(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
+
+    /** Partial specialization that allows passing a binary Eigen expression for val,
+      * similar to the partial specialization for insert.
+      */
+    template <typename BinaryOp, typename ValueType1, typename ValueType2>
+    void update(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val);
+
     /** update the current available values without adding new ones */
     void update(const Values& values);
 
@@ -266,14 +303,26 @@ namespace gtsam {
 
     /**
      * Update a set of variables.
-     * If any variable key doe not exist, then perform an insert.
+     * If any variable key does not exist, then perform an insert.
      */
     void insert_or_assign(const Values& values);
 
     /// Templated version to insert_or_assign a variable with the given j.
     template <typename ValueType>
     void insert_or_assign(Key j, const ValueType& val);
 
+    /** Partial specialization that allows passing a unary Eigen expression for val,
+      * similar to the partial specialization for insert.
+      */
+    template <typename UnaryOp, typename ValueType>
+    void insert_or_assign(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
+
+    /** Partial specialization that allows passing a binary Eigen expression for val,
+      * similar to the partial specialization for insert.
+      */
+    template <typename BinaryOp, typename ValueType1, typename ValueType2>
+    void insert_or_assign(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val);
+
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(Key j);
 

gtsam/nonlinear/tests/testValues.cpp

@@ -134,6 +134,44 @@ TEST( Values, insert_good )
   CHECK(assert_equal(expected, cfg1));
 }
 
+/* ************************************************************************* */
+TEST( Values, insert_expression )
+{
+  Point2 p1(0.1, 0.2);
+  Point2 p2(0.3, 0.4);
+  Point2 p3(0.5, 0.6);
+  Point2 p4(p1 + p2 + p3);
+  Point2 p5(-p1);
+  Point2 p6(2.0*p1);
+
+  Values cfg1, cfg2;
+  cfg1.insert(key1, p1 + p2 + p3);
+  cfg1.insert(key2, -p1);
+  cfg1.insert(key3, 2.0*p1);
+  cfg2.insert(key1, p4);
+  cfg2.insert(key2, p5);
+  cfg2.insert(key3, p6);
+
+  CHECK(assert_equal(cfg1, cfg2));
+
+  Point3 p7(0.1, 0.2, 0.3);
+  Point3 p8(0.4, 0.5, 0.6);
+  Point3 p9(0.7, 0.8, 0.9);
+  Point3 p10(p7 + p8 + p9);
+  Point3 p11(-p7);
+  Point3 p12(2.0*p7);
+
+  Values cfg3, cfg4;
+  cfg3.insert(key1, p7 + p8 + p9);
+  cfg3.insert(key2, -p7);
+  cfg3.insert(key3, 2.0*p7);
+  cfg4.insert(key1, p10);
+  cfg4.insert(key2, p11);
+  cfg4.insert(key3, p12);
+
+  CHECK(assert_equal(cfg3, cfg4));
+}
+
 /* ************************************************************************* */
 TEST( Values, insert_bad )
 {
@@ -167,6 +205,23 @@ TEST( Values, update_element )
   CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
 }
 
+/* ************************************************************************* */
+TEST(Values, update_element_with_expression)
+{
+  Values cfg;
+  Vector3 v1(5.0, 6.0, 7.0);
+  Vector3 v2(8.0, 9.0, 1.0);
+
+  cfg.insert(key1, v1);
+  CHECK(cfg.size() == 1);
+  CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
+
+  cfg.update(key1, 2.0*v1 + v2);
+  CHECK(cfg.size() == 1);
+  CHECK(assert_equal((2.0*v1 + v2).eval(), cfg.at<Vector3>(key1)));
+}
+
+/* ************************************************************************* */
 TEST(Values, InsertOrAssign) {
   Values values;
   Key X(0);
@@ -183,6 +238,25 @@ TEST(Values, InsertOrAssign) {
   EXPECT(assert_equal(values.at<double>(X), y));
 }
 
+/* ************************************************************************* */
+TEST(Values, InsertOrAssignWithExpression) {
+  Values values,expected;
+  Key X(0);
+  Vector3 x{1.0, 2.0, 3.0};
+  Vector3 y{4.0, 5.0, 6.0};
+
+  CHECK(values.size() == 0);
+  // This should perform an insert.
+  Vector3 z = x + y;
+  values.insert_or_assign(X, x + y);
+  EXPECT(assert_equal(values.at<Vector3>(X), z));
+
+  // This should perform an update.
+  z = 2.0*x - 3.0*y;
+  values.insert_or_assign(X, 2.0*x - 3.0*y);
+  EXPECT(assert_equal(values.at<Vector3>(X), z));
+}
+
 /* ************************************************************************* */
 TEST(Values, basic_functions)
 {