From 5125f08823f8d4d8970419f3709afff6a0620545 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Wed, 31 Jul 2019 00:45:20 +0200 Subject: [PATCH] Add namespace SE2 and use it --- include/core/math/SE2.h | 22 ++++++++++++++-------- src/processor/processor_diff_drive.cpp | 2 +- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index 925359e9b..a32254de5 100644 --- a/include/core/math/SE2.h +++ b/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_ */ diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 7af4c10f8..ffdce5bc9 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/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 ----------------------------------------------- -- GitLab