12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added evaluation of expressions when inserting in Values

release/4.3a0
Adam Rutkowski 2023-07-07 16:42:58 -05:00
parent 5dfaf4d0a4
commit 5ce7c812a9
3 changed files with 158 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/nonlinear/Values-inl.h
View File

@ -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());
}
}

56
gtsam/nonlinear/Values.h
View File

@ -29,11 +29,6 @@
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/VectorSpace.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/unique_ptr.hpp>
#endif
#include <memory>
#include <string>
#include <utility>
@ -245,6 +240,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 +278,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,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 <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);

74
gtsam/nonlinear/tests/testValues.cpp
View File

@ -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)
{