Even more slow progress

gtsam/base/concepts.h

@@ -16,6 +16,19 @@
 #include <boost/static_assert.hpp>
 #include <boost/type_traits/is_base_of.hpp>
 
+// 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 {
 
 

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)

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
   /// @{

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
     /// @{
 

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<imuBias::ConstantBias> : public boost::true_type {
-};
-
-template<>
-struct is_manifold<imuBias::ConstantBias> : public boost::true_type {
-};
-
-template<>
-struct dimension<imuBias::ConstantBias> : public boost::integral_constant<int, 6> {
-};
-
-}
+struct traits_x<imuBias::ConstantBias> : public internal::LieGroup<imuBias::ConstantBias> {};
 
 } // namespace gtsam