diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index 925359e9bf04758777909d8ae866cb8b2a0919a2..a32254de578f636a696dd91148bbd6cabac2b34f 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 0515376e459be99732f24d97ea9667d7616a2177..02c67109f0a75224d9e4c36ee24b2cb33a995bec 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 7af4c10f8a09807f234799c8fc5d6ea034aa9fc1..ffdce5bc9886d7421219d611b4d1f9f16dea175c 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 c922ae4b0b6e80ddcf75090d09bcf04d58406e1e..357c36f2e90977342fd089e968ea280d12d2d08f 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) {