fix(cam): depth scaling corrected

Author: juelg

extensions/rcs_realsense/src/rcs_realsense/camera.py

@@ -286,8 +286,8 @@ def to_ts(frame: rs.frame) -> float:
             elif rs.stream.depth == stream.stream_type():
                 frame = frameset.get_depth_frame()
                 depth = DataFrame(
-                    data=(to_numpy(frame).astype(np.float64) * device.depth_scale / BaseCameraSet.DEPTH_SCALE).astype(
-                        np.int16
+                    data=(to_numpy(frame).astype(np.float64) * device.depth_scale * BaseCameraSet.DEPTH_SCALE).astype(
+                        np.uint16
                     ),
                     timestamp=to_ts(frame),
                     intrinsics=device.depth_intrinsics,

python/rcs/camera/sim.py

@@ -10,7 +10,7 @@
 from rcs._core.sim import FrameSet as _FrameSet
 from rcs._core.sim import SimCameraConfig
 from rcs._core.sim import SimCameraSet as _SimCameraSet
-from rcs.camera.interface import CameraFrame, DataFrame, Frame, FrameSet
+from rcs.camera.interface import BaseCameraSet, CameraFrame, DataFrame, Frame, FrameSet
 
 from rcs import sim
 
@@ -77,13 +77,15 @@ def _cpp_to_python_frames(self, cpp_frameset: _FrameSet | None) -> FrameSet | No
                     data=color_np_frame,
                     timestamp=cpp_frameset.timestamp,
                     intrinsics=self._intrinsics(color_name),
-                    extrinsics=self._extrinsics(color_frame),
+                    extrinsics=self._extrinsics(color_name),
                 ),
                 depth=DataFrame(
-                    data=depth_np_frame,
+                    data= (depth_np_frame * BaseCameraSet.DEPTH_SCALE).astype(
+                        np.uint16
+                    ),
                     timestamp=cpp_frameset.timestamp,
-                    intrinsics=self._intrinsics(depth_frame),
-                    extrinsics=self._extrinsics(depth_frame),
+                    intrinsics=self._intrinsics(depth_name),
+                    extrinsics=self._extrinsics(depth_name),
                 ),
             )
             frame = Frame(camera=cameraframe, avg_timestamp=cpp_frameset.timestamp)

python/rcs/envs/base.py

@@ -604,12 +604,13 @@ def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False):
             params.update(
                 {
                     f"/{name}/{self.DEPTH_KEY}/frame": {
+                        # values metric but scaled with factor rcs.BaseCameraSet.DEPTH_SCALE to fit into uint16
                         "height": camera_set.config(name).resolution_height,
                         "width": camera_set.config(name).resolution_width,
                         "color_dim": 1,
                         "dtype": np.uint16,
-                        "low": -np.inf,
-                        "high": np.inf,
+                        "low": 0,
+                        "high": 65535,
                     }
                     for name in camera_set.camera_names
                 }