Add namespace SE2 and use it

include/core/math/SE2.h

@@ -22,15 +22,20 @@
 
 namespace wolf
 {
+namespace SO2{
 
 template<typename T>
-Eigen::Matrix<T, 2, 2> exp_SO2(const T theta)
+Eigen::Matrix<T, 2, 2> exp(const T theta)
 {
     return Eigen::Rotation2D<T>(theta).matrix();
 }
+} // namespace SO2
+
+
+namespace SE2{
 
 template<typename T>
-Eigen::Matrix2s V_SE2(const T theta)
+Eigen::Matrix2s V_helper(const T theta)
 {
     T s;   //   sin(theta)   / theta
     T c_1; // (1-cos(theta)) / theta
@@ -52,13 +57,13 @@ Eigen::Matrix2s V_SE2(const T theta)
 }
 
 template<class D1, class D2>
-void exp_SE2(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
+void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
 {
     MatrixSizeCheck<3, 1>::check(_tau);
     MatrixSizeCheck<3, 1>::check(_delta);
 
     // [1] eq. 156
-    _delta.head(2) = V_SE2(_tau(2)) * _tau.head(2);
+    _delta.head(2) = V_helper(_tau(2)) * _tau.head(2);
     _delta(2) = pi2pi(_tau(2));
 }
 
@@ -90,7 +95,7 @@ Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
 }
 
 template<class D1, class D2, class D3>
-void exp_SE2(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
+void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
 {
     MatrixSizeCheck<3, 1>::check(_tau);
     MatrixSizeCheck<3, 1>::check(_delta);
@@ -100,17 +105,18 @@ void exp_SE2(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>&
 
     // [1] eq. 156
     T theta = pi2pi(_tau(2));
-    Eigen::Matrix<T, 2, 2> V = V_SE2(theta);
+    Eigen::Matrix<T, 2, 2> V = V_helper(theta);
     _delta.head(2) = V * _tau.head(2);
     _delta(2) = theta;
 
     // Jacobian is the composite definition [1] eq. 89, with jacobian blocks:
-    //   J_Vp_p = V: see V_SE2 below, eq. 158
-    //   J_Vp_theta: see fcn helper below
+    //   J_Vp_p = V: see V_helper, eq. 158
+    //   J_Vp_theta: see fcn helper
     //   J_theta_theta = 1; eq. 126
     _J_delta_tau << V, J_Vp_theta(_tau.template head<2>(), theta), 0.0, 0.0, 1.0;
 }
 
+} // namespace SE2
 } // namespacs wolf
 
 #endif /* MATH_SE2_H_ */

src/processor/processor_diff_drive.cpp

@@ -119,7 +119,7 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
 
     Matrix3s J_delta_tangent;
 
-    exp_SE2(tangent, _delta, J_delta_tangent);
+    SE2::exp(tangent, _delta, J_delta_tangent);
 
 
     /// 3. delta covariance -----------------------------------------------