feat(sim): intrinsic and extrinsic as additional camera data

- added extrinsic and intrinsic matrices to camera interface
- added extrinsic and intrinsic to mujoco camera set

Author: juelg

python/rcs/camera/interface.py

@@ -1,8 +1,9 @@
 import logging
 from dataclasses import dataclass
 from datetime import datetime
-from typing import Any, Protocol
+from typing import Any, Literal, Protocol
 
+import numpy as np
 from rcs._core.common import BaseCameraConfig
 
 logger = logging.getLogger(__name__)
@@ -14,6 +15,8 @@ class DataFrame:
     data: Any
     # timestamp in posix time
     timestamp: float | None = None
+    intrinsics: np.ndarray[tuple[Literal[3, 4]], np.dtype[np.float64]] | None = None
+    extrinsics: np.ndarray[tuple[Literal[4, 4]], np.dtype[np.float64]] | None = None
 
 
 @dataclass(kw_only=True)

python/rcs/camera/sim.py

@@ -1,9 +1,12 @@
 import logging
 from datetime import datetime
+from typing import Literal
 
+import mujoco
 import numpy as np
 
 # from rcs._core.common import BaseCameraConfig
+from rcs._core import common
 from rcs._core.sim import FrameSet as _FrameSet
 from rcs._core.sim import SimCameraConfig
 from rcs._core.sim import SimCameraSet as _SimCameraSet
@@ -68,14 +71,46 @@ def _cpp_to_python_frames(self, cpp_frameset: _FrameSet | None) -> FrameSet | No
                 near = self._sim.model.vis.map.znear * extent
                 far = self._sim.model.vis.map.zfar * extent
                 depth_np_frame = near / (1 - depth_np_frame * (1 - near / far))
+
+            intrinsics = self._intrinsics(color_name)
+            extrinsics = self._extrinsics(color_name)
+
             cameraframe = CameraFrame(
-                color=DataFrame(data=color_np_frame, timestamp=cpp_frameset.timestamp),
-                depth=DataFrame(data=depth_np_frame, timestamp=cpp_frameset.timestamp),
+                color=DataFrame(
+                    data=color_np_frame, timestamp=cpp_frameset.timestamp, intrinsics=intrinsics, extrinsics=extrinsics
+                ),
+                depth=DataFrame(
+                    data=depth_np_frame, timestamp=cpp_frameset.timestamp, intrinsics=intrinsics, extrinsics=extrinsics
+                ),
             )
             frame = Frame(camera=cameraframe, avg_timestamp=cpp_frameset.timestamp)
             frames[color_name] = frame
         return FrameSet(frames=frames, avg_timestamp=cpp_frameset.timestamp)
 
+    def _intrinsics(self, camera_name) -> np.ndarray[tuple[Literal[3, 4]], np.dtype[np.float64]]:
+        cam_id = mujoco.mj_name2id(self._sim.model, mujoco.mjtObj.mjOBJ_CAMERA, self.cameras[camera_name].identifier)
+        fovy = self._sim.model.cam_fovy[cam_id]
+        fx = fy = 0.5 * self.cameras[camera_name].resolution_height / np.tan(fovy * np.pi / 360)
+        return np.array(
+            [
+                [fx, 0, (self.cameras[camera_name].resolution_width - 1) / 2, 0],
+                [0, fy, (self.cameras[camera_name].resolution_height - 1) / 2, 0],
+                [0, 0, 1, 0],
+            ]
+        )
+
+    def _extrinsics(self, camera_name) -> np.ndarray[tuple[Literal[4, 4]], np.dtype[np.float64]]:
+        cam_id = mujoco.mj_name2id(self._sim.model, mujoco.mjtObj.mjOBJ_CAMERA, self.cameras[camera_name].identifier)
+        xpos = self._sim.data.cam_xpos[cam_id]
+        xmat = self._sim.data.cam_xmat[cam_id].reshape(3, 3)
+
+        cam = common.Pose(rotation=xmat, translation=xpos)
+        # put z axis infront
+        rotation_p = common.Pose(rpy_vector=[np.pi, 0, 0], translation=[0, 0, 0])
+        cam = cam * rotation_p
+
+        return cam.inverse().pose_matrix()
+
     def config(self, camera_name: str) -> SimCameraConfig:
         """Should return the configuration of the camera with the given name."""
         return self.cameras[camera_name]