plexus-python-common 1.0.62__tar.gz → 1.0.64__tar.gz

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: plexus-python-common
-Version: 1.0.62
+Version: 1.0.64
 Classifier: Programming Language :: Python :: 3
 Classifier: Programming Language :: Python :: 3.12
 Classifier: Programming Language :: Python :: 3.13

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/src/plexus/common/carto/OSMFile.py

@@ -5,7 +5,7 @@ import lxml.etree
 from plexus.common.carto.OSMNode import OSMNode
 from plexus.common.carto.OSMTags import OSMTags
 from plexus.common.carto.OSMWay import OSMWay
-from plexus.common.proj import Coord, Proj
+from plexus.common.utils.gisutils import Coord, Proj
 
 
 class OSMFile(object):

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/src/plexus/common/carto/OSMNode.py

@@ -1,7 +1,7 @@
 from iker.common.utils.strutils import repr_data
 
 from plexus.common.carto.OSMTags import OSMTags
-from plexus.common.proj import Coord
+from plexus.common.utils.gisutils import Coord
 
 
 class OSMNode(object):

plexus_python_common-1.0.62/src/plexus/common/proj.py → plexus_python_common-1.0.64/src/plexus/common/utils/gisutils.py

@@ -5,10 +5,111 @@ from typing import Annotated, Self
 import numpy as np
 import pydantic as pdt
 import pyproj
+import pyquaternion as pyquat
 from iker.common.utils.funcutils import singleton
 from iker.common.utils.strutils import parse_params_string, repr_data, str_conv
 
-from plexus.common.pose import Pose
+
+class Pose(object):
+    @classmethod
+    def from_numbers(
+        cls,
+        px: float,
+        py: float,
+        pz: float,
+        qx: float,
+        qy: float,
+        qz: float,
+        qw: float,
+        ts: float = 0,
+    ) -> Self:
+        """
+        Constructs a pose from numbers representing position and orientation
+        """
+        return Pose(ts, np.array([px, py, pz], dtype=np.float64), np.array([qw, qx, qy, qz], dtype=np.float64))
+
+    @classmethod
+    def add(cls, x: Self, d: Self) -> Self:
+        """
+        Performs pose SE3 addition, as x + d = y
+        """
+        xq = pyquat.Quaternion(x.q)
+        dq = pyquat.Quaternion(d.q)
+
+        yp = x.p + xq.rotate(d.p)
+        yq = xq * dq
+
+        return Pose(0, yp, yq.normalised.elements)
+
+    @classmethod
+    def sub(cls, y: Self, x: Self) -> Self:
+        """
+        Performs pose SE3 subtraction, as x + d = y => d = y - x
+        """
+        xq = pyquat.Quaternion(x.q)
+        yq = pyquat.Quaternion(y.q)
+
+        dp = xq.inverse.rotate(y.p - x.p)
+        dq = xq.inverse * yq
+
+        return Pose(0, dp, dq.normalised.elements)
+
+    @classmethod
+    def interpolate(cls, a: Self, b: Self, t: float) -> Self:
+        """
+        Interpolates between two given poses, as a * t + b * (1 - t)
+
+        :return: interpolated pose
+        """
+        ts = a.ts + (b.ts - a.ts) * t
+        p = a.p + (b.p - a.p) * t
+        q = pyquat.Quaternion.slerp(pyquat.Quaternion(a.q), pyquat.Quaternion(b.q), t)
+        return Pose(ts, p, q.normalised.elements)
+
+    def __init__(self, ts: float, p: np.ndarray, q: np.ndarray):
+        """
+        Represents a pose
+
+        :param ts: timestamp
+        :param p: position vector
+        :param q: orientation quaternion
+        """
+        self.ts = ts
+        self.p = p
+        self.q = q
+
+    def matrix(self) -> np.ndarray:
+        """
+        Returns the transformation matrix of this pose
+
+        :return: transformation matrix
+        """
+        r = pyquat.Quaternion(self.q).rotation_matrix
+        t = self.p[:, None]
+        return np.block([[r, t], [np.zeros((1, 3), dtype=np.float64), np.ones((1, 1), dtype=np.float64)]])
+
+    def translate(self, v: np.ndarray) -> np.ndarray:
+        """
+        Translate the given vector with the pose position
+
+        :param v: the vector to be translated
+
+        :return: translated vector
+        """
+        return self.p + v
+
+    def rotate(self, v: np.ndarray) -> np.ndarray:
+        """
+        Rotates the given vector with the pose orientation
+
+        :param v: the vector to be rotated
+
+        :return: rotated vector
+        """
+        return pyquat.Quaternion(self.q).rotate(v)
+
+    def __str__(self):
+        return repr_data(self)
 
 
 class Proj(ABC):

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/src/plexus/common/utils/tagutils.py

@@ -1,6 +1,7 @@
 import contextlib
 import dataclasses
 import datetime
+import math
 import os
 import pathlib
 import textwrap
@@ -16,6 +17,7 @@ import sqlalchemy as sa
 import sqlalchemy.dialects.sqlite as sa_sqlite
 import sqlalchemy.orm as sa_orm
 from iker.common.utils.dbutils import ConnectionMaker
+from iker.common.utils.dtutils import dt_from_ts_us, dt_to_ts_us
 from iker.common.utils.funcutils import memorized, singleton
 from iker.common.utils.iterutils import batched, head_or_none
 from iker.common.utils.iterutils import dicttree
@@ -48,6 +50,8 @@ __all__ = [
     "tag_cache_file_path",
     "TagCache",
     "tag_cache",
+    "standard_clip_duration_us",
+    "populate_clip_ranges",
 ]
 
 
@@ -1093,3 +1097,52 @@ def tag_cache(*, identifier: str | None = None, file_path: str | None = None) ->
     if file_path is not None:
         return TagCache(file_path=file_path)
     return TagCache(file_path=tag_cache_file_path())
+
+
+
+@singleton
+def standard_clip_duration_us() -> int:
+    """Duration of clips to split samples into, in microseconds, which is fixed to 20 seconds for now."""
+    return 20 * 1_000_000  # 20 seconds
+
+
+
+def populate_clip_ranges(
+    data_begin_dt: datetime.datetime,
+    data_end_dt: datetime.datetime,
+    *,
+    clip_duration_us: int = None
+) -> Generator[tuple[datetime.datetime, datetime.datetime, datetime.datetime, datetime.datetime]]:
+    """
+    Generate clip begin datetime, clip data begin datetime, and clip data end datetime for each clip slot that overlaps
+    with the given data begin and end datetimes.
+    The clip begin datetime is the beginning of the clip slot, the clip data begin datetime is the maximum of the clip
+    begin datetime and the data begin datetime, and the clip data end datetime is the minimum of the clip end datetime
+    and the data end datetime.
+
+    :param data_begin_dt: The beginning datetime of the data.
+    :param data_end_dt: The end datetime of the data.
+    :param clip_duration_us: The duration of each clip in microseconds. If not provided,
+                             the standard clip duration will be used.
+    :return: A generator of tuples containing the clip begin datetime, clip data begin datetime,
+             and clip data end datetime
+    """
+    clip_duration_us = clip_duration_us or standard_clip_duration_us()
+
+    if data_begin_dt == data_end_dt:
+        clip_slot = math.floor(dt_to_ts_us(data_begin_dt) / clip_duration_us)
+        clip_begin_dt = dt_from_ts_us(clip_slot * clip_duration_us)
+        clip_end_dt = dt_from_ts_us((clip_slot + 1) * clip_duration_us)
+        yield clip_begin_dt, clip_end_dt, data_begin_dt, data_end_dt
+        return
+
+    begin_clip_slot = math.floor(dt_to_ts_us(data_begin_dt) / clip_duration_us)
+    end_clip_slot = math.ceil(dt_to_ts_us(data_end_dt)  / clip_duration_us)
+
+    for clip_slot in range(begin_clip_slot, end_clip_slot):
+        clip_begin_dt = dt_from_ts_us(clip_slot * clip_duration_us)
+        clip_end_dt = dt_from_ts_us((clip_slot + 1) * clip_duration_us)
+        clip_data_begin_dt = max(clip_begin_dt, data_begin_dt)
+        clip_data_end_dt = min(clip_end_dt, data_end_dt)
+
+        yield clip_begin_dt, clip_end_dt, clip_data_begin_dt, clip_data_end_dt

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/src/plexus_python_common.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: plexus-python-common
-Version: 1.0.62
+Version: 1.0.64
 Classifier: Programming Language :: Python :: 3
 Classifier: Programming Language :: Python :: 3.12
 Classifier: Programming Language :: Python :: 3.13

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/src/plexus_python_common.egg-info/SOURCES.txt

@@ -39,8 +39,6 @@ resources/unittest/s3utils/dir.foo/dir.foo.bar/dir.foo.bar.baz/file.foo.bar.baz
 resources/unittest/s3utils_archive/archive.compressed.zip
 resources/unittest/s3utils_archive/archive.uncompressed.zip
 src/plexus/common/__init__.py
-src/plexus/common/pose.py
-src/plexus/common/proj.py
 src/plexus/common/carto/OSMFile.py
 src/plexus/common/carto/OSMNode.py
 src/plexus/common/carto/OSMTags.py
@@ -56,6 +54,7 @@ src/plexus/common/utils/bagutils.py
 src/plexus/common/utils/config.py
 src/plexus/common/utils/datautils.py
 src/plexus/common/utils/dockerutils.py
+src/plexus/common/utils/gisutils.py
 src/plexus/common/utils/jsonutils.py
 src/plexus/common/utils/ormutils.py
 src/plexus/common/utils/pathutils.py
@@ -73,8 +72,6 @@ src/plexus_python_common.egg-info/top_level.txt
 test/testenv.py
 test/plexus_tests/__init__.py
 test/plexus_tests/common/__init__.py
-test/plexus_tests/common/pose_test.py
-test/plexus_tests/common/proj_test.py
 test/plexus_tests/common/carto/__init__.py
 test/plexus_tests/common/carto/osm_file_test.py
 test/plexus_tests/common/carto/osm_tags_test.py
@@ -82,6 +79,7 @@ test/plexus_tests/common/utils/__init__.py
 test/plexus_tests/common/utils/bagutils_test.py
 test/plexus_tests/common/utils/datautils_test.py
 test/plexus_tests/common/utils/dockerutils_test.py
+test/plexus_tests/common/utils/gisutils_test.py
 test/plexus_tests/common/utils/jsonutils_test.py
 test/plexus_tests/common/utils/ormutils_test.py
 test/plexus_tests/common/utils/pathutils_test.py

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/test/plexus_tests/common/carto/osm_file_test.py

@@ -4,8 +4,8 @@ import unittest.mock
 import lxml.etree
 
 from plexus.common.carto import OSMFile
-from plexus.common.proj import Coord
-from plexus.common.proj import make_proj
+from plexus.common.utils.gisutils import Coord
+from plexus.common.utils.gisutils import make_proj
 
 
 class MockedTree(object):

plexus_python_common-1.0.62/test/plexus_tests/common/proj_test.py → plexus_python_common-1.0.64/test/plexus_tests/common/utils/gisutils_test.py

@@ -1,11 +1,73 @@
+import math
 import random
 import unittest
 
 import ddt
+import numpy as np
 import pydantic as pdt
+import pyquaternion as pyquat
+from iker.common.utils.randutils import randomizer
 
-from plexus.common.proj import Coord, EQDCProj, UTMProj, WebMercProj
-from plexus.common.proj import make_proj
+from plexus.common.utils.gisutils import Coord, EQDCProj, UTMProj, WebMercProj
+from plexus.common.utils.gisutils import Pose
+from plexus.common.utils.gisutils import make_proj
+
+
+@ddt.ddt
+class PoseTest(unittest.TestCase):
+
+    @staticmethod
+    def random_pose() -> Pose:
+        q = pyquat.Quaternion(axis=randomizer().random_unit_vector(3),
+                              angle=math.pi * randomizer().next_float(0.0, 0.5))
+        return Pose(0, np.array(randomizer().random_unit_vector(3)), q.normalised.elements)
+
+    def assert_array_almost_equal(self, xs: np.ndarray, ys: np.ndarray, delta: float):
+        self.assertEqual(len(xs), len(ys))
+        for x, y in zip(xs, ys):
+            self.assertAlmostEqual(x, y, delta=delta)
+
+    def test_builtin_init(self):
+        for _ in range(0, 10000):
+            expect = PoseTest.random_pose()
+            actual = Pose(0, expect.p, expect.q)
+
+            self.assert_array_almost_equal(expect.p, actual.p, delta=1e-9)
+            self.assert_array_almost_equal(expect.q, actual.q, delta=1e-9)
+
+    def test_add_sub(self):
+        for _ in range(0, 10000):
+            pose = PoseTest.random_pose()
+            delta = PoseTest.random_pose()
+
+            result = Pose.sub(Pose.add(pose, delta), pose)
+
+            self.assert_array_almost_equal(delta.p, result.p, delta=1e-9)
+            self.assert_array_almost_equal(delta.q, result.q, delta=1e-9)
+
+    def test_interpolate(self):
+        for _ in range(0, 10000):
+            t = randomizer().next_float()
+
+            pose1 = PoseTest.random_pose()
+            pose2 = PoseTest.random_pose()
+
+            inter1 = Pose.interpolate(pose1, pose2, t)
+            inter2 = Pose.interpolate(pose2, pose1, 1.0 - t)
+
+            self.assert_array_almost_equal(inter1.p, inter2.p, delta=1e-9)
+            self.assert_array_almost_equal(inter1.q, inter2.q, delta=1e-9)
+
+    def test_matrix(self):
+        for _ in range(0, 10000):
+            point = np.array(randomizer().random_unit_vector(3))
+
+            pose = PoseTest.random_pose()
+
+            expect = pose.translate(pose.rotate(point))
+            result = np.matmul(pose.matrix(), np.array([*point, 1.0])[:, None]).transpose()[0][:3]
+
+            self.assert_array_almost_equal(expect, result, delta=1e-9)
 
 
 @ddt.ddt

{plexus_python_common-1.0.62 → plexus_python_common-1.0.64}/test/plexus_tests/common/utils/tagutils_test.py

@@ -10,6 +10,7 @@ from iker.common.utils.iterutils import last
 from iker.common.utils.jsonutils import JsonObject
 
 from plexus.common.utils.tagutils import MutableTagset, Tag, TagCache, Tagset
+from plexus.common.utils.tagutils import populate_clip_ranges
 from plexus.common.utils.tagutils import predefined_tagsets, render_tagset_markdown_readme
 from plexus.common.utils.tagutils import tag_cache_file_path
 
@@ -497,3 +498,177 @@ class TagUtilsTest(unittest.TestCase):
                              tag_records_count * 2 * (tags_count - tagset_tags_count) // tags_count)
             self.assertEqual(len(list(dst_target_cache.iter_tags(tagsets=[tagset], tagset_inverted=False))),
                              tag_records_count * 2 * tagset_tags_count // tags_count)
+
+    data_populate_clip_ranges = [
+        (
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:50.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:01:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:50.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:15.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:15.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:30.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:30.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:05.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:05.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:05.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:05.000000+00:00")),
+            ],
+        )
+    ]
+
+    @ddt.idata(data_populate_clip_ranges)
+    @ddt.unpack
+    def test_populate_clip_ranges(self, data_begin_dt, data_end_dt, expected):
+        result = list(populate_clip_ranges(data_begin_dt, data_end_dt))
+        self.assertEqual(result, expected)
+
+    data_populate_clip_ranges__customized_duration = [
+        (
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:50.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:30.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:30.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:30.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:30.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:50.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:40.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:50.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:15.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:15.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:30.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00")),
+                (dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:30.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:20.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:30.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:00.000000+00:00")),
+            ],
+        ),
+        (
+            dt_parse_iso("2023-01-01T00:00:05.000000+00:00"),
+            dt_parse_iso("2023-01-01T00:00:05.000000+00:00"),
+            [
+                (dt_parse_iso("2023-01-01T00:00:00.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:10.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:05.000000+00:00"),
+                 dt_parse_iso("2023-01-01T00:00:05.000000+00:00")),
+            ],
+        )
+    ]
+
+    @ddt.idata(data_populate_clip_ranges__customized_duration)
+    @ddt.unpack
+    def test_populate_clip_ranges__customized_duration(self, data_begin_dt, data_end_dt, expected):
+        result = list(populate_clip_ranges(data_begin_dt, data_end_dt, clip_duration_us=10_000_000))
+        self.assertEqual(result, expected)

plexus_python_common-1.0.62/src/plexus/common/pose.py

@@ -1,107 +0,0 @@
-from typing import Self
-
-import numpy as np
-import pyquaternion as pyquat
-from iker.common.utils.strutils import repr_data
-
-
-class Pose(object):
-    @classmethod
-    def from_numbers(
-        cls,
-        px: float,
-        py: float,
-        pz: float,
-        qx: float,
-        qy: float,
-        qz: float,
-        qw: float,
-        ts: float = 0,
-    ) -> Self:
-        """
-        Constructs a pose from numbers representing position and orientation
-        """
-        return Pose(ts, np.array([px, py, pz], dtype=np.float64), np.array([qw, qx, qy, qz], dtype=np.float64))
-
-    @classmethod
-    def add(cls, x: Self, d: Self) -> Self:
-        """
-        Performs pose SE3 addition, as x + d = y
-        """
-        xq = pyquat.Quaternion(x.q)
-        dq = pyquat.Quaternion(d.q)
-
-        yp = x.p + xq.rotate(d.p)
-        yq = xq * dq
-
-        return Pose(0, yp, yq.normalised.elements)
-
-    @classmethod
-    def sub(cls, y: Self, x: Self) -> Self:
-        """
-        Performs pose SE3 subtraction, as x + d = y => d = y - x
-        """
-        xq = pyquat.Quaternion(x.q)
-        yq = pyquat.Quaternion(y.q)
-
-        dp = xq.inverse.rotate(y.p - x.p)
-        dq = xq.inverse * yq
-
-        return Pose(0, dp, dq.normalised.elements)
-
-    @classmethod
-    def interpolate(cls, a: Self, b: Self, t: float) -> Self:
-        """
-        Interpolates between two given poses, as a * t + b * (1 - t)
-
-        :return: interpolated pose
-        """
-        ts = a.ts + (b.ts - a.ts) * t
-        p = a.p + (b.p - a.p) * t
-        q = pyquat.Quaternion.slerp(pyquat.Quaternion(a.q), pyquat.Quaternion(b.q), t)
-        return Pose(ts, p, q.normalised.elements)
-
-    def __init__(self, ts: float, p: np.ndarray, q: np.ndarray):
-        """
-        Represents a pose
-
-        :param ts: timestamp
-        :param p: position vector
-        :param q: orientation quaternion
-        """
-        self.ts = ts
-        self.p = p
-        self.q = q
-
-    def matrix(self) -> np.ndarray:
-        """
-        Returns the transformation matrix of this pose
-
-        :return: transformation matrix
-        """
-        r = pyquat.Quaternion(self.q).rotation_matrix
-        t = self.p[:, None]
-        return np.block([[r, t], [np.zeros((1, 3), dtype=np.float64), np.ones((1, 1), dtype=np.float64)]])
-
-    def translate(self, v: np.ndarray) -> np.ndarray:
-        """
-        Translate the given vector with the pose position
-
-        :param v: the vector to be translated
-
-        :return: translated vector
-        """
-        return self.p + v
-
-    def rotate(self, v: np.ndarray) -> np.ndarray:
-        """
-        Rotates the given vector with the pose orientation
-
-        :param v: the vector to be rotated
-
-        :return: rotated vector
-        """
-        return pyquat.Quaternion(self.q).rotate(v)
-
-    def __str__(self):
-        return repr_data(self)

plexus_python_common-1.0.62/test/plexus_tests/common/pose_test.py

@@ -1,66 +0,0 @@
-import math
-import unittest
-
-import ddt
-import numpy as np
-import pyquaternion as pyquat
-from iker.common.utils.randutils import randomizer
-
-from plexus.common.pose import Pose
-
-
-@ddt.ddt
-class PoseTest(unittest.TestCase):
-
-    @staticmethod
-    def random_pose() -> Pose:
-        q = pyquat.Quaternion(axis=randomizer().random_unit_vector(3),
-                              angle=math.pi * randomizer().next_float(0.0, 0.5))
-        return Pose(0, np.array(randomizer().random_unit_vector(3)), q.normalised.elements)
-
-    def assert_array_almost_equal(self, xs: np.ndarray, ys: np.ndarray, delta: float):
-        self.assertEqual(len(xs), len(ys))
-        for x, y in zip(xs, ys):
-            self.assertAlmostEqual(x, y, delta=delta)
-
-    def test_builtin_init(self):
-        for _ in range(0, 10000):
-            expect = PoseTest.random_pose()
-            actual = Pose(0, expect.p, expect.q)
-
-            self.assert_array_almost_equal(expect.p, actual.p, delta=1e-9)
-            self.assert_array_almost_equal(expect.q, actual.q, delta=1e-9)
-
-    def test_add_sub(self):
-        for _ in range(0, 10000):
-            pose = PoseTest.random_pose()
-            delta = PoseTest.random_pose()
-
-            result = Pose.sub(Pose.add(pose, delta), pose)
-
-            self.assert_array_almost_equal(delta.p, result.p, delta=1e-9)
-            self.assert_array_almost_equal(delta.q, result.q, delta=1e-9)
-
-    def test_interpolate(self):
-        for _ in range(0, 10000):
-            t = randomizer().next_float()
-
-            pose1 = PoseTest.random_pose()
-            pose2 = PoseTest.random_pose()
-
-            inter1 = Pose.interpolate(pose1, pose2, t)
-            inter2 = Pose.interpolate(pose2, pose1, 1.0 - t)
-
-            self.assert_array_almost_equal(inter1.p, inter2.p, delta=1e-9)
-            self.assert_array_almost_equal(inter1.q, inter2.q, delta=1e-9)
-
-    def test_matrix(self):
-        for _ in range(0, 10000):
-            point = np.array(randomizer().random_unit_vector(3))
-
-            pose = PoseTest.random_pose()
-
-            expect = pose.translate(pose.rotate(point))
-            result = np.matmul(pose.matrix(), np.array([*point, 1.0])[:, None]).transpose()[0][:3]
-
-            self.assert_array_almost_equal(expect, result, delta=1e-9)