Add radar extrinsics to Calibration (#67)

Author: mrafidashti

zod/data_classes/calibration.py

@@ -4,11 +4,11 @@
 
 import json
 from dataclasses import dataclass
-from typing import Any, Dict
+from typing import Any, Dict, Optional
 
 import numpy as np
 
-from zod.constants import EGO, Camera, CoordinateFrame, Lidar
+from zod.constants import EGO, Camera, CoordinateFrame, Lidar, Radar
 from zod.data_classes.geometry import Pose
 from zod.utils.geometry import transform_points
 
@@ -28,10 +28,16 @@ class CameraCalibration:
     field_of_view: np.ndarray  # horizontal, vertical (degrees)
 
 
+@dataclass
+class RadarCalibration:
+    extrinsics: Pose  # radar pose in the ego frame
+
+
 @dataclass
 class Calibration:
     lidars: Dict[Lidar, LidarCalibration]
     cameras: Dict[Camera, CameraCalibration]
+    radars: Optional[Dict[Radar, RadarCalibration]] = None
 
     @classmethod
     def from_dict(cls, calib_dict: Dict[str, Any]) -> Calibration:
@@ -48,7 +54,14 @@ def from_dict(cls, calib_dict: Dict[str, Any]) -> Calibration:
                 field_of_view=np.array(calib_dict["FC"]["field_of_view"]),
             ),
         }
-        return cls(lidars=lidars, cameras=cameras)
+
+        if "radar_extrinsics" in calib_dict["FC"]:
+            radars = {
+                Radar.FRONT: RadarCalibration(extrinsics=Pose(np.array(calib_dict["FC"]["radar_extrinsics"]))),
+            }
+            return cls(lidars=lidars, cameras=cameras, radars=radars)
+        else:
+            return cls(lidars=lidars, cameras=cameras)
 
     @classmethod
     def from_json_path(cls, json_path: str) -> Calibration:
@@ -85,5 +98,12 @@ def get_extrinsics(self, frame: CoordinateFrame) -> Pose:
             return self.lidars[frame].extrinsics
         elif isinstance(frame, Camera):
             return self.cameras[frame].extrinsics
+        elif isinstance(frame, Radar):
+            if self.radars is None:
+                err = "No radar calibration info available!"
+                err += "\nPlease download the latest version of ZOD to access radar data. "
+                err += "\nRadar is available for ZOD Sequences and Drives only."
+                raise ValueError(err)
+            return self.radars[frame].extrinsics
         else:
             raise ValueError(f"Unknown frame {frame}")