diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 324ed111f..f804528a2 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -16,6 +16,19 @@ #include #include +// This is a helper to ease the transition to the new traits defined in this file. +// Uncomment this if you want all methods that are tagged as not implemented +// to cause compilation errors. +// #define COMPILE_ERROR_NOT_IMPLEMENTED + +#ifdef COMPILE_ERROR_NOT_IMPLEMENTED +#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \ +"This method is required by the new concepts framework but has not been implemented yet."); +#else +#define CONCEPT_NOT_IMPLEMENTED \ + throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet."); +#endif + namespace gtsam { diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index c9aab4185..a32f84269 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -110,6 +110,11 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { #endif } +/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose +Vector Pose2::localCoordinates(const Pose2& p2, OptionalJacobian<3,3> Hthis, OptionalJacobian<3,3> Hother) const { + CONCEPT_NOT_IMPLEMENTED; +} + /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 33782fdb4..7dc1add70 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -143,6 +143,9 @@ public: /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose Vector localCoordinates(const Pose2& p2) const; + /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose + Vector localCoordinates(const Pose2& p2, OptionalJacobian<3,3> Hthis, OptionalJacobian<3,3> Hother) const; + /// @} /// @name Lie Group /// @{ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 7fc9406e8..853d41643 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -154,10 +154,16 @@ public: Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; + /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose + Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; - /// @} + /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose + Vector6 localCoordinates(const Pose3& T2, + OptionalJacobian<6,6> Horigin, OptionalJacobian<6,6> Hother) const { + CONCEPT_NOT_IMPLEMENTED; + } + + /// @} /// @name Lie Group /// @{ diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 466d93203..54ab9b6c5 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -213,26 +213,10 @@ namespace imuBias { /// @} }; // ConstantBias class - - -} // namespace ImuBias - -// Define GTSAM traits -namespace traits { +} // namespace imuBias template<> -struct is_group : public boost::true_type { -}; - -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -} +struct traits_x : public internal::LieGroup {}; } // namespace gtsam