diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 5b0009a7f..66fefc518 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -107,6 +107,8 @@ protected: T value_; ///< The wrapped value public: + // Only needed for serialization. + GenericValue(){} /// Construct from value GenericValue(const T& value) : @@ -244,7 +246,7 @@ public: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("GenericValue", - boost::serialization::base_object >(*this)); + boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("value", value_); } diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index c2c530429..e246a0a6c 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -363,11 +363,14 @@ struct traits_x : public internal::ScalarTraits {}; // traits for any double Eigen matrix template struct traits_x< Eigen::Matrix > { - BOOST_STATIC_ASSERT_MSG(M != Eigen::Dynamic && N != Eigen::Dynamic, - "This traits class only supports fixed-size matrices."); + BOOST_STATIC_ASSERT_MSG( + M != Eigen::Dynamic && N != Eigen::Dynamic, + "These traits are only valid on fixed-size types."); + // Typedefs required by all manifold types. typedef vector_space_tag structure_category; - enum { dimension = M * N }; + enum { dimension = (M == Eigen::Dynamic ? Eigen::Dynamic : + (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)) }; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix ManifoldType; @@ -462,7 +465,7 @@ struct traits_x< Eigen::Matrix > { template struct Canonical { BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::structure_category>::value), + (boost::is_base_of::structure_category>::value), "This type's trait does not assert it is a manifold (or derived)"); typedef traits_x Traits; typedef typename Traits::TangentVector TangentVector; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index cb3269e8b..65958ca0b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -488,6 +488,6 @@ private: template -struct traits_x< PinholeCamera > : public internal::Manifold > {}; +struct traits_x< PinholeCamera > : public internal::LieGroup > {}; } // \ gtsam diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 9767eba60..a078219fc 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -43,10 +43,6 @@ class AdaptAutoDiff { typedef typename Canonical1::TangentVector Vector1; typedef typename Canonical2::TangentVector Vector2; - // Instantiate function and charts - CanonicalT chartT; - Canonical1 chart1; - Canonical2 chart2; F f; public: @@ -57,8 +53,8 @@ public: using ceres::internal::AutoDiff; // Make arguments - Vector1 v1 = chart1.local(a1); - Vector2 v2 = chart2.local(a2); + Vector1 v1 = Canonical1::Local(a1); + Vector2 v2 = Canonical2::Local(a2); bool success; VectorT result; @@ -84,7 +80,7 @@ public: if (!success) throw std::runtime_error( "AdaptAutoDiff: function call resulted in failure"); - return chartT.retract(result); + return CanonicalT::Retract(result); } }; diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 3f39730ff..3e727b5da 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -46,23 +46,12 @@ template struct pack { typedef T type; }; } -#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \ - typedef gtsam::ChartValue > UNIQUE_NAME; \ - BOOST_CLASS_EXPORT( UNIQUE_NAME ); - /* ************************************************************************* */ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; -CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3); -CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2); -CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2); -CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler); - - - /* ************************************************************************* */ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 941728d8c..7f99c0c54 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -62,15 +62,6 @@ public: Vector localCoordinates(const TestValue&) const { return Vector(); } }; -namespace gtsam { -namespace traits { -template <> -struct is_manifold : public boost::true_type {}; -template <> -struct dimension : public boost::integral_constant {}; -} -} - /* ************************************************************************* */ TEST( Values, equals1 ) { @@ -170,9 +161,9 @@ TEST(Values, basic_functions) Values values; const Values& values_c = values; values.insert(2, Vector3()); - values.insert(4, Vector(3)); + values.insert(4, Vector3()); values.insert(6, Matrix23()); - values.insert(8, Matrix(2,3)); + values.insert(8, Matrix23()); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key);