diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2e0b496386a8251562e4daa2c0a7e4225122a6de..739948e8ff520de361ad5fe4b7904c1597b72855 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 0000000000000000000000000000000000000000..a25fba7a119d97c8064492cbdec29162fc3b3440
--- /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_ */