import numpy as np
import random
from typing import List, Optional, Tuple, Union
import open3d as o3d
from PIL import Image
from sklearn.neighbors import KDTree
REFERENCE_SIZE = 100
CAMERA_VIEWS = {
"3rd_person": {
"zoom": 0.12,
"front": np.array([1, 0, 0.5], dtype=np.float32), # Camera front vector
"lookat": np.array([1, 0.0, 0.0], dtype=np.float32), # Point camera looks at
"up": np.array([-0.5, 0, 1], dtype=np.float32), # Camera up direction
},
"3rd_person_opposite": {
"zoom": 0.12,
"front": np.array([-1, 0, 0.5], dtype=np.float32), # Camera front vector
"lookat": np.array([1, 0.0, 0.0], dtype=np.float32), # Point camera looks at
"up": np.array([-0.5, 0, 1], dtype=np.float32), # Camera up direction
},
"top": {
"zoom": 0.025,
"front": np.array([0, 0, -1], dtype=np.float32), # Looking straight down
"lookat": np.array([1, 0.0, 0.0], dtype=np.float32), # Same target point
"up": np.array([0, 1, 0], dtype=np.float32), # Y axis is "up" in image
},
"side": {
"zoom": 0.012,
"front": np.array(
[0, -1, 0], dtype=np.float32
), # Looking from positive Y toward origin
"lookat": np.array([1, 0.0, 0.0], dtype=np.float32), # Same target point
"up": np.array([0, 0, 1], dtype=np.float32), # Z axis is up
},
}
[docs]
class Sampler:
"""Init point cloud sampler
:param point_cloud_size: Total number of points in the point cloud
:type point_cloud_size: int
:param search_tree: Search tree for the point cloud data
:type search_tree: KDTree
:param sampler_name: Sampler name (e.g. random, spatially_regular)
:type sampler_name: str
:param num_classes: Number of classes in the dataset
:type num_classes: int
:param seed: Random seed, defaults to 42
:type seed: int, optional
:raises NotImplementedError: _description_
"""
def __init__(
self,
point_cloud_size: int,
search_tree: KDTree,
sampler_name: str,
num_classes: int,
seed: int = 42,
):
np.random.seed(seed)
random.seed(seed)
self.search_tree = search_tree
self.num_classes = num_classes
self.p = np.random.rand(point_cloud_size) * 1e-3
self.min_p = float(np.min(self.p[-1]))
self.test_probs = np.zeros(
(point_cloud_size, self.num_classes), dtype=np.float32
)
if sampler_name == "random":
self.sample = self.random
elif sampler_name == "spatially_regular":
self.sample = self.spatially_regular
else:
raise NotImplementedError(
f"Sampler {self.model_cfg['sampler']} not implemented"
)
def _get_indices(
self, point_cloud_size: int, num_points: int, center_point: np.ndarray
) -> np.ndarray:
"""Get indices to sample given a center point
:param point_cloud_size: Current point cloud size
:type point_cloud_size: int
:param num_points: Number of points to sample
:type num_points: int
:param center_point: Center point for sampling
:type center_point: np.ndarray
:return: Indices of points to sample
:rtype: np.ndarray
"""
# Sample only if the number of points is less than the required number of points
if point_cloud_size < num_points:
diff = num_points - point_cloud_size
indices = np.array(range(point_cloud_size))
indices = list(indices) + list(random.choices(indices, k=diff))
indices = np.asarray(indices)
else:
indices = self.search_tree.query(center_point, k=num_points)[1][0]
return indices
[docs]
def random(
self, points: np.ndarray, num_points: int
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Random sampling
:param points: Point cloud data
:type points: np.ndarray
:param num_points: Number of points to sample
:type num_points: int
:return: Sampled points, and their respective indices and center point
:rtype: Tuple[np.ndarray, np.ndarray, np.ndarray]
"""
# Get center point randomly
center_idx = np.random.choice(len(points), 1)
center_point = points[center_idx, :].reshape(1, -1)
# Get indices to sample and shuffle them
indices = self._get_indices(points.shape[0], num_points, center_point)
random.shuffle(indices)
# Get sampled points
points = points[indices]
return points, indices, center_point
[docs]
def spatially_regular(
self,
points: np.ndarray,
num_points: Optional[int] = None,
radius: Optional[float] = None,
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Spatially regular sampling
:param points: Point cloud data
:type points: np.ndarray
:param num_points: Number of points to sample
:type num_points: Optional[int]
:param radius: Radius for spatially regular sampling
:type radius: Optional[float]
:return: Sampled points, and their respective indices and center point
:rtype: Tuple[np.ndarray, np.ndarray, np.ndarray]
"""
n = 0
while n < 2:
# Keep as center point the one with the lowest probability
center_idx = np.argmin(self.p)
center_point = points[center_idx, :].reshape(1, -1)
# Get indices to sample
if radius is not None:
indices = self.search_tree.query_radius(center_point, r=radius)[0]
elif num_points is not None:
indices = self._get_indices(points.shape[0], num_points, center_point)
else:
raise ValueError("Either num_points or radius must be provided")
n = len(indices)
# Special case, less than 2 points in the cloud
if n < 2:
self.p[center_idx] += 0.001
# Shuffle indices and sample
random.shuffle(indices)
points = points[indices]
# Use inverse distance to center point as an increment in probability to be
# sampled in the future
dists = np.sum(np.square((points - center_point).astype(np.float32)), axis=1)
delta = np.square(1 - dists / np.max(dists))
self.p[indices] += delta
new_min = float(np.min(self.p))
self.min_p = new_min
return points, indices, center_point
[docs]
def recenter(points: np.ndarray, dims: List[int]) -> np.ndarray:
"""Recenter a point cloud along the specified dimensions
:param points: Point cloud data
:type points: np.ndarray
:param dims: Dimensions to recenter
:type dims: List[int]
:return: Recentred point cloud data
:rtype: np.ndarray
"""
points[:, dims] = points[:, dims] - points.mean(0)[dims]
return points
[docs]
def build_point_cloud(
points: np.ndarray, colors: np.ndarray
) -> o3d.geometry.PointCloud:
"""Build a point cloud
:param points: Point cloud data
:type points: np.ndarray
:param colors: Colors for the point cloud data
:type colors: np.ndarray
:return: Point cloud
:rtype: o3d.geometry.PointCloud
"""
point_cloud = o3d.geometry.PointCloud()
point_cloud.points = o3d.utility.Vector3dVector(points)
point_cloud.colors = o3d.utility.Vector3dVector(colors)
return point_cloud
[docs]
def view_point_cloud(points: np.ndarray, colors: np.ndarray):
"""Visualize a single point cloud
:param points: Point cloud data
:type points: np.ndarray
:param colors: Colors for the point cloud data
:type colors: np.ndarray
"""
point_cloud = build_point_cloud(points, colors)
o3d.visualization.draw_geometries([point_cloud])
[docs]
def render_point_cloud(
points: np.ndarray,
colors: np.ndarray,
camera_view: Union[str, dict] = "3rd_person",
bg_color: Optional[List[float]] = [0.0, 0.0, 0.0, 1.0],
color_jitter: float = 0.05,
point_size: float = 3.0,
resolution: Tuple[int, int] = (1920, 1080),
render_origin: bool = False,
origin_size: float = 0.5,
) -> Image:
"""Render a given point cloud from a specific camera view and return the image
:param points: Point cloud data
:type points: np.ndarray
:param colors: Colors for the point cloud data
:type colors: np.ndarray
:param camera_view: Camera view (either ID or dictionary containing camera definition), defaults to "3rd_person"
:type camera_view: Union[str, dict], optional
:param bg_color: Background color, defaults to black -> [0., 0., 0., 1.]
:type bg_color: Optional[List[float]], optional
:param color_jitter: Jitters the colors by a random value between [-color_jitter, color_jitter], defaults to 0.05
:type color_jitter: float, optional
:param point_size: Point size, defaults to 3.0
:type point_size: float, optional
:param resolution: Render resolution, defaults to (1920, 1080)
:type resolution: Tuple[int, int], optional
:param render_origin: Whether to render the origin axes, defaults to False
:type render_origin: bool, optional
:param origin_size: Size of the origin axes, defaults to 0.5
:type origin_size: float, optional
:return: Rendered point cloud
:rtype: Image
"""
if isinstance(camera_view, dict):
# If camera_view is a dictionary, use it directly
view_settings = camera_view
elif isinstance(camera_view, str):
# If camera_view is a string, look it up in predefined views
assert camera_view in CAMERA_VIEWS, f"Camera view {camera_view} not implemented"
view_settings = CAMERA_VIEWS[camera_view]
# Add color jitter if needed
if color_jitter > 0:
jitter = np.random.uniform(-color_jitter, color_jitter, (points.shape[0], 1))
colors += jitter
# Build point cloud object
point_cloud = build_point_cloud(points, colors)
# Set up the offscreen renderer
width, height = resolution
renderer = o3d.visualization.rendering.OffscreenRenderer(width, height)
# Create material and add the point cloud to the scene
material = o3d.visualization.rendering.MaterialRecord()
material.shader = "defaultUnlit" # Use unlit shader for better visibility of colors
material.sRGB_color = True
material.point_size = point_size
renderer.scene.add_geometry("point_cloud", point_cloud, material)
# Add origin axes for reference
if render_origin:
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=origin_size, origin=[0, 0, 0]
)
coord_material = o3d.visualization.rendering.MaterialRecord()
coord_material.shader = "defaultUnlit" # Also unlit for visibility
renderer.scene.add_geometry("coordinate_frame", coord_frame, coord_material)
# Set the background color
renderer.scene.set_background(bg_color)
# Set up the camera
camera_distance = 1 / view_settings["zoom"]
camera_position = view_settings["lookat"] + view_settings["front"] * camera_distance
renderer.setup_camera(
vertical_field_of_view=60.0, # Field of view in degrees
center=view_settings["lookat"],
eye=camera_position,
up=view_settings["up"],
)
# Render the scene to an image
image = np.asarray(renderer.render_to_image())
image = Image.fromarray(image)
# Cleanup
renderer.scene.clear_geometry()
return image
[docs]
def read_semantickitti_points(fname: str, has_intensity: bool = True) -> np.ndarray:
"""Read points from a binary file in SemanticKITTI format
:param fname: Binary file containing points
:type fname: str
:param has_intensity: Whether the points have intensity values, defaults to True
:type has_intensity: bool
:return: Numpy array containing points
:rtype: np.ndarray
"""
points = np.fromfile(fname, dtype=np.float32)
points = points.reshape((-1, 4 if has_intensity else 3))
if not has_intensity:
empty_intensity = np.zeros((points.shape[0], 1), dtype=np.float32)
points = np.concatenate([points, empty_intensity], axis=1)
return points
[docs]
def read_semantickitti_label(fname: str) -> Tuple[np.ndarray, np.ndarray]:
"""Read labels from a binary file in SemanticKITTI format
:param fname: Binary file containing labels
:type fname: str
:return: Numpy arrays containing semantic and instance labels
:rtype: Tuple[np.ndarray, np.ndarray]
"""
label = np.fromfile(fname, dtype=np.uint32)
label = label.reshape((-1))
semantic_label = label & 0xFFFF
instance_label = label >> 16
return semantic_label, instance_label