diff --git a/CMakeLists.txt b/CMakeLists.txt index 1880fedef..f58596520 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,8 +40,6 @@ option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) -option(GTSAM_LINK_BINARIES_AGAINST_CONVENIENCE_LIBS - "Enable/Disable linking tests against the convenience libraries for faster debugging" ON) # Add the Quaternion Build Flag if requested if (GTSAM_USE_QUATERNIONS) diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am deleted file mode 100644 index 625c57564..000000000 --- a/examples/vSLAMexample/Makefile.am +++ /dev/null @@ -1,39 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM Examples -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# Examples -noinst_PROGRAMS = vSFMexample -vSFMexample_SOURCES = vSFMexample.cpp vSLAMutils.cpp - -noinst_PROGRAMS += vISAMexample -vISAMexample_SOURCES = vISAMexample.cpp vSLAMutils.cpp - -headers += Feature2D.h vSLAMutils.h - -noinst_HEADERS = $(headers) - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) -LDADD = ../../gtsam/libgtsam.la -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./% - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ -#---------------------------------------------------------------------------------------------------- diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 296013062..8a28c8dad 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -15,7 +15,6 @@ endif (GTSAM_BUILD_CONVENIENCE_LIBRARIES) # exclude certain files # note the source dir on each set (tests_exclude - "${CMAKE_CURRENT_SOURCE_DIR}/testTupleValues.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp" ) diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp deleted file mode 100644 index d98afd5f6..000000000 --- a/tests/testTupleValues.cpp +++ /dev/null @@ -1,624 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testTupleValues.cpp - * @author Richard Roberts - * @author Alex Cunningham - */ - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -using namespace gtsam; -using namespace std; - -static const double tol = 1e-5; - -typedef TypedSymbol PoseKey; -typedef TypedSymbol PointKey; -typedef Values PoseValues; -typedef Values PointValues; -typedef TupleValues2 TestValues; - -/* ************************************************************************* */ -TEST( TupleValues, constructors ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues::Values1 cfg1; - cfg1.insert(PoseKey(1), x1); - cfg1.insert(PoseKey(2), x2); - TestValues::Values2 cfg2; - cfg2.insert(PointKey(1), l1); - cfg2.insert(PointKey(2), l2); - - TestValues actual(cfg1, cfg2), expected; - expected.insert(PoseKey(1), x1); - expected.insert(PoseKey(2), x2); - expected.insert(PointKey(1), l1); - expected.insert(PointKey(2), l2); - - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( TupleValues, insert_equals1 ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues expected; - expected.insert(PoseKey(1), x1); - expected.insert(PoseKey(2), x2); - expected.insert(PointKey(1), l1); - expected.insert(PointKey(2), l2); - - TestValues actual; - actual.insert(PoseKey(1), x1); - actual.insert(PoseKey(2), x2); - actual.insert(PointKey(1), l1); - actual.insert(PointKey(2), l2); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( TupleValues, insert_equals2 ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - TestValues values2; - values2.insert(PoseKey(1), x1); - values2.insert(PoseKey(2), x2); - values2.insert(PointKey(1), l1); - - CHECK(!values1.equals(values2)); - - values2.insert(2, Point2(9,11)); - - CHECK(!values1.equals(values2)); -} - -/* ************************************************************************* */ -TEST( TupleValues, insert_duplicate ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(1, x1); // 3 - values1.insert(2, x2); // 6 - values1.insert(1, l1); // 8 - values1.insert(2, l2); // 10 - CHECK_EXCEPTION(values1.insert(2, l1), KeyAlreadyExists); // still 10 !!!! -} - -/* ************************************************************************* */ -TEST( TupleValues, size_dim ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - EXPECT(values1.size() == 4); - EXPECT(values1.dim() == 10); -} - -/* ************************************************************************* */ -TEST(TupleValues, at) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - EXPECT(assert_equal(x1, values1[PoseKey(1)])); - EXPECT(assert_equal(x2, values1[PoseKey(2)])); - EXPECT(assert_equal(l1, values1[PointKey(1)])); - EXPECT(assert_equal(l2, values1[PointKey(2)])); - - CHECK_EXCEPTION(values1[PoseKey(3)], KeyDoesNotExist); - CHECK_EXCEPTION(values1[PointKey(3)], KeyDoesNotExist); -} - -/* ************************************************************************* */ -TEST(TupleValues, zero_expmap_logmap) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - Ordering o; o += "x1", "x2", "l1", "l2"; - VectorValues expected_zero(values1.dims(o)); - expected_zero[o["x1"]] = zero(3); - expected_zero[o["x2"]] = zero(3); - expected_zero[o["l1"]] = zero(2); - expected_zero[o["l2"]] = zero(2); - - CHECK(assert_equal(expected_zero, values1.zero(o))); - - VectorValues delta(values1.dims(o)); - delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2); - delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5); - delta[o["l1"]] = Vector_(2, 1.0, 1.1); - delta[o["l2"]] = Vector_(2, 1.3, 1.4); - - TestValues expected; - expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(PoseKey(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5))); - expected.insert(PointKey(1), Point2(5.0, 6.1)); - expected.insert(PointKey(2), Point2(10.3, 11.4)); - - TestValues actual = values1.retract(delta, o); - CHECK(assert_equal(expected, actual)); - - // Check log - VectorValues expected_log = delta; - VectorValues actual_log = values1.localCoordinates(actual, o); - CHECK(assert_equal(expected_log, actual_log)); -} - -/* ************************************************************************* */ - -// some key types -typedef TypedSymbol LamKey; -typedef TypedSymbol Pose3Key; -typedef TypedSymbol Point3Key; -typedef TypedSymbol Point3Key2; - -// some values types -typedef Values PoseValues; -typedef Values PointValues; -typedef Values LamValues; -typedef Values Pose3Values; -typedef Values Point3Values; -typedef Values Point3Values2; - -// some TupleValues types -typedef TupleValues > ValuesA; -typedef TupleValues > > ValuesB; - -typedef TupleValues1 TuplePoseValues; -typedef TupleValues1 TuplePointValues; -typedef TupleValues2 SimpleValues; - -/* ************************************************************************* */ -TEST(TupleValues, slicing) { - PointKey l1(1), l2(2); - Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0); - PoseKey x1(1), x2(2); - Pose2 x1_val(1.0, 2.0, 0.3), x2_val(3.0, 4.0, 0.4); - - PoseValues liePoseValues; - liePoseValues.insert(x1, x1_val); - liePoseValues.insert(x2, x2_val); - - PointValues liePointValues; - liePointValues.insert(l1, l1_val); - liePointValues.insert(l2, l2_val); - - // construct TupleValues1 from the base values - TuplePoseValues tupPoseValues1(liePoseValues); - EXPECT(assert_equal(liePoseValues, tupPoseValues1.first(), tol)); - - TuplePointValues tupPointValues1(liePointValues); - EXPECT(assert_equal(liePointValues, tupPointValues1.first(), tol)); - -// // construct a TupleValues2 from a TupleValues1 -// SimpleValues pairValues1(tupPoseValues1); -// EXPECT(assert_equal(liePoseValues, pairValues1.first(), tol)); -// EXPECT(pairValues1.second().empty()); -// -// SimpleValues pairValues2(tupPointValues1); -// EXPECT(assert_equal(liePointValues, pairValues2.second(), tol)); -// EXPECT(pairValues1.first().empty()); - -} - -/* ************************************************************************* */ -TEST(TupleValues, basic_functions) { - // create some tuple values - ValuesA valuesA; - ValuesB valuesB; - - PoseKey x1(1); - PointKey l1(1); - LamKey L1(1); - Pose2 pose1(1.0, 2.0, 0.3); - Point2 point1(2.0, 3.0); - LieVector lam1 = LieVector(2.3); - - // Insert - valuesA.insert(x1, pose1); - valuesA.insert(l1, point1); - - valuesB.insert(x1, pose1); - valuesB.insert(l1, point1); - valuesB.insert(L1, lam1); - - // bracket operator - EXPECT(assert_equal(valuesA[x1], pose1)); - EXPECT(assert_equal(valuesA[l1], point1)); - EXPECT(assert_equal(valuesB[x1], pose1)); - EXPECT(assert_equal(valuesB[l1], point1)); - EXPECT(assert_equal(valuesB[L1], lam1)); - - // exists - EXPECT(valuesA.exists(x1)); - EXPECT(valuesA.exists(l1)); - EXPECT(valuesB.exists(x1)); - EXPECT(valuesB.exists(l1)); - EXPECT(valuesB.exists(L1)); - - // at - EXPECT(assert_equal(valuesA.at(x1), pose1)); - EXPECT(assert_equal(valuesA.at(l1), point1)); - EXPECT(assert_equal(valuesB.at(x1), pose1)); - EXPECT(assert_equal(valuesB.at(l1), point1)); - EXPECT(assert_equal(valuesB.at(L1), lam1)); - - // size - EXPECT(valuesA.size() == 2); - EXPECT(valuesB.size() == 3); - - // dim - EXPECT(valuesA.dim() == 5); - EXPECT(valuesB.dim() == 6); - - // erase - valuesA.erase(x1); - CHECK(!valuesA.exists(x1)); - CHECK(valuesA.size() == 1); - valuesA.erase(l1); - CHECK(!valuesA.exists(l1)); - CHECK(valuesA.size() == 0); - valuesB.erase(L1); - CHECK(!valuesB.exists(L1)); - CHECK(valuesB.size() == 2); - - // clear - valuesA.clear(); - EXPECT(valuesA.size() == 0); - valuesB.clear(); - EXPECT(valuesB.size() == 0); - - // empty - EXPECT(valuesA.empty()); - EXPECT(valuesB.empty()); -} - -/* ************************************************************************* */ -TEST(TupleValues, insert_values) { - ValuesB values1, values2, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - LamKey L1(1), L2(2); - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - Point2 point1(2.0, 3.0), point2(5.0, 6.0); - LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5); - - values1.insert(x1, pose1); - values1.insert(l1, point1); - values1.insert(L1, lam1); - - values2.insert(x2, pose2); - values2.insert(l2, point2); - values2.insert(L2, lam2); - - values1.insert(values2); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(L1, lam1); - expected.insert(x2, pose2); - expected.insert(l2, point2); - expected.insert(L2, lam2); - - CHECK(assert_equal(expected, values1)); -} - -/* ************************************************************************* */ -TEST( TupleValues, update_element ) -{ - TupleValues2 cfg; - Pose2 x1(2.0, 1.0, 2.0), x2(3.0, 4.0, 5.0); - Point2 l1(1.0, 2.0), l2(3.0, 4.0); - PoseKey xk(1); - PointKey lk(1); - - cfg.insert(xk, x1); - CHECK(cfg.size() == 1); - CHECK(assert_equal(x1, cfg.at(xk))); - - cfg.update(xk, x2); - CHECK(cfg.size() == 1); - CHECK(assert_equal(x2, cfg.at(xk))); - - cfg.insert(lk, l1); - CHECK(cfg.size() == 2); - CHECK(assert_equal(l1, cfg.at(lk))); - - cfg.update(lk, l2); - CHECK(cfg.size() == 2); - CHECK(assert_equal(l2, cfg.at(lk))); -} - -/* ************************************************************************* */ -TEST( TupleValues, equals ) -{ - Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7); - PoseKey x1k(1), x2k(2); - Point2 l1(4,5), l2(9,10); - PointKey l1k(1), l2k(2); - - ValuesA values1, values2, values3, values4, values5; - - values1.insert(x1k, x1); - values1.insert(x2k, x2); - values1.insert(l1k, l1); - values1.insert(l2k, l2); - - values2.insert(x1k, x1); - values2.insert(x2k, x2); - values2.insert(l1k, l1); - values2.insert(l2k, l2); - - values3.insert(x2k, x2); - values3.insert(l1k, l1); - - values4.insert(x1k, x1); - values4.insert(x2k, x2_alt); - values4.insert(l1k, l1); - values4.insert(l2k, l2); - - ValuesA values6(values1); - - EXPECT(assert_equal(values1,values2)); - EXPECT(assert_equal(values1,values1)); - EXPECT(assert_inequal(values1,values3)); - EXPECT(assert_inequal(values1,values4)); - EXPECT(assert_inequal(values1,values5)); - EXPECT(assert_equal(values1, values6)); -} - -/* ************************************************************************* */ -TEST(TupleValues, expmap) -{ - Pose2 x1(1,2,3), x2(6,7,8); - PoseKey x1k(1), x2k(2); - Point2 l1(4,5), l2(9,10); - PointKey l1k(1), l2k(2); - - Ordering o; o += "x1", "x2", "l1", "l2"; - - ValuesA values1; - values1.insert(x1k, x1); - values1.insert(x2k, x2); - values1.insert(l1k, l1); - values1.insert(l2k, l2); - - VectorValues delta(values1.dims(o)); - delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2); - delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5); - delta[o["l1"]] = Vector_(2, 1.0, 1.1); - delta[o["l2"]] = Vector_(2, 1.3, 1.4); - - ValuesA expected; - expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5))); - expected.insert(l1k, Point2(5.0, 6.1)); - expected.insert(l2k, Point2(10.3, 11.4)); - - CHECK(assert_equal(expected, values1.retract(delta, o))); - CHECK(assert_equal(delta, values1.localCoordinates(expected, o))); -} - -/* ************************************************************************* */ -TEST(TupleValues, expmap_typedefs) -{ - Pose2 x1(1,2,3), x2(6,7,8); - PoseKey x1k(1), x2k(2); - Point2 l1(4,5), l2(9,10); - PointKey l1k(1), l2k(2); - - Ordering o; o += "x1", "x2", "l1", "l2"; - - TupleValues2 values1, expected, actual; - values1.insert(x1k, x1); - values1.insert(x2k, x2); - values1.insert(l1k, l1); - values1.insert(l2k, l2); - - VectorValues delta(values1.dims(o)); - delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2); - delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5); - delta[o["l1"]] = Vector_(2, 1.0, 1.1); - delta[o["l2"]] = Vector_(2, 1.3, 1.4); - - expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.retract(Vector_(3, 1.3, 1.4, 1.5))); - expected.insert(l1k, Point2(5.0, 6.1)); - expected.insert(l2k, Point2(10.3, 11.4)); - - CHECK(assert_equal(expected, TupleValues2(values1.retract(delta, o)))); - //CHECK(assert_equal(delta, values1.localCoordinates(expected))); -} - -/* ************************************************************************* */ -TEST(TupleValues, typedefs) -{ - TupleValues2 values1; - TupleValues3 values2; - TupleValues4 values3; - TupleValues5 values4; - TupleValues6 values5; -} - -/* ************************************************************************* */ -TEST( TupleValues, pairvalues_style ) -{ - PoseKey x1(1); - PointKey l1(1); - LamKey L1(1); - Pose2 pose1(1.0, 2.0, 0.3); - Point2 point1(2.0, 3.0); - LieVector lam1 = LieVector(2.3); - - PoseValues values1; values1.insert(x1, pose1); - PointValues values2; values2.insert(l1, point1); - LamValues values3; values3.insert(L1, lam1); - - // Constructor - TupleValues3 values(values1, values2, values3); - - // access - CHECK(assert_equal(values1, values.first())); - CHECK(assert_equal(values2, values.second())); - CHECK(assert_equal(values3, values.third())); -} - -/* ************************************************************************* */ -TEST(TupleValues, insert_values_typedef) { - - TupleValues4 values1, values2, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - LamKey L1(1), L2(2); - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - Point2 point1(2.0, 3.0), point2(5.0, 6.0); - LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5); - - values1.insert(x1, pose1); - values1.insert(l1, point1); - values1.insert(L1, lam1); - - values2.insert(x2, pose2); - values2.insert(l2, point2); - values2.insert(L2, lam2); - - values1.insert(values2); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(L1, lam1); - expected.insert(x2, pose2); - expected.insert(l2, point2); - expected.insert(L2, lam2); - - CHECK(assert_equal(expected, values1)); -} - -/* ************************************************************************* */ -TEST(TupleValues, partial_insert) { - TupleValues3 init, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - LamKey L1(1), L2(2); - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - Point2 point1(2.0, 3.0), point2(5.0, 6.0); - LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5); - - init.insert(x1, pose1); - init.insert(l1, point1); - init.insert(L1, lam1); - - PoseValues cfg1; - cfg1.insert(x2, pose2); - - init.insertSub(cfg1); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(L1, lam1); - expected.insert(x2, pose2); - - CHECK(assert_equal(expected, init)); -} - -/* ************************************************************************* */ -TEST(TupleValues, update) { - TupleValues3 init, superset, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - Point2 point1(2.0, 3.0), point2(5.0, 6.0); - - init.insert(x1, pose1); - init.insert(l1, point1); - - - Pose2 pose1_(1.0, 2.0, 0.4); - Point2 point1_(2.0, 4.0); - superset.insert(x1, pose1_); - superset.insert(l1, point1_); - superset.insert(x2, pose2); - superset.insert(l2, point2); - init.update(superset); - - expected.insert(x1, pose1_); - expected.insert(l1, point1_); - - CHECK(assert_equal(expected, init)); -} - -/* ************************************************************************* */ -TEST(TupleValues, arbitrary_ordering ) { - TupleValues1 values; - PoseKey x1(1), x2(2); - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - values.insert(x1, pose1); - values.insert(x2, pose2); - Ordering::shared_ptr actual = values.orderingArbitrary(); - Ordering expected; expected += x1, x2; - EXPECT(assert_equal(expected, *actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */