primlib 0.3.0__py3-none-any.whl

primlib/point_cloud.py

@@ -0,0 +1,711 @@
+from collections.abc import Callable
+from typing import override
+
+import matplotlib.pyplot as plt
+import numpy as np
+from numpy.typing import ArrayLike
+from scipy.sparse import csr_array
+from scipy.sparse.csgraph import connected_components
+from scipy.spatial import KDTree
+from trafolib.trafo3d import Trafo3d
+
+from primlib.vertices import Vertices
+
+
+class PointCloud(Vertices):
+    """
+    A class representing a point cloud of 3d points.
+    """
+
+    def __init__(
+        self,
+        vertices: ArrayLike | None = None,
+        normals: ArrayLike | None = None,
+        colors: ArrayLike | None = None,
+    ) -> None:
+        """Constructor
+
+        :param vertices: Vertices/points of the point cloud, shape (n, 3)
+        :param normals: Normals for each point, shape (n, 3), normalized
+        :param colors: RGB colors for each point, shape (n, 3), values [0.0..1.0]
+        """
+        super().__init__(vertices, normals, colors)
+
+    def __len__(self) -> int:
+        """Number of points in point cloud
+        :returns: Number of points
+        """
+        return self._vertices.shape[0]
+
+    @property
+    def size(self) -> int:
+        """Number of points in point cloud
+        :returns: Number of points
+        """
+        return self._vertices.shape[0]
+
+    @property
+    def empty(self) -> bool:
+        """Check if point cloud is empty
+        :returns: True if point cloud has no points
+        """
+        return self.size == 0
+
+    @override
+    def __str__(self) -> str:
+        """Get string representation of point cloud
+        :returns: String representation
+        """
+        if self.empty:
+            return "PointCloud(empty)"
+        pmin = self.min()
+        pmax = self.max()
+        result = (
+            f"PointCloud({self.size:_} vertices: "
+            + f"X[{pmin[0]:.3f}..{pmax[0]:.3f}], "
+            + f"Y[{pmin[1]:.3f}..{pmax[1]:.3f}], "
+            + f"Z[{pmin[2]:.3f}..{pmax[2]:.3f}]"
+        )
+        if self.has_normals:
+            result += ", normals"
+        if self.has_colors:
+            result += ", colors"
+        result += ")"
+        return result
+
+    @classmethod
+    def create_plane(
+        cls,
+        scale: float | ArrayLike = 1.0,
+        resolution: int | ArrayLike = (10, 10),
+    ) -> "PointCloud":
+        """Create point cloud with points arranged as grid in a 2d plane
+
+        Grid is located in X/Y plane and starts at (0.0, 0.0, 0.0) and
+        extends to (scale, scale, 0.0).
+
+        :param scale: Scale of grid for X and Y as single float or 2d
+        :param resolution: Number of points in X and Y
+        :returns: Resulting point cloud
+        """
+        if isinstance(scale, float):
+            scale = np.array((scale, scale))
+        else:
+            scale = np.asarray(scale)
+            if scale.size != 2:
+                raise Exception("Provide proper scale.")
+        if isinstance(resolution, int):
+            resolution = np.array((resolution, resolution), dtype=int)
+        else:
+            resolution = np.asarray(resolution, dtype=int)
+            if resolution.size != 2:
+                raise Exception("Provide proper resolution.")
+        xs = np.linspace(0.0, 1.0, resolution[0])
+        ys = np.linspace(0.0, 1.0, resolution[1])
+        (
+            xx,
+            yy,
+        ) = np.meshgrid(xs, ys, indexing="xy")
+        vertices = np.empty((resolution[0] * resolution[1], 3))
+        vertices[:, 0] = scale[0] * xx.flatten()
+        vertices[:, 1] = scale[1] * yy.flatten()
+        vertices[:, 2] = 0.0
+        return cls(vertices)
+
+    @classmethod
+    def create_grid(
+        cls,
+        scale: float | ArrayLike = 1.0,
+        resolution: int | ArrayLike = (10, 10, 10),
+    ) -> "PointCloud":
+        """Create point cloud with point arranged as 3d grid
+
+        Grid starts at (0.0, 0.0, 0.0) and extends to (scale, scale, scale).
+
+        :param scale: Scale of grid for X, Y and Z as single float or 3d
+        :param resolution: Number of points in X, Y and Z
+        :returns: Resulting point cloud
+        """
+        if isinstance(scale, float):
+            scale = np.array((scale, scale, scale))
+        else:
+            scale = np.asarray(scale)
+            if scale.size != 3:
+                raise Exception("Provide proper scale.")
+        if isinstance(resolution, int):
+            resolution = np.array((resolution, resolution, resolution), dtype=int)
+        else:
+            resolution = np.asarray(resolution, dtype=int)
+            if resolution.size != 3:
+                raise Exception("Provide proper resolution.")
+        xs = np.linspace(0.0, 1.0, resolution[0])
+        ys = np.linspace(0.0, 1.0, resolution[1])
+        zs = np.linspace(0.0, 1.0, resolution[2])
+        xx, yy, zz = np.meshgrid(xs, ys, zs, indexing="xy")
+        vertices = np.empty((resolution[0] * resolution[1] * resolution[2], 3))
+        vertices[:, 0] = scale[0] * xx.flatten()
+        vertices[:, 1] = scale[1] * yy.flatten()
+        vertices[:, 2] = scale[2] * zz.flatten()
+        return cls(vertices)
+
+    @classmethod
+    def create_random(
+        cls,
+        scale: float | ArrayLike = 1.0,
+        num_points: int = 1000,
+        rng: np.random.Generator | None = None,
+    ) -> "PointCloud":
+        """Create point cloud of random points
+
+        Points are uniformly distributed between (0.0, 0.0, 0.0)
+        and (scale, scale, scale).
+
+        :param scale: Scale of grid for X, Y and Z as single float or 3d
+        :param rng: NumPy random number generator
+        :returns: Resulting point cloud
+        """
+        if isinstance(scale, float):
+            scale = np.array((scale, scale, scale))
+        else:
+            scale = np.asarray(scale)
+            if scale.size != 3:
+                raise Exception("Provide proper scale.")
+        if rng is None:
+            rng = np.random.default_rng()
+        vertices = np.empty((num_points, 3))
+        vertices[:, 0] = rng.uniform(0.0, scale[0], num_points)
+        vertices[:, 1] = rng.uniform(0.0, scale[1], num_points)
+        vertices[:, 2] = rng.uniform(0.0, scale[2], num_points)
+        return PointCloud(vertices)
+
+    def select(self, inlier_mask_or_indices: np.ndarray) -> None:
+        """Select points from point cloud by mask or indices
+
+        :param inlier_mask_or_indices: Inlier point mask or indices
+        """
+        inlier_mask_or_indices = np.asarray(inlier_mask_or_indices)
+        if inlier_mask_or_indices.size == 0:
+            self._vertices = np.zeros((0, 3))
+            self._normals = np.zeros((0, 3))
+            self._colors = np.zeros((0, 3))
+        else:
+            self._vertices = self._vertices[inlier_mask_or_indices, :]
+            if self.has_normals:
+                self._normals = self._normals[inlier_mask_or_indices, :]
+            if self.has_colors:
+                self._colors = self._colors[inlier_mask_or_indices, :]
+
+    def get_selected(self, inlier_mask_or_indices: np.ndarray) -> "PointCloud":
+        """Get new point cloud of selected points by mask or indices
+
+        :param inlier_mask_or_indices: Inlier point mask or indices
+        :returns: Resulting point cloud
+        """
+        result = self.copy()
+        result.select(inlier_mask_or_indices)
+        return result
+
+    def filter_radius_outliers(self, num_points: int, radius: float) -> np.ndarray:
+        """Radius outlier filtering for point cloud
+
+        :param num_points: Minimum number of neighbors to keep point
+        :param radius: Radius defining point neighborhood
+        :returns: Inlier mask
+        """
+        counts = self.kdtree().query_ball_point(
+            self._vertices, r=radius, return_length=True, workers=-1
+        )
+        return counts >= num_points
+
+    def estimate_normals(self, cluster_size=30) -> None:
+        """Estimate normals for each point of point cloud
+
+        For each point uses a neighborhood of points to estimate normals
+
+        Direction of normals is computed but orientation may be wrong.
+
+        :param cluster_size: Number of neighboring points
+        """
+        if cluster_size < 3:
+            raise ValueError("cluster_size must be at least 3")
+        if cluster_size > self.size:
+            raise ValueError("cluster_size cannot exceed point cloud size")
+        _, indices = self.kdtree().query(self.vertices, k=cluster_size, workers=-1)
+        points = self.vertices[indices]
+        centroids = np.mean(points, axis=1, keepdims=True)
+        C = points - centroids
+        covariances = np.matmul(C.transpose(0, 2, 1), C) / (cluster_size - 1)
+        _, v = np.linalg.eigh(covariances)
+        self.normals = v[:, :, 0]
+
+    def orient_normals_towards_position(self, position: ArrayLike) -> None:
+        """Orients normals towards a 3d position (e.g. a camera)
+
+        :param position: 3d position
+        """
+        towards_position = np.asarray(position) - self._vertices
+        dots = np.sum(self._normals * towards_position, axis=1)
+        flip_mask = dots < 0
+        self._normals[flip_mask] *= -1
+
+    def segment_model(
+        self,
+        model_fit: Callable[[ArrayLike], dict],
+        model_abs_distance: Callable[[ArrayLike, dict], np.ndarray],
+        threshold: float,
+        num_samples: int,
+        num_iterations: int,
+        rng: np.random.Generator | None = None,
+    ) -> tuple[dict, np.ndarray]:
+        """Segment point cloud using RANSAC based on model
+
+        Function does not check weather num_samples is large enough
+        to estimate model
+
+        :param model_fit: Model fitting/estimation function: Takes n
+            points as input and returns model parameters as dict
+        :param model_abs_distance: Model distance function: Takes n
+            points and model parameters as dict as input and returns
+            absolute distances of model to points
+        :param threshold: Distance threshold to determine which points
+            are considered outliers
+        :param num_samples: Number of random samples used to estimate model
+        :param num_iterations: Number of attempts in RANSAC to find
+            model with more inliers
+        :param rng: NumPy random number generator
+        :returns: Model parameters and boolean inlier mask
+        """
+        if rng is None:
+            rng = np.random.default_rng()
+        best_inlier_mask = np.zeros(self.size, dtype=bool)
+        for _ in range(num_iterations):
+            indices = rng.choice(self.size, num_samples, replace=False)
+            model = model_fit(self._vertices[indices])
+            distances = model_abs_distance(self._vertices, model)
+            inlier_mask = distances <= threshold
+            if np.sum(inlier_mask) > np.sum(best_inlier_mask):
+                best_inlier_mask = inlier_mask
+        if not np.any(best_inlier_mask):
+            raise RuntimeError("RANSAC could not find a valid model")
+        return model_fit(self._vertices[best_inlier_mask]), best_inlier_mask
+
+    @staticmethod
+    def _model_fit_plane(points: ArrayLike) -> dict:
+        """Helper method to estimate 3d plane from points
+
+        The plane is parameterized as Hesse normal form:
+        E(x,y,z): nx * x + ny * y + nz * z + d = 0
+        with "normal": (nx, ny, nz) the normal vector of the plane
+        and "distance": d the distance between the plane and
+        origin (0, 0, 0).
+
+        :param points: n sample points, shape (n, 3)
+        :returns: Plane model as a dict
+        """
+        points = np.asarray(points)
+        centroid = np.mean(points, axis=0)
+        centered_points = points - centroid
+        _, _, Vt = np.linalg.svd(centered_points)
+        n = Vt[-1]
+        d = -np.dot(centroid, n)
+        if d < 0:
+            n = -n
+            d = -d
+        return {"normal": n, "distance": d}
+
+    @staticmethod
+    def _model_abs_distance_plane(points: ArrayLike, model: dict) -> np.ndarray:
+        """Helper method to estimate distance between points and 3d plane
+
+        The plane is parameterized as Hesse normal form:
+        E(x,y,z): nx * x + ny * y + nz * z + d = 0
+        with "normal": (nx, ny, nz) the normal vector of the plane
+        and "distance": d the distance between the plane and
+        origin (0, 0, 0).
+
+        :param points: n sample points, shape (n, 3)
+        :param model: Plane model as a dict
+        :returns: Absolute distances, shape (n,)
+        """
+        return np.abs(np.dot(points, model["normal"]) + model["distance"])
+
+    def segment_plane(
+        self,
+        threshold: float,
+        num_samples: int,
+        num_iterations: int,
+        rng: np.random.Generator | None = None,
+    ) -> tuple[dict, np.ndarray]:
+        """Segment 3d plane in point cloud using RANSAC
+
+        :param threshold: Distance threshold to determine which points
+            are considered outliers
+        :param num_samples: Number of random samples used to estimate model
+        :param num_iterations: Number of attempts in RANSAC to find
+            model with more inliers
+        :param rng: NumPy random number generator
+        :returns: Model parameters and boolean inlier mask
+        """
+        if num_samples < 3:
+            raise Exception("Not enough samples to fit a plane")
+        return self.segment_model(
+            PointCloud._model_fit_plane,
+            PointCloud._model_abs_distance_plane,
+            threshold,
+            num_samples,
+            num_iterations,
+            rng,
+        )
+
+    @staticmethod
+    def _model_fit_sphere(points: ArrayLike) -> dict:
+        """Helper method to estimate 3d sphere from points
+
+        The sphere is parameterized with "center": (x0, y0, z0)
+        the center of the sphere and "radius": the radius of
+        the sphere.
+
+        :param points: n sample points, shape (n, 3)
+        :returns: Sphere model as a dict
+        """
+        points = np.asarray(points)
+        # Set up design matrix A and target vector b for algebraic sphere fitting
+        # with center=(x0, y0, z0) and radius R
+        # 2*x0*x + 2*y0*y + 2*z0*z + D = x^2 + y^2 + z^2
+        # where D = R^2 - x0^2 - y0^2 - z0^2
+        A = np.column_stack((2 * points, np.ones(len(points))))
+        b = np.sum(points**2, axis=1)
+        # Solve the least squares problem: A * [x0, y0, z0, D]^T = b
+        params, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
+        x0, y0, z0, d = params
+        center = np.array([x0, y0, z0])
+        radius_squared = d + np.dot(center, center)
+        radius = np.sqrt(max(0.0, radius_squared))
+        return {"center": center, "radius": radius}
+
+    @staticmethod
+    def _model_abs_distance_sphere(points: ArrayLike, model: dict) -> np.ndarray:
+        """Helper method to estimate distance between points and 3d sphere
+
+        The sphere is parameterized with "center": (x0, y0, z0)
+        the center of the sphere and "radius": the radius of
+        the sphere.
+
+        :param points: n sample points, shape (n, 3)
+        :param model: Sphere model as a dict
+        :returns: Absolute distances, shape (n,)
+        """
+        distances_to_center = np.linalg.norm(points - model["center"], axis=-1)
+        return np.abs(distances_to_center - model["radius"])
+
+    def segment_sphere(
+        self,
+        threshold: float,
+        num_samples: int,
+        num_iterations: int,
+        rng: np.random.Generator | None = None,
+    ) -> tuple[dict, np.ndarray]:
+        """Segment 3d sphere in point cloud using RANSAC
+
+        :param threshold: Distance threshold to determine which points
+            are considered outliers
+        :param num_samples: Number of random samples used to estimate model
+        :param num_iterations: Number of attempts in RANSAC to find
+            model with more inliers
+        :param rng: NumPy random number generator
+        :returns: Model parameters and boolean inlier mask
+        """
+        if num_samples < 4:
+            raise Exception("Not enough samples to fit a sphere")
+        return self.segment_model(
+            PointCloud._model_fit_sphere,
+            PointCloud._model_abs_distance_sphere,
+            threshold,
+            num_samples,
+            num_iterations,
+            rng,
+        )
+
+    def random_downsample(
+        self, keep_ratio: float, rng: np.random.Generator | None = None
+    ) -> "PointCloud":
+        """Downsample point cloud by randomly selecting points to keep
+
+        Downsampling a point cloud with 100 points and keep_ratio=0.13 keeps
+        13 random points and removes the other 87.
+
+        :param keep_ratio: Ratio of points to keep, between 0.0 and 1.0
+        :param rng: NumPy random number generator
+        :return: Downsampled point cloud
+        """
+        if rng is None:
+            rng = np.random.default_rng()
+        keep_ratio = np.clip(keep_ratio, 0.0, 1.0)
+        num_keep = np.round(keep_ratio * self.size).astype(int)
+        keep_indices = rng.choice(self.size, num_keep, replace=False)
+        return self.get_selected(keep_indices)
+
+    def voxel_downsample(self, voxel_size: float) -> "PointCloud":
+        """Downsample point cloud using voxel downsampling.
+
+        Points within the same voxel are replaced by their centroid.
+        Colors are averaged and normals are averaged and normalized.
+
+        :param voxel_size: Size of voxel as a scalar
+        :return: Downsampled point cloud
+        """
+        if voxel_size <= 0:
+            raise ValueError("voxel_size must be positive")
+        if self.empty:
+            return self.copy()
+        # Determine voxel indices
+        voxel_indices = np.floor(self._vertices / voxel_size).astype(int)
+        # Group points by voxel
+        _, inverse_indices = np.unique(voxel_indices, axis=0, return_inverse=True)
+        num_voxels = inverse_indices.max() + 1
+        # Count points per voxel
+        counts = np.bincount(inverse_indices)
+        counts = counts[:, None]
+        # Aggregate vertices
+        vertices = np.zeros((num_voxels, 3))
+        np.add.at(vertices, inverse_indices, self._vertices)
+        vertices /= counts
+        # Aggregate colors
+        colors = None
+        if self.has_colors:
+            colors = np.zeros((num_voxels, 3))
+            np.add.at(colors, inverse_indices, self._colors)
+            colors /= counts
+        # Aggregate normals
+        normals = None
+        if self.has_normals:
+            normals = np.zeros((num_voxels, 3))
+            np.add.at(normals, inverse_indices, self._normals)
+            normals /= counts
+            # Normalize averaged normals
+            lengths = np.linalg.norm(normals, axis=1, keepdims=True)
+            # Handle opposing normals (resulting in zero-length vectors)
+            valid = lengths[:, 0] > 1e-12
+            normals[valid] /= lengths[valid]
+            normals[~valid] = (
+                self._normals[np.argmax(np.bincount(inverse_indices)[inverse_indices])]
+                if False
+                else normals[~valid]
+            )
+        return PointCloud(vertices, normals, colors)
+
+    def distances(
+        self, other: "ArrayLike | PointCloud"
+    ) -> tuple[np.ndarray, np.ndarray]:
+        """Compute unsigned distance of point cloud "other" to "self"
+
+        For every point in "other" we find the closest point in "self" and
+        the distance between them.
+
+        This distance calculation is not symmetrical, self.distances(other)
+        is not the same as other.distances(self).
+
+        :param other: Other point cloud with n points
+        :returns: Distances of shape (n,) and Indices of nearest
+            points in self as integer
+        """
+        kdtree = self.kdtree()
+        if isinstance(other, PointCloud):
+            points = other.vertices
+        else:
+            points = np.asarray(other, dtype=float)
+        dists, indices = kdtree.query(points, 1, workers=-1)
+        return dists, indices
+
+    def chamfer_distance(self, other: "ArrayLike | PointCloud") -> float:
+        """Computes Chamfer distance between two point clouds
+
+        :param other: Other point cloud
+        :returns: Chamfer distance
+        """
+        dist0, _ = self.distances(other)
+        if isinstance(other, PointCloud):
+            dist1, _ = other.distances(self)
+        else:
+            dist1, _ = PointCloud(vertices=other).distances(self)
+        return float(0.5 * np.mean(dist0) + 0.5 * np.mean(dist1))
+
+    def estimate_transform(self, other: "ArrayLike | PointCloud") -> Trafo3d:
+        """Estimate rigid transformation between two point clouds
+
+        Point clouds have to have the same number of points and the first
+        point in "self" should correspond with the first point in "other"
+        and so on.
+
+        Solves the constraint orthogonal Procrustes problem using the
+        Kabsch-Umeyama algorithm.
+
+        :param other: Other point cloud with n points
+        :returns: Transformation from self to other
+        """
+        if isinstance(other, PointCloud):
+            other = other.vertices
+        else:
+            other = np.asarray(other)
+        if self.size != other.shape[0]:
+            raise Exception(
+                "Needs two corresponding point clouds of same size "
+                "to estimate transform"
+            )
+        # Centroids
+        centroid_A = np.mean(other, axis=0)
+        centroid_B = np.mean(self._vertices, axis=0)
+        # De-mean point clouds
+        AA = other - centroid_A
+        BB = self._vertices - centroid_B
+        # Covariance matrix
+        H = AA.T @ BB
+        # Singular value decomposition
+        U, _, Vt = np.linalg.svd(H)
+        R = Vt.T @ U.T
+        # Handle reflection case (det(R) == -1)
+        if np.linalg.det(R) < 0:
+            Vt[-1, :] *= -1
+            R = Vt.T @ U.T
+        return Trafo3d(t=centroid_B - R @ centroid_A, mat=R)
+
+    def icp(
+        self,
+        other: "ArrayLike | PointCloud",
+        max_iterations: int = 100,
+        tolerance: float = 1e-6,
+    ) -> Trafo3d:
+        """Point cloud registration using Iterative Closest Point (ICP)
+
+        No correspondences between point clouds needed. If the point clouds
+        are far apart, algorithm may end in local minimum. Use other methods
+        (like keypoint/feature matching) first for a better initial value.
+
+        Warning: ICP is not necessarily a symmetric operation, so
+        self.icp(other) != other.icp(self).
+
+        :param other: Other point cloud with n points
+        :param max_iterations: Number maximum number of iterations
+        :param tolerance: If mean distance improvement from iteration to
+            iteration is smaller than tolerance, stop iteration
+        :returns: Transformation from self to other
+        """
+        if isinstance(other, PointCloud):
+            other = other.vertices
+        else:
+            other = np.asarray(other)
+        prev_err = np.inf
+        if self.size >= other.shape[0]:
+            # Move "other" until it fits to "self"
+            # Optimizes distances from transformed other to self
+            kdtree = self.kdtree()
+            dst = other.copy()
+            for _ in range(max_iterations):
+                distances, indices = kdtree.query(dst)
+                trafo = self.get_selected(indices).estimate_transform(dst)
+                dst = trafo * dst
+                err = np.abs(np.mean(distances))
+                if np.abs(prev_err - err) < tolerance:
+                    break
+                prev_err = err
+            return PointCloud(other).estimate_transform(dst).inverse
+        else:
+            # Move "self" until it fits to "other"
+            # Optimizes distances from transformed self to other
+            kdtree = KDTree(other)
+            src = self._vertices.copy()
+            for _ in range(max_iterations):
+                distances, indices = kdtree.query(src)
+                trafo = PointCloud(other[indices]).estimate_transform(src)
+                src = trafo * src
+                err = np.abs(np.mean(distances))
+                if np.abs(prev_err - err) < tolerance:
+                    break
+                prev_err = err
+            return self.estimate_transform(src)
+
+    def cluster_dbscan(self, eps: float, min_points: int) -> np.ndarray:
+        """Cluster point cloud using DBSCAN algorithm
+
+        Ester, M. and Kriegel, H.-P. and Sander J. and Xu X.: A density-based
+            algorithm for discovering clusters in large spatial databases
+            with noise, 1996.
+
+        :param eps: Maximum distance between two points to be considered neighbors
+        :param min_points: Minimum number of points (including the point itself)
+            within eps for a point to be a core point
+        """
+        # Count number of neighbors for each point in eps environment
+        kdtree = self.kdtree()
+        neighbors = kdtree.query_ball_point(self._vertices, r=eps, workers=-1)
+        num_neighbors = np.fromiter(
+            (len(nb) for nb in neighbors), dtype=int, count=self.size
+        )
+        # Core points have at least min_points neighbors
+        is_core = num_neighbors >= min_points
+        # Build graph containing only core-to-core connections
+        rows = np.repeat(np.arange(self.size), num_neighbors)
+        cols = np.concatenate(neighbors) if self.size > 0 else np.empty(0, dtype=int)
+        keep = is_core[rows] & is_core[cols]
+        core_rows = rows[keep]
+        core_cols = cols[keep]
+        graph = csr_array(
+            (np.ones(core_rows.shape[0], dtype=np.int8), (core_rows, core_cols)),
+            shape=(self.size, self.size),
+        )
+        # Calculate connected components using core points only
+        _, comp = connected_components(graph, directed=False, return_labels=True)
+        # Initialize labels: -1 means noise
+        labels = np.full(self.size, -1, dtype=np.int64)
+        # Assign cluster labels to core points
+        labels[is_core] = comp[is_core]
+        # Assign border points to neighboring core points
+        for i in np.flatnonzero(~is_core):
+            core_neighbors = np.asarray(neighbors[i], dtype=int)
+            core_neighbors = core_neighbors[is_core[core_neighbors]]
+            if core_neighbors.size > 0:
+                labels[i] = labels[core_neighbors[0]]
+        # Relabel clusters consecutively starting at zero
+        clustered = labels >= 0
+        _, labels[clustered] = np.unique(labels[clustered], return_inverse=True)
+        if False:
+            # 2D visualization projected into X/Y plane;
+            # use for illustration with few points only
+            _, ax = plt.subplots()
+            is_noise = labels == -1
+            is_border = ~is_core & ~is_noise
+            # Core points: Data points that have at least min_points
+            # neighbors within a radius eps
+            ax.plot(
+                self._vertices[is_core, 0],
+                self._vertices[is_core, 1],
+                "go",
+                label="Core",
+            )
+            # Border points: Points that have fewer neighbors than min_points,
+            # but fall within the eps radius of a core point
+            ax.plot(
+                self._vertices[is_border, 0],
+                self._vertices[is_border, 1],
+                "yo",
+                label="Border",
+            )
+            # Points that are neither core nor border points
+            ax.plot(
+                self._vertices[is_noise, 0],
+                self._vertices[is_noise, 1],
+                "ro",
+                label="Noise",
+            )
+            # Draw graph connections
+            for row, col in zip(*graph.nonzero(), strict=True):
+                ax.plot(
+                    (self._vertices[row, 0], self._vertices[col, 0]),
+                    (self._vertices[row, 1], self._vertices[col, 1]),
+                    "-k",
+                )
+            for pos, label in zip(self._vertices[:, 0:2], labels, strict=True):
+                ax.text(*pos, f"{label}")
+            ax.legend()
+            plt.show()
+        return labels