From f75c1241415fea37aeec4c16039abdc036dc65a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Thu, 18 Jun 2020 18:19:22 +0200 Subject: [PATCH] Add laser_tools.h with some helper fcns --- CMakeLists.txt | 1 + include/laser/math/laser_tools.h | 49 ++++++++++++++++++++++++++++++++ 2 files changed, 50 insertions(+) create mode 100644 include/laser/math/laser_tools.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e0b49638..739948e8f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,6 +146,7 @@ INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS}) SET(HDRS_COMMON ) SET(HDRS_MATH + include/laser/math/laser_tools.h ) SET(HDRS_UTILS ) diff --git a/include/laser/math/laser_tools.h b/include/laser/math/laser_tools.h new file mode 100644 index 000000000..a25fba7a1 --- /dev/null +++ b/include/laser/math/laser_tools.h @@ -0,0 +1,49 @@ +/* + * laser_tools.h + * + * Created on: Jun 17, 2020 + * Author: jsola + */ + +#ifndef INCLUDE_LASER_MATH_LASER_TOOLS_H_ +#define INCLUDE_LASER_MATH_LASER_TOOLS_H_ + +#include <core/common/wolf.h> +#include <core/state_block/state_composite.h> + +#include <laser_scan_utils/laser_scan_utils.h> +#include <laser_scan_utils/icp.h> + +using namespace Eigen; + +namespace wolf +{ +namespace laser +{ + +inline Eigen::Isometry2d trf2isometry(Vector2d translation, Vector1d rotation) +{ + Isometry2d T = Translation2d(translation) * Rotation2Dd(rotation(0)); + + return T; +} + +inline Eigen::Isometry2d trf2isometry(Vector3d trf) +{ + Isometry2d T = Translation2d(trf.head<2>()) * Rotation2Dd(trf(2)); + + return T; +} + +inline Eigen::Isometry2d trf2isometry(VectorComposite trf) +{ + return trf2isometry(trf.at('P'), trf.at('O')); +} + +} // namespace laser + +} // namespace wolf + + + +#endif /* INCLUDE_LASER_MATH_LASER_TOOLS_H_ */ -- GitLab