diff --git a/octreelib/grid/grid.py b/octreelib/grid/grid.py index b47c0d4..98f523d 100644 --- a/octreelib/grid/grid.py +++ b/octreelib/grid/grid.py @@ -10,7 +10,7 @@ GridVisualizationType, VisualizationConfig, ) -from octreelib.internal.point import RawPointCloud, RawPoint, CloudManager +from octreelib.internal.point import PointCloud, Point, CloudManager from octreelib.internal.voxel import Voxel from octreelib.octree_manager import OctreeManager @@ -35,12 +35,12 @@ def __init__(self, grid_config: GridConfig): super().__init__(grid_config) # {pose -> list of voxel coordinates} - self.__pose_voxel_coordinates: Dict[int, List[RawPoint]] = {} + self.__pose_voxel_coordinates: Dict[int, List[Point]] = {} # {voxel coordinates hash -> octree} self.__octrees: Dict[int, grid_config.octree_type] = {} - def insert_points(self, pose_number: int, points: RawPointCloud): + def insert_points(self, pose_number: int, points: PointCloud): """ Insert points to the grid :param pose_number: pose to which the cloud is inserted @@ -71,7 +71,7 @@ def insert_points(self, pose_number: int, points: RawPointCloud): pose_number, voxel_points ) - def map_leaf_points(self, function: Callable[[RawPointCloud], RawPointCloud]): + def map_leaf_points(self, function: Callable[[PointCloud], PointCloud]): """ Transforms point cloud in each leaf node of each octree using the function :param function: transformation function PointCloud -> PointCloud @@ -93,7 +93,7 @@ def get_leaf_points(self, pose_number: int) -> List[Voxel]: [], ) - def get_points(self, pose_number: int) -> RawPointCloud: + def get_points(self, pose_number: int) -> PointCloud: """ :param pose_number: Pose number. :return: All points inside the grid. @@ -104,7 +104,7 @@ def get_points(self, pose_number: int) -> RawPointCloud: def subdivide( self, - subdivision_criteria: List[Callable[[RawPointCloud], bool]], + subdivision_criteria: List[Callable[[PointCloud], bool]], pose_numbers: Optional[List[int]] = None, ): """ @@ -118,7 +118,7 @@ def subdivide( subdivision_criteria, pose_numbers ) - def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): + def filter(self, filtering_criteria: List[Callable[[PointCloud], bool]]): """ Filters nodes of each octree with points by criteria :param filtering_criteria: Filtering Criteria @@ -209,7 +209,7 @@ def n_nodes(self, pose_number: int) -> int: """ return sum([octree.n_nodes(pose_number) for octree in self.__octrees.values()]) - def __get_voxel_for_point(self, point: RawPoint) -> RawPoint: + def __get_voxel_for_point(self, point: Point) -> Point: """ Method to get coordinates of a voxel where the given point would be stored. :param point: Point. diff --git a/octreelib/grid/grid_base.py b/octreelib/grid/grid_base.py index fbe2078..5fed5b5 100644 --- a/octreelib/grid/grid_base.py +++ b/octreelib/grid/grid_base.py @@ -5,7 +5,7 @@ import numpy as np -from octreelib.internal.point import RawPoint, RawPointCloud +from octreelib.internal.point import Point, PointCloud from octreelib.internal.voxel import Voxel from octreelib.internal.typing import T from octreelib.octree.octree_base import OctreeConfigBase @@ -63,7 +63,7 @@ class GridConfigBase(ABC): octree_config: OctreeConfigBase debug: bool = False grid_voxel_edge_length: int = 1 - corner: RawPoint = field(default_factory=lambda: np.array(([0.0, 0.0, 0.0]))) + corner: Point = field(default_factory=lambda: np.array(([0.0, 0.0, 0.0]))) @property def compatible_octree_types(self): @@ -102,7 +102,7 @@ def __init__( self._grid_config = grid_config @abstractmethod - def insert_points(self, pose_number: int, points: List[RawPoint]) -> None: + def insert_points(self, pose_number: int, points: List[Point]) -> None: """ Insert points to the according octree. If an octree for this pos does not exist, a new octree is created @@ -112,7 +112,7 @@ def insert_points(self, pose_number: int, points: List[RawPoint]) -> None: pass @abstractmethod - def get_points(self, pose_number: int) -> List[RawPoint]: + def get_points(self, pose_number: int) -> List[Point]: """ Returns points for a specific pos :param pose_number: the desired pos @@ -121,7 +121,7 @@ def get_points(self, pose_number: int) -> List[RawPoint]: pass @abstractmethod - def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]): + def subdivide(self, subdivision_criteria: List[Callable[[PointCloud], bool]]): """ Subdivides all octrees :param subdivision_criteria: criteria for subdivision. @@ -130,7 +130,7 @@ def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]) pass @abstractmethod - def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): + def filter(self, filtering_criteria: List[Callable[[PointCloud], bool]]): """ Filters nodes of each octree with points by criterion :param filtering_criteria: @@ -138,7 +138,7 @@ def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): pass @abstractmethod - def map_leaf_points(self, function: Callable[[RawPointCloud], RawPointCloud]): + def map_leaf_points(self, function: Callable[[PointCloud], PointCloud]): """ Transform point cloud in each node of each octree using the function :param function: transformation function PointCloud -> PointCloud diff --git a/octreelib/internal/point.py b/octreelib/internal/point.py index 4ce89f4..4b40750 100644 --- a/octreelib/internal/point.py +++ b/octreelib/internal/point.py @@ -1,23 +1,24 @@ from __future__ import annotations import itertools -from typing import Annotated, Literal, List +from typing import Annotated, Literal, List, Tuple import numpy as np import numpy.typing as npt -__all__ = ["RawPoint", "RawPointCloud", "CloudManager"] +__all__ = ["Point", "PointCloud", "CloudManager"] """ -RawPoint and RawPointCloud are intended to be used in the methods +Point and PointCloud are intended to be used in the methods which interact with the user or the methods which facilitate those. These are meant to be the main types for Points and Point Clouds to be used by user. """ -RawPoint = Annotated[npt.NDArray[np.float_], Literal[3]] -RawPointCloud = Annotated[npt.NDArray[np.float_], Literal["N", 3]] +Point = Annotated[npt.NDArray[np.float_], Literal[3]] +PointCloud = Annotated[npt.NDArray[np.float_], Literal["N", 3]] +HashablePoint = Tuple[float, float, float] class CloudManager: @@ -28,7 +29,7 @@ def __init__(self): raise TypeError("This class is not intended to be instantiated") @classmethod - def hash_point(cls, point: RawPoint): + def hash_point(cls, point: Point): """ Hash point. :param point: Point to hash. @@ -44,7 +45,7 @@ def empty(cls): return np.empty((0, 3), dtype=float) @classmethod - def add(cls, points_a: RawPoint, points_b: RawPoint): + def add(cls, points_a: Point, points_b: Point): """ Add two point clouds. :param points_a: First point cloud. @@ -55,7 +56,7 @@ def add(cls, points_a: RawPoint, points_b: RawPoint): @classmethod def distribute_grid( - cls, points: RawPointCloud, voxel_size: float, grid_start: RawPoint + cls, points: PointCloud, voxel_size: float, grid_start: Point ): """ Distribute points into voxels. @@ -76,8 +77,8 @@ def distribute_grid( @classmethod def distribute( - cls, points: RawPointCloud, corner_min: RawPoint, edge_length: float - ) -> List[RawPointCloud]: + cls, points: PointCloud, corner_min: Point, edge_length: float + ) -> List[PointCloud]: """ Distribute points into 8 octants. :param points: Points to distribute. diff --git a/octreelib/internal/voxel.py b/octreelib/internal/voxel.py index 1933b4f..67965fc 100644 --- a/octreelib/internal/voxel.py +++ b/octreelib/internal/voxel.py @@ -5,8 +5,8 @@ from octreelib.internal.interfaces import WithID from octreelib.internal.point import ( - RawPoint, - RawPointCloud, + Point, + PointCloud, CloudManager, ) @@ -24,7 +24,7 @@ class VoxelBase(WithID): def __init__( self, - corner_min: RawPoint, + corner_min: Point, edge_length: float, ): self._corner_min = corner_min @@ -44,7 +44,7 @@ def __init__( WithID.__init__(self, self._static_voxel_id_map[voxel_position_hash]) - def is_point_geometrically_inside(self, point: RawPoint) -> bool: + def is_point_geometrically_inside(self, point: Point) -> bool: """ This method checks if the point is inside the voxel geometrically. :param point: Point to check. @@ -86,23 +86,23 @@ class Voxel(VoxelBase): def __init__( self, - corner_min: RawPoint, + corner_min: Point, edge_length: float, - points: Optional[RawPointCloud] = None, + points: Optional[PointCloud] = None, ): super().__init__(corner_min, edge_length) - self._points: RawPointCloud = ( + self._points: PointCloud = ( points if points is not None else np.empty((0, 3), dtype=float) ) - def get_points(self) -> RawPointCloud: + def get_points(self) -> PointCloud: """ :return: Points, which are stored inside the voxel. """ return self._points.copy() - def insert_points(self, points: RawPointCloud): + def insert_points(self, points: PointCloud): """ :param points: Points to insert """ diff --git a/octreelib/octree/octree.py b/octreelib/octree/octree.py index d9e5ee6..4352e79 100644 --- a/octreelib/octree/octree.py +++ b/octreelib/octree/octree.py @@ -7,7 +7,7 @@ import numpy as np -from octreelib.internal import RawPointCloud, T, Voxel, CloudManager +from octreelib.internal import PointCloud, T, Voxel, CloudManager from octreelib.octree.octree_base import OctreeBase, OctreeNodeBase, OctreeConfigBase __all__ = ["OctreeNode", "Octree", "OctreeConfig"] @@ -36,7 +36,7 @@ def __unify(self): self._has_children = False self._children = [] - def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]): + def subdivide(self, subdivision_criteria: List[Callable[[PointCloud], bool]]): """ Subdivide node based on the subdivision criteria. :param subdivision_criteria: list of criteria for subdivision @@ -62,7 +62,7 @@ def subdivide_as(self, other: OctreeNode): elif self._has_children: self.__unify() - def get_points(self) -> RawPointCloud: + def get_points(self) -> PointCloud: """ :return: Points inside the octree node. """ @@ -74,7 +74,7 @@ def get_points(self) -> RawPointCloud: points = np.vstack((points, child.get_points())) return points - def insert_points(self, points: RawPointCloud): + def insert_points(self, points: PointCloud): """ :param points: Points to insert. """ @@ -85,7 +85,7 @@ def insert_points(self, points: RawPointCloud): else: self._points = np.vstack([self._points, points]) - def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): + def filter(self, filtering_criteria: List[Callable[[PointCloud], bool]]): """ Filter nodes with points by filtering criteria :param filtering_criteria: List of filtering criteria functions. @@ -99,10 +99,10 @@ def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): elif not all([criterion(self._points) for criterion in filtering_criteria]): self._points = np.empty((0, 3), dtype=float) - def map_leaf_points(self, function: Callable[[RawPointCloud], RawPointCloud]): + def map_leaf_points(self, function: Callable[[PointCloud], PointCloud]): """ Transform point cloud in the node using the function. - :param function: Transformation function RawPointCloud -> RawPointCloud. + :param function: Transformation function PointCloud -> PointCloud. """ if self._has_children: for child in self._children: @@ -169,7 +169,7 @@ class Octree(OctreeBase, Generic[T]): _node_type = OctreeNode - def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]): + def subdivide(self, subdivision_criteria: List[Callable[[PointCloud], bool]]): """ Subdivide node based on the subdivision criteria. :param subdivision_criteria: list of criteria for subdivision @@ -179,29 +179,29 @@ def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]) def subdivide_as(self, other: Octree): self._root.subdivide_as(other._root) - def get_points(self) -> RawPointCloud: + def get_points(self) -> PointCloud: """ :return: Points, which are stored inside the Octree. """ return self._root.get_points() - def insert_points(self, points: RawPointCloud): + def insert_points(self, points: PointCloud): """ :param points: Points to insert """ self._root.insert_points(points) - def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): + def filter(self, filtering_criteria: List[Callable[[PointCloud], bool]]): """ Filter nodes with points by filtering criteria :param filtering_criteria: List of filtering criteria functions """ self._root.filter(filtering_criteria) - def map_leaf_points(self, function: Callable[[RawPointCloud], RawPointCloud]): + def map_leaf_points(self, function: Callable[[PointCloud], PointCloud]): """ transform point cloud in the node using the function - :param function: transformation function RawPointCloud -> RawPointCloud + :param function: transformation function PointCloud -> PointCloud """ self._root.map_leaf_points(function) diff --git a/octreelib/octree/octree_base.py b/octreelib/octree/octree_base.py index ea94fe9..a420e33 100644 --- a/octreelib/octree/octree_base.py +++ b/octreelib/octree/octree_base.py @@ -6,7 +6,7 @@ import numpy as np -from octreelib.internal.point import RawPoint, RawPointCloud +from octreelib.internal.point import Point, PointCloud from octreelib.internal.voxel import Voxel __all__ = ["OctreeConfigBase", "OctreeBase", "OctreeNodeBase"] @@ -35,7 +35,7 @@ class OctreeNodeBase(Voxel, ABC): and are not stored in the parent node. """ - def __init__(self, corner_min: RawPoint, edge_length: float, position: int): + def __init__(self, corner_min: Point, edge_length: float, position: int): super().__init__(corner_min, edge_length) self._position = position self._points: np.empty((0, 3), dtype=float) @@ -67,7 +67,7 @@ def n_points(self): return @abstractmethod - def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): + def filter(self, filtering_criteria: List[Callable[[PointCloud], bool]]): """ Filter nodes with points by filtering criteria :param filtering_criteria: List of filtering criteria functions @@ -75,10 +75,10 @@ def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): pass @abstractmethod - def map_leaf_points(self, function: Callable[[RawPointCloud], RawPointCloud]): + def map_leaf_points(self, function: Callable[[PointCloud], PointCloud]): """ transform point cloud in the node using the function - :param function: transformation function RawPointCloud -> RawPointCloud + :param function: transformation function PointCloud -> PointCloud """ pass @@ -90,7 +90,7 @@ def get_leaf_points(self) -> List[Voxel]: pass @abstractmethod - def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]): + def subdivide(self, subdivision_criteria: List[Callable[[PointCloud], bool]]): """ Subdivide node based on the subdivision criteria. :param subdivision_criteria: list of criteria for subdivision @@ -98,7 +98,7 @@ def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]) pass @abstractmethod - def get_points(self) -> RawPointCloud: + def get_points(self) -> PointCloud: """ :return: Points, which are stored inside the node. """ @@ -119,7 +119,7 @@ class OctreeBase(Voxel, ABC): def __init__( self, octree_config: OctreeConfigBase, - corner_min: RawPoint, + corner_min: Point, edge_length: float, ): super().__init__(corner_min, edge_length) @@ -151,7 +151,7 @@ def n_points(self): pass @abstractmethod - def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): + def filter(self, filtering_criteria: List[Callable[[PointCloud], bool]]): """ filter nodes with points by criterion :param filtering_criteria: @@ -159,7 +159,7 @@ def filter(self, filtering_criteria: List[Callable[[RawPointCloud], bool]]): pass @abstractmethod - def map_leaf_points(self, function: Callable[[RawPointCloud], RawPointCloud]): + def map_leaf_points(self, function: Callable[[PointCloud], PointCloud]): """ transform point cloud in each node using the function :param function: transformation function PointCloud -> PointCloud @@ -175,7 +175,7 @@ def get_leaf_points(self) -> List[Voxel]: pass @abstractmethod - def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]): + def subdivide(self, subdivision_criteria: List[Callable[[PointCloud], bool]]): """ Subdivide node based on the subdivision criteria. :param subdivision_criteria: list of criteria for subdivision @@ -183,12 +183,12 @@ def subdivide(self, subdivision_criteria: List[Callable[[RawPointCloud], bool]]) pass @abstractmethod - def get_points(self) -> RawPointCloud: + def get_points(self) -> PointCloud: """ :return: Points, which are stored inside the octree. """ pass @abstractmethod - def insert_points(self, points: RawPointCloud): + def insert_points(self, points: PointCloud): pass diff --git a/octreelib/octree_manager/multi_pose_manager.py b/octreelib/octree_manager/multi_pose_manager.py index c40b5f1..e8a3385 100644 --- a/octreelib/octree_manager/multi_pose_manager.py +++ b/octreelib/octree_manager/multi_pose_manager.py @@ -3,7 +3,7 @@ import numpy as np from octreelib.internal.voxel import Voxel, VoxelBase -from octreelib.internal.point import RawPointCloud, RawPoint +from octreelib.internal.point import PointCloud, Point from octreelib.octree.octree import Octree, OctreeConfig __all__ = ["OctreeManager"] @@ -11,7 +11,7 @@ class OctreeManager(VoxelBase): def __init__( - self, octree_config: OctreeConfig, corner_min: RawPoint, edge_length: float + self, octree_config: OctreeConfig, corner_min: Point, edge_length: float ): super().__init__(corner_min, edge_length) self._octree_config = octree_config @@ -20,7 +20,7 @@ def __init__( def subdivide( self, - subdivision_criteria: List[Callable[[RawPointCloud], bool]], + subdivision_criteria: List[Callable[[PointCloud], bool]], pose_numbers: Optional[List[int]], ): if pose_numbers is None: @@ -42,7 +42,7 @@ def subdivide( def map_leaf_points( self, - function: Callable[[RawPointCloud], RawPointCloud], + function: Callable[[PointCloud], PointCloud], pose_numbers: Optional[List[int]] = None, ): if pose_numbers is None: @@ -53,7 +53,7 @@ def map_leaf_points( def filter( self, - filtering_criteria: List[Callable[[RawPointCloud], bool]], + filtering_criteria: List[Callable[[PointCloud], bool]], pose_numbers: Optional[List[int]], ): if pose_numbers is None: @@ -73,7 +73,7 @@ def get_leaf_points(self, pose_number: Optional[int] = None) -> List[Voxel]: ) return self._octrees.get(pose_number, self._empty_octree).get_leaf_points() - def get_points(self, pose_number: Optional[int] = None) -> RawPointCloud: + def get_points(self, pose_number: Optional[int] = None) -> PointCloud: """ :param pose_number: Desired pose number. :return: Points for this pose which are stored inside the octree. @@ -111,7 +111,7 @@ def n_nodes(self, pose_number: Optional[int] = None) -> int: # return sum(octree.n_nodes for octree in self._octrees.values()) return self._octrees.get(pose_number, self._empty_octree).n_nodes - def insert_points(self, pose_number: int, points: RawPointCloud): + def insert_points(self, pose_number: int, points: PointCloud): """ :param pose_number: :param points: Points to insert diff --git a/test/grid/test_grid.py b/test/grid/test_grid.py index 39b37b1..c299387 100644 --- a/test/grid/test_grid.py +++ b/test/grid/test_grid.py @@ -1,13 +1,13 @@ import numpy as np import pytest -from octreelib.internal import RawPointCloud +from octreelib.internal import PointCloud from octreelib.grid import Grid, GridConfig from octreelib.octree import OctreeConfig, Octree from octreelib.octree_manager import OctreeManager -def points_are_same(points_first: RawPointCloud, points_second: RawPointCloud): +def points_are_same(points_first: PointCloud, points_second: PointCloud): return set(map(str, points_first.tolist())) == set(map(str, points_second.tolist())) diff --git a/test/octree/test_multi_pose.py b/test/octree/test_multi_pose.py index ce3d0fb..27ed551 100644 --- a/test/octree/test_multi_pose.py +++ b/test/octree/test_multi_pose.py @@ -1,12 +1,12 @@ import numpy as np import pytest -from octreelib.internal import RawPointCloud, Voxel +from octreelib.internal import PointCloud, Voxel from octreelib.octree import OctreeConfig from octreelib.octree_manager import OctreeManager -def points_are_same(points_first: RawPointCloud, points_second: RawPointCloud): +def points_are_same(points_first: PointCloud, points_second: PointCloud): return set(map(str, points_first.tolist())) == set(map(str, points_second.tolist()))