From d4a1e3ece4cc0de346f79d41f2a1593f03f8c1e2 Mon Sep 17 00:00:00 2001 From: Janick Martinez Esturo Date: Wed, 6 May 2026 13:48:06 +0200 Subject: [PATCH] feat(ncore_vis): Add option to recenter world coordinates near the origin Some datasets (like waymo) might have large local world offsets that can cause rendering artifacts in WebGL due to floating-point precision issues and requires explicitl initial recentering to the scene in the viewer. This change avoids both issues by allowing to recenter to the first pose (if available) to the viewers origin. --- docs/tools/ncore_vis.rst | 3 + tools/ncore_vis/cli.py | 8 +++ tools/ncore_vis/components/camera.py | 1 + tools/ncore_vis/components/cuboids.py | 1 + tools/ncore_vis/components/lidar.py | 3 +- tools/ncore_vis/components/point_clouds.py | 1 + tools/ncore_vis/components/radar.py | 3 +- tools/ncore_vis/components/trajectory.py | 3 + tools/ncore_vis/data_loader.py | 65 ++++++++++++++++++++++ tools/ncore_vis/server.py | 7 ++- 10 files changed, 92 insertions(+), 3 deletions(-) diff --git a/docs/tools/ncore_vis.rst b/docs/tools/ncore_vis.rst index 7cee0d6d..f5cfd7bb 100644 --- a/docs/tools/ncore_vis.rst +++ b/docs/tools/ncore_vis.rst @@ -60,6 +60,9 @@ Global Options * - ``--world-frame-id`` - ``world`` - Pose graph frame ID for the world/map reference + * - ``--recenter-world/--no-recenter-world`` + - on + - Whether to recenter the viewer world frame to the rig's initial pose * - ``--debug`` - off - Start a debugpy remote debugging session diff --git a/tools/ncore_vis/cli.py b/tools/ncore_vis/cli.py index fff7a6fb..9236cf16 100644 --- a/tools/ncore_vis/cli.py +++ b/tools/ncore_vis/cli.py @@ -47,6 +47,7 @@ class CLIBaseParams: port: int rig_frame_id: str world_frame_id: str + recenter_world: bool debug: bool debug_port: int @@ -56,6 +57,12 @@ class CLIBaseParams: @click.option("--port", type=int, default=8080, help="Server port") @click.option("--rig-frame-id", type=str, default="rig", help="Pose graph frame ID for the rig/vehicle body") @click.option("--world-frame-id", type=str, default="world", help="Pose graph frame ID for the world/map reference") +@click.option( + "--recenter-world/--no-recenter-world", + default=True, + help="Recenter the scene near the origin by subtracting the first rig pose translation. " + "Prevents rendering artifacts caused by large world-frame offsets.", +) @click.option("--debug", is_flag=True, default=False, help="Start a debugpy remote debugging session") @click.option("--debug-port", type=int, default=5678, help="Port on which debugpy will wait for a client to connect") @click.pass_context @@ -133,6 +140,7 @@ def run(params: CLIBaseParams, loader: SequenceLoaderProtocol) -> None: port=params.port, rig_frame_id=params.rig_frame_id, world_frame_id=params.world_frame_id, + recenter_world=params.recenter_world, ) server.start() diff --git a/tools/ncore_vis/components/camera.py b/tools/ncore_vis/components/camera.py index e7d82d52..4555abab 100644 --- a/tools/ncore_vis/components/camera.py +++ b/tools/ncore_vis/components/camera.py @@ -583,6 +583,7 @@ def _update_camera(self, camera_id: str) -> None: T_camera_world = cam.get_frames_T_sensor_target( self.data_loader.world_frame_id, frame_idx, FrameTimepoint.END ) + T_camera_world = self.data_loader.rebase_world_se3(T_camera_world) position, wxyz = se3_to_position_wxyz(T_camera_world) # Pose frame diff --git a/tools/ncore_vis/components/cuboids.py b/tools/ncore_vis/components/cuboids.py index a6a261c8..4b8b08c7 100644 --- a/tools/ncore_vis/components/cuboids.py +++ b/tools/ncore_vis/components/cuboids.py @@ -124,6 +124,7 @@ def _update_cuboids(self) -> None: for i, obs in enumerate(observations): bbox = obs.bbox3 T_bbox_world = se3_from_centroid_euler(bbox.centroid, bbox.rot) + T_bbox_world = self.data_loader.rebase_world_se3(T_bbox_world) position, wxyz = se3_to_position_wxyz(T_bbox_world) color = self.renderer.get_class_color(obs.class_id) diff --git a/tools/ncore_vis/components/lidar.py b/tools/ncore_vis/components/lidar.py index 327f20d9..5c1ef8c3 100644 --- a/tools/ncore_vis/components/lidar.py +++ b/tools/ncore_vis/components/lidar.py @@ -416,7 +416,8 @@ def _transform_to_world(self, lidar_id: str, frame_idx: int, points_sensor: np.n T_sensor_world = sensor.get_frames_T_sensor_target( self.data_loader.world_frame_id, frame_idx, FrameTimepoint.END ) - return transform_point_cloud(points_sensor, T_sensor_world) + points_world = transform_point_cloud(points_sensor, T_sensor_world) + return self.data_loader.rebase_world_points(points_world) def _get_fused_point_cloud( self, diff --git a/tools/ncore_vis/components/point_clouds.py b/tools/ncore_vis/components/point_clouds.py index 63f679e6..ec5ca22b 100644 --- a/tools/ncore_vis/components/point_clouds.py +++ b/tools/ncore_vis/components/point_clouds.py @@ -273,6 +273,7 @@ def _update_point_cloud(self, source_id: str) -> None: target_frame_timestamp_us=pc.reference_frame_timestamp_us, pose_graph=self.data_loader.pose_graph, ).xyz + points_world = self.data_loader.rebase_world_points(points_world) colors = self._colorize_points(source_id, pc, points_world) diff --git a/tools/ncore_vis/components/radar.py b/tools/ncore_vis/components/radar.py index 5d10f0f9..51195856 100644 --- a/tools/ncore_vis/components/radar.py +++ b/tools/ncore_vis/components/radar.py @@ -414,7 +414,8 @@ def _transform_to_world(self, radar_id: str, frame_idx: int, points_sensor: np.n T_sensor_world = sensor.get_frames_T_sensor_target( self.data_loader.world_frame_id, frame_idx, FrameTimepoint.END ) - return transform_point_cloud(points_sensor, T_sensor_world) + points_world = transform_point_cloud(points_sensor, T_sensor_world) + return self.data_loader.rebase_world_points(points_world) def _get_fused_point_cloud( self, diff --git a/tools/ncore_vis/components/trajectory.py b/tools/ncore_vis/components/trajectory.py index 73d135ac..fe350d2b 100644 --- a/tools/ncore_vis/components/trajectory.py +++ b/tools/ncore_vis/components/trajectory.py @@ -109,6 +109,8 @@ def populate_scene(self) -> None: if poses.shape[0] == 0: return + poses = self.data_loader.rebase_world_se3(poses) + with self.client.atomic(): for i in range(poses.shape[0]): T = poses[i] @@ -178,6 +180,7 @@ def _place_rig_frame(self, pose: Optional[np.ndarray]) -> None: self._rig_frame_handle.visible = False return + pose = self.data_loader.rebase_world_se3(pose) position, wxyz = se3_to_position_wxyz(pose) if self._rig_frame_handle is None: diff --git a/tools/ncore_vis/data_loader.py b/tools/ncore_vis/data_loader.py index a88fd20c..ea7687f6 100644 --- a/tools/ncore_vis/data_loader.py +++ b/tools/ncore_vis/data_loader.py @@ -54,10 +54,12 @@ def __init__( loader: SequenceLoaderProtocol, rig_frame_id: Optional[str] = "rig", world_frame_id: str = "world", + recenter_world: bool = True, ) -> None: self._loader: SequenceLoaderProtocol = loader self._rig_frame_id: Optional[str] = rig_frame_id self._world_frame_id: str = world_frame_id + self._recenter_world: bool = recenter_world self._warned_pose_paths: set = set() # Build cuboid DataFrame and tracks eagerly (thread-safe: done once at init) @@ -100,6 +102,69 @@ def world_frame_id(self) -> str: """Pose graph frame ID for the world/map reference.""" return self._world_frame_id + # ------------------------------------------------------------------ + # World recentering + # ------------------------------------------------------------------ + + @functools.cached_property + def world_origin_offset(self) -> np.ndarray: + """Translation offset subtracted from world coordinates to place the scene near the origin. + + When ``recenter_world`` is enabled, returns the translation component of the + first rig-to-world pose (i.e. the rig position at t=0). This ensures that all + rendered geometry is close to the coordinate origin, preventing floating-point + precision issues in the WebGL renderer. + + Returns ``[0, 0, 0]`` when recentering is disabled or trajectory data is unavailable. + """ + if not self._recenter_world: + return np.zeros(3, dtype=np.float64) + + if self._rig_frame_id is None: + return np.zeros(3, dtype=np.float64) + + interval = self._loader.sequence_timestamp_interval_us + try: + poses = self.pose_graph.evaluate_poses( + self._rig_frame_id, self._world_frame_id, np.array([interval.start], dtype=np.uint64) + ) + except KeyError: + return np.zeros(3, dtype=np.float64) + + offset = poses[0, :3, 3].astype(np.float64).copy() + logger.info("World origin offset (recenter): [%.2f, %.2f, %.2f]", offset[0], offset[1], offset[2]) + return offset + + def rebase_world_se3(self, T: np.ndarray) -> np.ndarray: + """Subtract :attr:`world_origin_offset` from the translation of SE3 matrix/matrices. + + Args: + T: SE3 matrix of shape ``[4, 4]`` or batch ``[N, 4, 4]``. + + Returns: + *T* with the translation column adjusted. + """ + offset = self.world_origin_offset + if not offset.any(): + return T + T[..., :3, 3] -= offset + return T + + def rebase_world_points(self, points: np.ndarray) -> np.ndarray: + """Subtract :attr:`world_origin_offset` from XYZ point coordinates. + + Args: + points: Array of shape ``[N, 3]`` or ``[N, 3+]``. + + Returns: + *points* with XYZ columns adjusted. + """ + offset = self.world_origin_offset + if not offset.any(): + return points + points[:, :3] -= offset + return points + @functools.lru_cache(maxsize=None) def get_camera_sensor(self, camera_id: str) -> CameraSensorProtocol: """Return a camera sensor by ID (cached).""" diff --git a/tools/ncore_vis/server.py b/tools/ncore_vis/server.py index 85ecd366..c8dac468 100644 --- a/tools/ncore_vis/server.py +++ b/tools/ncore_vis/server.py @@ -45,17 +45,22 @@ def __init__( port: int, rig_frame_id: Optional[str] = "rig", world_frame_id: str = "world", + recenter_world: bool = True, ) -> None: self._loader: SequenceLoaderProtocol = loader self._host: str = host self._port: int = port self._rig_frame_id: Optional[str] = rig_frame_id self._world_frame_id: str = world_frame_id + self._recenter_world: bool = recenter_world def start(self) -> None: """Create the data loader, start the viser server, and block forever.""" self._data_loader = DataLoader( - self._loader, rig_frame_id=self._rig_frame_id, world_frame_id=self._world_frame_id + self._loader, + rig_frame_id=self._rig_frame_id, + world_frame_id=self._world_frame_id, + recenter_world=self._recenter_world, ) self._renderers: Dict[int, NCoreVisRenderer] = {}