add real cam intrinsics

leggedrobotics · Jul 31, 2023 · eb4a865 · eb4a865
1 parent bc9c552
commit eb4a865
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 2 deletions.
diff --git a/wild_visual_navigation_orbit/camera_wrapper.py b/wild_visual_navigation_orbit/camera_wrapper.py
@@ -2,6 +2,7 @@
 import carb
 import numpy as np
 import torch
+import math
 from typing import List, Dict, Optional, Sequence, Union
 
 from omni.isaac.orbit.sensors.camera.camera import Camera
@@ -46,14 +47,54 @@
 )
 
 
+WILL_DEPTH_CFG = PinholeCameraCfg(
+    sensor_tick=1/30.0,
+    height=540,
+    width=720,
+    data_types=["rgb", "distance_to_camera"],
+    usd_params=PinholeCameraCfg.UsdCameraCfg(
+        focal_length=1.0, horizontal_aperture=1.25086, clipping_range=(0.1, 1.0e5)
+    ),
+)
+
+
+k_wide_angle_rosbag = [575.6050407221768, 0.0, 745.7312198525915, 0.0, 578.564849365178, 519.5207040671075, 0.0, 0.0, 1.0]
+k_depth_rosbag = [423.54608154296875, 0.0, 427.6981506347656, 0.0, 423.54608154296875, 240.177734375, 0.0, 0.0, 1.0]
+
+def ha_from_K(K, w):
+    # inverse of Camera.compute_k()
+    return .5*math.tan(math.atan(w / (2*K[0])))
+
+ha_wide_angle = ha_from_K(k_wide_angle_rosbag, 1440)
+ha_depth = ha_from_K(k_depth_rosbag, 848)
+
+WILL_WIDE_ANGLE_CFG = PinholeCameraCfg(
+    sensor_tick=1/30.0,
+    height=1080,
+    width=1440,
+    data_types=["rgb", "distance_to_camera"],
+    usd_params=PinholeCameraCfg.UsdCameraCfg(
+        focal_length=1.0, horizontal_aperture=ha_wide_angle, clipping_range=(0.1, 1.0e5)
+    ),
+)
+
+WILL_DEPTH_CFG = PinholeCameraCfg(
+    sensor_tick=1/30.0,
+    height=480,
+    width=848,
+    data_types=["rgb", "distance_to_camera"],
+    usd_params=PinholeCameraCfg.UsdCameraCfg(
+        focal_length=1.0, horizontal_aperture=ha_depth, clipping_range=(0.1, 1.0e5)
+    ),
+)
+
 
 class CameraWrapper(Camera):
     """
     TO get LATEST frame use:
         - Camera.data.output
             or 
         - call get_most_recent_frame()
-    
     """
 
     def __init__(self, cfg: Union[PinholeCameraCfg, FisheyeCameraCfg], device: str = "cpu"):

diff --git a/wild_visual_navigation_orbit/voxseg_standalone.py b/wild_visual_navigation_orbit/voxseg_standalone.py
@@ -35,7 +35,7 @@
 from debugviz import DebugViz
 
 # Camera Imports (Sean)
-from camera_wrapper import CameraWrapper, ANYMAL_D_CFG
+from camera_wrapper import CameraWrapper, ANYMAL_D_CFG, ANYMAL_DEPTH_CAMERA_CFG
 
 # Planning Imports
 from ws_isaac_planner.planner import PlanningAgent