Add laser_tools.h with some helper fcns

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
   )

include/laser/math/laser_tools.h

+/*
+ * 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_ */