feat(ncore_vis): Add option to recenter world coordinates near the origin

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

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()
 

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

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)
 

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,

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)
 

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,

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:

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)."""

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