diff --git a/.cproject b/.cproject index 3886fae80..be5c4a324 100644 --- a/.cproject +++ b/.cproject @@ -2578,6 +2578,14 @@ true true + + make + -j4 + testLieVector.run + true + true + true + make -j5 diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index cc691a3c3..0e06efdb2 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -58,10 +58,8 @@ struct LieVector : public Vector { /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } + /// @name Testable + /// @{ /** print @param name optional string naming the object */ GTSAM_EXPORT void print(const std::string& name="") const; @@ -71,72 +69,25 @@ struct LieVector : public Vector { return gtsam::equal(vector(), expected.vector(), tol); } - // Manifold requirements + /// @} + /// @name VectorSpace requirements + /// @{ + + /** get the underlying vector */ + Vector vector() const { + return static_cast(*this); + } /** Returns dimensionality of the tangent space */ size_t dim() const { return this->size(); } - /** Update the LieVector with a tangent space update */ - LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - - /** @return the local coordinates of another object */ - Vector localCoordinates(const LieVector& t2) const { return t2 - *this; } - - // Group requirements - /** identity - NOTE: no known size at compile time - so zero length */ static LieVector identity() { throw std::runtime_error("LieVector::identity(): Don't use this function"); return LieVector(); } - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - LieVector compose(const LieVector& p, - OptionalJacobian<-1,-1> H1 = boost::none, - OptionalJacobian<-1,-1> H2 = boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); - - return LieVector(vector() + p); - } - - /** between operation */ - LieVector between(const LieVector& l2, - OptionalJacobian<-1,-1> H1 = boost::none, - OptionalJacobian<-1,-1> H2 = boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieVector(l2.vector() - vector()); - } - - /** invert the object and yield a new one */ - LieVector inverse(OptionalJacobian<-1,-1> H=boost::none) const { - if(H) *H = -eye(dim()); - - return LieVector(-1.0 * vector()); - } - - // Lie functions - - /** Expmap around identity */ - static LieVector Expmap(const Vector& v, OptionalJacobian<-1, -1> H = - boost::none) { - if (H) - throw std::runtime_error("LieVector::Expmap derivative not implemented"); - return LieVector(v); - } - - /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieVector& p, OptionalJacobian<-1, -1> H = - boost::none) { - if (H) - throw std::runtime_error("LieVector::Logmap derivative not implemented"); - return p; - } + /// @} private: diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index c1dface2e..81e03c63c 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -25,7 +25,21 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieVector) GTSAM_CONCEPT_LIE_INST(LieVector) -/* ************************************************************************* */ +//****************************************************************************** +TEST(LieVector , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(LieVector , Invariants) { + Vector v = Vector3(1.0, 2.0, 3.0); + LieVector lie1(v), lie2(v); + check_manifold_invariants(lie1, lie2); +} + +//****************************************************************************** TEST( testLieVector, construction ) { Vector v = Vector3(1.0, 2.0, 3.0); LieVector lie1(v), lie2(v); @@ -35,17 +49,19 @@ TEST( testLieVector, construction ) { EXPECT(assert_equal(lie1, lie2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testLieVector, other_constructors ) { Vector init = Vector2(10.0, 20.0); LieVector exp(init); - double data[] = {10,20}; - LieVector b(2,data); + double data[] = { 10, 20 }; + LieVector b(2, data); EXPECT(assert_equal(exp, b)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ -