new package with small TUM trajectory cli tools

mola_traj_tools/CMakeLists.txt

@@ -0,0 +1,37 @@
+# ------------------------------------------------------------------------------
+#        A Modular Optimization framework for Localization and mApping
+#                               (MOLA)
+#
+# Copyright (C) 2018-2024, Jose Luis Blanco-Claraco, contributors (AUTHORS.md)
+# All rights reserved.
+# See LICENSE file
+# ------------------------------------------------------------------------------
+
+# Minimum CMake vesion: limited by CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS
+cmake_minimum_required(VERSION 3.4)
+
+# Tell CMake we'll use C++ for use in its tests/flags
+project(mola_traj_tools LANGUAGES CXX)
+
+# MOLA CMake scripts: "mola_xxx()"
+find_package(mola_common REQUIRED)
+
+# find dependencies:
+find_package(mrpt-poses)
+
+# ----------------------
+# define app targets:
+
+mola_add_executable(
+	TARGET  traj_ypr2tum
+	SOURCES src/traj_ypr2tum.cpp
+	LINK_LIBRARIES
+		mrpt::poses
+)
+
+mola_add_executable(
+	TARGET  traj_tf
+	SOURCES src/traj_tf.cpp
+	LINK_LIBRARIES
+		mrpt::poses
+)

mola_traj_tools/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2005-2019, Individual contributors, see AUTHORS file
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

mola_traj_tools/README.md

@@ -0,0 +1,51 @@
+# mola_traj_tools
+CLI tools to manipulate trajectory files as a complement to [evo](https://github.com/MichaelGrupp/evo).
+
+<!-- toc -->
+
+- [Build and install](#build-and-install)
+- [Documentation](#documentation)
+- [License](#license)
+
+<!-- tocstop -->
+
+## Build and install
+Refer to the [root MOLA repository](https://github.com/MOLAorg/mola) for compilation instructions.
+
+To install from the ROS repositories:
+
+    sudo apt install ros-${ROS_DISTRO}-mola-traj-tools
+
+## Documentation
+
+### traj_ypr2tum
+
+This tool can be used to convert a TXT file with a trajectory in this format:
+
+```
+# t [unix timestamp, double]  x y z [meters]  yaw pitch roll [radians]
+t x y z yaw pitch roll
+```
+
+into the [TUM format](https://github.com/MichaelGrupp/evo/wiki/Formats#tum---tum-rgb-d-dataset-trajectory-format).
+
+Usage:
+
+```bash
+  traj_ypr2tum INPUT.ypr OUTPUT.tum
+```
+
+### traj_tf
+
+This tool takes an **input** trajectory file in the [TUM format](https://github.com/MichaelGrupp/evo/wiki/Formats#tum---tum-rgb-d-dataset-trajectory-format),
+a SE(3) **transformation**, and applies it to the input, writing the modified trajectory to an **output** file.
+
+Usage:
+
+```bash
+  traj_tf INPUT.tum OUTPUT.tum "[x y z yaw_deg pitch_deg roll_deg]"
+```
+
+
+## License
+This package is released under the BSD-3-clause license.

mola_traj_tools/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<!-- This is a ROS package file, intended to allow this library to be built
+     side-by-side to ROS packages in a catkin/ament environment.
+-->
+<package format="3">
+  <name>mola_traj_tools</name>
+  <version>0.2.2</version>
+  <description>CLI tools to manipulate trajectory files as a complement to the evo package</description>
+
+  <author     email="[email protected]">Jose-Luis Blanco-Claraco</author>
+  <maintainer email="[email protected]">Jose-Luis Blanco-Claraco</maintainer>
+
+  <url type="website">https://github.com/MOLAorg/</url>
+
+  <license file="LICENSE">BSD</license>
+
+  <depend>mola_common</depend>
+  <depend>mrpt2</depend>
+
+  <doc_depend>doxygen</doc_depend>
+
+  <!-- Minimum entries to release non-catkin pkgs: -->
+  <buildtool_depend>cmake</buildtool_depend>
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+  <!-- End -->
+
+</package>

mola_traj_tools/src/traj_tf.cpp

@@ -0,0 +1,55 @@
+/* -------------------------------------------------------------------------
+ *   A Modular Optimization framework for Localization and mApping  (MOLA)
+ * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
+ * See LICENSE for license information.
+ * ------------------------------------------------------------------------- */
+
+#include <mrpt/core/exceptions.h>
+#include <mrpt/poses/CPose3D.h>
+#include <mrpt/poses/CPose3DInterpolator.h>
+
+#include <iostream>
+
+int main(int argc, char** argv)
+{
+    try
+    {
+        if (argc != 4)
+        {
+            std::cerr << "Usage: " << argv[0]
+                      << " INPUT.tum OUTPUT.tum \"[x y z yaw_deg pitch_deg "
+                         "roll_deg]\""
+                      << std::endl;
+            return 1;
+        }
+
+        const std::string sIn  = argv[1];
+        const std::string sOut = argv[2];
+        const std::string sTF  = argv[3];
+
+        mrpt::poses::CPose3DInterpolator pIn;
+        pIn.loadFromTextFile_TUM(sIn);
+
+        std::cout << "Loaded: " << pIn.size() << " poses.\n";
+        ASSERT_(!pIn.empty());
+
+        auto in0 = pIn.begin()->second;
+
+        // Apply tf:
+        const auto tf = mrpt::poses::CPose3D::FromString(sTF);
+        std::cout << "tf: " << tf << "\n";
+
+        for (auto& [t, pose] : pIn)  //
+            pose = (tf + mrpt::poses::CPose3D(pose - in0)).asTPose();
+
+        // save:
+        pIn.saveToTextFile_TUM(sOut);
+
+        return 0;
+    }
+    catch (const std::exception& e)
+    {
+        std::cerr << "Exception: " << e.what() << std::endl;
+        return 1;
+    }
+}

mola_traj_tools/src/traj_ypr2tum.cpp

@@ -0,0 +1,37 @@
+/* -------------------------------------------------------------------------
+ *   A Modular Optimization framework for Localization and mApping  (MOLA)
+ * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
+ * See LICENSE for license information.
+ * ------------------------------------------------------------------------- */
+
+#include <mrpt/core/exceptions.h>
+#include <mrpt/poses/CPose3DInterpolator.h>
+
+#include <iostream>
+
+int main(int argc, char** argv)
+{
+    try
+    {
+        if (argc != 3)
+        {
+            std::cerr << "Usage: " << argv[0] << " INPUT.ypr  OUTPUT.tum"
+                      << std::endl;
+            return 1;
+        }
+
+        const std::string sIn  = argv[1];
+        const std::string sOut = argv[2];
+
+        mrpt::poses::CPose3DInterpolator p;
+        p.loadFromTextFile(sIn);
+        p.saveToTextFile_TUM(sOut);
+
+        return 0;
+    }
+    catch (const std::exception& e)
+    {
+        std::cerr << "Exception: " << e.what() << std::endl;
+        return 1;
+    }
+}