From 263d4e163c017302891be7e3181c147baa08fd93 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Wed, 17 Dec 2014 22:53:56 +0100 Subject: [PATCH] more progress, need to fix testSO3.cpp and testManifold.cpp --- gtsam/base/LieVector.h | 3 + gtsam/geometry/Rot2.h | 3 +- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/geometry/SO3.h | 4 + gtsam/geometry/tests/testPose3.cpp | 10 +- gtsam/geometry/tests/testRot3.cpp | 11 +- gtsam/geometry/tests/testSO3.cpp | 12 +- gtsam/geometry/tests/testStereoPoint2.cpp | 9 +- gtsam/geometry/tests/testUnit3.cpp | 4 +- tests/testManifold.cpp | 150 +++++++++++----------- 11 files changed, 114 insertions(+), 96 deletions(-) diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index b5e21b777..36e020bcb 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -39,6 +39,9 @@ struct LieVector : public Vector { /** initialize from a normal vector */ LieVector(const Vector& v) : Vector(v) {} + template + LieVector(const V& v) : Vector(v) {} + // Currently TMP constructor causes ICE on MSVS 2013 #if (_MSC_VER < 1800) /** initialize from a fixed size normal vector */ diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index aeaf88146..c1dc93b22 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -111,7 +111,8 @@ namespace gtsam { inline static Rot2 identity() { return Rot2(); } /** The inverse rotation - negative angle */ - Rot2 inverse() const { + Rot2 inverse(OptionalJacobian<1,1> H = boost::none) const { + if (H) *H = -I_1x1; return Rot2(c_, -s_); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 68095810c..9e99c4b82 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -213,7 +213,7 @@ namespace gtsam { } /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; + Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index fc3490fb5..84a8135c5 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -158,7 +158,7 @@ Matrix3 Rot3::transpose() const { } /* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { +Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -rot_; return Rot3(Matrix3(transpose())); } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index c8770fe3b..df66ad2f1 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -35,6 +35,7 @@ class SO3: public Matrix3 { protected: public: + enum { dimension=3 }; /// Constructor from Eigen Matrix template @@ -48,6 +49,9 @@ public: } }; +template<> +struct traits_x : public internal::LieGroup {}; + } // end namespace gtsam diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 52f721f41..798836005 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -683,14 +683,14 @@ Vector testDerivExpmapInv(const Vector6& dxi) { TEST( Pose3, dExpInv_TLN) { Matrix res = Pose3::dExpInv_exp(xi); - Matrix numericalDerivExpmapInv = numericalDerivative11( + Matrix numericalDerivExpmapInv = numericalDerivative11( testDerivExpmapInv, Vector6::Zero(), 1e-5); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { +Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi) * v; } @@ -700,7 +700,7 @@ TEST( Pose3, adjoint) { Matrix actualH; Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); - Matrix numericalH = numericalDerivative21( + Matrix numericalH = numericalDerivative21( testDerivAdjoint, screw::xi, screw::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); @@ -708,7 +708,7 @@ TEST( Pose3, adjoint) { } /* ************************************************************************* */ -Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { +Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi).transpose() * v; } @@ -720,7 +720,7 @@ TEST( Pose3, adjointTranspose) { Matrix actualH; Vector actual = Pose3::adjointTranspose(xi, v, actualH); - Matrix numericalH = numericalDerivative21( + Matrix numericalH = numericalDerivative21( testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c89d2dacb..4c0333f69 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +//#include #include @@ -43,7 +43,6 @@ TEST( Rot3, chart) { Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished(); Rot3 rot3(R); - CHECK_CHART_CONCEPT(rot3); } /* ************************************************************************* */ @@ -148,10 +147,10 @@ TEST( Rot3, retract) Vector v = zero(3); CHECK(assert_equal(R, R.retract(v))); - // test Canonical coordinates - Canonical chart; - Vector v2 = chart.local(R); - CHECK(assert_equal(R, chart.retract(v2))); +// // test Canonical coordinates +// Canonical chart; +// Vector v2 = chart.local(R); +// CHECK(assert_equal(R, chart.retract(v2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index b7c488ed8..03e3654e4 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -23,6 +23,7 @@ using namespace gtsam; typedef OptionalJacobian<3,3> SO3Jacobian; +#if 0 //****************************************************************************** TEST(SO3 , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -47,10 +48,9 @@ TEST(SO3 , Local) { Vector3 z_axis(0, 0, 1); SO3 q1(Eigen::AngleAxisd(0, z_axis)); SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); - typedef manifold::traits::DefaultChart::type Chart; SO3Jacobian H1,H2; Vector3 expected(0, 0, 0.1); - Vector3 actual = Chart::Local(q1, q2, H1, H2); + Vector3 actual = traits_x::Local(q1, q2, H1, H2); EXPECT(assert_equal((Vector)expected,actual)); } @@ -59,22 +59,26 @@ TEST(SO3 , Retract) { Vector3 z_axis(0, 0, 1); SO3 q(Eigen::AngleAxisd(0, z_axis)); SO3 expected(Eigen::AngleAxisd(0.1, z_axis)); - typedef manifold::traits::DefaultChart::type Chart; Vector3 v(0, 0, 0.1); - SO3 actual = Chart::Retract(q, v); + SO3 actual = traits_x::Retract(q, v); EXPECT(actual.isApprox(expected)); } +#endif + //****************************************************************************** TEST(SO3 , Compose) { + EXPECT(false); } //****************************************************************************** TEST(SO3 , Between) { + EXPECT(false); } //****************************************************************************** TEST(SO3 , Inverse) { + EXPECT(false); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 8e504ba0e..5496b8aac 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -26,9 +26,14 @@ using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) -GTSAM_CONCEPT_LIE_INST(StereoPoint2) +//GTSAM_CONCEPT_LIE_INST(StereoPoint2) +//****************************************************************************** +TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsManifold)); +} + /* ************************************************************************* */ TEST(StereoPoint2, constructor) { StereoPoint2 p1(1, 2, 3), p2 = p1; @@ -49,7 +54,7 @@ TEST(StereoPoint2, Lie) { } /* ************************************************************************* */ -TEST( StereoPoint2, expmap) { +TEST( StereoPoint2, retract) { Vector d(3); d(0) = 1; d(1) = -1; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 072f3b7ad..1d0c28ded 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -115,13 +115,13 @@ TEST(Unit3, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 1927ba5c6..66ef80db3 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -40,165 +40,167 @@ 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); +TEST(Manifold, IsManifold) { + + //BOOST_CONCEPT_ASSERT((IsManifold)); // integer is not a manifold + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + // dynamic not working yet + //BOOST_CONCEPT_ASSERT((IsManifold)); + //BOOST_CONCEPT_ASSERT((IsManifold)); + } /* ************************************************************************* */ // 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); + //using namespace traits; + EXPECT_LONGS_EQUAL(2, traits_x::dimension); + EXPECT_LONGS_EQUAL(8, traits_x::dimension); + EXPECT_LONGS_EQUAL(1, traits_x::dimension); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x::dimension); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x::dimension); } /* ************************************************************************* */ // charts TEST(Manifold, DefaultChart) { - DefaultChart chart1; - EXPECT(chart1.local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); - EXPECT(chart1.retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); + //DefaultChart chart1; + EXPECT(traits_x::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); + EXPECT(traits_x::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); Vector v2(2); v2 << 1, 0; - DefaultChart chart2; - EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); - EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); + //DefaultChart chart2; + EXPECT(assert_equal(v2, traits_x::Local(Vector2(0, 0), Vector2(1, 0)))); + EXPECT(traits_x::Retract(Vector2(0, 0), v2) == Vector2(1, 0)); { typedef Matrix2 ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1, 3, 2, 4; // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! - EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m); + EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits_x::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits_x::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m); } { typedef Eigen::Matrix ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1, 2; - EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); + EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits_x::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits_x::Retract(m, Vector2(1, 2)) == 2 * m); } { typedef Eigen::Matrix ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1; - EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m); + EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits_x::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits_x::Retract(m, ManifoldPoint::Ones()) == 2 * m); } - DefaultChart chart3; + //DefaultChart chart3; Vector v1(1); v1 << 1; - EXPECT(assert_equal(v1, chart3.local(0, 1))); - EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); + EXPECT(assert_equal(v1, traits_x::Local(0, 1))); + EXPECT_DOUBLES_EQUAL(traits_x::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! Vector z = zero(2), v(2); v << 1, 0; - DefaultChart chart4; - EXPECT(assert_equal(chart4.local(z, v), v)); - EXPECT(assert_equal(chart4.retract(z, v), v)); + //DefaultChart chart4; + EXPECT(assert_equal(traits_x::Local(z, v), v)); + EXPECT(assert_equal(traits_x::Retract(z, v), v)); Vector v3(3); v3 << 1, 1, 1; Rot3 I = Rot3::identity(); Rot3 R = I.retract(v3); - DefaultChart chart5; - EXPECT(assert_equal(v3, chart5.local(I, R))); - EXPECT(assert_equal(chart5.retract(I, v3), R)); + //DefaultChart chart5; + EXPECT(assert_equal(v3, traits_x::Local(I, R))); + EXPECT(assert_equal(traits_x::Retract(I, v3), R)); // Check zero vector - DefaultChart chart6; - EXPECT(assert_equal(zero(3), chart6.local(R, R))); + //DefaultChart chart6; + EXPECT(assert_equal(zero(3), traits_x::Local(R, R))); } /* ************************************************************************* */ // 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())); - EXPECT(assert_equal(Point2(), traits::zero::value())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); - EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); -} - -/* ************************************************************************* */ -// identity -TEST(Manifold, _identity) { - EXPECT(assert_equal(Pose3(), traits::identity::value())); - EXPECT(assert_equal(Cal3Bundler(), traits::identity::value())); - EXPECT(assert_equal(Camera(), traits::identity::value())); - EXPECT(assert_equal(Point2(), traits::identity::value())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity::value()))); - EXPECT_DOUBLES_EQUAL(0.0, traits::identity::value(), 0.0); -} +//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())); +// EXPECT(assert_equal(Point2(), traits::zero::value())); +// EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); +// EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); +//} +// +///* ************************************************************************* */ +//// identity +//TEST(Manifold, _identity) { +// EXPECT(assert_equal(Pose3(), traits::identity::value())); +// EXPECT(assert_equal(Cal3Bundler(), traits::identity::value())); +// EXPECT(assert_equal(Camera(), traits::identity::value())); +// EXPECT(assert_equal(Point2(), traits::identity::value())); +// EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity::value()))); +// EXPECT_DOUBLES_EQUAL(0.0, traits::identity::value(), 0.0); +//} /* ************************************************************************* */ // charts TEST(Manifold, Canonical) { Canonical chart1; - EXPECT(chart1.local(Point2(1, 0))==Vector2(1, 0)); - EXPECT(chart1.retract(Vector2(1, 0))==Point2(1, 0)); + EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0)); + EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0)); Vector v2(2); v2 << 1, 0; Canonical chart2; - EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0)))); - EXPECT(chart2.retract(v2)==Vector2(1, 0)); + EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0)))); + EXPECT(chart2.Retract(v2)==Vector2(1, 0)); Canonical chart3; Eigen::Matrix v1; v1 << 1; - EXPECT(chart3.local(1)==v1); - EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); + EXPECT(chart3.Local(1)==v1); + EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9); Canonical chart4; Point3 point(1, 2, 3); Vector v3(3); v3 << 1, 2, 3; - EXPECT(assert_equal(v3, chart4.local(point))); - EXPECT(assert_equal(chart4.retract(v3), point)); + EXPECT(assert_equal(v3, chart4.Local(point))); + EXPECT(assert_equal(chart4.Retract(v3), point)); Canonical chart5; Pose3 pose(Rot3::identity(), point); Vector v6(6); v6 << 0, 0, 0, 1, 2, 3; - EXPECT(assert_equal(v6, chart5.local(pose))); - EXPECT(assert_equal(chart5.retract(v6), pose)); + EXPECT(assert_equal(v6, chart5.Local(pose))); + EXPECT(assert_equal(chart5.Retract(v6), pose)); Canonical chart6; Cal3Bundler cal0(0, 0, 0); Camera camera(Pose3(), cal0); Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9, chart6.local(camera))); - EXPECT(assert_equal(chart6.retract(z9), camera)); + EXPECT(assert_equal(z9, chart6.Local(camera))); + EXPECT(assert_equal(chart6.Retract(z9), camera)); Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() Camera camera2(pose, cal); Vector v9(9); v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; - EXPECT(assert_equal(v9, chart6.local(camera2))); - EXPECT(assert_equal(chart6.retract(v9), camera2)); + EXPECT(assert_equal(v9, chart6.Local(camera2))); + EXPECT(assert_equal(chart6.Retract(v9), camera2)); } /* ************************************************************************* */