evo - pypi Package Compare versions

Comparing version 1.30.6 to 1.31.0

evo/__init__.py

@@ -11,3 +11,3 @@ import logging
 
-__version__ = "v1.30.6"
+__version__ = "v1.31.0"
 
@@ -14,0 +14,0 @@

evo/core/trajectory.py

@@ -331,2 +331,23 @@ # -*- coding: UTF8 -*-
 
+    def _jumps(self, dist: float) -> np.ndarray:
+        jumps = np.where(self.distances[1:] - self.distances[:-1] > dist)
+        if len(jumps[0]) == 0:
+            return np.array([0, self.num_poses])
+        return np.concatenate([[0], jumps[0] + 1, [self.num_poses]])
+
+    def split_distance_gaps(self,
+                            dist: float) -> typing.Sequence["PosePath3D"]:
+        """
+        Determines translation gaps in the path and splits it into multiple
+        paths at the gaps.
+        :param dist: distance threshold for gap detection in meters
+        """
+        if self.num_poses < 2:
+            return [self]
+        jumps = self._jumps(dist)
+        return [
+            PosePath3D(poses_se3=self.poses_se3[jumps[i]:jumps[i + 1]])
+            for i in range(len(jumps) - 1)
+        ]
+
     def check(self) -> typing.Tuple[bool, dict]:
@@ -454,2 +475,58 @@ """
 
+    def split_time_gaps(self,
+                        dt: float) -> typing.Sequence["PoseTrajectory3D"]:
+        """
+        Determines time gaps in the trajectory and splits it into multiple
+        trajectories at the gaps.
+        :param dt: time threshold for gap detection in seconds
+        """
+        if self.num_poses < 2:
+            return [self]
+        gaps = np.where(self.timestamps[1:] - self.timestamps[:-1] > dt)[0]
+        if len(gaps) == 0:
+            return [self]
+        gaps = np.concatenate([[0], gaps + 1, [self.num_poses]])
+        return [
+            PoseTrajectory3D(timestamps=self.timestamps[gaps[i]:gaps[i + 1]],
+                             poses_se3=self.poses_se3[gaps[i]:gaps[i + 1]])
+            for i in range(len(gaps) - 1)
+        ]
+
+    def split_distance_gaps(
+            self, dist: float) -> typing.Sequence["PoseTrajectory3D"]:
+        """
+        Determines translation gaps in the path and splits it into multiple
+        trajectories at the gaps.
+        :param dist: distance threshold for gap detection in meters
+        """
+        if self.num_poses < 2:
+            return [self]
+        jumps = self._jumps(dist)
+        return [
+            PoseTrajectory3D(timestamps=self.timestamps[jumps[i]:jumps[i + 1]],
+                             poses_se3=self.poses_se3[jumps[i]:jumps[i + 1]])
+            for i in range(len(jumps) - 1)
+        ]
+
+    def split_speed_outliers(
+            self, v_max: float) -> typing.Sequence["PoseTrajectory3D"]:
+        """
+        Splits the trajectory into multiple trajectories at speed outliers.
+        Can be used for example to handle jumps due to tracking loss.
+
+        :param v_max: speed threshold for outlier detection in m/s
+        """
+        if self.num_poses < 2:
+            return [self]
+        speeds = self.speeds
+        outliers = np.where(speeds > v_max)[0]
+        if len(outliers) == 0:
+            return [self]
+        jumps = np.concatenate([[0], outliers + 1, [self.num_poses]])
+        return [
+            PoseTrajectory3D(timestamps=self.timestamps[jumps[i]:jumps[i + 1]],
+                             poses_se3=self.poses_se3[jumps[i]:jumps[i + 1]])
+            for i in range(len(jumps) - 1)
+        ]
+
     def check(self) -> typing.Tuple[bool, dict]:
@@ -456,0 +533,0 @@ if self.num_poses == 0:

evo/main_traj.py

@@ -98,4 +98,6 @@ #!/usr/bin/env python
                 if len(topics) == 0:
-                    die("No topics of supported types: {}".format(" ".join(
-                        file_interface.SUPPORTED_ROS_MSGS)))
+                    die("Found no topics of supported types:\n\n- {}"
+                        "\n\nIf you want to load TF trajectories, "
+                        "specify them like: /tf:map.base_link".format(
+                            "\n- ".join(file_interface.SUPPORTED_ROS_MSGS)))
             else:
@@ -102,0 +104,0 @@ topics = args.topics

evo/tools/file_interface.py

@@ -261,5 +261,7 @@ # -*- coding: UTF8 -*-
 
-def read_bag_trajectory(reader: typing.Union[Rosbag1Reader,
-                                             Rosbag2Reader], topic: str,
-                        cache_tf_tree: bool = False) -> PoseTrajectory3D:
+def read_bag_trajectory(
+    reader: typing.Union[Rosbag1Reader, Rosbag2Reader], topic: str,
+    cache_tf_tree: bool = False,
+    cache_hash_source: tf_id.HashSource = tf_id.HashSource.READER_INSTANCE
+) -> PoseTrajectory3D:
     """
@@ -272,2 +274,4 @@ :param reader: opened bag reader (rosbags.rosbag2 or rosbags.rosbag1)
                   the same reader.
+    :param cache_hash_source: Determines whether to cache per reader instance
+        (default) or per bag filename (e.g. if a bag is opened multiple times).
     :return: trajectory.PoseTrajectory3D
@@ -284,3 +288,4 @@ """
         from evo.tools import tf_cache
-        tf_tree_cache = (tf_cache.instance(reader.__hash__())
+        tf_tree_cache = (tf_cache.instance(
+            tf_id.hash_bag(reader, cache_hash_source))
                          if cache_tf_tree else tf_cache.TfCache())
@@ -287,0 +292,0 @@ return tf_tree_cache.get_trajectory(reader, identifier=topic)

evo/tools/plot.py

@@ -686,11 +686,13 @@ # -*- coding: UTF8 -*-
 
-def trajectories(fig: Figure, trajectories: typing.Union[
-        trajectory.PosePath3D, typing.Sequence[trajectory.PosePath3D],
-        typing.Dict[str, trajectory.PosePath3D]], plot_mode=PlotMode.xy,
-                 title: str = "", subplot_arg: int = 111,
-                 plot_start_end_markers: bool = False,
+def trajectories(fig_or_ax: typing.Union[Figure, Axes],
+                 trajectories: typing.Union[
+                     trajectory.PosePath3D,
+                     typing.Sequence[trajectory.PosePath3D],
+                     typing.Dict[str, trajectory.PosePath3D]],
+                 plot_mode=PlotMode.xy, title: str = "",
+                 subplot_arg: int = 111, plot_start_end_markers: bool = False,
                  length_unit: Unit = Unit.meters) -> None:
     """
     high-level function for plotting multiple trajectories
-    :param fig: matplotlib figure
+    :param fig: matplotlib figure, or maptplotlib axes
     :param trajectories: instance or container of PosePath3D or derived
@@ -706,3 +708,6 @@ - if it's a dictionary, the keys (names) will be used as labels
     """
-    ax = prepare_axis(fig, plot_mode, subplot_arg, length_unit)
+    if isinstance(fig_or_ax, Axes):
+        ax = fig_or_ax
+    else:
+        ax = prepare_axis(fig_or_ax, plot_mode, subplot_arg, length_unit)
     if title:
@@ -709,0 +714,0 @@ ax.set_title(title)

evo/tools/tf_id.py

@@ -21,4 +21,9 @@ """
 
+import enum
 import re
+from typing import Union
 
+from rosbags.rosbag1.reader import Reader as Rosbag1Reader
+from rosbags.rosbag2.reader import Reader as Rosbag2Reader
+
 from evo import EvoException
@@ -33,2 +38,23 @@
 
[e-mail]
+class HashSource(enum.Enum):
+    """
+    For choosing the source of the hash value when hashing a rosbag reader.
+    See hash_bag() for usage.
+    """
+    READER_INSTANCE = "reader_instance"
+    BAG_FILENAME = "filename"
+
+
+def hash_bag(reader: Union[Rosbag1Reader, Rosbag2Reader],
+             hash_source: HashSource) -> int:
+    """
+    Convenience function to hash a rosbag reader instance or its filename,
+    for using it as a key to tf_cache.instance()
+    """
+    if hash_source == HashSource.BAG_FILENAME:
+        return hash(reader.path)
+    return hash(reader)
+
+
 def split_id(identifier: str) -> tuple:
@@ -35,0 +61,0 @@ tf_topic, _, identifier = identifier.partition(":")

PKG-INFO

 Metadata-Version: 2.4
 Name: evo
-Version: 1.30.6
+Version: 1.31.0
 Summary: Python package for the evaluation of odometry and SLAM
@@ -5,0 +5,0 @@ Project-URL: Homepage, https://github.com/MichaelGrupp/evo

test/test_trajectory.py

@@ -195,3 +195,27 @@ #!/usr/bin/env python
 
+    def test_split_distance_gaps(self):
+        """
+        Checks if the path can be split correctly at a distance threshold.
+        """
+        path = PosePath3D(poses_se3=[
+            lie.se3(r=np.eye(3), t=np.array([0, 0, 0])),
+            lie.se3(r=np.eye(3), t=np.array([0, 0, 1])),
+            lie.se3(r=np.eye(3), t=np.array([0, 0, 2])),
+            lie.se3(r=np.eye(3), t=np.array([0, 0, 4])),
+            lie.se3(r=np.eye(3), t=np.array([0, 0, 7])),
+            lie.se3(r=np.eye(3), t=np.array([0, 0, 8])),
+        ])
+        splits = path.split_distance_gaps(1.5)
+        self.assertEqual(len(splits), 3)
+        self.assertEqual(splits[0].num_poses, 3)
+        self.assertEqual(splits[0].path_length, 2.)
+        self.assertEqual(splits[1].num_poses, 1)
+        self.assertEqual(splits[1].path_length, 0.)
+        self.assertEqual(splits[2].num_poses, 2)
+        self.assertEqual(splits[2].path_length, 1.)
+        splits = path.split_distance_gaps(999.)
+        self.assertEqual(len(splits), 1)
+        self.assertEqual(splits[0].num_poses, path.num_poses)
 
+
 class TestPoseTrajectory3D(unittest.TestCase):
@@ -249,3 +273,31 @@ def test_equals(self):
 
+    def test_split_time_gaps(self):
+        """
+        Checks if the trajectory can be split correctly with a time threshold.
+        """
+        traj = helpers.fake_trajectory(5, 1.)
+        traj.timestamps[2:5] += 10.
+        traj.timestamps[4] += 10.
+        splits = traj.split_time_gaps(5.)
+        self.assertEqual(len(splits), 3)
+        self.assertEqual(splits[0].num_poses, 2)
+        self.assertEqual(splits[1].num_poses, 2)
+        self.assertEqual(splits[2].num_poses, 1)
 
+    def test_split_speed_outliers(self):
+        """
+        Checks if the trajectory can be split correctly with a speed threshold.
+        """
+        traj = PoseTrajectory3D(
+            timestamps=np.array([0, 1, 2]), poses_se3=[
+                lie.se3(r=np.eye(3), t=np.array([0, 0, 0])),
+                lie.se3(r=np.eye(3), t=np.array([0, 0, 1])),
+                lie.se3(r=np.eye(3), t=np.array([0, 0, 4])),
+            ])
+        splits = traj.split_speed_outliers(2.)
+        self.assertEqual(len(splits), 2)
+        self.assertEqual(splits[0].num_poses, 2)
+        self.assertEqual(splits[1].num_poses, 1)
+
+
 class TestTrajectoryAlignment(unittest.TestCase):
@@ -252,0 +304,0 @@ def test_se3_alignment(self):

test/data/bla.py

-import matplotlib.pyplot as plt
-import numpy as np
-import matplotlib as mpl
-
-# Fixing random state for reproducibility
-np.random.seed(19680801)
-
-mpl.rcParams.update({
-        # NOTE: don't call tight_layout manually anymore. See warning here:
-        # https://matplotlib.org/stable/users/explain/axes/constrainedlayout_guide.html
-        "figure.constrained_layout.use": True,
-        #"savefig.bbox": "tight",
-})
-
-def randrange(n, vmin, vmax):
-    """
-    Helper function to make an array of random numbers having shape (n, )
-    with each number distributed Uniform(vmin, vmax).
-    """
-    return (vmax - vmin)*np.random.rand(n) + vmin
-
-fig = plt.figure()
-ax = fig.add_subplot(projection='3d')
-
-n = 100
-
-# For each set of style and range settings, plot n random points in the box
-# defined by x in [23, 32], y in [0, 100], z in [zlow, zhigh].
-for m, zlow, zhigh in [('o', -50, -25), ('^', -30, -5)]:
-    xs = randrange(n, 23, 32)
-    ys = randrange(n, 0, 100)
-    zs = randrange(n, zlow, zhigh)
-    ax.scatter(xs, ys, zs, marker=m)
-
-ax.set_xlabel('X Label')
-ax.set_ylabel('Y Label')
-ax.set_zlabel('Z Label')
-
-plt.savefig("bla.png")
-
-#plt.show()

test/data/test.py

-import matplotlib.pyplot as plt
-import numpy as np
-
-# Fixing random state for reproducibility
-np.random.seed(19680801)
-
-
-def randrange(n, vmin, vmax):
-    """
-    Helper function to make an array of random numbers having shape (n, )
-    with each number distributed Uniform(vmin, vmax).
-    """
-    return (vmax - vmin)*np.random.rand(n) + vmin
-
-fig = plt.figure()
-ax = fig.add_subplot(projection='3d')
-
-n = 100
-
-# For each set of style and range settings, plot n random points in the box
-# defined by x in [23, 32], y in [0, 100], z in [zlow, zhigh].
-for m, zlow, zhigh in [('o', -50, -25), ('^', -30, -5)]:
-    xs = randrange(n, 23, 32)
-    ys = randrange(n, 0, 100)
-    zs = randrange(n, zlow, zhigh)
-    ax.scatter(xs, ys, zs, marker=m)
-
-ax.set_xlabel('X Label')
-ax.set_ylabel('Y Label')
-ax.set_zlabel('Z Label')
-
-plt.show()

Alert delta unavailable

Currently unable to show alert delta for PyPI packages.

Improved metrics

Lines of code

11105

0.89%

Worsened metrics

Total package byte prevSize

14270581

-8.02%

Number of package files

187

-7.88%