Basic lidar tracker (still glitchy)

supervisord.conf

@@ -23,8 +23,8 @@ directory=%(here)s
 autostart=false
 
 [program:video]
-# command=uv run trap_video_source  --homography ../DATASETS/hof3/homography.json   --video-src ../DATASETS/hof3/hof3-cam-demo-twoperson.mp4  --calibration ../DATASETS/hof3/calibration.json --video-loop
+command=uv run trap_video_source  --homography ../DATASETS/hof3/homography.json   --video-src ../DATASETS/hof3/hof3-cam-demo-twoperson.mp4  --calibration ../DATASETS/hof3/calibration.json --video-loop
-command=uv run trap_video_source  --homography ../DATASETS/hof3-cam-baumer-cropped/homography.json   --video-src gige://../DATASETS/hof3-cam-baumer-cropped/gige_config.json --calibration ../DATASETS/hof3-cam-baumer-cropped/calibration.json
+# command=uv run trap_video_source  --homography ../DATASETS/hof3-cam-baumer-cropped/homography.json   --video-src gige://../DATASETS/hof3-cam-baumer-cropped/gige_config.json --calibration ../DATASETS/hof3-cam-baumer-cropped/calibration.json
 directory=%(here)s
 directory=%(here)s
 

trap/base.py

@@ -3,6 +3,7 @@ from __future__ import annotations
 from abc import ABC, abstractmethod
 import argparse
 from collections import defaultdict
+from copy import deepcopy
 from enum import IntFlag
 from itertools import cycle
 import json
@@ -252,6 +253,20 @@ class FisheyeCamera(DistortedCamera):
     
 
 
+class UndistortedCamera(DistortedCamera):
+    def __init__(self, fps = 10):
+        self.fps = fps
+        self.H = np.eye(3,3)
+    
+    def undistort_img(self, img: MatLike):
+        return deepcopy(img)
+
+    def undistort_points(self, distorted_points: PointList):
+        return deepcopy(distorted_points)
+        
+    
+
+
 class Camera(DistortedCamera):
     def __init__(self, mtx: cv2.Mat, dist: cv2.Mat, w: float, h: float, H: cv2.Mat, fps: float):
         self.mtx = mtx
@@ -372,7 +387,13 @@ class Track:
         if not self.created_at:
             self.created_at = time.time()
         if not self.updated_at:
-            self.update_at = time.time()
+            self.updated_at = time.time()
+    
+    def track_age(self) -> float:
+        return time.time() - self.created_at
+    
+    def track_update_dt(self) -> float:
+        return time.time() - self.updated_at
 
     def get_projected_history(self, H: Optional[cv2.Mat] = None, camera: Optional[DistortedCamera]= None) -> np.array:
         foot_coordinates = [d.get_foot_coords() for d in self.history]

trap/cv_renderer.py

@@ -117,7 +117,7 @@ class CvRenderer(Node):
         # return process
 
     def run(self):
-        frame = None
+        self.frame = None
         prediction_frame = None
         tracker_frame = None
 
@@ -129,6 +129,8 @@ class CvRenderer(Node):
         cv2.moveWindow("frame", 0, -1)
         if self.config.full_screen:
             cv2.setWindowProperty("frame",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
+
+        cv2.setMouseCallback('frame',self.click_print_position)
         # bgsub = cv2.createBackgroundSubtractorMOG2(120, 50, detectShadows=True)
 
         while self.run_loop():
@@ -140,7 +142,7 @@ class CvRenderer(Node):
             #     continue
         
             try:
-                frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
+                self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK)
             except zmq.ZMQError as e:
                 # idx = frame.index if frame else "NONE"
                 # logger.debug(f"reuse video frame {idx}")
@@ -149,7 +151,7 @@ class CvRenderer(Node):
             #     logger.debug(f'new video frame  {frame.index}')
 
             
-            if frame is None:
+            if self.frame is None:
                 # might need to wait a few iterations before first frame comes available
                 time.sleep(.1)
                 continue
@@ -180,7 +182,7 @@ class CvRenderer(Node):
                 pass
 
             if first_time is None:
-                    first_time = frame.time
+                    first_time = self.frame.time
 
             # img = frame.img
             # save_file = Path("videos/snap.png")
@@ -188,9 +190,9 @@ class CvRenderer(Node):
             #     img = frame.camera.img_to_world(frame.img, 100)
             #     cv2.imwrite(save_file, img)
 
-            img = decorate_frame(frame, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.detections, self.config.render_clusters, self.debug_lines, self.scale)
+            img = decorate_frame(self.frame, tracker_frame, prediction_frame, first_time, self.config, self.tracks, self.predictions, self.detections, self.config.render_clusters, self.debug_lines, self.scale)
 
-            logger.debug(f"write frame {frame.time - first_time:.3f}s")
+            logger.debug(f"write frame {self.frame.time - first_time:.3f}s")
             if self.out_writer:
                 self.out_writer.write(img)
             if self.streaming_process:
@@ -203,10 +205,10 @@ class CvRenderer(Node):
             # clear out old tracks & predictions:
             
             for track_id, track in list(self.tracks.items()):
-                if get_animation_position(track, frame) == 1:
+                if get_animation_position(track, self.frame) == 1:
                     self.tracks.pop(track_id)
             for prediction_id, track in list(self.predictions.items()):
-                if get_animation_position(track, frame) == 1:
+                if get_animation_position(track, self.frame) == 1:
                     self.predictions.pop(prediction_id)
 
         logger.info('Stopping')
@@ -274,6 +276,18 @@ class CvRenderer(Node):
                             type=str,
                             default="../DATASETS/hof3/map_hof.svg")
         return render_parser
+
+    def click_print_position(self, event,x,y,flags,param):
+
+        # if event == cv2.EVENT_LBUTTONDBLCLK:
+        if event == cv2.EVENT_LBUTTONUP:
+            if not self.frame:
+                return
+            scale = 100
+            print("click position:", x/scale, y/scale)
+            # self.frame.camera.points_img_to_world([[x, y]], 1)
+            # cv2.circle(img,(x,y),100,(255,0,0),-1)
+            mouseX,mouseY = x,y
     
 # colorset = itertools.product([0,255], repeat=3) # but remove white
 # colorset = [(0, 0, 0),
@@ -345,7 +359,7 @@ def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame,
                 [detection.l, detection.t],
                 [detection.l + detection.w, detection.t + detection.h],
             ]
-            points = frame.camera.points_img_to_world(points, scale)
+            points = tracker_frame.camera.points_img_to_world(points, scale)
             points = [to_point(p) for p in points] # to int
             
             w = points[1][0]-points[2][0]
@@ -363,7 +377,7 @@ def decorate_frame(frame: Frame, tracker_frame: Frame, prediction_frame: Frame,
     else:
         for track_id, track in tracks.items():
             inv_H = np.linalg.pinv(tracker_frame.H)
-            draw_track_projected(img, track, int(track_id), frame.camera, conversion)
+            draw_track_projected(img, track, int(track_id), tracker_frame.camera, conversion)
     
     for line in debug_lines:
         for rp1, rp2 in zip(line.points, line.points[1:]):

trap/lidar_tracker.py

@@ -1,17 +1,19 @@
 from argparse import ArgumentParser
-from collections import deque, namedtuple
+from collections import defaultdict, deque, namedtuple
 from copy import copy
+from pathlib import Path
+import select
 import socket
-from typing import List
+from typing import DefaultDict, 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.base import Camera, Detection, Frame, Track, UndistortedCamera
 from trap.node import Node
-
+from bytetracker import BYTETracker
-
 import velodyne_decoder as vd
 
 
@@ -29,18 +31,57 @@ ResultTuple = namedtuple("StampCloudTuple", ("stamp", "points"))
 # 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)
+    sock = 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
+    # To increase the buffer at network device, you might want to 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
+    sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 4 * 1024 * 1024)  # 4MB
-    s.bind((ip, port))
+    sock.bind((ip, port))
+    sock.setblocking(False)
+    inputs = [sock]
+
+    
     while True:
-        # aggre
+        readable, _, _ = select.select([sock], [], [], 1.0)
-        data, address = s.recvfrom(vd.PACKET_SIZE * 2)
+        latest_data = {}
-        recv_stamp = time.time()
+
-        result = decoder.decode(recv_stamp, data, as_pcl_structs)
+        if sock in readable:
-        # this is like read_pcap
+            # Drain all packets, keep only the last one, this prevents buffer build-up
-        if result is not None:
+            # in situations where the processing is not keeping up with the LiDAR speed
-            yield ResultTuple(*result)  
+            while True:
+                try:
+                    data, addr = sock.recvfrom(vd.PACKET_SIZE * 2)
+                    recv_stamp = time.time()
+                    result = decoder.decode(recv_stamp, data, as_pcl_structs)
+                    if result is not None:
+                        latest_data[addr] = ResultTuple(*result)
+                except BlockingIOError:
+                    break  # No more packets in the queue
+
+            for addr, data in latest_data.items():
+                yield data
+        else:
+            logger.warning("No LiDAR data in the last second.")
+        
+
+
+def filter_raw_points_by_distance(points: np.ndarray, center: np.ndarray, radius):
+  """
+  Filters an Open3D point cloud based on distance from a center point.
+
+  Args:
+    pointcloud: An Open3D PointCloud object.
+    center: A NumPy array representing the center point (x, y, z).
+    radius: The maximum distance from the center point to keep points.
+
+  Returns:
+    A new Open3D PointCloud object containing only the points within the specified radius.
+  """
+
+  distances = np.linalg.norm(points - center, axis=1)  # Calculate distance from each point to the center
+  indices = np.where(distances <= radius)[0]  # Find indices of points within the radius
+
+  filtered_points = points[indices]
+
+  return filtered_points 
 
 class LidarTracker(Node):
     def setup(self):
@@ -51,14 +92,23 @@ class LidarTracker(Node):
         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)
+        if self.config.pcap and self.config.pcap.exists():
+            # self.sequence_generator = vd.read_pcap(self.config.pcap, config, time_range=[1761139039+100,None])
+            self.sequence_generator = vd.read_pcap(self.config.pcap, config, time_range=[1761139039+30,None])
+            # self.sequence_generator = vd.read_pcap(self.config.pcap, config)
+        else:
+            self.sequence_generator = read_live_data(self.config.ip, self.config.port, config)
+
+        self.tracker = BYTETracker(frame_rate=10)
+
+        self.tracks: DefaultDict[str, Track] = defaultdict(lambda: Track())
 
         self.room_filter = VoxelOccupancyFilter(
-            voxel_size=0.3,
+            voxel_size=.5,
             history_size=80,
             history_interval=8, #not every frame needs to impact history
             ready_for_action_at=25,
-            static_threshold=0.5
+            static_threshold=0.3
         )
     
     def run(self):
@@ -70,14 +120,19 @@ class LidarTracker(Node):
             # Set the point size
             render_option = vis.get_render_option()
             render_option.point_size = 1.0  # Smaller point size
+            render_option.background_color = np.array([0,0,0])  # Smaller point size
+            # render_option.camera
 
             view_ctl = vis.get_view_control()
-            view_ctl.set_lookat([0, 0, 0])
+            # front_direction = np.array([0,0,-1])
-            view_ctl.set_up([0, 1, 0])
+            # front_direction = front_direction / np.linalg.norm(front_direction)  # Normalize!
-            front_direction = np.array([0.5, 0.5, -1.0])
+            # view_ctl.set_front(front_direction.tolist()) # Convert to list
-            front_direction = front_direction / np.linalg.norm(front_direction)  # Normalize!
+            # view_ctl.set_lookat([0, 0, 0])
-            view_ctl.set_front(front_direction.tolist()) # Convert to list
+            # view_ctl.set_up([0, 0, 1])
-            view_ctl.set_zoom(0.9)
+            # view_ctl.set_zoom(1)
+            # view_ctl.set_front([0, 0, -1])
+            # view_ctl.set_up([1, 0, 0])
+            # view_ctl.set_zoom(1.5)
 
             geom_added = False
         
@@ -87,32 +142,61 @@ class LidarTracker(Node):
             live_filtered_pcd = o3d.geometry.PointCloud()
             live_filtered_pcd.paint_uniform_color([0.0, 0.7, 0.2])
 
+            coordinate_indicator = o3d.geometry.TriangleMesh.create_coordinate_frame()
+            vis.add_geometry(coordinate_indicator, False)
+
+
+            ground_lines = o3d.geometry.LineSet(
+                o3d.utility.Vector3dVector([[0,0,0], [4,4,0], [4,-4,0], [-4,-4,0], [-4,4,0]]),
+                o3d.utility.Vector2iVector([[0,1], [0,2], [0,3], [0,4], [1,2], [2,3], [3,4], [4,1]])
+            )
+            ground_lines.paint_uniform_color([0,1,0])
+            vis.add_geometry(ground_lines, False)
+
+
         ground_normal = np.array([1,1,0])
 
         found_planes = False
         
-        
+        transform_matrix = np.array([[0.8457653099655785, -0.4625540400071917, -0.26594134791689383, -6.176546342430921], [-0.4412543343066861, -0.886591089552013, 0.13874744099455633, 4.588060710249307], [-0.29995941877926136, -4.340500985936626e-17, -0.9539519626719197, 0.6514765464071752], [0.0, 0.0, 0.0, 1.0]])
+        # transform_matrix = np.array([[0.8393535624356702, -0.4568202093538991, -0.2946199137064719, 7.480984770326982], [-0.46710069069557, -0.8833469220220505, 0.03892505755796417, 1.2182001838057144], [-0.2780333468817559, 0.1049452794556715, -0.9548214211786936, -1.598860001424085], [0.0, 0.0, 0.0, 1.0]])
+
+        camera = UndistortedCamera()
+
+        frame_idx = 0
         while self.run_loop():
+            frame_idx += 1
+
             stamp, lidar_points = next(self.sequence_generator)
+            # print(len(lidar_points))
+            # print(stamp)
         
-            
+            raw_frame_points = velodyne_to_points(lidar_points)
-            frame_points = velodyne_to_points(lidar_points)
+
+            raw_frame_points = filter_raw_points_by_distance(raw_frame_points, np.array([0,0,0]), 15)
+
             current_pcd = o3d.geometry.PointCloud()
-            current_pcd.points = points_to_o3d(frame_points)
+            current_pcd.points = points_to_o3d(raw_frame_points)
+            R = current_pcd.get_rotation_matrix_from_xyz((0,0, np.pi / 4+.1))
+            current_pcd.rotate(R)
+            current_pcd.translate(  [9.7, -11.14, 0])
+            current_pcd = flip_points_along_axis(current_pcd, 1)
+            # current_pcd.scale(np.array([1,-1, 1]), )
+            # current_pcd.transform(transform_matrix)
 
-            if not found_planes:
+            # if not found_planes:
-                self.planes = segment_planes(current_pcd, 4, distance_threshold=.2)
+            #     self.planes = segment_planes(current_pcd, 4, distance_threshold=.2)
-                found_planes = True
+            #     found_planes = True
-                self.logger.info(f"Found {len(self.planes)} planes in points cloud")
+            #     self.logger.info(f"Found {len(self.planes)} planes in points cloud")
 
-                if self.config.viz:
+            #     if self.config.viz:
-                    for i, plane in enumerate(self.planes):
+            #         for i, plane in enumerate(self.planes):
-                        color = [0,0,0]
+            #             color = [0,0,0]
-                        color[i%3] = 1
+            #             color[i%3] = 1
-                        # color = [1*i/len(self.planes),0,0]
+            #             # color = [1*i/len(self.planes),0,0]
-                        plane.paint_uniform_color(color)
+            #             plane.paint_uniform_color(color)
-                        print(f"Add plane {len(plane.points)} points")
+            #             print(f"Add plane {len(plane.points)} points")
-                        vis.add_geometry(plane, True)
+            #             vis.add_geometry(plane, True)
                         
 
 
@@ -161,15 +245,73 @@ class LidarTracker(Node):
                 # 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)
 
+                # append confidence and class (placeholders)
+                
+                detections = np.ones((boxes.shape[0],boxes.shape[1]+2)); 
+                detections[:,:-2] = boxes
+                # detections = np.array([
+                #     # coords in xyxy
+                #     box.tolist() + [1,1] for box in boxes
+                # ])
+
+                online_tracks = self.tracker.update(detections)
+                self.logger.debug(f"online tracks: {[t[4] for t in online_tracks]}")
+                removed_tracks = self.tracker.removed_stracks
+                # active_stracks = [track for track in self.tracker.tracked_stracks if track.is_activated]
+                active_stracks = [track for track in self.tracker.tracked_stracks if track.is_activated]
+                detections = [Detection.from_bytetrack(track, frame_idx) for track in active_stracks]
+
+
+                for detection in detections:
+                    track = self.tracks[detection.track_id]
+                    track.track_id = detection.track_id # for new tracks
+                    track.fps = 10
+                    track.frame_index = frame_idx
+                    track.updated_at = time.time()
+                    # track.fps = self.config.camera.fps # for new tracks
+                    track.history.append(detection) # add to history
+                
+                for t in removed_tracks:
+                    if t.track_id in self.tracks:
+                        del self.tracks[t.track_id]
+                    # TODO: fix this oddity:
+                    # else:
+                    #     logger.debug(f"removing non-existing track? id: {t.track_id}")
+
+                # reset for each iteration
+                # self.tracker.removed_stracks = []
+
+                active_track_ids = [d.track_id for d in detections]
+                active_tracks = {t.track_id: t.get_with_interpolated_history() for t in self.tracks.values() if t.track_id in active_track_ids}
+                # active_tracks =  displacement_filter.apply_to_dict(active_tracks, frame.camera)# a filter to remove just detecting static objects
+
+                # frame = Frame(frame_idx, None, time.time(), self.tracks, camera.H, camera)
+                frame = Frame(frame_idx, None, time.time(), active_tracks,
+                               camera.H, camera)
+
+                if self.config.smooth_tracks:
+                    frame = self.smoother.smooth_frame_tracks(frame)
+                
+                self.track_sock.send_pyobj(frame)
+                
 
 
                 if len(centroids):
                     self.logger.debug(f"{centroids=}")
 
                 if self.config.viz:
-                    line_sets = boxes_to_lineset(boxes, 0)
+                    line_sets = []
-                    for s in line_sets:
+
-                        vis.add_geometry(s, False)
+                    for box in boxes:
+                        line_sets.append(box_to_lineset(box, 0, [.3,.3,.3]))
+
+                    for track in active_stracks:
+                        color = RGB_COLORS[track.track_id % len(RGB_COLORS)]
+
+                        line_sets.append(box_to_lineset(track.tlbr,  0, color))
+
+                    for line_set in line_sets:
+                        vis.add_geometry(line_set, False)
             elif self.config.viz:
                 line_sets = []
                 live_filtered_pcd.points = o3d.utility.Vector3dVector(np.empty([0,3]))
@@ -196,13 +338,14 @@ class LidarTracker(Node):
 
                 if not geom_added:
                     vis.add_geometry(live_filtered_pcd, True)
-                    # vis.add_geometry(live_pcd, True)
+                    vis.add_geometry(live_pcd, True)
                     for s in spheres:
                         vis.add_geometry(s, False)
                     geom_added = True
                 else:
+                    # vis.remove_geometry(live_pcd, False)
+                    vis.update_geometry(live_pcd)
                     vis.update_geometry(live_filtered_pcd)
-                    # vis.update_geometry(live_pcd)
                     for s in spheres:
                         vis.add_geometry(s, False)
 
@@ -233,6 +376,14 @@ class LidarTracker(Node):
                         help='Port of the incomming ip packets',
                         type=int,
                         default=2368)
+        argparser.add_argument('--pcap',
+                        help='Read pcap instead of socket',
+                        type=Path,
+                        default=None)
+        # argparser.add_argument('--pcap-offset',
+        #                 help='offset in seconds',
+        #                 type=int,
+        #                 default=None)
         argparser.add_argument('--min-box-area',
                         help='Minimum area of bounding boxes to consider them for tracking',
                         type=float,
@@ -342,10 +493,13 @@ def get_cluster_boxes(points_xy, labels, min_area = 0):
         boxes.append([x_min, y_min, x_max, y_max])
         centroids.append(centroid)
 
+    if not len(boxes):
+        return np.empty([0,4]), np.empty([0,2])
+    
     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]:
+def box_to_lineset(box, 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:
@@ -354,38 +508,35 @@ def boxes_to_lineset(boxes, z_height=0.0, color=[1.0, 0.0, 0.0]) -> List[o3d.geo
         z_height (float): The z-coordinate height of the boxes.
         color (list): The color of the boxes (RGB).
     """
-    sets = []
+    x_min, y_min, x_max, y_max = box
-    for box in boxes:
-        x_min, y_min, x_max, y_max = box
 
-        # Define the 8 corners of the box in 3D
+    # Define the 8 corners of the box in 3D
-        corners = np.array([
+    corners = np.array([
-            [x_min, y_min, z_height],
+        [x_min, y_min, z_height],
-            [x_max, y_min, z_height],
+        [x_max, y_min, z_height],
-            [x_max, y_max, z_height],
+        [x_max, y_max, z_height],
-            [x_min, y_max, z_height],
+        [x_min, y_max, z_height],
-            [x_min, y_min, z_height + 1.0],  # Top corners
+        [x_min, y_min, z_height + 1.0],  # Top corners
-            [x_max, y_min, z_height + 1.0],
+        [x_max, y_min, z_height + 1.0],
-            [x_max, y_max, z_height + 1.0],
+        [x_max, y_max, z_height + 1.0],
-            [x_min, 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
+    # Create lines from the corners to form the edges of the box
-        line_idxs = []
+    line_idxs = []
-        for i in range(8):
+    for i in range(8):
-            for j in range(i + 1, 8):
+        for j in range(i + 1, 8):
-                line_idxs.append([i, j])
+            line_idxs.append([i, j])
 
-        line_idxs = np.array(line_idxs)
+    line_idxs = np.array(line_idxs)
 
-        points = o3d.utility.Vector3dVector(corners)
+    points = o3d.utility.Vector3dVector(corners)
-        lines = o3d.utility.Vector2iVector(line_idxs)
+    lines = o3d.utility.Vector2iVector(line_idxs)
 
-        # Create a LineSet from the lines
+    # Create a LineSet from the lines
-        line_set = o3d.geometry.LineSet(points, lines)
+    line_set = o3d.geometry.LineSet(points, lines)
-        line_set.colors = o3d.utility.Vector3dVector(np.tile(color, (len(line_idxs), 1)))  # Assign color
+    line_set.colors = o3d.utility.Vector3dVector(np.tile(color, (len(line_idxs), 1)))  # Assign color
-        sets.append(line_set)
+    return line_set
-    return sets
 
         
 
@@ -432,6 +583,7 @@ class VoxelOccupancyFilter:
         self.history_size = history_size
         self.static_threshold = static_threshold
         self.voxel_history = deque(maxlen=history_size)
+        self.static_voxels = {}
 
         self.ready_for_action_at = ready_for_action_at if ready_for_action_at else history_size
 
@@ -461,30 +613,30 @@ class VoxelOccupancyFilter:
         """
         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:
+            if voxel not in self.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:
+            current_voxel_set = self._point_cloud_to_voxel_set(current_pcd)
             self.voxel_history.append(current_voxel_set)
+            voxel_counts = {}
+            # Count voxel occurrences in history
+        
+            for voxel_set in self.voxel_history:
+                for v in voxel_set:
+                    voxel_counts[v] = voxel_counts.get(v, 0) + 1
+            # Determine static voxels
+            self.static_voxels = {v for v, count in voxel_counts.items()
+                            if count / len(self.voxel_history) >= self.static_threshold}
 
         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}")
@@ -630,3 +782,42 @@ def project_to_plane(point_cloud, plane_normal: np.ndarray):
     # projected_pc.points = o3d.utility.Vector3dVector(projected_points)
 
     return projected_points
+
+
+RGB_COLORS = [
+    (255, 0, 0),
+    (0, 255, 0),
+    # (0, 0, 255),# blue used for missing map
+    (0, 255, 255),
+    (255, 255, 0),
+]
+
+
+def flip_points_along_axis(pcd, axis):
+    """
+    Flips the points of an Open3D PointCloud along a specified axis.
+
+    Args:
+        pcd (open3d.geometry.PointCloud): The input PointCloud.
+        axis (int): The axis to flip along (0 for X, 1 for Y, 2 for Z).
+
+    Returns:
+        open3d.geometry.PointCloud: The PointCloud with flipped points.
+    """
+
+    points = np.asarray(pcd.points)
+    flipped_points = points.copy()
+
+    if axis == 0:  # Flip along X-axis
+        flipped_points[:, 0] = -flipped_points[:, 0]
+    elif axis == 1:  # Flip along Y-axis
+        flipped_points[:, 1] = -flipped_points[:, 1]
+    elif axis == 2:  # Flip along Z-axis
+        flipped_points[:, 2] = -flipped_points[:, 2]
+    else:
+        raise ValueError("Invalid axis.  Must be 0, 1, or 2.")
+
+    flipped_pcd = o3d.geometry.PointCloud()
+    flipped_pcd.points = o3d.utility.Vector3dVector(flipped_points)
+
+    return flipped_pcd

trap/video_sources.py

@@ -96,8 +96,14 @@ class GigE(VideoSource):
             # print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(20000)) # shutter 1/50 (hence; 1000000/shutter)
             print('exposure time', self.camera.f.ExposureAutoMaxValue.Set(60000)) # otherwise it becomes too blurry in movements
             print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Get())
-            print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Set(35))
+            print('brightness targt', self.camera.f.BrightnessAutoNominalValue.Set(value=35))
+            # print('brightness targt', self.camera.f.Auto.Set(neoapi.BrightnessCorrection_On))
+            # print('brightness targt', self.camera.f.BrightnessCorrection.Set(neoapi.BrightnessCorrection_On))
+            # print('brightness targt', self.camera.f.BrightnessCorrection.Set(neoapi.BrightnessCorrection_On))
             print('exposure time', self.camera.f.ExposureTime.Get())
+            print('LUTEnable', self.camera.f.LUTEnable.Get())
+            print('LUTEnable', self.camera.f.LUTEnable.Set(True))
+            # print('LUTEnable', self.camera.f.LUTEnable.Set(False))
             print('Gamma', self.camera.f.Gamma.Set(0.45))
 
             # neoapi.region