enhancement #138: fix P values

Author: FabienDanieau

pollen_vision/pollen_vision/camera_wrappers/depthai/cam_config.py

@@ -3,6 +3,7 @@
 import json
 from typing import Dict, List, Optional, Tuple
 
+import cv2
 import depthai as dai
 import numpy as np
 import numpy.typing as npt
@@ -66,6 +67,10 @@ def __init__(
         }
         self.calib: dai.CalibrationHandler = dai.CalibrationHandler()
 
+        # lazy init, camera needs to be connected to
+        self.P_left: Optional[cv2.UMat] = None
+        self.P_right: Optional[cv2.UMat] = None
+
     def get_device_info(self) -> dai.DeviceInfo:
         """Returns a dai.DeviceInfo object with the mx_id.
         This allows connecting to multiple devices plugged in the host machine at the same time,
@@ -147,6 +152,30 @@ def to_string(self) -> str:
 
         return ret_string
 
+    def compute_projection_matrices(self) -> Tuple[cv2.UMat, cv2.UMat]:
+        left_socket = get_socket_from_name("left", self.name_to_socket)
+        right_socket = get_socket_from_name("right", self.name_to_socket)
+
+        left_D = np.array(self.calib.getDistortionCoefficients(left_socket))
+        right_D = np.array(self.calib.getDistortionCoefficients(right_socket))
+
+        R = np.array(self.calib.getStereoRightRectificationRotation())
+
+        T = np.array(self.calib.getCameraTranslationVector(left_socket, right_socket))
+        T *= 0.01  # to meter for ROS
+
+        R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
+            self.get_K_left(),
+            left_D,
+            self.get_K_right(),
+            right_D,
+            self.undistort_resolution,
+            R,
+            T,
+            flags=0,
+        )
+        return P1, P2
+
     def to_ROS_msg(
         self, side: str = "left"
     ) -> Tuple[int, int, str, List[float], npt.NDArray[np.float32], npt.NDArray[np.float32], npt.NDArray[np.float32]]:
@@ -159,24 +188,17 @@ def to_ROS_msg(
             distortion_model = "equidistant"
         D = self.calib.getDistortionCoefficients(get_socket_from_name(side, self.name_to_socket))
 
+        if self.P_left is None or self.P_right is None:
+            self.P_left, self.P_right = self.compute_projection_matrices()
+
         if side == "left":
             K = self.get_K_left().flatten()
             R = np.array(self.calib.getStereoLeftRectificationRotation()).flatten()
-            P_t = np.zeros(3).reshape((3, 1))  # Tx, Ty, 0
-            P = np.hstack((self.get_K_left(), P_t)).flatten()
+            P = np.array(self.P_left).flatten()
 
         else:
             K = self.get_K_right().flatten()
             R = np.array(self.calib.getStereoRightRectificationRotation()).flatten()
-            Extrinsics = np.array(
-                self.calib.getCameraExtrinsics(
-                    srcCamera=get_socket_from_name("left", self.name_to_socket),
-                    dstCamera=get_socket_from_name("right", self.name_to_socket),
-                )
-            ).reshape((4, 4))
-            P_t = np.zeros(3).reshape((3, 1))  # Tx, Ty, 0
-            P_t[0] = Extrinsics[0, 3]  # Tx
-            P_t[1] = Extrinsics[1, 3]  # Ty
-            P = np.hstack((self.get_K_right(), P_t)).flatten()
+            P = np.array(self.P_right).flatten()
 
         return height, width, distortion_model, D, K, R, P