From 224b71d696bcba7703dd30450c9b5a2fa43ee7ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Oct 2014 12:54:28 +0200 Subject: [PATCH] Created testManifold --- .../nonlinear/tests/testAdaptAutoDiff.cpp | 102 ------------ tests/testManifold.cpp | 149 ++++++++++++++++++ 2 files changed, 149 insertions(+), 102 deletions(-) create mode 100644 tests/testManifold.cpp diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index d697a382f..397c91c5f 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -135,108 +135,6 @@ struct SnavelyProjection { }; -/* ************************************************************************* */ -// is_manifold -TEST(Manifold, _is_manifold) { - using namespace traits; - EXPECT(!is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); -} - -/* ************************************************************************* */ -// dimension -TEST(Manifold, _dimension) { - using namespace traits; - EXPECT_LONGS_EQUAL(2, dimension::value); - EXPECT_LONGS_EQUAL(8, dimension::value); - EXPECT_LONGS_EQUAL(1, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); -} - -/* ************************************************************************* */ -// charts -TEST(Manifold, DefaultChart) { - - DefaultChart chart1(Point2(0, 0)); - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); - - DefaultChart chart2(Vector2(0, 0)); - EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); - - DefaultChart chart3(0); - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); - - // Dynamic does not work yet ! -// Vector z = zero(2), v(2); -// v << 1, 0; -// DefaultChart chart4(z); -// EXPECT(chart4.apply(v)==v); -// EXPECT(chart4.retract(v)==v); -} - -/* ************************************************************************* */ -// zero -TEST(Manifold, _zero) { - EXPECT(assert_equal(Pose3(),traits::zero::value())); - Cal3Bundler cal(0,0,0); - EXPECT(assert_equal(cal,traits::zero::value())); - EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); -} - -/* ************************************************************************* */ -// charts -TEST(Manifold, Canonical) { - - Canonical chart1; - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); - - Canonical chart2; - EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0))); - EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); - - Canonical chart3; - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.apply(1)==v1); - EXPECT(chart3.retract(v1)==1); - - Canonical chart4; - Point3 point(1,2,3); - Vector3 v3(1,2,3); - EXPECT(assert_equal((Vector)chart4.apply(point),v3)); - EXPECT(assert_equal(chart4.retract(v3),point)); - - Canonical chart5; - Pose3 pose(Rot3::identity(),point); - Vector6 v6; v6 << 0,0,0,1,2,3; - EXPECT(assert_equal((Vector)chart5.apply(pose),v6)); - EXPECT(assert_equal(chart5.retract(v6),pose)); - - Canonical chart6; - Cal3Bundler cal0(0,0,0); - Camera camera(Pose3(),cal0); - Vector9 z9 = Vector9::Zero(); - EXPECT(assert_equal((Vector)chart6.apply(camera),z9)); - EXPECT(assert_equal(chart6.retract(z9),camera)); - - Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() - Camera camera2(pose,cal); - Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0; - EXPECT(assert_equal((Vector)chart6.apply(camera2),v9)); - EXPECT(assert_equal(chart6.retract(v9),camera2)); -} - /* ************************************************************************* */ // New-style numerical derivatives using manifold_traits template diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp new file mode 100644 index 000000000..e43cde102 --- /dev/null +++ b/tests/testManifold.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include +#include +#include + +#undef CHECK +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +/* ************************************************************************* */ +// is_manifold +TEST(Manifold, _is_manifold) { + using namespace traits; + EXPECT(!is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); +} + +/* ************************************************************************* */ +// dimension +TEST(Manifold, _dimension) { + using namespace traits; + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, DefaultChart) { + + DefaultChart chart1(Point2(0, 0)); + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + + DefaultChart chart2(Vector2(0, 0)); + EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + DefaultChart chart3(0); + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + // Dynamic does not work yet ! +// Vector z = zero(2), v(2); +// v << 1, 0; +// DefaultChart chart4(z); +// EXPECT(chart4.apply(v)==v); +// EXPECT(chart4.retract(v)==v); +} + +/* ************************************************************************* */ +// zero +TEST(Manifold, _zero) { + EXPECT(assert_equal(Pose3(),traits::zero::value())); + Cal3Bundler cal(0,0,0); + EXPECT(assert_equal(cal,traits::zero::value())); + EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, Canonical) { + + Canonical chart1; + EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + + Canonical chart2; + EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0))); + EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.retract(v1)==1); + + Canonical chart4; + Point3 point(1,2,3); + Vector3 v3(1,2,3); + EXPECT(assert_equal((Vector)chart4.apply(point),v3)); + EXPECT(assert_equal(chart4.retract(v3),point)); + + Canonical chart5; + Pose3 pose(Rot3::identity(),point); + Vector6 v6; v6 << 0,0,0,1,2,3; + EXPECT(assert_equal((Vector)chart5.apply(pose),v6)); + EXPECT(assert_equal(chart5.retract(v6),pose)); + + Canonical chart6; + Cal3Bundler cal0(0,0,0); + Camera camera(Pose3(),cal0); + Vector9 z9 = Vector9::Zero(); + EXPECT(assert_equal((Vector)chart6.apply(camera),z9)); + EXPECT(assert_equal(chart6.retract(z9),camera)); + + Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() + Camera camera2(pose,cal); + Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0; + EXPECT(assert_equal((Vector)chart6.apply(camera2),v9)); + EXPECT(assert_equal(chart6.retract(v9),camera2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +