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