From f74db707ac6a6c57eb24750c459c18b6b8ea7f52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0=20Ortega?= <jsola@iri.upc.edu> Date: Wed, 31 Jul 2019 00:56:42 +0200 Subject: [PATCH] Se2 --- include/core/math/SE2.h | 22 ++++++++++++++-------- include/core/math/SE3.h | 2 +- src/processor/processor_diff_drive.cpp | 2 +- test/gtest_SE3.cpp | 2 +- 4 files changed, 17 insertions(+), 11 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/include/core/math/SE3.h b/include/core/math/SE3.h index 0515376e4..02c67109f 100644 --- a/include/core/math/SE3.h +++ b/include/core/math/SE3.h @@ -35,7 +35,7 @@ namespace wolf { -namespace three_D { +namespace SE3 { using namespace Eigen; template<typename D1, typename D2> 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 ----------------------------------------------- diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp index c922ae4b0..357c36f2e 100644 --- a/test/gtest_SE3.cpp +++ b/test/gtest_SE3.cpp @@ -13,7 +13,7 @@ using namespace Eigen; using namespace wolf; -using namespace three_D; +using namespace SE3; TEST(SE3, exp_0) { -- GitLab