SDK 1.14 - Tof decoding

depthai_sdk/examples/StereoComponent/depth_score.py

@@ -0,0 +1,12 @@
+from depthai_sdk import OakCamera
+
+def callback(packet):
+    print(packet.depth_score)
+
+with OakCamera() as oak:
+    stereo = oak.create_stereo('800p', fps=60)
+
+    stereo.config_output(depth_score=True)
+    stereo.config_output(depth_score=True)
+    oak.callback(stereo.out.disparity, callback)
+    oak.start(blocking=True)

depthai_sdk/examples/ToFComponent/tof.py

@@ -0,0 +1,8 @@
+from depthai_sdk import OakCamera
+
+with OakCamera() as oak:
+    tof = oak.create_tof("cama")
+    depth_q = oak.queue(tof.out.depth).queue
+    amplitude_q = oak.queue(tof.out.amplitude).queue
+    oak.visualize([tof.out.depth, tof.out.amplitude])
+    oak.start(blocking=True)

depthai_sdk/examples/ToFComponent/tof_align.py

@@ -0,0 +1,19 @@
+from depthai_sdk import OakCamera
+from depthai_sdk.classes.packets import DisparityDepthPacket
+import cv2
+from depthai_sdk.visualize.visualizer import Visualizer
+
+with OakCamera() as oak:
+    cam_c = oak.create_camera('CAM_C')
+    tof = oak.create_tof("CAM_A", align_to=cam_c)
+    depth_q = oak.queue(tof.out.depth).queue
+
+    vis = Visualizer() # Only for depth colorization
+    oak.start()
+    while oak.running():
+        depth: DisparityDepthPacket = depth_q.get()
+        colored_depth = depth.get_colorized_frame(vis)
+        cv2.imshow("depth", colored_depth)
+        cv2.imshow('Weighted', cv2.addWeighted(depth.aligned_frame.getCvFrame(), 0.5, colored_depth, 0.5, 0))
+        if cv2.waitKey(1) == ord('q'):
+            break

depthai_sdk/examples/mixed/collision_avoidance.py

@@ -1,6 +1,7 @@
 from depthai_sdk import OakCamera
 from depthai_sdk.visualize.configs import StereoColor
 from depthai_sdk.classes.packets import DisparityDepthPacket
+from depthai_sdk.visualize.visualizers.opencv_visualizer import OpenCvVisualizer
 import math
 import depthai as dai
 import cv2
@@ -10,38 +11,9 @@
 CRITICAL = 500 # 50cm, red
 
 slc_data = []
+fontType = cv2.FONT_HERSHEY_TRIPLEX
 
-def cb(packet: DisparityDepthPacket):
-    global slc_data
-    fontType = cv2.FONT_HERSHEY_TRIPLEX
-
-    depthFrameColor = packet.visualizer.draw(packet.frame)
-
-    for depthData in slc_data:
-        roi = depthData.config.roi
-        roi = roi.denormalize(width=depthFrameColor.shape[1], height=depthFrameColor.shape[0])
-
-        xmin = int(roi.topLeft().x)
-        ymin = int(roi.topLeft().y)
-        xmax = int(roi.bottomRight().x)
-        ymax = int(roi.bottomRight().y)
-
-        coords = depthData.spatialCoordinates
-        distance = math.sqrt(coords.x ** 2 + coords.y ** 2 + coords.z ** 2)
-
-        if distance == 0: # Invalid
-            continue
-
-        if distance < CRITICAL:
-            color = (0, 0, 255)
-            cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=4)
-            cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color)
-        elif distance < WARNING:
-            color = (0, 140, 255)
-            cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=2)
-            cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color)
-
-    cv2.imshow('0_depth', depthFrameColor)
+
 
 with OakCamera() as oak:
     stereo = oak.create_stereo('720p')
@@ -54,7 +26,6 @@ def cb(packet: DisparityDepthPacket):
     stereo.config_postprocessing(colorize=StereoColor.RGBD, colormap=cv2.COLORMAP_BONE)
     stereo.config_stereo(confidence=50, lr_check=True, extended=True)
 
-    oak.visualize([stereo], fps=True, callback=cb)
 
     slc = oak.pipeline.create(dai.node.SpatialLocationCalculator)
     for x in range(15):
@@ -73,10 +44,41 @@ def cb(packet: DisparityDepthPacket):
     slc_out.setStreamName('slc')
     slc.out.link(slc_out.input)
 
+    stereoQ = oak.queue(stereo.out.depth).get_queue()
+
     oak.start() # Start the pipeline (upload it to the OAK)
 
-    q = oak.device.getOutputQueue('slc') # Create output queue after calling start()
+    slcQ = oak.device.getOutputQueue('slc') # Create output queue after calling start()
+    vis = OpenCvVisualizer()
     while oak.running():
-        if q.has():
-            slc_data = q.get().getSpatialLocations()
-        oak.poll()
+        oak.poll()
+        packet: DisparityDepthPacket = stereoQ.get()
+        slc_data = slcQ.get().getSpatialLocations()
+
+        depthFrameColor = packet.get_colorized_frame(vis)
+
+        for depthData in slc_data:
+            roi = depthData.config.roi
+            roi = roi.denormalize(width=depthFrameColor.shape[1], height=depthFrameColor.shape[0])
+
+            xmin = int(roi.topLeft().x)
+            ymin = int(roi.topLeft().y)
+            xmax = int(roi.bottomRight().x)
+            ymax = int(roi.bottomRight().y)
+
+            coords = depthData.spatialCoordinates
+            distance = math.sqrt(coords.x ** 2 + coords.y ** 2 + coords.z ** 2)
+
+            if distance == 0: # Invalid
+                continue
+
+            if distance < CRITICAL:
+                color = (0, 0, 255)
+                cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=4)
+                cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color)
+            elif distance < WARNING:
+                color = (0, 140, 255)
+                cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=2)
+                cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color)
+
+        cv2.imshow('Frame', depthFrameColor)

depthai_sdk/requirements.txt

@@ -4,7 +4,7 @@ opencv-contrib-python>4
 blobconverter>=1.4.1
 pytube>=12.1.0
 --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/
-depthai==2.24.0
+depthai
 PyTurboJPEG==1.6.4
 marshmallow==3.17.0
 xmltodict

depthai_sdk/src/depthai_sdk/classes/packets.py

@@ -119,18 +119,26 @@ def __init__(self,
                  disparity_map: Optional[np.ndarray] = None,
                  colorize: StereoColor = None,
                  colormap: int = None,
-                 mono_frame: Optional[dai.ImgFrame] = None,
+                 aligned_frame: Optional[dai.ImgFrame] = None,
+                 confidence_map: Optional[np.ndarray] = None
                  ):
         """
         disparity_map might be filtered, eg. if WLS filter is enabled
         """
         super().__init__(name=name, msg=img)
-        self.mono_frame = mono_frame
+        self.aligned_frame = aligned_frame
         self.disparity_map = disparity_map
         self.multiplier = multiplier
         self.colorize = colorize
         self.colormap = colormap
 
+        self.confidence_map = confidence_map
+        self.depth_score = None
+        if self.confidence_map:
+            values = 1 - (self.confidence_map.getData() / 255)
+            values_no_outliers = values[np.logical_and(values > 0.0, values < 1.0)]
+            self.depth_score = np.mean(values_no_outliers)
+
     def get_disparity(self) -> np.ndarray:
         if self.disparity_map is not None:
             return self.disparity_map
@@ -142,9 +150,9 @@ def get_colorized_frame(self, visualizer) -> np.ndarray:
         colorized_disp = frame * self.multiplier
 
         try:
-            mono_frame = self.mono_frame.getCvFrame()
+            aligned_frame = self.aligned_frame.getCvFrame()
         except AttributeError:
-            mono_frame = None
+            aligned_frame = None
 
         stereo_config = visualizer.config.stereo
 
@@ -155,7 +163,7 @@ def get_colorized_frame(self, visualizer) -> np.ndarray:
             colormap = stereo_config.colormap
             colormap[0] = [0, 0, 0]  # Invalidate pixels 0 to be black
 
-        if mono_frame is not None and colorized_disp.ndim == 2 and mono_frame.ndim == 3:
+        if aligned_frame is not None and colorized_disp.ndim == 2 and aligned_frame.ndim == 3:
             colorized_disp = colorized_disp[..., np.newaxis]
 
         if colorize == StereoColor.GRAY:
@@ -164,7 +172,7 @@ def get_colorized_frame(self, visualizer) -> np.ndarray:
             colorized_disp = cv2.applyColorMap(colorized_disp.astype(np.uint8), colormap)
         elif colorize == StereoColor.RGBD:
             colorized_disp = cv2.applyColorMap(
-                (colorized_disp + mono_frame * 0.5).astype(np.uint8), colormap
+                (colorized_disp + aligned_frame * 0.5).astype(np.uint8), colormap
             )
         return colorized_disp
 
@@ -182,8 +190,9 @@ def __init__(self,
                  img_frame: dai.ImgFrame,
                  colorize: StereoColor = None,
                  colormap: int = None,
-                 mono_frame: Optional[dai.ImgFrame] = None,
+                 aligned_frame: Optional[dai.ImgFrame] = None,
                  disp_scale_factor=255 / 95,
+                 confidence_map=None
                  ):
         # DepthPacket.__init__(self, name=name, msg=img_frame)
         super().__init__(
@@ -193,7 +202,8 @@ def __init__(self,
             multiplier=255 / 95,
             colorize=colorize,
             colormap=colormap,
-            mono_frame=mono_frame,
+            aligned_frame=aligned_frame,
+            confidence_map=confidence_map
         )
         self.disp_scale_factor = disp_scale_factor
 

depthai_sdk/src/depthai_sdk/components/camera_helper.py

@@ -221,6 +221,7 @@ def getClosesResolution(sensor: dai.CameraFeatures,
     desired, i = (width, 0) if width is not None else (height, 1)
 
     resolutions = [get_sensor_resolution(type, conf.width, conf.height) for conf in sensor.configs if conf.type == type]
+    resolutions = [res for res in resolutions if res is not None]
 
     for (res, size) in resolutions:
         err = abs(size[i] - desired)

depthai_sdk/src/depthai_sdk/components/stereo_component.py

@@ -103,6 +103,10 @@ def __init__(self,
             'sigma': None
         }
 
+        # Output config
+        self.enable_depth_score = False
+        self.validate_calibration = False
+
         self._undistortion_offset: Optional[int] = None
 
         if not self._replay:
@@ -320,6 +324,23 @@ def config_wls(self,
             'sigma': wls_sigma,
         }
 
+    def config_output(self,
+                      depth_score: bool = None,
+                      validate_calibration: bool = None
+                      ) -> None:
+        """
+        Configures output streams.
+
+        Args:
+            depth_score: True to include depth score in the output packets.
+            validate_calibration: Check if the calibration is valid during the runtime (done on-host) and warn
+            the user if it's not. This can be used to detect if the calibration is invalid (e.g. due to temperature drift).
+        """
+        if depth_score is not None:
+            self.enable_depth_score = depth_score
+        if validate_calibration is not None:
+            self.validate_calibration = validate_calibration
+
     def set_colormap(self, colormap: dai.Colormap):
         """
         Sets the colormap to use for colorizing the disparity map. Used for on-device postprocessing.
@@ -429,14 +450,20 @@ def get_fourcc(self) -> Optional[str]:
     Available outputs (to the host) of this component
     """
 
-    def _mono_frames(self):
+    def _aligned_frames(self):
         """
-        Create mono frames output if WLS filter is enabled or colorize is set to RGBD
+        Create aligned frames output if WLS filter is enabled or colorize is set to RGBD
         """
-        mono_frames = None
+        aligned_frame = None
         if self.wls_config['enabled'] or self._colorize == StereoColor.RGBD:
-            mono_frames = StreamXout(self._right_stream)
-        return mono_frames
+            aligned_frame = StreamXout(self._right_stream)
+        return aligned_frame
+
+    def _try_get_confidence_map(self):
+        confidence_map = None
+        if self.enable_depth_score:
+            confidence_map = StreamXout(self.node.confidenceMap, name='depth_score')
+        return confidence_map
 
     class Out:
         class DepthOut(ComponentOutput):
@@ -445,10 +472,12 @@ def __call__(self, device: dai.Device) -> XoutBase:
                     device=device,
                     frames=StreamXout(self._comp.depth),
                     dispScaleFactor=depth_to_disp_factor(device, self._comp.node),
-                    mono_frames=self._comp._mono_frames(),
+                    aligned_frame=self._comp._aligned_frames(),
                     colorize=self._comp._colorize,
                     colormap=self._comp._postprocess_colormap,
-                    ir_settings=self._comp.ir_settings
+                    ir_settings=self._comp.ir_settings,
+                    confidence_map=self._comp._try_get_confidence_map()
+
                 ).set_comp_out(self)
 
         class DisparityOut(ComponentOutput):
@@ -458,11 +487,13 @@ def __call__(self, device: dai.Device, fourcc: Optional[str] = None) -> XoutBase
                     frames=StreamXout(self._comp.encoder.bitstream) if fourcc else
                     StreamXout(self._comp.disparity),
                     disp_factor=255.0 / self._comp.node.getMaxDisparity(),
-                    mono_frames=self._comp._mono_frames(),
+                    aligned_frame=self._comp._aligned_frames(),
                     colorize=self._comp._colorize,
+                    fourcc=fourcc,
                     colormap=self._comp._postprocess_colormap,
                     wls_config=self._comp.wls_config,
                     ir_settings=self._comp.ir_settings,
+                    confidence_map=self._comp._try_get_confidence_map()
                 ).set_comp_out(self)
 
         class RectifiedLeftOut(ComponentOutput):