diff --git a/gtsam.h b/gtsam.h index d86887e8b..826f8472e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -286,6 +286,99 @@ virtual class GenericValue : gtsam::Value { void serializable() const; }; +#include +class LieScalar { + // Standard constructors + LieScalar(); + LieScalar(double d); + + // Standard interface + double value() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieScalar& expected, double tol) const; + + // Group + static gtsam::LieScalar identity(); + gtsam::LieScalar inverse() const; + gtsam::LieScalar compose(const gtsam::LieScalar& p) const; + gtsam::LieScalar between(const gtsam::LieScalar& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieScalar retract(Vector v) const; + Vector localCoordinates(const gtsam::LieScalar& t2) const; + + // Lie group + static gtsam::LieScalar Expmap(Vector v); + static Vector Logmap(const gtsam::LieScalar& p); +}; + +#include +class LieVector { + // Standard constructors + LieVector(); + LieVector(Vector v); + + // Standard interface + Vector vector() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieVector& expected, double tol) const; + + // Group + static gtsam::LieVector identity(); + gtsam::LieVector inverse() const; + gtsam::LieVector compose(const gtsam::LieVector& p) const; + gtsam::LieVector between(const gtsam::LieVector& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieVector retract(Vector v) const; + Vector localCoordinates(const gtsam::LieVector& t2) const; + + // Lie group + static gtsam::LieVector Expmap(Vector v); + static Vector Logmap(const gtsam::LieVector& p); + + // enabling serialization functionality + void serialize() const; +}; + +#include +class LieMatrix { + // Standard constructors + LieMatrix(); + LieMatrix(Matrix v); + + // Standard interface + Matrix matrix() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieMatrix& expected, double tol) const; + + // Group + static gtsam::LieMatrix identity(); + gtsam::LieMatrix inverse() const; + gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; + gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::LieMatrix & t2) const; + + // Lie group + static gtsam::LieMatrix Expmap(Vector v); + static Vector Logmap(const gtsam::LieMatrix& p); + + // enabling serialization functionality + void serialize() const; +}; + //************************************************************************* // geometry //************************************************************************* diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b80f1626c..b4a33943e 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -39,12 +39,6 @@ set (excluded_sources #"") set (excluded_headers #"") "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" - "${CMAKE_CURRENT_SOURCE_DIR}/base/LieMatrix.h" - "${CMAKE_CURRENT_SOURCE_DIR}/base/LieVector.h" - "${CMAKE_CURRENT_SOURCE_DIR}/base/LieScalar.h" - "${CMAKE_CURRENT_SOURCE_DIR}/base/deprecated/LieMatrix.h" - "${CMAKE_CURRENT_SOURCE_DIR}/base/deprecated/LieVector.h" - "${CMAKE_CURRENT_SOURCE_DIR}/base/deprecated/LieScalar.h" ) if(GTSAM_USE_QUATERNIONS) @@ -62,7 +56,6 @@ foreach(subdir ${gtsam_subdirs}) # Build convenience libraries file(GLOB_RECURSE subdir_srcs "${subdir}/*.cpp" "${subdir}/*.h") # Include header files so they show up in Visual Studio list(REMOVE_ITEM subdir_srcs ${excluded_sources}) - list(REMOVE_ITEM subdir_srcs ${excluded_headers}) file(GLOB subdir_test_files "${subdir}/tests/*") list(REMOVE_ITEM subdir_srcs ${subdir_test_files}) # Remove test files from sources compiled into library gtsam_assign_source_folders("${subdir_srcs}") # Create MSVC structure diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 8060ae7f4..235cd30f3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -299,7 +299,7 @@ GTSAM_EXPORT std::pair qr(const Matrix& A); * @param A is the input matrix, and is the output * @param clear_below_diagonal enables zeroing out below diagonal */ -GTSAM_EXPORT void inplace_QR(Matrix& A); +void inplace_QR(Matrix& A); /** * Imperative algorithm for in-place full elimination with diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp new file mode 100644 index 000000000..8c68bf8a0 --- /dev/null +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLieMatrix.cpp + * @author Richard Roberts + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(LieMatrix) +GTSAM_CONCEPT_LIE_INST(LieMatrix) + +/* ************************************************************************* */ +TEST( LieMatrix, construction ) { + Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); + LieMatrix lie1(m), lie2(m); + + EXPECT(traits::GetDimension(m) == 4); + EXPECT(assert_equal(m, lie1.matrix())); + EXPECT(assert_equal(lie1, lie2)); +} + +/* ************************************************************************* */ +TEST( LieMatrix, other_constructors ) { + Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0).finished(); + LieMatrix exp(init); + double data[] = {10,30,20,40}; + LieMatrix b(2,2,data); + EXPECT(assert_equal(exp, b)); +} + +/* ************************************************************************* */ +TEST(LieMatrix, retract) { + LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0).finished()); + Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); + + LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); + LieMatrix actual = traits::Retract(init,update); + + EXPECT(assert_equal(expected, actual)); + + Vector expectedUpdate = update; + Vector actualUpdate = traits::Local(init,actual); + + EXPECT(assert_equal(expectedUpdate, actualUpdate)); + + Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); + Vector actualLogmap = traits::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); + EXPECT(assert_equal(expectedLogmap, actualLogmap)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp new file mode 100644 index 000000000..74f5e0d41 --- /dev/null +++ b/gtsam/base/tests/testLieScalar.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLieScalar.cpp + * @author Kai Ni + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(LieScalar) +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(check_group_invariants(lie1, lie2)); + CHECK(check_manifold_invariants(lie1, lie2)); +} + +/* ************************************************************************* */ +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(traits::dimension == 1); + EXPECT(assert_equal(lie1, lie2)); +} + +/* ************************************************************************* */ +TEST( testLieScalar, localCoordinates ) { + LieScalar lie1(1.), lie2(3.); + + Vector1 actual = traits::Local(lie1, lie2); + EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp new file mode 100644 index 000000000..76c4fc490 --- /dev/null +++ b/gtsam/base/tests/testLieVector.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLieVector.cpp + * @author Alex Cunningham + */ + +#include +#include +#include +#include + +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); + + EXPECT(lie1.dim() == 3); + EXPECT(assert_equal(v, lie1.vector())); + 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); + EXPECT(assert_equal(exp, b)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index 3fd2e9003..305aa7ca9 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -15,10 +15,20 @@ */ #include +#include #include using namespace gtsam; +/* ************************************************************************* */ +TEST( testTestableAssertions, optional ) { + typedef boost::optional OptionalScalar; + LieScalar x(1.0); + OptionalScalar ox(x), dummy = boost::none; + EXPECT(assert_equal(ox, ox)); + EXPECT(assert_equal(x, ox)); + EXPECT(assert_equal(dummy, dummy)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }