diff --git a/.cproject b/.cproject
index be5c4a324..8afdf5942 100644
--- a/.cproject
+++ b/.cproject
@@ -2586,6 +2586,14 @@
true
true
+
+ make
+ -j4
+ testLieScalar.run
+ true
+ true
+ true
+
make
-j5
diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h
index 7ecd2d50e..631b88553 100644
--- a/gtsam/base/LieScalar.h
+++ b/gtsam/base/LieScalar.h
@@ -49,6 +49,9 @@ namespace gtsam {
/** Automatic conversion to underlying value */
operator double() const { return d_; }
+ /// @name Testable
+ /// @{
+
/** print @param name optional string naming the object */
void print(const std::string& name="") const;
@@ -57,65 +60,7 @@ namespace gtsam {
return fabs(expected.d_ - d_) <= tol;
}
- // Manifold requirements
-
- /** Returns dimensionality of the tangent space */
- size_t dim() const { return 1; }
- static size_t Dim() { return 1; }
-
- /** Update the LieScalar with a tangent space update */
- LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
-
- /** @return the local coordinates of another object */
- Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())).finished(); }
-
- // Group requirements
-
- /** identity */
- static LieScalar identity() {
- return LieScalar();
- }
-
- /** compose with another object */
- LieScalar compose(const LieScalar& p,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = eye(1);
- if(H2) *H2 = eye(1);
- return LieScalar(d_ + p.d_);
- }
-
- /** between operation */
- LieScalar between(const LieScalar& l2,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = -eye(1);
- if(H2) *H2 = eye(1);
- return LieScalar(l2.value() - value());
- }
-
- /** invert the object and yield a new one */
- LieScalar inverse() const {
- return LieScalar(-1.0 * value());
- }
-
- // Lie functions
-
- /** Expmap around identity */
- static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
-
- /** Logmap around identity - just returns with default cast back */
- static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); }
-
- /// Left-trivialized derivative of the exponential map
- static Matrix dexpL(const Vector& v) {
- return eye(1);
- }
-
- /// Left-trivialized derivative inverse of the exponential map
- static Matrix dexpInvL(const Vector& v) {
- return eye(1);
- }
+ /// @}
private:
double d_;
diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp
index 946a342fc..9055298e5 100644
--- a/gtsam/base/tests/testLieScalar.cpp
+++ b/gtsam/base/tests/testLieScalar.cpp
@@ -27,6 +27,20 @@ GTSAM_CONCEPT_LIE_INST(LieScalar)
const double tol=1e-9;
+//******************************************************************************
+TEST(LieScalar , Concept) {
+ BOOST_CONCEPT_ASSERT((IsGroup));
+ BOOST_CONCEPT_ASSERT((IsManifold));
+ BOOST_CONCEPT_ASSERT((IsLieGroup));
+}
+
+//******************************************************************************
+TEST(LieScalar , Invariants) {
+ LieScalar lie1(2), lie2(3);
+ check_group_invariants(lie1, lie2);
+ check_manifold_invariants(lie1, lie2);
+}
+
/* ************************************************************************* */
TEST( testLieScalar, construction ) {
double d = 2.;
@@ -34,7 +48,7 @@ TEST( testLieScalar, construction ) {
EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol);
EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol);
- EXPECT(lie1.dim() == 1);
+ EXPECT(traits_x::dimension == 1);
EXPECT(assert_equal(lie1, lie2));
}
@@ -42,7 +56,8 @@ TEST( testLieScalar, construction ) {
TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.);
- EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2)));
+ Vector1 actual = traits_x::Local(lie1, lie2);
+ EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual));
}
/* ************************************************************************* */