diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 4626306b6..fc9004930 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -33,6 +33,9 @@ namespace gtsam { /** wrap a double */ LieScalar(double d) : d_(d) {} + /** access the underlying value */ + double value() const { return d_; } + /** print @param s optional string naming the object */ inline void print(const std::string& name="") const { std::cout << name << ": " << d_ << std::endl; @@ -45,8 +48,10 @@ namespace gtsam { /** * Returns dimensionality of the tangent space + * with member and static versions */ inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } /** * Returns Exponential map update of T diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 87560780a..a5428f803 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -20,11 +20,15 @@ using namespace gtsam; +const double tol=1e-9; + /* ************************************************************************* */ TEST( testLieScalar, construction ) { double d = 2.; LieScalar lie1(d), lie2(d); + EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); + EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); EXPECT(lie1.dim() == 1); EXPECT(assert_equal(lie1, lie2)); }