diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index f907dd879..2aa151a14 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -18,8 +18,7 @@ #pragma once -#include -#include +#include #include #include @@ -28,33 +27,41 @@ namespace gtsam { -template struct traits_x; +namespace detail { +// By default, we assume an Identity element +template +struct Origin { T operator()() { return traits_x::Identity();} }; + +// but dimple manifolds don't have one, so we just use the default constructor +template +struct Origin { T operator()() { return T();} }; + +} // \ detail + +/** + * Canonical is a template that creates canonical coordinates for a given type. + * A simple manifold type (i.e., not a Lie Group) has no concept of identity, + * hence in that case we use the value given by the default constructor T() as + * the origin of a "canonical coordinate" parameterization. + */ template struct Canonical { + + GTSAM_CONCEPT_MANIFOLD_TYPE(T) + typedef traits_x Traits; - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), - "This type's trait does not assert it is a manifold (or derived)"); - typedef typename Traits::TangentVector TangentVector; enum { dimension = Traits::dimension }; - typedef OptionalJacobian ChartJacobian; + typedef typename Traits::TangentVector TangentVector; + typedef typename Traits::structure_category Category; + typedef detail::Origin Origin; static TangentVector Local(const T& other) { - return Traits::Local(Traits::Identity(), other); + return Traits::Local(Origin()(), other); } static T Retract(const TangentVector& v) { - return Traits::Retract(Traits::Identity(), v); - } - - static TangentVector Local(const T& other, ChartJacobian Hother) { - return Traits::Local(Traits::Identity(), other, boost::none, Hother); - } - - static T Retract(const T& origin, const TangentVector& v, - ChartJacobian Horigin, ChartJacobian Hv) { - return Traits::Retract(Traits::Identity(), v, boost::none, Hv); + return Traits::Retract(Origin()(), v); } }; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 93aaa94b0..0286e9845 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -33,15 +33,35 @@ using boost::assign::list_of; using boost::assign::map_list_of; +namespace gtsam { + +// Special version of Cal3Bundler so that default constructor = 0,0,0 +struct Cal: public Cal3Bundler { + Cal(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : + Cal3Bundler(f, k1, k2, u0, v0) { + } + Cal retract(const Vector& d) const { + return Cal(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + } + Vector3 localCoordinates(const Cal& T2) const { + return T2.vector() - vector(); + } +}; + +template<> +struct traits_x : public internal::Manifold {}; + +// With that, camera below behaves like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +} + using namespace std; using namespace gtsam; -// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector -typedef PinholeCamera Camera; - /* ************************************************************************* */ // charts -TEST(Manifold, Canonical) { +TEST(AdaptAutoDiff, Canonical) { Canonical chart1; EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0)); @@ -73,21 +93,17 @@ TEST(Manifold, Canonical) { EXPECT(assert_equal(v6, chart5.Local(pose))); EXPECT(assert_equal(chart5.Retract(v6), pose)); -#if 0 // TODO: Canonical should not require group? - Canonical chart6; - Cal3Bundler cal0(0, 0, 0); + Canonical chart6; + Cal cal0; + Vector z3 = Vector3::Zero(); + EXPECT(assert_equal(z3, chart6.Local(cal0))); + EXPECT(assert_equal(chart6.Retract(z3), cal0)); + + Canonical chart7; Camera camera(Pose3(), cal0); Vector z9 = Vector9::Zero(); - 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)); -#endif + EXPECT(assert_equal(z9, chart7.Local(camera))); + EXPECT(assert_equal(chart7.Retract(z9), camera)); } /* ************************************************************************* */ @@ -137,7 +153,7 @@ struct Projective { /* ************************************************************************* */ // Test Ceres AutoDiff -TEST(Expression, AutoDiff) { +TEST(AdaptAutoDiff, AutoDiff) { using ceres::internal::AutoDiff; // Instantiate function @@ -181,7 +197,7 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { throw std::runtime_error("Snavely fail"); } -TEST(Expression, AutoDiff2) { +TEST(AdaptAutoDiff, AutoDiff2) { using ceres::internal::AutoDiff; // Instantiate function @@ -212,12 +228,12 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } -/* ************************************************************************* * +/* ************************************************************************* */ // Test AutoDiff wrapper Snavely -TEST(Expression, AutoDiff3) { +TEST(AdaptAutoDiff, AutoDiff3) { // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0)); Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! typedef AdaptAutoDiff Adaptor; @@ -233,17 +249,17 @@ TEST(Expression, AutoDiff3) { Matrix E2 = numericalDerivative22(Adaptor(), P, X); // Get derivatives with AutoDiff, not gives RowMajor results! - OptionalJacobian<2,9> H1; - OptionalJacobian<2,3> H2; + Matrix29 H1; + Matrix23 H2; Point2 actual2 = snavely(P, X, H1, H2); EXPECT(assert_equal(expected,actual2,1e-9)); - EXPECT(assert_equal(E1,*H1,1e-8)); - EXPECT(assert_equal(E2,*H2,1e-8)); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); } -/* ************************************************************************* * +/* ************************************************************************* */ // Test AutoDiff wrapper in an expression -TEST(Expression, Snavely) { +TEST(AdaptAutoDiff, Snavely) { Expression P(1); Expression X(2); typedef AdaptAutoDiff Adaptor;