mostly ManifoldTemplate cleaner scheme hourra

liuzhenboo · Jul 29, 2018 · b61b9e6 · b61b9e6
1 parent 1b7383f
commit b61b9e6
Show file tree

Hide file tree

Showing 19 changed files with 100 additions and 86 deletions.
diff --git a/examples/se2_smooth_trajectory.cpp b/examples/se2_smooth_trajectory.cpp
@@ -25,10 +25,10 @@ class ConstraintSmoothness
   using JacobianMap = typename internal::traits_ceres<Manifold>::ConstraintJacobianMap;
 
   template <typename _Scalar>
-  using ManifoldTemplate = typename _Manifold::template ManifoldTemplate<_Scalar>;
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   template <typename _Scalar>
-  using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
   static constexpr int DoF = Manifold::DoF;
   static constexpr int RepSize = Manifold::RepSize;
@@ -170,10 +170,10 @@ class ConstraintVelocity
   using JacobianMap = typename internal::traits_ceres<Manifold>::ConstraintJacobianMap;
 
   template <typename _Scalar>
-  using ManifoldTemplate = typename _Manifold::template ManifoldTemplate<_Scalar>;
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   template <typename _Scalar>
-  using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
   static constexpr int DoF = Manifold::DoF;
   static constexpr int RepSize = Manifold::RepSize;
@@ -266,10 +266,10 @@ class ConstraintAcceleration
   using JacobianMap = typename internal::traits_ceres<Manifold>::ConstraintJacobianMap;
 
   template <typename _Scalar>
-  using ManifoldTemplate = typename _Manifold::template ManifoldTemplate<_Scalar>;
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   template <typename _Scalar>
-  using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
   static constexpr int DoF = Manifold::DoF;
   static constexpr int RepSize = Manifold::RepSize;

diff --git a/include/manif/algorithms/interpolation.h b/include/manif/algorithms/interpolation.h
@@ -3,7 +3,7 @@
 
 #include "manif/impl/manifold_base.h"
 
-#include <iostream>
+//#include <iostream>
 
 namespace manif
 {
@@ -252,9 +252,9 @@ interpolate_smooth(const ManifoldBase<_Derived>& ma,
 
   if (std::abs(phi - phis) > 1e-8)
   {
-    std::cout << "phi : " << phi << "\n";
-    std::cout << "smoothing_phi : " << phis << "\n";
-    std::cout << "abs diff : " << std::abs(phi - phis) << "\n";
+//    std::cout << "phi : " << phi << "\n";
+//    std::cout << "smoothing_phi : " << phis << "\n";
+//    std::cout << "abs diff : " << std::abs(phi - phis) << "\n";
     MANIF_THROW("PHI AINT EQUAL !");
   }
 

diff --git a/include/manif/ceres/ceres_traits.h b/include/manif/ceres/ceres_traits.h
@@ -2,7 +2,7 @@
 #define _MANIF_MANIF_CERES_TRAITS_H_
 
 #include "manif/impl/manifold_base.h"
-#include <Eigen/Core>
+#include "manif/impl/eigen.h"
 
 namespace manif
 {

diff --git a/include/manif/ceres/constraint.h b/include/manif/ceres/constraint.h
@@ -19,10 +19,10 @@ class Constraint
   using JacobianMap = typename internal::traits_ceres<Manifold>::ConstraintJacobianMap;
 
   template <typename _Scalar>
-  using ManifoldTemplate = typename _Manifold::template ManifoldTemplate<_Scalar>;
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   template <typename _Scalar>
-  using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
   static constexpr int DoF = Manifold::DoF;
   static constexpr int RepSize = Manifold::RepSize;

diff --git a/include/manif/ceres/local_parametrization.h b/include/manif/ceres/local_parametrization.h
@@ -18,12 +18,10 @@ class LocalParameterization
   using JacobianMap = typename internal::traits_ceres<Manifold>::LocalParamJacobianMap;
 
   template <typename _Scalar>
-  using ManifoldTemplate =
-  typename _Manifold::template ManifoldTemplate<_Scalar>;
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   template <typename _Scalar>
-  using TangentTemplate =
-  typename Tangent::template TangentTemplate<_Scalar>;
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
 public:
 

diff --git a/include/manif/ceres/objective.h b/include/manif/ceres/objective.h
@@ -18,10 +18,10 @@ class Objective : public ceres::CostFunction
   using JacobianMap = typename internal::traits_ceres<Manifold>::ObjectiveJacobianMap;
 
   template <typename _Scalar>
-  using ManifoldTemplate = typename _Manifold::template ManifoldTemplate<_Scalar>;
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   template <typename _Scalar>
-  using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
 public:
 

diff --git a/include/manif/impl/eigen_checks.h → include/manif/impl/eigen.h b/include/manif/impl/eigen_checks.h → include/manif/impl/eigen.h
@@ -1,5 +1,8 @@
-#ifndef _EIGEN_CHECKS_H_
-#define _EIGEN_CHECKS_H_
+#ifndef _MANIF_MANIF_EIGEN_H_
+#define _MANIF_MANIF_EIGEN_H_
+
+
+#include <Eigen/Core>
 
 /**
  * @note static_cast<int> to avoid -Wno-enum-compare
@@ -97,4 +100,44 @@
   assert_is_colmajor_vector(x); \
   assert_cols_dim(x, dim);
 
-#endif /* _EIGEN_CHECKS_H_ */
+namespace manif {
+namespace internal {
+
+/**
+ * @brief traitscast specialization that come handy when writing thing like
+ * using Matrix3f = typename traitscast<Matrix3d, float>::cast;
+ */
+template <typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols, typename NewScalar>
+struct traitscast<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, NewScalar>
+{
+  using cast = Eigen::Matrix<NewScalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>;
+};
+
+} /* namespace internal */
+
+template <typename _Scalar>
+Eigen::Matrix<_Scalar, 2, 2>
+skew2(const _Scalar v)
+{
+  return (Eigen::Matrix<_Scalar, 2, 2>() <<
+             _Scalar(0.), -v,
+             v, _Scalar(0.) ).finished();
+}
+
+template <typename _Derived>
+Eigen::Matrix<typename _Derived::Scalar, 3, 3>
+skew3(const Eigen::MatrixBase<_Derived>& v)
+{
+  assert_vector_dim(v, 3);
+
+  using T = typename _Derived::Scalar;
+
+  return (Eigen::Matrix<T, 3, 3>() <<
+             T(0.),  -v(2),   +v(1),
+            +v(2),    T(0.),  -v(0),
+            -v(1),   +v(0),    T(0.) ).finished();
+}
+
+} /* namespace manif */
+
+#endif /* _MANIF_MANIF_EIGEN_H_ */
diff --git a/include/manif/impl/manifold_base.h b/include/manif/impl/manifold_base.h
@@ -31,10 +31,8 @@ struct ManifoldBase
 
   using OptJacobianRef = tl::optional<Jacobian&>;
 
-  /// @todo find something sexier
-  template <typename T>
-  using ManifoldTemplate =
-  typename internal::traits<_Derived>::template ManifoldTemplate<T>;
+  template <typename _Scalar>
+  using ManifoldTemplate = typename manif::internal::traitscast<Manifold, _Scalar>::cast;
 
   /// @todo this is an implicit conversion operator,
   /// evaluate how bad it is to use it.
@@ -634,6 +632,15 @@ _Stream& operator << (
   return s;
 }
 
+/// Traits specialization
+
+namespace internal {
+template <template <typename> class _Derived, typename _NewScalar, typename _Scalar>
+struct traitscast<ManifoldBase<_Derived<_Scalar>>, _NewScalar>
+{
+  using cast = ManifoldBase<_Derived<_NewScalar>>;
+};
+} /* namespace internal */
 } /* namespace manif */
 
 #endif /* _MANIF_MANIF_MANIFOLD_BASE_H_ */
diff --git a/include/manif/impl/se2/SE2.h b/include/manif/impl/se2/SE2.h
@@ -23,9 +23,6 @@ struct traits<SE2<_Scalar>>
 {
   using Scalar = _Scalar;
 
-  template <typename T>
-  using ManifoldTemplate = SE2<T>;
-
   using Manifold = SE2<_Scalar>;
   using Tangent  = SE2Tangent<_Scalar>;
 

diff --git a/include/manif/impl/se2/SE2Tangent.h b/include/manif/impl/se2/SE2Tangent.h
@@ -15,9 +15,6 @@ namespace internal
 template <typename _Scalar>
 struct traits<SE2Tangent<_Scalar>>
 {
-  template <typename T>
-  using TangentTemplate = SE2Tangent<T>;
-
   using Scalar = _Scalar;
 
   using Manifold = SE2<_Scalar>;

diff --git a/include/manif/impl/se3/SE3.h b/include/manif/impl/se3/SE3.h
@@ -23,9 +23,6 @@ struct traits<SE3<_Scalar>>
 {
   using Scalar = _Scalar;
 
-  template <typename T>
-  using ManifoldTemplate = SE3<T>;
-
   using Manifold = SE3<_Scalar>;
   using Tangent  = SE3Tangent<_Scalar>;
 

diff --git a/include/manif/impl/se3/SE3Tangent.h b/include/manif/impl/se3/SE3Tangent.h
@@ -15,9 +15,6 @@ namespace internal
 template <typename _Scalar>
 struct traits<SE3Tangent<_Scalar>>
 {
-  template <typename T>
-  using TangentTemplate = SE3Tangent<T>;
-
   using Scalar = _Scalar;
 
   using Manifold = SE3<_Scalar>;

diff --git a/include/manif/impl/so2/SO2.h b/include/manif/impl/so2/SO2.h
@@ -19,9 +19,6 @@ namespace internal
 template <typename _Scalar>
 struct traits<SO2<_Scalar>>
 {
-  template <typename T>
-  using ManifoldTemplate = SO2<T>;
-
   using Scalar = _Scalar;
 
   using Manifold = SO2<_Scalar>;

diff --git a/include/manif/impl/so2/SO2Tangent.h b/include/manif/impl/so2/SO2Tangent.h
@@ -15,9 +15,6 @@ namespace internal
 template <typename _Scalar>
 struct traits<SO2Tangent<_Scalar>>
 {
-  template <typename T>
-  using TangentTemplate = SO2Tangent<T>;
-
   using Scalar = _Scalar;
 
   using Manifold = SO2<_Scalar>;

diff --git a/include/manif/impl/so3/SO3.h b/include/manif/impl/so3/SO3.h
@@ -19,9 +19,6 @@ namespace internal
 template <typename _Scalar>
 struct traits<SO3<_Scalar>>
 {
-  template <typename T>
-  using ManifoldTemplate = SO3<T>;
-
   using Scalar = _Scalar;
 
   using Manifold = SO3<_Scalar>;

diff --git a/include/manif/impl/so3/SO3Tangent.h b/include/manif/impl/so3/SO3Tangent.h
@@ -15,9 +15,6 @@ namespace internal
 template <typename _Scalar>
 struct traits<SO3Tangent<_Scalar>>
 {
-  template <typename T>
-  using TangentTemplate = SO3Tangent<T>;
-
   using Scalar = _Scalar;
 
   using Manifold = SO3<_Scalar>;

diff --git a/include/manif/impl/tangent_base.h b/include/manif/impl/tangent_base.h
@@ -27,9 +27,8 @@ struct TangentBase
 
   using OptJacobianRef = tl::optional<Jacobian&>;
 
-  template <typename T>
-  using TangentTemplate =
-  typename internal::traits<_Derived>::template TangentTemplate<T>;
+  template <typename _Scalar>
+  using TangentTemplate = typename manif::internal::traitscast<Tangent, _Scalar>::cast;
 
   /// @todo this is an implicit conversion operator,
   /// evaluate how bad it is to use it.
@@ -400,6 +399,16 @@ _Stream& operator << (
   return s;
 }
 
+/// Traits specialization
+
+namespace internal {
+template <template <typename> class _Derived, typename _NewScalar, typename _Scalar>
+struct traitscast<TangentBase<_Derived<_Scalar>>, _NewScalar>
+{
+  using cast = TangentBase<_Derived<_NewScalar>>;
+};
+} /* namespace internal */
+
 } /* namespace manif */
 
 #endif /* _MANIF_MANIF_TANGENT_BASE_H_ */
diff --git a/include/manif/impl/traits.h b/include/manif/impl/traits.h
@@ -3,10 +3,8 @@
 
 #include <type_traits>
 
-namespace manif
-{
-namespace internal
-{
+namespace manif {
+namespace internal {
 
 template <typename T> struct traits;
 
@@ -32,15 +30,22 @@ template <typename _ManifoldBase> struct ManifoldProperties;
 //  using Base = TangentBase<_Derived>;
 //};
 
+template <typename _Class, typename _NewScalar>
+struct traitscast;
+
+template <template <typename> class _Class, typename _NewScalar, typename _Scalar>
+struct traitscast<_Class<_Scalar>, _NewScalar>
+{
+  using cast = _Class<_NewScalar>;
+};
+
 } /* namespace internal */
 } /* namespace manif */
 
 ////// Garbage / messing arround
 
-namespace manif
-{
-namespace internal
-{
+namespace manif {
+namespace internal {
 
 template<class, typename T> struct has_rplus_impl : std::false_type {};
 template<typename T> struct

diff --git a/include/manif/impl/utils.h b/include/manif/impl/utils.h
@@ -1,8 +1,7 @@
 #ifndef _MANIF_MANIF_UTILS_H_
 #define _MANIF_MANIF_UTILS_H_
 
-#include "manif/impl/eigen_checks.h"
-#include <Eigen/Core>
+#include "manif/impl/eigen.h"
 
 namespace manif
 {
@@ -16,29 +15,6 @@ T pi2pi(T angle)
   return angle;
 }
 
-template <typename _Scalar>
-Eigen::Matrix<_Scalar, 2, 2>
-skew2(const _Scalar v)
-{
-  return (Eigen::Matrix<_Scalar, 2, 2>() <<
-             _Scalar(0.), -v,
-             v, _Scalar(0.) ).finished();
-}
-
-template <typename _Derived>
-Eigen::Matrix<typename _Derived::Scalar, 3, 3>
-skew3(const Eigen::MatrixBase<_Derived>& v)
-{
-  assert_vector_dim(v, 3);
-
-  using T = typename _Derived::Scalar;
-
-  return (Eigen::Matrix<T, 3, 3>() <<
-             T(0.),  -v(2),   +v(1),
-            +v(2),    T(0.),  -v(0),
-            -v(1),   +v(0),    T(0.) ).finished();
-}
-
 /**
  * @brief Conversion to radians
  * @param deg angle in degrees