From 5ce7c812a99f2a36aacd5c3352820ae8511060f9 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Fri, 7 Jul 2023 16:42:58 -0500 Subject: [PATCH 1/2] Added evaluation of expressions when inserting in Values --- gtsam/nonlinear/Values-inl.h | 34 +++++++++++++ gtsam/nonlinear/Values.h | 56 ++++++++++++++++++--- gtsam/nonlinear/tests/testValues.cpp | 74 ++++++++++++++++++++++++++++ 3 files changed, 158 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 61ea6684a5..a93f9570e6 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -243,16 +243,50 @@ namespace gtsam { insert(j, static_cast(GenericValue(val))); } + // partial specialization to insert an expression involving unary operators + template + void Values::insert(Key j, const Eigen::CwiseUnaryOp& val) { + insert(j, val.eval()); + } + + // partial specialization to insert an expression involving binary operators + template + void Values::insert(Key j, const Eigen::CwiseBinaryOp& val) { + insert(j, val.eval()); + } + // update with templated value template void Values::update(Key j, const ValueType& val) { update(j, static_cast(GenericValue(val))); } + // partial specialization to update with an expression involving unary operators + template + void Values::update(Key j, const Eigen::CwiseUnaryOp& val) { + update(j, val.eval()); + } + + // partial specialization to update with an expression involving binary operators + template + void Values::update(Key j, const Eigen::CwiseBinaryOp& val) { + update(j, val.eval()); + } + // insert_or_assign with templated value template void Values::insert_or_assign(Key j, const ValueType& val) { insert_or_assign(j, static_cast(GenericValue(val))); } + template + void Values::insert_or_assign(Key j, const Eigen::CwiseUnaryOp& val) { + insert_or_assign(j, val.eval()); + } + + template + void Values::insert_or_assign(Key j, const Eigen::CwiseBinaryOp& val) { + insert_or_assign(j, val.eval()); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 75cfc24e0f..7e95852a5b 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,11 +29,6 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -#include -#endif - - #include #include #include @@ -245,6 +240,31 @@ namespace gtsam { template 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 + void insert(Key j, const Eigen::CwiseUnaryOp& 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 + void insert(Key j, const Eigen::CwiseBinaryOp& val); + /// version for double void insertDouble(Key j, double c) { insert(j,c); } @@ -258,6 +278,18 @@ namespace gtsam { template 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 + void update(Key j, const Eigen::CwiseUnaryOp& val); + + /** Partial specialization that allows passing a binary Eigen expression for val, + * similar to the partial specialization for insert. + */ + template + void update(Key j, const Eigen::CwiseBinaryOp& val); + /** update the current available values without adding new ones */ void update(const Values& values); @@ -266,7 +298,7 @@ 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); @@ -274,6 +306,18 @@ namespace gtsam { template 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 + void insert_or_assign(Key j, const Eigen::CwiseUnaryOp& val); + + /** Partial specialization that allows passing a binary Eigen expression for val, + * similar to the partial specialization for insert. + */ + template + void insert_or_assign(Key j, const Eigen::CwiseBinaryOp& val); + /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(Key j); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 1af18ef957..2a7bede307 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/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(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(key1))); + + cfg.update(key1, 2.0*v1 + v2); + CHECK(cfg.size() == 1); + CHECK(assert_equal((2.0*v1 + v2).eval(), cfg.at(key1))); +} + +/* ************************************************************************* */ TEST(Values, InsertOrAssign) { Values values; Key X(0); @@ -183,6 +238,25 @@ TEST(Values, InsertOrAssign) { EXPECT(assert_equal(values.at(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(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(X), z)); +} + /* ************************************************************************* */ TEST(Values, basic_functions) { From 609351e25b1ccc0ed8e24a187e907467e1c2bf87 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Thu, 13 Jul 2023 12:45:32 -0500 Subject: [PATCH 2/2] added boost unique_ptr header that was removed in previous commit --- gtsam/nonlinear/Values.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 7e95852a5b..21e8e281f9 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,6 +29,11 @@ #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + + #include #include #include