Add camera instrinsics config files; and class to convert points base…

angel_system/data/common/calibrate_camera_bboxes.py

@@ -0,0 +1,49 @@
+import configparser
+import kwimage
+
+import numpy as np
+
+
+class CameraCalibration:
+    def __init__(
+        self,
+        src_calibration_fp="config/camera_intrinsics/hololens2.conf",
+        src_calibration_key="RIGHT_CAM_HD",
+        dst_calibration_fp="config/camera_intrinsics/zed.conf",
+        dst_calibration_key="RIGHT_CAM_HD"
+    ):
+        # Read the camera intrinsics
+        configparser = configparser.ConfigParser()
+        src_calibration_config = configparser.read(src_calibration_fp)
+        dst_calibration_config = configparser.read(dst_calibration_fp)
+
+        src_intrinsics = src_calibration_config[src_calibration_key]
+        dst_intrinsics = dst_calibration_config[dst_calibration_key]
+
+        self.k_src = np.array(
+            [src_intrinsics["fx"], src_intrinsics["s"],  src_intrinsics["cx"]],
+            [0,                    src_intrinsics["fy"], src_intrinsics["cy"]],
+            [0,                    0,                    1]
+        )
+        self.k_dst = np.array(
+            [dst_intrinsics["fx"], dst_intrinsics["s"],  dst_intrinsics["cx"]],
+            [0,                    dst_intrinsics["fy"], dst_intrinsics["cy"]],
+            [0,                    0,                    1]
+        )
+
+        self.k = np.matmul(self.k_dst, np.linalg.inv(self.k_src))
+
+    def calibrate_bbox(self, bbox: kwimage.Box):
+        # Make sure the bbox is tlbr
+        bbox = bbox.toformat("tlbr")
+        tl = bbox.data[:1]
+        br = bbox.data[1:]
+
+        # Convert coordinates from src to dst camera
+        tl_dst = numpy.matmul(self.k, tl)
+        br_dst = numpy.matmul(self.k, br)
+
+        tl_final = tl_dst / tl_dst[2]
+        br_final = br_dst / br_dst[2]
+
+        return tl_final + br_final

config/camera_intrinsics/hololens2.conf

@@ -0,0 +1,41 @@
+# Helpful resource: https://de.mathworks.com/help/vision/ug/camera-calibration.html
+
+[PV_CAM_1280x720]
+ImageHeight=720
+ImageWidth=1280
+FrameRate=30
+# focal length
+fx=986.7971
+fy=986.1311
+# principal point
+cx=628.5365
+cy=338.7137
+# radial distortion
+k1=0  
+k2=0
+k3=0
+# tangential distortion
+p1=0
+p2=0
+# skew coefficient
+s=0
+
+[PV_CAM_760x428]
+ImageHeight=428
+ImageWidth=760
+FrameRate=30
+# focal length
+fx=589.9811
+fy=589.1793
+# principal point
+cx=314.2682
+cy=169.3568
+# radial distortion
+k1=0  
+k2=0
+k3=0
+# tangential distortion
+p1=0
+p2=0
+# skew coefficient
+s=0

config/camera_intrinsics/zed.conf

@@ -0,0 +1,35 @@
+# Helpful resource: https://de.mathworks.com/help/vision/ug/camera-calibration.html
+
+[LEFT_CAM_HD]
+# focal length
+fx=699.25
+fy=699.31
+# principal point
+cx=671.745
+cy=368.2635
+# radial distortion
+k1=-0.174627
+k2=0.0277376
+k3=9.87232e-11
+# tangential distortion
+p1=0.000100253
+p2=7.86794e-05
+# skew coefficient
+s=0
+
+[RIGHT_CAM_HD]
+# focal length
+fx=699.73
+fy=699.865
+# principal point
+cx=643.675
+cy=366.9085
+# radial distortion
+k1=-0.173558
+k2=0.0269429
+k3=3.36205e-11
+# tangential distortion
+p1=-4.90694e-05
+p2=-0.000155324
+# skew coefficient
+s=0