WIP lidar
This commit is contained in:
parent
e24e270a42
commit
bcb2369046
3 changed files with 830 additions and 0 deletions
|
|
@ -37,6 +37,8 @@ dependencies = [
|
||||||
"superfsmon>=1.2.3",
|
"superfsmon>=1.2.3",
|
||||||
"noise>=1.2.2",
|
"noise>=1.2.2",
|
||||||
"svgpathtools>=1.7.1",
|
"svgpathtools>=1.7.1",
|
||||||
|
"velodyne-decoder>=3.1.0",
|
||||||
|
"open3d>=0.19.0",
|
||||||
]
|
]
|
||||||
|
|
||||||
[project.scripts]
|
[project.scripts]
|
||||||
|
|
@ -53,6 +55,7 @@ model_train = "trap.models.train:train"
|
||||||
trap_video_source = "trap.frame_emitter:FrameEmitter.parse_and_start"
|
trap_video_source = "trap.frame_emitter:FrameEmitter.parse_and_start"
|
||||||
trap_video_writer = "trap.frame_writer:FrameWriter.parse_and_start"
|
trap_video_writer = "trap.frame_writer:FrameWriter.parse_and_start"
|
||||||
trap_tracker = "trap.tracker:Tracker.parse_and_start"
|
trap_tracker = "trap.tracker:Tracker.parse_and_start"
|
||||||
|
trap_lidar = "trap.lidar_tracker:LidarTracker.parse_and_start"
|
||||||
trap_stage = "trap.stage:Stage.parse_and_start"
|
trap_stage = "trap.stage:Stage.parse_and_start"
|
||||||
trap_prediction = "trap.prediction_server:PredictionServer.parse_and_start"
|
trap_prediction = "trap.prediction_server:PredictionServer.parse_and_start"
|
||||||
trap_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start"
|
trap_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start"
|
||||||
|
|
@ -71,6 +74,9 @@ baumer-neoapi = { path = "../../Downloads/Baumer_neoAPI_1.5.0_lin_x86_64_python/
|
||||||
foucault = { git = "https://git.rubenvandeven.com/r/conductofconduct" }
|
foucault = { git = "https://git.rubenvandeven.com/r/conductofconduct" }
|
||||||
opencv-python = {path="./opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl"}
|
opencv-python = {path="./opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl"}
|
||||||
|
|
||||||
|
[tool.uv.workspace]
|
||||||
|
members = ["CenterTrack"]
|
||||||
|
|
||||||
[build-system]
|
[build-system]
|
||||||
requires = ["hatchling"]
|
requires = ["hatchling"]
|
||||||
build-backend = "hatchling.build"
|
build-backend = "hatchling.build"
|
||||||
|
|
|
||||||
632
trap/lidar_tracker.py
Normal file
632
trap/lidar_tracker.py
Normal file
|
|
@ -0,0 +1,632 @@
|
||||||
|
from argparse import ArgumentParser
|
||||||
|
from collections import deque, namedtuple
|
||||||
|
from copy import copy
|
||||||
|
import socket
|
||||||
|
from typing import List
|
||||||
|
import numpy as np
|
||||||
|
import open3d as o3d
|
||||||
|
from sklearn.cluster import DBSCAN
|
||||||
|
from scipy.spatial.distance import cdist
|
||||||
|
import logging
|
||||||
|
import time
|
||||||
|
from trap.node import Node
|
||||||
|
|
||||||
|
|
||||||
|
import velodyne_decoder as vd
|
||||||
|
|
||||||
|
|
||||||
|
logger = logging.getLogger('lidar')
|
||||||
|
# import sort
|
||||||
|
# or ByteTrack
|
||||||
|
|
||||||
|
|
||||||
|
# This is what is used by velodyne_decoder internally on read_pcap:
|
||||||
|
ResultTuple = namedtuple("StampCloudTuple", ("stamp", "points"))
|
||||||
|
# In bag reader this is: Result = namedtuple("ResultTuple", ("stamp", "points", "topic", "frame_id"))
|
||||||
|
|
||||||
|
|
||||||
|
# like velodyne_decoder.read_pcap but with live streamed data
|
||||||
|
# see https://github.com/valgur/velodyne_decoder/issues/4
|
||||||
|
def read_live_data(ip, port, config, as_pcl_structs=False):
|
||||||
|
decoder = vd.StreamDecoder(config)
|
||||||
|
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
# for this to work run sudo sysctl -w net.core.rmem_max=4194304 && sudo sysctl -w net.core.rmem_default=4194304
|
||||||
|
s.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 2 * 1024 * 1024) # 4MB
|
||||||
|
s.bind((ip, port))
|
||||||
|
while True:
|
||||||
|
# aggre
|
||||||
|
data, address = s.recvfrom(vd.PACKET_SIZE * 2)
|
||||||
|
recv_stamp = time.time()
|
||||||
|
result = decoder.decode(recv_stamp, data, as_pcl_structs)
|
||||||
|
# this is like read_pcap
|
||||||
|
if result is not None:
|
||||||
|
yield ResultTuple(*result)
|
||||||
|
|
||||||
|
class LidarTracker(Node):
|
||||||
|
def setup(self):
|
||||||
|
if self.config.smooth_tracks:
|
||||||
|
self.logger.warning("Smoothing not yet implemented")
|
||||||
|
self.track_sock = self.pub(self.config.zmq_trajectory_addr)
|
||||||
|
|
||||||
|
calibration = vd.Calibration.read( "VLP-16.yaml")
|
||||||
|
config = vd.Config(model=vd.Model.VLP16, calibration=calibration)
|
||||||
|
|
||||||
|
self.sequence_generator = read_live_data(self.config.ip, self.config.port, config)
|
||||||
|
|
||||||
|
self.room_filter = VoxelOccupancyFilter(
|
||||||
|
voxel_size=0.3,
|
||||||
|
history_size=80,
|
||||||
|
history_interval=8, #not every frame needs to impact history
|
||||||
|
ready_for_action_at=25,
|
||||||
|
static_threshold=0.5
|
||||||
|
)
|
||||||
|
|
||||||
|
def run(self):
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
vis = o3d.visualization.Visualizer()
|
||||||
|
vis.create_window()
|
||||||
|
|
||||||
|
# Set the point size
|
||||||
|
render_option = vis.get_render_option()
|
||||||
|
render_option.point_size = 1.0 # Smaller point size
|
||||||
|
|
||||||
|
view_ctl = vis.get_view_control()
|
||||||
|
view_ctl.set_lookat([0, 0, 0])
|
||||||
|
view_ctl.set_up([0, 1, 0])
|
||||||
|
front_direction = np.array([0.5, 0.5, -1.0])
|
||||||
|
front_direction = front_direction / np.linalg.norm(front_direction) # Normalize!
|
||||||
|
view_ctl.set_front(front_direction.tolist()) # Convert to list
|
||||||
|
view_ctl.set_zoom(0.9)
|
||||||
|
|
||||||
|
geom_added = False
|
||||||
|
|
||||||
|
live_pcd = o3d.geometry.PointCloud()
|
||||||
|
live_pcd.paint_uniform_color([0.2, 0.2, 0.7])
|
||||||
|
|
||||||
|
live_filtered_pcd = o3d.geometry.PointCloud()
|
||||||
|
live_filtered_pcd.paint_uniform_color([0.0, 0.7, 0.2])
|
||||||
|
|
||||||
|
ground_normal = np.array([1,1,0])
|
||||||
|
|
||||||
|
found_planes = False
|
||||||
|
|
||||||
|
|
||||||
|
while self.run_loop():
|
||||||
|
stamp, lidar_points = next(self.sequence_generator)
|
||||||
|
|
||||||
|
|
||||||
|
frame_points = velodyne_to_points(lidar_points)
|
||||||
|
current_pcd = o3d.geometry.PointCloud()
|
||||||
|
current_pcd.points = points_to_o3d(frame_points)
|
||||||
|
|
||||||
|
if not found_planes:
|
||||||
|
self.planes = segment_planes(current_pcd, 4, distance_threshold=.2)
|
||||||
|
found_planes = True
|
||||||
|
self.logger.info(f"Found {len(self.planes)} planes in points cloud")
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
for i, plane in enumerate(self.planes):
|
||||||
|
color = [0,0,0]
|
||||||
|
color[i%3] = 1
|
||||||
|
# color = [1*i/len(self.planes),0,0]
|
||||||
|
plane.paint_uniform_color(color)
|
||||||
|
print(f"Add plane {len(plane.points)} points")
|
||||||
|
vis.add_geometry(plane, True)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# a = time.perf_counter()
|
||||||
|
filtered_pcd = self.room_filter.remove_static(current_pcd)
|
||||||
|
|
||||||
|
stat_static = len(filtered_pcd.points)
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
live_pcd.points = current_pcd.points
|
||||||
|
|
||||||
|
if self.room_filter.initialised:
|
||||||
|
|
||||||
|
# filtered_pcd, _ = filtered_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
|
||||||
|
filtered_pcd, _ = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.2)
|
||||||
|
|
||||||
|
stat_denoise = len(filtered_pcd.points)
|
||||||
|
|
||||||
|
|
||||||
|
# down sample
|
||||||
|
filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.05)
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
live_filtered_pcd.points = filtered_pcd.points
|
||||||
|
|
||||||
|
|
||||||
|
# live_pcd.colors = o3d.utility.Vector3dVector(np.asarray(rgb_image)[mask].reshape(-1, 3) / 255.0)
|
||||||
|
|
||||||
|
stat_downsampled = len(filtered_pcd.points)
|
||||||
|
# b = time.perf_counter()
|
||||||
|
# filtered_pcd2 = room_filter2.remove_static(current_pcd)
|
||||||
|
# c = time.perf_counter()
|
||||||
|
|
||||||
|
# print(f"after static object removal: {stat_static} / after outlier removal: {stat_denoise} / after_downsample: {stat_downsampled}")
|
||||||
|
|
||||||
|
# pcd.points = filtered_pcd.points
|
||||||
|
|
||||||
|
points_2d = project_to_xy(filtered_pcd)
|
||||||
|
# points_2d = project_to_plane(filtered_pcd, ground_normal)
|
||||||
|
|
||||||
|
# non_ground = remove_ground(pcd)
|
||||||
|
# points_2d = project_to_plane(pro)
|
||||||
|
# points_2d = non_ground[:, :2]
|
||||||
|
labels = cluster_2d(points_2d)
|
||||||
|
|
||||||
|
# boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= 0.3*0.3)
|
||||||
|
boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= self.config.min_box_area)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if len(centroids):
|
||||||
|
self.logger.debug(f"{centroids=}")
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
line_sets = boxes_to_lineset(boxes, 0)
|
||||||
|
for s in line_sets:
|
||||||
|
vis.add_geometry(s, False)
|
||||||
|
elif self.config.viz:
|
||||||
|
line_sets = []
|
||||||
|
live_filtered_pcd.points = o3d.utility.Vector3dVector(np.empty([0,3]))
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# tracks = tracker.update(centroids)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# Build tracked spheres
|
||||||
|
spheres = []
|
||||||
|
# for _, pos in tracks.items():
|
||||||
|
# sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.15)
|
||||||
|
# sphere.translate([pos[0], pos[1], 0])
|
||||||
|
# sphere.paint_uniform_color([1, 0, 0])
|
||||||
|
# spheres.append(sphere)
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
|
||||||
|
color = [0.5, 0.5, 0.7] if self.room_filter.initialised else [0.7, 0.7, 0.7]
|
||||||
|
live_pcd.paint_uniform_color(color)
|
||||||
|
live_filtered_pcd.paint_uniform_color([0.2, 0.7, 0.2])
|
||||||
|
|
||||||
|
if not geom_added:
|
||||||
|
vis.add_geometry(live_filtered_pcd, True)
|
||||||
|
# vis.add_geometry(live_pcd, True)
|
||||||
|
for s in spheres:
|
||||||
|
vis.add_geometry(s, False)
|
||||||
|
geom_added = True
|
||||||
|
else:
|
||||||
|
vis.update_geometry(live_filtered_pcd)
|
||||||
|
# vis.update_geometry(live_pcd)
|
||||||
|
for s in spheres:
|
||||||
|
vis.add_geometry(s, False)
|
||||||
|
|
||||||
|
vis.poll_events()
|
||||||
|
vis.update_renderer()
|
||||||
|
# time.sleep(0.1)
|
||||||
|
for s in line_sets:
|
||||||
|
vis.remove_geometry(s, False)
|
||||||
|
for s in spheres:
|
||||||
|
vis.remove_geometry(s, False)
|
||||||
|
|
||||||
|
|
||||||
|
if self.config.viz:
|
||||||
|
vis.destroy_window()
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def arg_parser(cls):
|
||||||
|
argparser = ArgumentParser()
|
||||||
|
argparser.add_argument('--zmq-trajectory-addr',
|
||||||
|
help='Manually specity communication addr for the trajectory messages',
|
||||||
|
type=str,
|
||||||
|
default="ipc:///tmp/feeds_traj")
|
||||||
|
argparser.add_argument('--ip',
|
||||||
|
help='IP of this computer on which to listen for IP packets broadcast by lidar (so NOT the ip of the Lidar itself)',
|
||||||
|
type=str,
|
||||||
|
default="192.168.0.70")
|
||||||
|
argparser.add_argument('--port',
|
||||||
|
help='Port of the incomming ip packets',
|
||||||
|
type=int,
|
||||||
|
default=2368)
|
||||||
|
argparser.add_argument('--min-box-area',
|
||||||
|
help='Minimum area of bounding boxes to consider them for tracking',
|
||||||
|
type=float,
|
||||||
|
default=.3*.2)
|
||||||
|
argparser.add_argument('--max-box-area',
|
||||||
|
help='Maximum area (m2) of bounding boxes to consider them for tracking',
|
||||||
|
type=float,
|
||||||
|
default=2)
|
||||||
|
argparser.add_argument("--smooth-tracks",
|
||||||
|
help="Smooth the tracker tracks before sending them to the predictor",
|
||||||
|
action='store_true')
|
||||||
|
argparser.add_argument("--viz",
|
||||||
|
help="Render pointclouds in open3d",
|
||||||
|
action='store_true')
|
||||||
|
|
||||||
|
return argparser
|
||||||
|
|
||||||
|
# ==== Ground Removal ====
|
||||||
|
def find_ground(pcd: o3d.geometry.PointCloud, distance_threshold=0.1):
|
||||||
|
|
||||||
|
_, inliers = pcd.segment_plane(distance_threshold=distance_threshold,
|
||||||
|
ransac_n=3,
|
||||||
|
num_iterations=100)
|
||||||
|
ground = pcd.select_by_index(inliers)
|
||||||
|
non_ground = pcd.select_by_index(inliers, invert=True)
|
||||||
|
return np.asarray(non_ground.points), np.asarray(ground)
|
||||||
|
|
||||||
|
|
||||||
|
def segment_planes(pcd, num_planes = 5, num_iterations=100, distance_threshold=0.1) -> List[o3d.geometry.PointCloud]:
|
||||||
|
"""
|
||||||
|
Segments a point cloud into multiple planes using RANSAC.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
pcd: The input point cloud.
|
||||||
|
num_iterations: The number of RANSAC iterations.
|
||||||
|
distance_threshold: The maximum distance for a point to be considered an inlier.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
A list of point clouds, each representing a segmented plane.
|
||||||
|
"""
|
||||||
|
planes = []
|
||||||
|
remaining_pcd = copy(pcd)
|
||||||
|
|
||||||
|
for i in range(num_planes):
|
||||||
|
if len(remaining_pcd.points) <= 100: # Stop when very few points remain
|
||||||
|
break
|
||||||
|
|
||||||
|
# Perform RANSAC plane segmentation
|
||||||
|
plane_def, inliers = remaining_pcd.segment_plane(distance_threshold=distance_threshold,
|
||||||
|
ransac_n=3,
|
||||||
|
num_iterations=num_iterations)
|
||||||
|
|
||||||
|
# Extract the plane
|
||||||
|
plane = remaining_pcd.select_by_index(inliers)
|
||||||
|
|
||||||
|
# Add the plane to the list
|
||||||
|
planes.append(plane)
|
||||||
|
|
||||||
|
# Remove the plane from the remaining point cloud
|
||||||
|
remaining_pcd = remaining_pcd.select_by_index(inliers, invert=True)
|
||||||
|
|
||||||
|
return planes
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# ==== Clustering and Centroids ====
|
||||||
|
def cluster_2d(points_xy, eps=0.5, min_samples=5):
|
||||||
|
if not len(points_xy):
|
||||||
|
return []
|
||||||
|
|
||||||
|
db = DBSCAN(eps=eps, min_samples=min_samples).fit(points_xy)
|
||||||
|
return db.labels_
|
||||||
|
|
||||||
|
def compute_centroids(points_xy, labels):
|
||||||
|
centroids = []
|
||||||
|
for label in set(labels):
|
||||||
|
if label == -1:
|
||||||
|
continue
|
||||||
|
cluster = points_xy[labels == label]
|
||||||
|
centroid = cluster.mean(axis=0)
|
||||||
|
centroids.append(centroid)
|
||||||
|
return np.array(centroids)
|
||||||
|
|
||||||
|
def get_cluster_boxes(points_xy, labels, min_area = 0):
|
||||||
|
if not len(labels):
|
||||||
|
return np.empty([0,4]), np.empty([0,2])
|
||||||
|
|
||||||
|
boxes = [] # Each box: [x1, y1, x2, y2]
|
||||||
|
centroids = []
|
||||||
|
unique_labels = set(labels)
|
||||||
|
|
||||||
|
for label in unique_labels:
|
||||||
|
if label == -1:
|
||||||
|
continue # Skip noise
|
||||||
|
cluster = points_xy[labels == label]
|
||||||
|
x_min, y_min = cluster.min(axis=0)
|
||||||
|
x_max, y_max = cluster.max(axis=0)
|
||||||
|
|
||||||
|
area = (x_max-x_min) * (y_max - y_min)
|
||||||
|
if area < min_area:
|
||||||
|
logger.warning(f"Dropping box {area} ")
|
||||||
|
continue
|
||||||
|
|
||||||
|
|
||||||
|
centroid = cluster.mean(axis=0)
|
||||||
|
|
||||||
|
boxes.append([x_min, y_min, x_max, y_max])
|
||||||
|
centroids.append(centroid)
|
||||||
|
|
||||||
|
return np.array(boxes), np.array(centroids)
|
||||||
|
|
||||||
|
|
||||||
|
def boxes_to_lineset(boxes, z_height=0.0, color=[1.0, 0.0, 0.0]) -> List[o3d.geometry.LineSet]:
|
||||||
|
"""Draws a list of 2D bounding boxes onto an Open3D visualization.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
boxes (list): A list of bounding boxes, where each box is a list
|
||||||
|
of [x_min, y_min, x_max, y_max].
|
||||||
|
z_height (float): The z-coordinate height of the boxes.
|
||||||
|
color (list): The color of the boxes (RGB).
|
||||||
|
"""
|
||||||
|
sets = []
|
||||||
|
for box in boxes:
|
||||||
|
x_min, y_min, x_max, y_max = box
|
||||||
|
|
||||||
|
# Define the 8 corners of the box in 3D
|
||||||
|
corners = np.array([
|
||||||
|
[x_min, y_min, z_height],
|
||||||
|
[x_max, y_min, z_height],
|
||||||
|
[x_max, y_max, z_height],
|
||||||
|
[x_min, y_max, z_height],
|
||||||
|
[x_min, y_min, z_height + 1.0], # Top corners
|
||||||
|
[x_max, y_min, z_height + 1.0],
|
||||||
|
[x_max, y_max, z_height + 1.0],
|
||||||
|
[x_min, y_max, z_height + 1.0],
|
||||||
|
])
|
||||||
|
|
||||||
|
# Create lines from the corners to form the edges of the box
|
||||||
|
line_idxs = []
|
||||||
|
for i in range(8):
|
||||||
|
for j in range(i + 1, 8):
|
||||||
|
line_idxs.append([i, j])
|
||||||
|
|
||||||
|
line_idxs = np.array(line_idxs)
|
||||||
|
|
||||||
|
points = o3d.utility.Vector3dVector(corners)
|
||||||
|
lines = o3d.utility.Vector2iVector(line_idxs)
|
||||||
|
|
||||||
|
# Create a LineSet from the lines
|
||||||
|
line_set = o3d.geometry.LineSet(points, lines)
|
||||||
|
line_set.colors = o3d.utility.Vector3dVector(np.tile(color, (len(line_idxs), 1))) # Assign color
|
||||||
|
sets.append(line_set)
|
||||||
|
return sets
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def velodyne_to_points(points) -> o3d.geometry.PointCloud:
|
||||||
|
"""lidar points have additional metadata. Strip that for clustering
|
||||||
|
"""
|
||||||
|
if points is None:
|
||||||
|
return []
|
||||||
|
|
||||||
|
try:
|
||||||
|
points_np = np.array(points, dtype=np.float64)
|
||||||
|
|
||||||
|
if points_np.shape[1] != 8:
|
||||||
|
raise RuntimeError(f"Warning: Point cloud has unexpected shape {points_np.shape}. Expected (N, 8). Skipping.")
|
||||||
|
|
||||||
|
# Extract x, y, and z coordinates
|
||||||
|
x = points_np[:, 0]
|
||||||
|
y = points_np[:, 1]
|
||||||
|
z = points_np[:, 2]
|
||||||
|
|
||||||
|
# Stack the coordinates to create a (N, 3) array
|
||||||
|
points_xyz = np.stack([x, y, z], axis=1)
|
||||||
|
|
||||||
|
except ValueError as e:
|
||||||
|
raise RuntimeError(f"Error converting points to NumPy array: {e}. Skipping.")
|
||||||
|
|
||||||
|
return points_xyz
|
||||||
|
|
||||||
|
def points_to_o3d(points_xyz: np.array) -> o3d.geometry.PointCloud:
|
||||||
|
return o3d.utility.Vector3dVector(points_xyz)
|
||||||
|
|
||||||
|
|
||||||
|
class VoxelOccupancyFilter:
|
||||||
|
def __init__(self, voxel_size=0.2, history_size=150, static_threshold=0.2, history_interval=1, ready_for_action_at=None):
|
||||||
|
"""
|
||||||
|
Initializes the voxel occupancy filter.
|
||||||
|
|
||||||
|
Parameters:
|
||||||
|
voxel_size (float): Size of each voxel cube in meters.
|
||||||
|
history_size (int): Number of past frames to keep.
|
||||||
|
static_threshold (float): Fraction of frames a voxel must appear in to be considered static.
|
||||||
|
"""
|
||||||
|
self.voxel_size = voxel_size
|
||||||
|
self.history_size = history_size
|
||||||
|
self.static_threshold = static_threshold
|
||||||
|
self.voxel_history = deque(maxlen=history_size)
|
||||||
|
|
||||||
|
self.ready_for_action_at = ready_for_action_at if ready_for_action_at else history_size
|
||||||
|
|
||||||
|
self.initialised = False
|
||||||
|
|
||||||
|
self.history_interval = history_interval
|
||||||
|
|
||||||
|
self.i = -1
|
||||||
|
|
||||||
|
def _point_cloud_to_voxel_set(self, pcd):
|
||||||
|
"""
|
||||||
|
Converts a point cloud to a set of voxel coordinate tuples.
|
||||||
|
"""
|
||||||
|
points = np.asarray(pcd.points)
|
||||||
|
voxel_coords = np.floor(points / self.voxel_size).astype(np.int32)
|
||||||
|
return set(map(tuple, voxel_coords))
|
||||||
|
|
||||||
|
def remove_static(self, current_pcd):
|
||||||
|
"""
|
||||||
|
Removes static points from the current point cloud.
|
||||||
|
|
||||||
|
Parameters:
|
||||||
|
current_pcd (open3d.geometry.PointCloud): Current point cloud frame.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
open3d.geometry.PointCloud: Filtered point cloud with static points removed.
|
||||||
|
"""
|
||||||
|
self.i = (self.i + 1) % self.history_interval
|
||||||
|
|
||||||
|
current_voxel_set = self._point_cloud_to_voxel_set(current_pcd)
|
||||||
|
|
||||||
|
# Count voxel occurrences in history
|
||||||
|
voxel_counts = {}
|
||||||
|
for voxel_set in self.voxel_history:
|
||||||
|
for v in voxel_set:
|
||||||
|
voxel_counts[v] = voxel_counts.get(v, 0) + 1
|
||||||
|
|
||||||
|
# Determine static voxels
|
||||||
|
static_voxels = {v for v, count in voxel_counts.items()
|
||||||
|
if count / len(self.voxel_history) >= self.static_threshold}
|
||||||
|
|
||||||
|
# Filter out static points from current point cloud
|
||||||
|
filtered_points = []
|
||||||
|
for pt in np.asarray(current_pcd.points):
|
||||||
|
voxel = tuple(np.floor(pt / self.voxel_size).astype(np.int32))
|
||||||
|
if voxel not in static_voxels:
|
||||||
|
filtered_points.append(pt)
|
||||||
|
|
||||||
|
# Update history: until full, from there every n frame
|
||||||
|
# this way, minimal history is necessary to still to encode participants, while
|
||||||
|
# handling more semi-permanent changes to the space
|
||||||
|
if self.i == 0 or len(self.voxel_history) < self.history_size:
|
||||||
|
self.voxel_history.append(current_voxel_set)
|
||||||
|
|
||||||
|
if not self.initialised and len(self.voxel_history) >= self.ready_for_action_at:
|
||||||
|
logger.info(f"Static object filter ready: {len(self.voxel_history)} historical items, keep max {self.history_size}")
|
||||||
|
self.initialised = True
|
||||||
|
|
||||||
|
# Return filtered point cloud
|
||||||
|
filtered_pcd = o3d.geometry.PointCloud()
|
||||||
|
if filtered_points:
|
||||||
|
filtered_pcd.points = o3d.utility.Vector3dVector(np.array(filtered_points))
|
||||||
|
return filtered_pcd
|
||||||
|
|
||||||
|
|
||||||
|
class DecayingVoxelOccupancyFilter:
|
||||||
|
def __init__(self, voxel_size=0.1, decay_rate=0.01, appear_increment=0.01, static_threshold=0.2):
|
||||||
|
"""
|
||||||
|
WIP!! Initializes the voxel occupancy filter with decay.
|
||||||
|
|
||||||
|
Parameters:
|
||||||
|
voxel_size (float): Size of each voxel in meters.
|
||||||
|
decay_rate (float): Amount to decay occupancy per frame.
|
||||||
|
appear_increment (float): Value added when a voxel appears in current frame.
|
||||||
|
static_threshold (float): Threshold to consider voxel as static.
|
||||||
|
"""
|
||||||
|
self.voxel_size = voxel_size
|
||||||
|
self.decay_rate = decay_rate
|
||||||
|
self.appear_increment = appear_increment
|
||||||
|
self.static_threshold = static_threshold
|
||||||
|
self.voxel_occupancy = {} # dict: voxel tuple -> occupancy score
|
||||||
|
|
||||||
|
def _point_cloud_to_voxel_set(self, pcd):
|
||||||
|
points = np.asarray(pcd.points)
|
||||||
|
voxel_coords = np.floor(points / self.voxel_size).astype(np.int32)
|
||||||
|
return set(map(tuple, voxel_coords))
|
||||||
|
|
||||||
|
def remove_static(self, current_pcd):
|
||||||
|
current_voxels = self._point_cloud_to_voxel_set(current_pcd)
|
||||||
|
|
||||||
|
# Decay all existing voxel scores
|
||||||
|
for voxel in list(self.voxel_occupancy.keys()):
|
||||||
|
self.voxel_occupancy[voxel] *= (1.0 - self.decay_rate)
|
||||||
|
if self.voxel_occupancy[voxel] < self.appear_increment:
|
||||||
|
del self.voxel_occupancy[voxel] # clean up low-score entries
|
||||||
|
|
||||||
|
# Update occupancy with current frame
|
||||||
|
for voxel in current_voxels:
|
||||||
|
self.voxel_occupancy[voxel] = self.voxel_occupancy.get(voxel, 0.0) + self.appear_increment
|
||||||
|
self.voxel_occupancy[voxel] = min(self.voxel_occupancy[voxel], 1.0) # clamp to max=1.0
|
||||||
|
|
||||||
|
# Identify static voxels
|
||||||
|
static_voxels = {v for v, score in self.voxel_occupancy.items() if score >= self.static_threshold}
|
||||||
|
|
||||||
|
# Filter points
|
||||||
|
filtered_points = []
|
||||||
|
for pt in np.asarray(current_pcd.points):
|
||||||
|
voxel = tuple(np.floor(pt / self.voxel_size).astype(np.int32))
|
||||||
|
if voxel not in static_voxels:
|
||||||
|
filtered_points.append(pt)
|
||||||
|
|
||||||
|
filtered_pcd = o3d.geometry.PointCloud()
|
||||||
|
if filtered_points:
|
||||||
|
filtered_pcd.points = o3d.utility.Vector3dVector(np.array(filtered_points))
|
||||||
|
|
||||||
|
return filtered_pcd
|
||||||
|
|
||||||
|
# def point_cloud_to_voxel_set(pcd, voxel_size):
|
||||||
|
# points = np.asarray(pcd.points)
|
||||||
|
# # Convert to voxel coordinates
|
||||||
|
# voxel_coords = np.floor(points / voxel_size).astype(np.int32)
|
||||||
|
# # Use set of tuple coordinates
|
||||||
|
# return set(map(tuple, voxel_coords))
|
||||||
|
|
||||||
|
# def remove_static_points(current_pcd, voxel_history, voxel_size, threshold=STATIC_THRESHOLD):
|
||||||
|
# current_voxels = point_cloud_to_voxel_set(current_pcd, voxel_size)
|
||||||
|
|
||||||
|
# # Count how many times each voxel appears in history
|
||||||
|
# voxel_counts = {}
|
||||||
|
# for vh in voxel_history:
|
||||||
|
# for v in vh:
|
||||||
|
# voxel_counts[v] = voxel_counts.get(v, 0) + 1
|
||||||
|
|
||||||
|
# # Mark voxels that are static
|
||||||
|
# static_voxels = {v for v, count in voxel_counts.items() if count / HISTORY_SIZE >= threshold}
|
||||||
|
# # print(len(static_voxels))
|
||||||
|
|
||||||
|
# # Filter out static points from current frame
|
||||||
|
# filtered_points = []
|
||||||
|
# for pt in np.asarray(current_pcd.points):
|
||||||
|
# voxel = tuple(np.floor(pt / voxel_size).astype(np.int32))
|
||||||
|
# if voxel not in static_voxels:
|
||||||
|
# filtered_points.append(pt)
|
||||||
|
|
||||||
|
# filtered_pcd = o3d.geometry.PointCloud()
|
||||||
|
# filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
|
||||||
|
# return filtered_pcd
|
||||||
|
|
||||||
|
def project_to_xy(pcd: o3d.geometry.PointCloud) -> np.ndarray:
|
||||||
|
"""
|
||||||
|
Projects a 3D point cloud to the XY plane.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
2D numpy array of shape (N, 2) with [x, y] coordinates.
|
||||||
|
"""
|
||||||
|
points = np.asarray(pcd.points)
|
||||||
|
return points[:, :2] # Drop Z
|
||||||
|
|
||||||
|
def project_to_plane(point_cloud, plane_normal: np.ndarray):
|
||||||
|
"""Projects a point cloud onto a plane.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
point_cloud (o3d.geometry.PointCloud): The input point cloud.
|
||||||
|
plane_normal (numpy.ndarray): The normal vector of the plane.
|
||||||
|
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
o3d.geometry.PointCloud: The projected point cloud.
|
||||||
|
"""
|
||||||
|
|
||||||
|
# 1. Calculate the rotation matrix to align the plane normal with the z-axis
|
||||||
|
plane_normal = plane_normal / np.linalg.norm(plane_normal) # Normalize
|
||||||
|
|
||||||
|
# Calculate a rotation matrix that rotates the plane normal to the z-axis.
|
||||||
|
# We use the Rodrigues' rotation formula. This requires finding an axis
|
||||||
|
# perpendicular to both the normal and the z-axis.
|
||||||
|
z_axis = np.array([0, 0, 1])
|
||||||
|
rotation_axis = np.cross(plane_normal, z_axis)
|
||||||
|
rotation_axis = rotation_axis / np.linalg.norm(rotation_axis)
|
||||||
|
|
||||||
|
# angle = np.arccos(np.dot(plane_normal, z_axis))
|
||||||
|
# print(rotation_axis, angle)
|
||||||
|
rotation_matrix = o3d.geometry.get_rotation_matrix_from_axis_angle(rotation_axis.astype(np.float64))
|
||||||
|
|
||||||
|
# 2. Transform the point cloud
|
||||||
|
transformed_pc = point_cloud.rotate(rotation_matrix, center=(0,0,0))
|
||||||
|
|
||||||
|
# 3. Translation to make the plane at z=0
|
||||||
|
translation_vector = -np.asarray(transformed_pc.get_center())
|
||||||
|
transformed_pc = transformed_pc.translate(translation_vector)
|
||||||
|
|
||||||
|
# 4. Drop Z-coordinate
|
||||||
|
points = np.asarray(transformed_pc.points)
|
||||||
|
projected_points = points[:, :2] # Take only x and y coordinates
|
||||||
|
# projected_pc = o3d.geometry.PointCloud()
|
||||||
|
# projected_pc.points = o3d.utility.Vector3dVector(projected_points)
|
||||||
|
|
||||||
|
return projected_points
|
||||||
192
uv.lock
192
uv.lock
|
|
@ -18,6 +18,15 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/2f/7a/874c46ad2d14998bc2eedac1133c5299e12fe728d2ce91b4d64f2fcc5089/absl_py-2.2.0-py3-none-any.whl", hash = "sha256:5c432cdf7b045f89c4ddc3bba196cabb389c0c321322f8dec68eecdfa732fdad", size = 276986 },
|
{ url = "https://files.pythonhosted.org/packages/2f/7a/874c46ad2d14998bc2eedac1133c5299e12fe728d2ce91b4d64f2fcc5089/absl_py-2.2.0-py3-none-any.whl", hash = "sha256:5c432cdf7b045f89c4ddc3bba196cabb389c0c321322f8dec68eecdfa732fdad", size = 276986 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "addict"
|
||||||
|
version = "2.4.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/85/ef/fd7649da8af11d93979831e8f1f8097e85e82d5bfeabc8c68b39175d8e75/addict-2.4.0.tar.gz", hash = "sha256:b3b2210e0e067a281f5646c8c5db92e99b7231ea8b0eb5f74dbdf9e259d4e494", size = 9186 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/6a/00/b08f23b7d7e1e14ce01419a467b583edbb93c6cdb8654e54a9cc579cd61f/addict-2.4.0-py3-none-any.whl", hash = "sha256:249bb56bbfd3cdc2a004ea0ff4c2b6ddc84d53bc2194761636eb314d5cfa5dfc", size = 3832 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "aiofiles"
|
name = "aiofiles"
|
||||||
version = "24.1.0"
|
version = "24.1.0"
|
||||||
|
|
@ -256,6 +265,15 @@ css = [
|
||||||
{ name = "tinycss2" },
|
{ name = "tinycss2" },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "blinker"
|
||||||
|
version = "1.9.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/21/28/9b3f50ce0e048515135495f198351908d99540d69bfdc8c1d15b73dc55ce/blinker-1.9.0.tar.gz", hash = "sha256:b4ce2265a7abece45e7cc896e98dbebe6cead56bcf805a3d23136d145f5445bf", size = 22460 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/10/cb/f2ad4230dc2eb1a74edf38f1a38b9b52277f75bef262d8908e60d957e13c/blinker-1.9.0-py3-none-any.whl", hash = "sha256:ba0efaa9080b619ff2f3459d1d500c57bddea4a6b424b60a91141db6fd2f08bc", size = 8458 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "bytetracker"
|
name = "bytetracker"
|
||||||
version = "0.3.2"
|
version = "0.3.2"
|
||||||
|
|
@ -352,6 +370,15 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/e6/75/49e5bfe642f71f272236b5b2d2691cf915a7283cc0ceda56357b61daa538/comm-0.2.2-py3-none-any.whl", hash = "sha256:e6fb86cb70ff661ee8c9c14e7d36d6de3b4066f1441be4063df9c5009f0a64d3", size = 7180 },
|
{ url = "https://files.pythonhosted.org/packages/e6/75/49e5bfe642f71f272236b5b2d2691cf915a7283cc0ceda56357b61daa538/comm-0.2.2-py3-none-any.whl", hash = "sha256:e6fb86cb70ff661ee8c9c14e7d36d6de3b4066f1441be4063df9c5009f0a64d3", size = 7180 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "configargparse"
|
||||||
|
version = "1.7.1"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/85/4d/6c9ef746dfcc2a32e26f3860bb4a011c008c392b83eabdfb598d1a8bbe5d/configargparse-1.7.1.tar.gz", hash = "sha256:79c2ddae836a1e5914b71d58e4b9adbd9f7779d4e6351a637b7d2d9b6c46d3d9", size = 43958 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/31/28/d28211d29bcc3620b1fece85a65ce5bb22f18670a03cd28ea4b75ede270c/configargparse-1.7.1-py3-none-any.whl", hash = "sha256:8b586a31f9d873abd1ca527ffbe58863c99f36d896e2829779803125e83be4b6", size = 25607 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "cycler"
|
name = "cycler"
|
||||||
version = "0.12.1"
|
version = "0.12.1"
|
||||||
|
|
@ -361,6 +388,26 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/e7/05/c19819d5e3d95294a6f5947fb9b9629efb316b96de511b418c53d245aae6/cycler-0.12.1-py3-none-any.whl", hash = "sha256:85cef7cff222d8644161529808465972e51340599459b8ac3ccbac5a854e0d30", size = 8321 },
|
{ url = "https://files.pythonhosted.org/packages/e7/05/c19819d5e3d95294a6f5947fb9b9629efb316b96de511b418c53d245aae6/cycler-0.12.1-py3-none-any.whl", hash = "sha256:85cef7cff222d8644161529808465972e51340599459b8ac3ccbac5a854e0d30", size = 8321 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "dash"
|
||||||
|
version = "3.2.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
dependencies = [
|
||||||
|
{ name = "flask" },
|
||||||
|
{ name = "importlib-metadata" },
|
||||||
|
{ name = "nest-asyncio" },
|
||||||
|
{ name = "plotly" },
|
||||||
|
{ name = "requests" },
|
||||||
|
{ name = "retrying" },
|
||||||
|
{ name = "setuptools" },
|
||||||
|
{ name = "typing-extensions" },
|
||||||
|
{ name = "werkzeug" },
|
||||||
|
]
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/80/37/8b5621e0a0b3c6e81a8b6cd3f033aa4b750f53e288dd1a494a887a8a06e9/dash-3.2.0.tar.gz", hash = "sha256:93300b9b99498f8b8ed267e61c455b4ee1282c7e4d4b518600eec87ce6ddea55", size = 7558708 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/d3/36/e0010483ca49b9bf6f389631ccea07b3ff6b678d14d8c7a0a4357860c36a/dash-3.2.0-py3-none-any.whl", hash = "sha256:4c1819588d83bed2cbcf5807daa5c2380c8c85789a6935a733f018f04ad8a6a2", size = 7900661 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "debugpy"
|
name = "debugpy"
|
||||||
version = "1.8.13"
|
version = "1.8.13"
|
||||||
|
|
@ -424,6 +471,15 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/8f/d7/9322c609343d929e75e7e5e6255e614fcc67572cfd083959cdef3b7aad79/docutils-0.21.2-py3-none-any.whl", hash = "sha256:dafca5b9e384f0e419294eb4d2ff9fa826435bf15f15b7bd45723e8ad76811b2", size = 587408 },
|
{ url = "https://files.pythonhosted.org/packages/8f/d7/9322c609343d929e75e7e5e6255e614fcc67572cfd083959cdef3b7aad79/docutils-0.21.2-py3-none-any.whl", hash = "sha256:dafca5b9e384f0e419294eb4d2ff9fa826435bf15f15b7bd45723e8ad76811b2", size = 587408 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "dpkt"
|
||||||
|
version = "1.9.8"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/c9/7d/52f17a794db52a66e46ebb0c7549bf2f035ed61d5a920ba4aaa127dd038e/dpkt-1.9.8.tar.gz", hash = "sha256:43f8686e455da5052835fd1eda2689d51de3670aac9799b1b00cfd203927ee45", size = 180073 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/11/79/479e2194c9096b92aecdf33634ae948d2be306c6011673e98ee1917f32c2/dpkt-1.9.8-py3-none-any.whl", hash = "sha256:4da4d111d7bf67575b571f5c678c71bddd2d8a01a3d57d489faf0a92c748fbfd", size = 194973 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "exceptiongroup"
|
name = "exceptiongroup"
|
||||||
version = "1.2.2"
|
version = "1.2.2"
|
||||||
|
|
@ -502,6 +558,23 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/4d/36/2a115987e2d8c300a974597416d9de88f2444426de9571f4b59b2cca3acc/filelock-3.18.0-py3-none-any.whl", hash = "sha256:c401f4f8377c4464e6db25fff06205fd89bdd83b65eb0488ed1b160f780e21de", size = 16215 },
|
{ url = "https://files.pythonhosted.org/packages/4d/36/2a115987e2d8c300a974597416d9de88f2444426de9571f4b59b2cca3acc/filelock-3.18.0-py3-none-any.whl", hash = "sha256:c401f4f8377c4464e6db25fff06205fd89bdd83b65eb0488ed1b160f780e21de", size = 16215 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "flask"
|
||||||
|
version = "3.1.2"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
dependencies = [
|
||||||
|
{ name = "blinker" },
|
||||||
|
{ name = "click" },
|
||||||
|
{ name = "itsdangerous" },
|
||||||
|
{ name = "jinja2" },
|
||||||
|
{ name = "markupsafe" },
|
||||||
|
{ name = "werkzeug" },
|
||||||
|
]
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/dc/6d/cfe3c0fcc5e477df242b98bfe186a4c34357b4847e87ecaef04507332dab/flask-3.1.2.tar.gz", hash = "sha256:bf656c15c80190ed628ad08cdfd3aaa35beb087855e2f494910aa3774cc4fd87", size = 720160 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/ec/f9/7f9263c5695f4bd0023734af91bedb2ff8209e8de6ead162f35d8dc762fd/flask-3.1.2-py3-none-any.whl", hash = "sha256:ca1d8112ec8a6158cc29ea4858963350011b5c846a414cdb7a954aa9e967d03c", size = 103308 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "fonttools"
|
name = "fonttools"
|
||||||
version = "4.56.0"
|
version = "4.56.0"
|
||||||
|
|
@ -675,6 +748,27 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/9c/1f/19ebc343cc71a7ffa78f17018535adc5cbdd87afb31d7c34874680148b32/ifaddr-0.2.0-py3-none-any.whl", hash = "sha256:085e0305cfe6f16ab12d72e2024030f5d52674afad6911bb1eee207177b8a748", size = 12314 },
|
{ url = "https://files.pythonhosted.org/packages/9c/1f/19ebc343cc71a7ffa78f17018535adc5cbdd87afb31d7c34874680148b32/ifaddr-0.2.0-py3-none-any.whl", hash = "sha256:085e0305cfe6f16ab12d72e2024030f5d52674afad6911bb1eee207177b8a748", size = 12314 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "importlib-metadata"
|
||||||
|
version = "8.7.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
dependencies = [
|
||||||
|
{ name = "zipp" },
|
||||||
|
]
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/76/66/650a33bd90f786193e4de4b3ad86ea60b53c89b669a5c7be931fac31cdb0/importlib_metadata-8.7.0.tar.gz", hash = "sha256:d13b81ad223b890aa16c5471f2ac3056cf76c5f10f82d6f9292f0b415f389000", size = 56641 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/20/b0/36bd937216ec521246249be3bf9855081de4c5e06a0c9b4219dbeda50373/importlib_metadata-8.7.0-py3-none-any.whl", hash = "sha256:e5dd1551894c77868a30651cef00984d50e1002d06942a7101d34870c5f02afd", size = 27656 },
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "importlib-resources"
|
||||||
|
version = "6.5.2"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/cf/8c/f834fbf984f691b4f7ff60f50b514cc3de5cc08abfc3295564dd89c5e2e7/importlib_resources-6.5.2.tar.gz", hash = "sha256:185f87adef5bcc288449d98fb4fba07cea78bc036455dd44c5fc4a2fe78fed2c", size = 44693 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/a4/ed/1f1afb2e9e7f38a545d628f864d562a5ae64fe6f7a10e28ffb9b185b4e89/importlib_resources-6.5.2-py3-none-any.whl", hash = "sha256:789cfdc3ed28c78b67a06acb8126751ced69a3d5f79c095a98298cd8a760ccec", size = 37461 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "ipykernel"
|
name = "ipykernel"
|
||||||
version = "6.29.5"
|
version = "6.29.5"
|
||||||
|
|
@ -1186,6 +1280,15 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/9c/fd/b247aec6add5601956d440488b7f23151d8343747e82c038af37b28d6098/multidict-6.2.0-py3-none-any.whl", hash = "sha256:5d26547423e5e71dcc562c4acdc134b900640a39abd9066d7326a7cc2324c530", size = 10266 },
|
{ url = "https://files.pythonhosted.org/packages/9c/fd/b247aec6add5601956d440488b7f23151d8343747e82c038af37b28d6098/multidict-6.2.0-py3-none-any.whl", hash = "sha256:5d26547423e5e71dcc562c4acdc134b900640a39abd9066d7326a7cc2324c530", size = 10266 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "narwhals"
|
||||||
|
version = "2.9.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/b7/95/aa46616f5e567ff5d262f4c207d5ca79cb2766010c786c351b8e7f930ef4/narwhals-2.9.0.tar.gz", hash = "sha256:d8cde40a6a8a7049d8e66608b7115ab19464acc6f305d136a8dc8ba396c4acfe", size = 584098 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/13/34/00c7ae8194074ed82b64e0bb7c24220eac5f77ac90c16e23cf0d2cfd2a03/narwhals-2.9.0-py3-none-any.whl", hash = "sha256:c59f7de4763004ae81691ce16df71b4e55aead0ead7ccde8c8f2ef8c9559c765", size = 422255 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "nbclient"
|
name = "nbclient"
|
||||||
version = "0.10.2"
|
version = "0.10.2"
|
||||||
|
|
@ -1350,6 +1453,32 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/19/77/538f202862b9183f54108557bfda67e17603fc560c384559e769321c9d92/numpy-1.26.4-cp310-cp310-win_amd64.whl", hash = "sha256:b97fe8060236edf3662adfc2c633f56a08ae30560c56310562cb4f95500022d5", size = 15808905 },
|
{ url = "https://files.pythonhosted.org/packages/19/77/538f202862b9183f54108557bfda67e17603fc560c384559e769321c9d92/numpy-1.26.4-cp310-cp310-win_amd64.whl", hash = "sha256:b97fe8060236edf3662adfc2c633f56a08ae30560c56310562cb4f95500022d5", size = 15808905 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "open3d"
|
||||||
|
version = "0.19.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
dependencies = [
|
||||||
|
{ name = "addict" },
|
||||||
|
{ name = "configargparse" },
|
||||||
|
{ name = "dash" },
|
||||||
|
{ name = "flask" },
|
||||||
|
{ name = "matplotlib" },
|
||||||
|
{ name = "nbformat" },
|
||||||
|
{ name = "numpy" },
|
||||||
|
{ name = "pandas" },
|
||||||
|
{ name = "pillow" },
|
||||||
|
{ name = "pyquaternion" },
|
||||||
|
{ name = "pyyaml" },
|
||||||
|
{ name = "scikit-learn" },
|
||||||
|
{ name = "tqdm" },
|
||||||
|
{ name = "werkzeug" },
|
||||||
|
]
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/5c/4b/91e8a4100adf0ccd2f7ad21dd24c2e3d8f12925396528d0462cfb1735e5a/open3d-0.19.0-cp310-cp310-macosx_10_15_universal2.whl", hash = "sha256:f7128ded206e07987cc29d0917195fb64033dea31e0d60dead3629b33d3c175f", size = 103086005 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/c7/45/13bc9414ee9db611cba90b9efa69f66f246560e8ade575f1ee5b7f7b5d31/open3d-0.19.0-cp310-cp310-manylinux_2_31_x86_64.whl", hash = "sha256:5b60234fa6a56a20caf1560cad4e914133c8c198d74d7b839631c90e8592762e", size = 447678387 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/bc/1c/0219416429f88ebc94fcb269fb186b153affe5b91dffe8f9062330d7776d/open3d-0.19.0-cp310-cp310-win_amd64.whl", hash = "sha256:18bb8b86e5fa9e582ed11b9651ff6e4a782e6778c9b8bfc344fc866dc8b5f49c", size = 69150378 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "opencv-python"
|
name = "opencv-python"
|
||||||
version = "4.10.0.84"
|
version = "4.10.0.84"
|
||||||
|
|
@ -1505,6 +1634,19 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/6d/45/59578566b3275b8fd9157885918fcd0c4d74162928a5310926887b856a51/platformdirs-4.3.7-py3-none-any.whl", hash = "sha256:a03875334331946f13c549dbd8f4bac7a13a50a895a0eb1e8c6a8ace80d40a94", size = 18499 },
|
{ url = "https://files.pythonhosted.org/packages/6d/45/59578566b3275b8fd9157885918fcd0c4d74162928a5310926887b856a51/platformdirs-4.3.7-py3-none-any.whl", hash = "sha256:a03875334331946f13c549dbd8f4bac7a13a50a895a0eb1e8c6a8ace80d40a94", size = 18499 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "plotly"
|
||||||
|
version = "6.3.1"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
dependencies = [
|
||||||
|
{ name = "narwhals" },
|
||||||
|
{ name = "packaging" },
|
||||||
|
]
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/0c/63/961d47c9ffd592a575495891cdcf7875dc0903ebb33ac238935714213789/plotly-6.3.1.tar.gz", hash = "sha256:dd896e3d940e653a7ce0470087e82c2bd903969a55e30d1b01bb389319461bb0", size = 6956460 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/3f/93/023955c26b0ce614342d11cc0652f1e45e32393b6ab9d11a664a60e9b7b7/plotly-6.3.1-py3-none-any.whl", hash = "sha256:8b4420d1dcf2b040f5983eed433f95732ed24930e496d36eb70d211923532e64", size = 9833698 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "prometheus-client"
|
name = "prometheus-client"
|
||||||
version = "0.21.1"
|
version = "0.21.1"
|
||||||
|
|
@ -1936,6 +2078,15 @@ socks = [
|
||||||
{ name = "pysocks" },
|
{ name = "pysocks" },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "retrying"
|
||||||
|
version = "1.4.2"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/c8/5a/b17e1e257d3e6f2e7758930e1256832c9ddd576f8631781e6a072914befa/retrying-1.4.2.tar.gz", hash = "sha256:d102e75d53d8d30b88562d45361d6c6c934da06fab31bd81c0420acb97a8ba39", size = 11411 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/67/f3/6cd296376653270ac1b423bb30bd70942d9916b6978c6f40472d6ac038e7/retrying-1.4.2-py3-none-any.whl", hash = "sha256:bbc004aeb542a74f3569aeddf42a2516efefcdaff90df0eb38fbfbf19f179f59", size = 10859 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "rfc3339-validator"
|
name = "rfc3339-validator"
|
||||||
version = "0.1.4"
|
version = "0.1.4"
|
||||||
|
|
@ -2552,6 +2703,7 @@ dependencies = [
|
||||||
{ name = "ipywidgets" },
|
{ name = "ipywidgets" },
|
||||||
{ name = "jsonlines" },
|
{ name = "jsonlines" },
|
||||||
{ name = "noise" },
|
{ name = "noise" },
|
||||||
|
{ name = "open3d" },
|
||||||
{ name = "opencv-python" },
|
{ name = "opencv-python" },
|
||||||
{ name = "pandas-helper-calc" },
|
{ name = "pandas-helper-calc" },
|
||||||
{ name = "pyglet" },
|
{ name = "pyglet" },
|
||||||
|
|
@ -2574,6 +2726,7 @@ dependencies = [
|
||||||
{ name = "trajectron-plus-plus" },
|
{ name = "trajectron-plus-plus" },
|
||||||
{ name = "tsmoothie" },
|
{ name = "tsmoothie" },
|
||||||
{ name = "ultralytics" },
|
{ name = "ultralytics" },
|
||||||
|
{ name = "velodyne-decoder" },
|
||||||
]
|
]
|
||||||
|
|
||||||
[package.metadata]
|
[package.metadata]
|
||||||
|
|
@ -2588,6 +2741,7 @@ requires-dist = [
|
||||||
{ name = "ipywidgets", specifier = ">=8.1.5,<9" },
|
{ name = "ipywidgets", specifier = ">=8.1.5,<9" },
|
||||||
{ name = "jsonlines", specifier = ">=4.0.0,<5" },
|
{ name = "jsonlines", specifier = ">=4.0.0,<5" },
|
||||||
{ name = "noise", specifier = ">=1.2.2" },
|
{ name = "noise", specifier = ">=1.2.2" },
|
||||||
|
{ name = "open3d", specifier = ">=0.19.0" },
|
||||||
{ name = "opencv-python", path = "opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl" },
|
{ name = "opencv-python", path = "opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl" },
|
||||||
{ name = "pandas-helper-calc", git = "https://github.com/scls19fr/pandas-helper-calc" },
|
{ name = "pandas-helper-calc", git = "https://github.com/scls19fr/pandas-helper-calc" },
|
||||||
{ name = "pyglet", specifier = ">=2.0.15,<3" },
|
{ name = "pyglet", specifier = ">=2.0.15,<3" },
|
||||||
|
|
@ -2610,6 +2764,7 @@ requires-dist = [
|
||||||
{ name = "trajectron-plus-plus", editable = "../Trajectron-plus-plus" },
|
{ name = "trajectron-plus-plus", editable = "../Trajectron-plus-plus" },
|
||||||
{ name = "tsmoothie", specifier = ">=1.0.5,<2" },
|
{ name = "tsmoothie", specifier = ">=1.0.5,<2" },
|
||||||
{ name = "ultralytics", specifier = "~=8.3" },
|
{ name = "ultralytics", specifier = "~=8.3" },
|
||||||
|
{ name = "velodyne-decoder", specifier = ">=3.1.0" },
|
||||||
]
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
|
|
@ -2764,6 +2919,34 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/a6/3d/7b22abbdb059d551507275a2815bc2b1974e3b9f6a13781c1eac9e858965/vbuild-0.8.2-py2.py3-none-any.whl", hash = "sha256:d76bcc976a1c53b6a5776ac947606f9e7786c25df33a587ebe33ed09dd8a1076", size = 9371 },
|
{ url = "https://files.pythonhosted.org/packages/a6/3d/7b22abbdb059d551507275a2815bc2b1974e3b9f6a13781c1eac9e858965/vbuild-0.8.2-py2.py3-none-any.whl", hash = "sha256:d76bcc976a1c53b6a5776ac947606f9e7786c25df33a587ebe33ed09dd8a1076", size = 9371 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "velodyne-decoder"
|
||||||
|
version = "3.1.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
dependencies = [
|
||||||
|
{ name = "dpkt" },
|
||||||
|
{ name = "importlib-resources" },
|
||||||
|
{ name = "numpy" },
|
||||||
|
{ name = "pyyaml" },
|
||||||
|
]
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/21/92/9f329e46b1b2e2f1010a4ca99b159633fd92404889066d3694b90c2589ca/velodyne_decoder-3.1.0.tar.gz", hash = "sha256:d87da7e1ccda58eddf573edb7c6a2b299fffd9b687e1ea5f24f2fc1b94a1abcd", size = 75189 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/66/c3/7c0cdf266c9007821614004f24367593e35ea1b794090d91a36e57b22df3/velodyne_decoder-3.1.0-cp310-cp310-macosx_10_15_x86_64.whl", hash = "sha256:a9a021d6a7e382bd8604d640434ccea777ca716f74e81eae68d0d825525ed863", size = 267943 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/6c/11/9e19533911eb9bd20fb2598ccf9579c3ec1fc908111a69e2f10ca2682397/velodyne_decoder-3.1.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:5c6a263fe2e5d5f6fcbda54aa5a4359ff74069e9c044414712be43b277e02607", size = 249930 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/64/31/2a509389f9496e62b71c6278c7fe27666d9d2e424e085e0b166abf8d918a/velodyne_decoder-3.1.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:dc2b5015a9d3286bbd0dac2168227892d74b69df276d8409288802163e0b29d2", size = 375722 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/10/bd/3214b3da85a892c57b179159b5dddcc3fc6a749ed8b8760c8f88a165660e/velodyne_decoder-3.1.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fdef9a6da33e77bbfbb504279590e087b40cb79e7a0d1d773d6850913970cac6", size = 396289 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/16/a8/32abf4ab8421a1874ecf6f2e47e0ea7ed9c95e24a3d88b6eff84177f6aaa/velodyne_decoder-3.1.0-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:cd1b8af06ed862cfd65a430febf523c3a84ca43eb2123783d2c5e7bf1b4e3744", size = 804012 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/3a/33/531c3cd86571b22b44a7d579fb6df550aaa554910576970d9311dee3e0cc/velodyne_decoder-3.1.0-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:0523003f672fee4de4e0b2444ee92891601ac173dd96d7687d26da46a47790df", size = 844248 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/f8/23/de92d024ed525517c308c5ca347b24fea110b45df363734628670589f832/velodyne_decoder-3.1.0-cp310-cp310-win_amd64.whl", hash = "sha256:8830cb92e031314ad809141b6eff7effeed94fd01f743a7e430b064d613401f2", size = 243310 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/32/db/f746aedcdf35a694ddf431c08aada87db2a6a03a3ed6cc64212f064a466f/velodyne_decoder-3.1.0-cp312-abi3-macosx_10_15_x86_64.whl", hash = "sha256:60dc598ab6bdb461351ad5923b66bc219bfda1801a355b15e4d76742344dda28", size = 265217 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/28/21/ba36a151b34f783005cdc281f3218ada97c63788a26d0be23689752995ec/velodyne_decoder-3.1.0-cp312-abi3-macosx_11_0_arm64.whl", hash = "sha256:47af0c685f1a80b915b179f417eef2499496fb1f15e5449aa34769f32cef67a2", size = 246741 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/fe/f3/6450f5d35ac345027b742b5f0225277c6de25158635a719d87b1998cfa59/velodyne_decoder-3.1.0-cp312-abi3-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:2c88fa75eefa60860802247c11cc58c8a9879dc308b5045d021009d18086a411", size = 372638 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/48/6e/4d47f1dfba4d100d861ca937dba645d3d7ca3f6dfd9879ef5c7098afa342/velodyne_decoder-3.1.0-cp312-abi3-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:2dcaddddcd410220166a5fce461a67b0cb6f098e64a79cfbb24559ca239ac87e", size = 393789 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/f4/2a/98fe5f64e3d826919f4e9399efd9460b742c679b891a1799759d9e2bf906/velodyne_decoder-3.1.0-cp312-abi3-musllinux_1_2_aarch64.whl", hash = "sha256:854f0c93522589c3ec0cf884a92566906dac2c606e36d59e42eff52e088a3c25", size = 801652 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/76/d8/6865ab284f2d924607c2f1a2a91041ac9511a2d3722f007fba5ad8b2a4db/velodyne_decoder-3.1.0-cp312-abi3-musllinux_1_2_x86_64.whl", hash = "sha256:8b82d3e55934bc2b078bbb211012a66c74ec98d2dc5965a64c4865be8bf4c5a7", size = 841421 },
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/82/d9/051784cb0eff7177f44f2d080fab545e612ddc8b47ed3b5ca5364b0abfe8/velodyne_decoder-3.1.0-cp312-abi3-win_amd64.whl", hash = "sha256:27e192dcb364460251e7f6ba3552541dcbb97e99a24b96a41e2f61f0e2036074", size = 242691 },
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "watchdog"
|
name = "watchdog"
|
||||||
version = "6.0.0"
|
version = "6.0.0"
|
||||||
|
|
@ -2938,3 +3121,12 @@ wheels = [
|
||||||
{ url = "https://files.pythonhosted.org/packages/fd/b7/2e9a5b18eb0fe24c3a0e8bae994e812ed9852ab4fd067c0107fadde0d5f0/yarl-1.18.3-cp310-cp310-win_amd64.whl", hash = "sha256:93b2e109287f93db79210f86deb6b9bbb81ac32fc97236b16f7433db7fc437d8", size = 90484 },
|
{ url = "https://files.pythonhosted.org/packages/fd/b7/2e9a5b18eb0fe24c3a0e8bae994e812ed9852ab4fd067c0107fadde0d5f0/yarl-1.18.3-cp310-cp310-win_amd64.whl", hash = "sha256:93b2e109287f93db79210f86deb6b9bbb81ac32fc97236b16f7433db7fc437d8", size = 90484 },
|
||||||
{ url = "https://files.pythonhosted.org/packages/f5/4b/a06e0ec3d155924f77835ed2d167ebd3b211a7b0853da1cf8d8414d784ef/yarl-1.18.3-py3-none-any.whl", hash = "sha256:b57f4f58099328dfb26c6a771d09fb20dbbae81d20cfb66141251ea063bd101b", size = 45109 },
|
{ url = "https://files.pythonhosted.org/packages/f5/4b/a06e0ec3d155924f77835ed2d167ebd3b211a7b0853da1cf8d8414d784ef/yarl-1.18.3-py3-none-any.whl", hash = "sha256:b57f4f58099328dfb26c6a771d09fb20dbbae81d20cfb66141251ea063bd101b", size = 45109 },
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "zipp"
|
||||||
|
version = "3.23.0"
|
||||||
|
source = { registry = "https://pypi.org/simple" }
|
||||||
|
sdist = { url = "https://files.pythonhosted.org/packages/e3/02/0f2892c661036d50ede074e376733dca2ae7c6eb617489437771209d4180/zipp-3.23.0.tar.gz", hash = "sha256:a07157588a12518c9d4034df3fbbee09c814741a33ff63c05fa29d26a2404166", size = 25547 }
|
||||||
|
wheels = [
|
||||||
|
{ url = "https://files.pythonhosted.org/packages/2e/54/647ade08bf0db230bfea292f893923872fd20be6ac6f53b2b936ba839d75/zipp-3.23.0-py3-none-any.whl", hash = "sha256:071652d6115ed432f5ce1d34c336c0adfd6a884660d1e9712a256d3d3bd4b14e", size = 10276 },
|
||||||
|
]
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue