Skip to content

Commit

Permalink
Merge pull request #1577 from arutkowski/insert_expressions_in_Values
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Jul 14, 2023
2 parents 5dfaf4d + 609351e commit 4de2d46
Show file tree
Hide file tree
Showing 3 changed files with 158 additions and 1 deletion.
34 changes: 34 additions & 0 deletions gtsam/nonlinear/Values-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

}
51 changes: 50 additions & 1 deletion gtsam/nonlinear/Values.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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); }

Expand All @@ -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);

Expand All @@ -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);

Expand Down
74 changes: 74 additions & 0 deletions gtsam/nonlinear/tests/testValues.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
{
Expand Down Expand Up @@ -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);
Expand All @@ -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)
{
Expand Down

0 comments on commit 4de2d46

Please sign in to comment.