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] = {}