From c600c90b264efd113ed5fd9caa32f8992bb6b0bd Mon Sep 17 00:00:00 2001 From: apirrone Date: Wed, 3 Jul 2024 11:40:41 +0200 Subject: [PATCH 01/17] update --- examples/camera_wrappers_examples/live_pcl.py | 3 +- .../CONFIG_IMX296_TOF.json | 10 + .../camera_wrappers/depthai/tof.py | 171 ++++++++++++++++++ .../camera_wrappers/depthai/wrapper.py | 16 +- scripts/aze.py | 159 ++++++++++++++++ scripts/tof_test.py | 16 +- 6 files changed, 368 insertions(+), 7 deletions(-) create mode 100644 pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json create mode 100644 pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py create mode 100644 scripts/aze.py diff --git a/examples/camera_wrappers_examples/live_pcl.py b/examples/camera_wrappers_examples/live_pcl.py index ee41283..150c683 100644 --- a/examples/camera_wrappers_examples/live_pcl.py +++ b/examples/camera_wrappers_examples/live_pcl.py @@ -7,7 +7,7 @@ get_config_file_path, get_config_files_names, ) -from pollen_vision.perception.utils import PCLVisualizer +from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer valid_configs = get_config_files_names() @@ -33,6 +33,7 @@ depth = data["depth"] rgb = data["left"] disparity = data["disparity"] + P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) diff --git a/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json b/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json new file mode 100644 index 0000000..2a113ad --- /dev/null +++ b/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json @@ -0,0 +1,10 @@ +{ + "socket_to_name": { + "CAM_B": "right", + "CAM_C": "left", + "CAM_A": "tof" + }, + "inverted": false, + "fisheye": true, + "mono": false +} \ No newline at end of file diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py new file mode 100644 index 0000000..87b51b0 --- /dev/null +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -0,0 +1,171 @@ +from datetime import timedelta +from typing import Dict, Tuple + +import cv2 +import depthai as dai +import numpy as np +import numpy.typing as npt +from pollen_vision.camera_wrappers.depthai.utils import ( + get_config_file_path, + get_socket_from_name, +) +from pollen_vision.camera_wrappers.depthai.wrapper import DepthaiWrapper + +# from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer + + +class TOFWrapper(DepthaiWrapper): + def __init__( + self, + cam_config_json: str, + fps: int = 30, + force_usb2: bool = False, + mx_id: str = "", + ) -> None: + super().__init__( + cam_config_json, + fps, + force_usb2=force_usb2, + resize=(640, 480), + rectify=True, + exposure_params=None, + mx_id=mx_id, + isp_scale=(2, 3), + ) + self.cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) + self.cvColorMap[0] = [0, 0, 0] + + def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], Dict[str, timedelta]]: + # message_group = self.queues["depth_left"].get() + # frame_left = message_group["left"].getCvFrame() + # frame_depth = message_group["depth"] + + # return {"left": frame_left, "depth": frame_depth}, None, None + + data, latency, ts = super().get_data() + for name, pkt in data.items(): + if name != "depth": + data[name] = pkt.getCvFrame() + else: + depth = np.array(pkt.getFrame()).astype(np.float32) + max_depth = (self.tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. + data[name] = depth # / max_depth + # print(max_depth) + # max_depth = (self.tof_camConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. + # depth = (depth / max_depth) * 255 # .astype(np.uint8) + # depth_colorized = np.interp(depth, (0, max_depth), (0, 255)).astype(np.uint8) + # depth_colorized = cv2.applyColorMap(depth_colorized, self.cvColorMap) + # data[name] = depth_colorized + # data[name] = np.array(pkt.getFrame()).astype(np.float32) + + return data, latency, ts + + def get_K(self) -> npt.NDArray[np.float32]: + return super().get_K(left=True) # type: ignore + + def _create_pipeline(self) -> dai.Pipeline: + pipeline = self._pipeline_basis() + + self.tof = pipeline.create(dai.node.ToF) + # self.tof.setNumShaves(1) + + self.tofConfig = self.tof.initialConfig.get() + self.tofConfig.enableOpticalCorrection = False + self.tofConfig.enablePhaseShuffleTemporalFilter = True + self.tofConfig.phaseUnwrappingLevel = 1 + self.tofConfig.phaseUnwrapErrorThreshold = 300 + self.tofConfig.enableFPPNCorrection = False + # self.tofConfig.median = dai.MedianFilter.KERNEL_7x7 + + self.tof.initialConfig.set(self.tofConfig) + + self.tof_cam = pipeline.create(dai.node.Camera) + self.tof_cam.setFps(self.cam_config.fps) + tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) + self.tof_cam.setBoardSocket(tof_socket) + + # self.sync = pipeline.create(dai.node.Sync) + # self.sync.setSyncThreshold(timedelta(seconds=(1 / self.cam_config.fps))) + + # self.align = pipeline.create(dai.node.ImageAlign) + + pipeline = self._create_output_streams(pipeline) + + return self._link_pipeline(pipeline) + + def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: + pipeline = super()._create_output_streams(pipeline) + + self.xout_tof = pipeline.create(dai.node.XLinkOut) + self.xout_tof.setStreamName("depth") + + # self.xout_tof_left = pipeline.create(dai.node.XLinkOut) + # self.xout_tof_left.setStreamName("depth_left") + + return pipeline + + def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: + self.left.isp.link(self.left_manip.inputImage) + self.right.isp.link(self.right_manip.inputImage) + + self.tof_cam.raw.link(self.tof.input) + self.tof.depth.link(self.xout_tof.input) + # self.tof.depth.link(self.align.input) + # self.left_manip.out.link(self.align.inputAlignTo) + # self.align.outputAligned.link(self.xout_tof.input) + + # self.left_manip.out.link(self.sync.inputs["left"]) + # self.align.outputAligned.link(self.sync.inputs["depth"]) + # self.sync.inputs["left"].setBlocking(False) + # self.left_manip.out.link(self.align.inputAlignTo) + # self.sync.out.link(self.xout_tof_left.input) + + self.right_manip.out.link(self.xout_right.input) + self.left_manip.out.link(self.xout_left.input) + return pipeline + # self.tof_cam.raw.link(self.tof.input) + # self.left.isp.link(self.left_manip.inputImage) + # self.right.isp.link(self.right_manip.inputImage) + + # self.left_manip.out.link(self.xout_left.input) + # self.right_manip.out.link(self.xout_right.input) + + # self.tof.depth.link(self.xout_tof.input) + + # return pipeline + + def _create_queues(self) -> Dict[str, dai.DataOutputQueue]: + queues: Dict[str, dai.DataOutputQueue] = super()._create_queues() + + queues["depth"] = self._device.getOutputQueue("depth", maxSize=1, blocking=False) + + return queues + + +mouse_x, mouse_y = 0, 0 + + +def cv_callback(event, x, y, flags, param): + global mouse_x, mouse_y + mouse_x, mouse_y = x, y + + +if __name__ == "__main__": + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=60) + + # P = PCLVisualizer(t.get_K()) + cv2.namedWindow("depth") + cv2.setMouseCallback("depth", cv_callback) + while True: + data, _, _ = t.get_data() + left = cv2.resize(data["left"], (640, 480)) + right = cv2.resize(data["right"], (640, 480)) + depth = data["depth"] + # cv2.imshow("left", left) + # cv2.imshow("right", right) + print(data["depth"][mouse_y, mouse_x]) + depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) + cv2.imshow("depth", depth) + # P.update(cv2.cvtColor(left, cv2.COLOR_BGR2RGB), depth) + # P.tick() + cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index 66345ca..ef8fec7 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -81,9 +81,19 @@ def _prepare(self) -> None: if socket_camToString[cam.socket] in self.cam_config.socket_to_name.keys(): connected_cameras_features.append(cam) - # Assuming both cameras are the same - width = connected_cameras_features[0].width - height = connected_cameras_features[0].height + # Ad hoc solution to NOT select the ToF sensor. Pretty bad + tmp = None + for cam in connected_cameras_features: + if "33D".lower() not in str(cam.sensorName).lower(): + tmp = cam + break + + width = tmp.width + height = tmp.height + + # # Assuming both cameras are the same + # width = connected_cameras_features[-1].width + # height = connected_cameras_features[-1].height self.cam_config.set_sensor_resolution((width, height)) diff --git a/scripts/aze.py b/scripts/aze.py new file mode 100644 index 0000000..86fdcdf --- /dev/null +++ b/scripts/aze.py @@ -0,0 +1,159 @@ +import time +from datetime import timedelta + +import cv2 +import depthai as dai +import numpy as np + +# This example is intended to run unchanged on an OAK-D-SR-PoE camera +FPS = 30.0 + +RGB_SOCKET = dai.CameraBoardSocket.CAM_C +TOF_SOCKET = dai.CameraBoardSocket.CAM_A +ALIGN_SOCKET = RGB_SOCKET + + +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + + +pipeline = dai.Pipeline() +# Define sources and outputs +camRgb = pipeline.create(dai.node.ColorCamera) +tof = pipeline.create(dai.node.ToF) +camTof = pipeline.create(dai.node.Camera) +sync = pipeline.create(dai.node.Sync) +align = pipeline.create(dai.node.ImageAlign) +out = pipeline.create(dai.node.XLinkOut) + +# ToF settings +camTof.setFps(FPS) +camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) +camTof.setBoardSocket(TOF_SOCKET) + +# rgb settings +camRgb.setBoardSocket(RGB_SOCKET) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) +camRgb.setFps(FPS) +camRgb.setIspScale(1, 2) + +out.setStreamName("out") + +sync.setSyncThreshold(timedelta(seconds=(1 / FPS))) + +# Linking +camRgb.isp.link(sync.inputs["rgb"]) +camTof.raw.link(tof.input) +tof.depth.link(align.input) +align.outputAligned.link(sync.inputs["depth_aligned"]) +sync.inputs["rgb"].setBlocking(False) +camRgb.isp.link(align.inputAlignTo) +sync.out.link(out.input) + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb): + """ + Update the rgb and depth weights used to blend depth/rgb image + + @param[in] percent_rgb The rgb weight expressed as a percentage (0..100) + """ + global depthWeight + global rgbWeight + rgbWeight = float(percentRgb) / 100.0 + depthWeight = 1.0 - rgbWeight + + +# Connect to device and start pipeline +with dai.Device(pipeline) as device: + queue = device.getOutputQueue("out", 8, False) + + # Configure windows; trackbar adjusts blending ratio of rgb/depth + rgbDepthWindowName = "rgb-depth" + + cv2.namedWindow(rgbDepthWindowName) + cv2.createTrackbar( + "RGB Weight %", + rgbDepthWindowName, + int(rgbWeight * 100), + 100, + updateBlendWeights, + ) + fpsCounter = FPSCounter() + while True: + messageGroup = queue.get() + fpsCounter.tick() + assert isinstance(messageGroup, dai.MessageGroup) + frameRgb = messageGroup["rgb"] + assert isinstance(frameRgb, dai.ImgFrame) + frameDepth = messageGroup["depth_aligned"] + assert isinstance(frameDepth, dai.ImgFrame) + + sizeRgb = frameRgb.getData().size + sizeDepth = frameDepth.getData().size + # Blend when both received + if frameDepth is not None: + cvFrame = frameRgb.getCvFrame() + # Colorize the aligned depth + alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) + # Resize depth to match the rgb frame + cv2.putText( + alignedDepthColorized, + f"FPS: {fpsCounter.getFps():.2f}", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 2, + ) + cv2.imshow("depth", alignedDepthColorized) + + blended = cv2.addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) + cv2.imshow(rgbDepthWindowName, blended) + + key = cv2.waitKey(1) + if key == ord("q"): + break diff --git a/scripts/tof_test.py b/scripts/tof_test.py index 083bc9c..7182756 100644 --- a/scripts/tof_test.py +++ b/scripts/tof_test.py @@ -36,8 +36,8 @@ def create_pipeline(): tof.initialConfig.set(tofConfig) cam_tof = pipeline.create(dai.node.Camera) - cam_tof.setFps(60) # ToF node will produce depth frames at /2 of this rate - cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_B) + cam_tof.setFps(100) # ToF node will produce depth frames at /2 of this rate + cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_A) cam_tof.raw.link(tof.input) xout = pipeline.create(dai.node.XLinkOut) @@ -109,7 +109,7 @@ def create_pipeline(): ] currentMedian = tofConfig.median nextMedian = medianSettings[(medianSettings.index(currentMedian) + 1) % len(medianSettings)] - print(f"Changing median to {nextMedian.name} from {currentMedian.name}") + # print(f"Changing median to {nextMedian.name} from {currentMedian.name}") tofConfig.median = nextMedian tofConfigInQueue.send(tofConfig) @@ -119,6 +119,16 @@ def create_pipeline(): depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8) depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap) + print("===") + print("tofConfig.enableOpticalCorrection", tofConfig.enableOpticalCorrection) + print("tofConfig.enablePhaseShuffleTemporalFilter", tofConfig.enablePhaseShuffleTemporalFilter) + print("tofConfig.phaseUnwrappingLevel", tofConfig.phaseUnwrappingLevel) + print("tofConfig.phaseUnwrapErrorThreshold", tofConfig.phaseUnwrapErrorThreshold) + print("tofConfig.enableFPPNCorrection", tofConfig.enableFPPNCorrection) + print("tofConfig.median", tofConfig.median) + print("tofConfig.enableTemperatureCorrection", tofConfig.enableTemperatureCorrection) + print("===") + cv2.imshow("Colorized depth", depth_colorized) counter += 1 From 00544e032a9e998919f7ccae64119eb5679f17a9 Mon Sep 17 00:00:00 2001 From: apirrone Date: Wed, 4 Sep 2024 17:17:21 +0200 Subject: [PATCH 02/17] tof align and point cloud wip --- examples/camera_wrappers_examples/live_pcl.py | 32 ++- .../camera_wrappers_examples/live_pcl_tof.py | 92 +++++++ .../camera_wrappers_examples/tof_example.py | 68 +++++ .../CONFIG_AR0234_TOF.json | 6 + .../CONFIG_IMX296_TOF.json | 18 +- .../pollen_vision/camera_wrappers/__init__.py | 1 + .../depthai/calibration/check_epilines.py | 2 +- .../depthai/calibration/flash.py | 3 +- .../depthai/calibration/test_flash.py | 28 ++ .../camera_wrappers/depthai/teleop.py | 2 +- .../camera_wrappers/depthai/tof.py | 154 ++++++----- .../camera_wrappers/depthai/wrapper.py | 89 ++++++- .../perception/utils/pcl_visualizer.py | 4 + scripts/align_example.py | 169 ++++++++++++ scripts/align_example2.py | 169 ++++++++++++ scripts/align_rgb_on_depth_test.py | 60 +++++ scripts/chatgpt.py | 94 +++++++ scripts/minimal_tof_test.py | 93 +++++++ scripts/read_calib.json | 247 ++++++++++++++++++ scripts/test.py | 7 + scripts/tof_and_encoding_test.py | 206 +++++++++++++++ scripts/tof_pcl_test.py | 66 +++++ scripts/tof_test_align.py | 27 ++ scripts/vanilla_tof_test.py | 184 +++++++++++++ 24 files changed, 1719 insertions(+), 102 deletions(-) create mode 100644 examples/camera_wrappers_examples/live_pcl_tof.py create mode 100644 examples/camera_wrappers_examples/tof_example.py create mode 100644 pollen_vision/config_files_vision/CONFIG_AR0234_TOF.json create mode 100644 pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py create mode 100644 scripts/align_example.py create mode 100644 scripts/align_example2.py create mode 100644 scripts/align_rgb_on_depth_test.py create mode 100644 scripts/chatgpt.py create mode 100644 scripts/minimal_tof_test.py create mode 100644 scripts/read_calib.json create mode 100644 scripts/test.py create mode 100644 scripts/tof_and_encoding_test.py create mode 100644 scripts/tof_pcl_test.py create mode 100644 scripts/tof_test_align.py create mode 100644 scripts/vanilla_tof_test.py diff --git a/examples/camera_wrappers_examples/live_pcl.py b/examples/camera_wrappers_examples/live_pcl.py index 150c683..2398254 100644 --- a/examples/camera_wrappers_examples/live_pcl.py +++ b/examples/camera_wrappers_examples/live_pcl.py @@ -2,7 +2,7 @@ import cv2 import numpy as np -from pollen_vision.camera_wrappers.depthai import SDKWrapper +from pollen_vision.camera_wrappers import SDKWrapper from pollen_vision.camera_wrappers.depthai.utils import ( get_config_file_path, get_config_files_names, @@ -23,24 +23,38 @@ w = SDKWrapper(get_config_file_path(args.config), compute_depth=True) -K = w.cam_config.get_K_left() +K = w.get_K() P = PCLVisualizer(K) + +mouse_x, mouse_y = 0, 0 + + +def cv_callback(event, x, y, flags, param): + global mouse_x, mouse_y + mouse_x, mouse_y = x, y + + +cv2.namedWindow("depth") +cv2.setMouseCallback("depth", cv_callback) while True: data, lat, _ = w.get_data() - print(lat["depthNode_left"], lat["depthNode_right"]) + # print(lat["depthNode_left"], lat["depthNode_right"]) depth = data["depth"] rgb = data["left"] - disparity = data["disparity"] + # disparity = data["disparity"] P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) - disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) - disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET) - cv2.imshow("disparity", disparity) - cv2.imshow("left", data["depthNode_left"]) - cv2.imshow("right", data["depthNode_right"]) + # disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) + # disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET) + # cv2.imshow("disparity", disparity) + # cv2.imshow("left", data["depthNode_left"]) + # cv2.imshow("right", data["depthNode_right"]) + cv2.imshow("depth", depth) + cv2.imshow("rgb", rgb) + print(depth[mouse_y, mouse_x]) key = cv2.waitKey(1) P.tick() diff --git a/examples/camera_wrappers_examples/live_pcl_tof.py b/examples/camera_wrappers_examples/live_pcl_tof.py new file mode 100644 index 0000000..758a7c2 --- /dev/null +++ b/examples/camera_wrappers_examples/live_pcl_tof.py @@ -0,0 +1,92 @@ +import argparse + +import cv2 +import numpy as np +from pollen_vision.camera_wrappers import TOFWrapper +from pollen_vision.camera_wrappers.depthai.utils import ( + get_config_file_path, + get_config_files_names, +) +from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer + +valid_configs = get_config_files_names() + +argParser = argparse.ArgumentParser(description="depth wrapper example") +argParser.add_argument( + "--config", + type=str, + required=True, + choices=valid_configs, + help=f"Configutation file name : {valid_configs}", +) +args = argParser.parse_args() + +w = TOFWrapper(get_config_file_path(args.config)) + +K = w.get_K() +P = PCLVisualizer(K) +P.add_frame("origin") + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +mouse_x, mouse_y = 0, 0 + + +def cv_callback(event, x, y, flags, param): + global mouse_x, mouse_y + mouse_x, mouse_y = x, y + + +cv2.namedWindow("depth") +cv2.setMouseCallback("depth", cv_callback) +while True: + data, lat, _ = w.get_data() + # print(lat["depthNode_left"], lat["depthNode_right"]) + + depth = data["depth"] + rgb = data["left"] + + colorized_depth = colorizeDepth(depth) + # disparity = data["disparity"] + + P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) + # P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) + + # disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) + # disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET) + # cv2.imshow("disparity", disparity) + # cv2.imshow("left", data["depthNode_left"]) + # cv2.imshow("right", data["depthNode_right"]) + cv2.imshow("depth", colorized_depth) + cv2.imshow("rgb", rgb) + print(depth[mouse_y, mouse_x]) + + key = cv2.waitKey(1) + P.tick() diff --git a/examples/camera_wrappers_examples/tof_example.py b/examples/camera_wrappers_examples/tof_example.py new file mode 100644 index 0000000..7fcd37d --- /dev/null +++ b/examples/camera_wrappers_examples/tof_example.py @@ -0,0 +1,68 @@ +import cv2 +import numpy as np +import depthai as dai +from pollen_vision.camera_wrappers import TOFWrapper +from pollen_vision.camera_wrappers.depthai.utils import get_config_file_path + + +t = TOFWrapper(get_config_file_path("CONFIG_AR0234_TOF"), fps=60) + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +mouse_x, mouse_y = 0, 0 + + +def cv_callback(event, x, y, flags, param): + global mouse_x, mouse_y + mouse_x, mouse_y = x, y + + +# P = PCLVisualizer(t.get_K()) +cv2.namedWindow("depth") +cv2.setMouseCallback("depth", cv_callback) +print(dai.__version__) +while True: + data, _, _ = t.get_data() + left = data["left"] + # right = data["right"] + # left = cv2.resize(left, (640, 480)) + # right = cv2.resize(right, (640, 480)) + depth = data["depth"] + colorized_depth = colorizeDepth(depth) + cv2.imshow("left", left) + # cv2.imshow("right", right) + print(data["depth"][mouse_y, mouse_x]) + # colorized_depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) + blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0) + cv2.imshow("blended", blended) + cv2.imshow("depth", depth) + # P.update(cv2.cvtColor(left, cv2.COLOR_BGR2RGB), depth) + # P.tick() + cv2.waitKey(1) diff --git a/pollen_vision/config_files_vision/CONFIG_AR0234_TOF.json b/pollen_vision/config_files_vision/CONFIG_AR0234_TOF.json new file mode 100644 index 0000000..5a8c94f --- /dev/null +++ b/pollen_vision/config_files_vision/CONFIG_AR0234_TOF.json @@ -0,0 +1,6 @@ +{ + "socket_to_name": { "CAM_B": "right", "CAM_C": "left", "CAM_D": "tof" }, + "inverted": false, + "fisheye": true, + "mono": false +} diff --git a/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json b/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json index 2a113ad..74487a5 100644 --- a/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json +++ b/pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json @@ -1,10 +1,10 @@ { - "socket_to_name": { - "CAM_B": "right", - "CAM_C": "left", - "CAM_A": "tof" - }, - "inverted": false, - "fisheye": true, - "mono": false -} \ No newline at end of file + "socket_to_name": { + "CAM_B": "right", + "CAM_C": "left", + "CAM_D": "tof" + }, + "inverted": false, + "fisheye": true, + "mono": false +} diff --git a/pollen_vision/pollen_vision/camera_wrappers/__init__.py b/pollen_vision/pollen_vision/camera_wrappers/__init__.py index 7bf35cc..a01bce5 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/__init__.py +++ b/pollen_vision/pollen_vision/camera_wrappers/__init__.py @@ -1,6 +1,7 @@ from .camera_wrapper import CameraWrapper # noqa: F401 from .depthai.sdk import SDKWrapper # noqa: F401 from .depthai.teleop import TeleopWrapper # noqa: F401 +from .depthai.tof import TOFWrapper # noqa: F401 # from .pollen_sdk_camera.pollen_sdk_camera_wrapper import ( # noqa: F401 # PollenSDKCameraWrapper, diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py index 03567dc..54ebd67 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py @@ -35,7 +35,7 @@ for name in data.keys(): _data[name] = data[name] epi = drawEpiLines(_data["left"], _data["right"], ARUCO_DICT) - epi = cv2.resize(epi, (0, 0), fx=0.9, fy=0.9) + epi = cv2.resize(epi, (0, 0), fx=0.5, fy=0.5) cv2.imshow("epi", epi) key = cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/flash.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/flash.py index ea8295b..bf00075 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/flash.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/flash.py @@ -24,7 +24,8 @@ required=True, help="Path to the calibration json file", ) +argParser.add_argument("--tof", action="store_true", help="Has tof sensor ?") args = argParser.parse_args() w = SDKWrapper(get_config_file_path(args.config)) -w.flash(args.calib_json_file) +w.flash(args.calib_json_file, tof=args.tof) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py new file mode 100644 index 0000000..8175a61 --- /dev/null +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py @@ -0,0 +1,28 @@ +# left -> right +# left -> tof +# tof -> left + +ch.setCameraExtrinsics( + left_socket, + tof_socket, + R_left_to_tof, + T_left_to_tof, + specTranslation=T_left_to_tof, +) + +ch.setCameraExtrinsics( + right_socket, + left_socket, + R_right_to_left, + T_right_to_left, + specTranslation=T_right_to_left, +) + +# Should I do this ? +ch.setCameraExtrinsics( + tof_socket, + tof_socket, + [0, 0, 0], + np.eye(3).tolist(), + specTranslation=[0, 0, 0], +) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/teleop.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/teleop.py index 178e74c..bcf3d95 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/teleop.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/teleop.py @@ -34,7 +34,7 @@ def __init__( cam_config_json, fps, force_usb2=force_usb2, - resize=(1280, 720), + resize=(960, 720), rectify=rectify, exposure_params=exposure_params, mx_id=mx_id, diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 87b51b0..676b5af 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -26,37 +26,55 @@ def __init__( cam_config_json, fps, force_usb2=force_usb2, - resize=(640, 480), - rectify=True, + resize=None, + rectify=False, exposure_params=None, mx_id=mx_id, - isp_scale=(2, 3), + # isp_scale=(2, 3), ) self.cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) self.cvColorMap[0] = [0, 0, 0] + self.cam_config.undistort_resolution = (640, 480) + + def crop_image(self, im, depth): + # threshold depth + thresholded_depth = np.where(depth > 0, 1, 0).astype(np.uint8) + depth_mask_bounding_box = cv2.boundingRect(thresholded_depth) + cropped_image = im[ + depth_mask_bounding_box[1] : depth_mask_bounding_box[1] + depth_mask_bounding_box[3], + depth_mask_bounding_box[0] : depth_mask_bounding_box[0] + depth_mask_bounding_box[2], + ] + cropped_depth = depth[ + depth_mask_bounding_box[1] : depth_mask_bounding_box[1] + depth_mask_bounding_box[3], + depth_mask_bounding_box[0] : depth_mask_bounding_box[0] + depth_mask_bounding_box[2], + ] + + cropped_image = cv2.resize(cropped_image, (640, 480)) + cropped_depth = cv2.resize(cropped_depth, (640, 480)) + + return cropped_image, cropped_depth def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], Dict[str, timedelta]]: - # message_group = self.queues["depth_left"].get() - # frame_left = message_group["left"].getCvFrame() - # frame_depth = message_group["depth"] - - # return {"left": frame_left, "depth": frame_depth}, None, None - - data, latency, ts = super().get_data() - for name, pkt in data.items(): - if name != "depth": - data[name] = pkt.getCvFrame() - else: - depth = np.array(pkt.getFrame()).astype(np.float32) - max_depth = (self.tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. - data[name] = depth # / max_depth - # print(max_depth) - # max_depth = (self.tof_camConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. - # depth = (depth / max_depth) * 255 # .astype(np.uint8) - # depth_colorized = np.interp(depth, (0, max_depth), (0, 255)).astype(np.uint8) - # depth_colorized = cv2.applyColorMap(depth_colorized, self.cvColorMap) - # data[name] = depth_colorized - # data[name] = np.array(pkt.getFrame()).astype(np.float32) + data: Dict[str, npt.NDArray[np.uint8]] = {} + latency: Dict[str, float] = {} + ts: Dict[str, timedelta] = {} + + messageGroup = self.queues["sync_out"].get() + + left = messageGroup["left"].getCvFrame() + right = messageGroup["right"].getCvFrame() + depth = messageGroup["depth_aligned"].getFrame() + + # Temporary, not ideal + cropped_left, cropped_depth = self.crop_image(left, depth) + + data["left"] = cropped_left + data["right"] = right + data["depth"] = cropped_depth + + # data["left"] = left + # data["right"] = right + # data["depth"] = depth return data, latency, ts @@ -66,28 +84,30 @@ def get_K(self) -> npt.NDArray[np.float32]: def _create_pipeline(self) -> dai.Pipeline: pipeline = self._pipeline_basis() + self.cam_tof = pipeline.create(dai.node.Camera) + self.cam_tof.setFps(self.cam_config.fps) + self.tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) + self.cam_tof.setBoardSocket(self.tof_socket) + self.tof = pipeline.create(dai.node.ToF) - # self.tof.setNumShaves(1) + # === Tof configuration === self.tofConfig = self.tof.initialConfig.get() - self.tofConfig.enableOpticalCorrection = False + self.tofConfig.enableFPPNCorrection = True + self.tofConfig.enableOpticalCorrection = False # True is ok ? self.tofConfig.enablePhaseShuffleTemporalFilter = True self.tofConfig.phaseUnwrappingLevel = 1 self.tofConfig.phaseUnwrapErrorThreshold = 300 - self.tofConfig.enableFPPNCorrection = False - # self.tofConfig.median = dai.MedianFilter.KERNEL_7x7 - + self.tofConfig.enableTemperatureCorrection = False # Not yet supported + self.tofConfig.enableWiggleCorrection = False + self.tofConfig.median = dai.MedianFilter.KERNEL_7x7 self.tof.initialConfig.set(self.tofConfig) + # ========================== - self.tof_cam = pipeline.create(dai.node.Camera) - self.tof_cam.setFps(self.cam_config.fps) - tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) - self.tof_cam.setBoardSocket(tof_socket) - - # self.sync = pipeline.create(dai.node.Sync) - # self.sync.setSyncThreshold(timedelta(seconds=(1 / self.cam_config.fps))) + self.align = pipeline.create(dai.node.ImageAlign) - # self.align = pipeline.create(dai.node.ImageAlign) + self.sync = pipeline.create(dai.node.Sync) + self.sync.setSyncThreshold(timedelta(seconds=(0.5 / self.cam_config.fps))) pipeline = self._create_output_streams(pipeline) @@ -96,11 +116,8 @@ def _create_pipeline(self) -> dai.Pipeline: def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: pipeline = super()._create_output_streams(pipeline) - self.xout_tof = pipeline.create(dai.node.XLinkOut) - self.xout_tof.setStreamName("depth") - - # self.xout_tof_left = pipeline.create(dai.node.XLinkOut) - # self.xout_tof_left.setStreamName("depth_left") + self.xout_sync = pipeline.create(dai.node.XLinkOut) + self.xout_sync.setStreamName("sync_out") return pipeline @@ -108,36 +125,32 @@ def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: self.left.isp.link(self.left_manip.inputImage) self.right.isp.link(self.right_manip.inputImage) - self.tof_cam.raw.link(self.tof.input) - self.tof.depth.link(self.xout_tof.input) - # self.tof.depth.link(self.align.input) - # self.left_manip.out.link(self.align.inputAlignTo) - # self.align.outputAligned.link(self.xout_tof.input) + self.left_manip.out.link(self.sync.inputs["left"]) + self.right_manip.out.link(self.sync.inputs["right"]) + self.sync.inputs["left"].setBlocking(False) + self.sync.inputs["right"].setBlocking(False) - # self.left_manip.out.link(self.sync.inputs["left"]) - # self.align.outputAligned.link(self.sync.inputs["depth"]) - # self.sync.inputs["left"].setBlocking(False) - # self.left_manip.out.link(self.align.inputAlignTo) - # self.sync.out.link(self.xout_tof_left.input) + self.cam_tof.raw.link(self.tof.input) - self.right_manip.out.link(self.xout_right.input) - self.left_manip.out.link(self.xout_left.input) - return pipeline - # self.tof_cam.raw.link(self.tof.input) - # self.left.isp.link(self.left_manip.inputImage) - # self.right.isp.link(self.right_manip.inputImage) + # Align depth on RGB WORKS + self.left_manip.out.link(self.align.inputAlignTo) + self.tof.depth.link(self.align.input) + + # Align RGB on depth + # self.left_manip.out.link(self.align.input) + # self.tof.depth.link(self.align.inputAlignTo) - # self.left_manip.out.link(self.xout_left.input) - # self.right_manip.out.link(self.xout_right.input) + self.align.outputAligned.link(self.sync.inputs["depth_aligned"]) - # self.tof.depth.link(self.xout_tof.input) + self.sync.out.link(self.xout_sync.input) - # return pipeline + return pipeline def _create_queues(self) -> Dict[str, dai.DataOutputQueue]: - queues: Dict[str, dai.DataOutputQueue] = super()._create_queues() + # queues: Dict[str, dai.DataOutputQueue] = super()._create_queues() - queues["depth"] = self._device.getOutputQueue("depth", maxSize=1, blocking=False) + queues: Dict[str, dai.DataOutputQueue] = {} + queues["sync_out"] = self._device.getOutputQueue("sync_out", maxSize=8, blocking=False) return queues @@ -151,18 +164,21 @@ def cv_callback(event, x, y, flags, param): if __name__ == "__main__": - t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=60) + t = TOFWrapper(get_config_file_path("CONFIG_AR0234_TOF"), fps=30) # P = PCLVisualizer(t.get_K()) cv2.namedWindow("depth") cv2.setMouseCallback("depth", cv_callback) + print(dai.__version__) while True: data, _, _ = t.get_data() - left = cv2.resize(data["left"], (640, 480)) - right = cv2.resize(data["right"], (640, 480)) + left = data["left"] + right = data["right"] + # left = cv2.resize(left, (640, 480)) + # right = cv2.resize(right, (640, 480)) depth = data["depth"] - # cv2.imshow("left", left) - # cv2.imshow("right", right) + cv2.imshow("left", left) + cv2.imshow("right", right) print(data["depth"][mouse_y, mouse_x]) depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) cv2.imshow("depth", depth) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index ef8fec7..3fbfc09 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -245,7 +245,7 @@ def _create_imageManip( manip.setMaxOutputFrameSize(resolution[0] * resolution[1] * 3) manip.initialConfig.setKeepAspectRatio(True) - manip.initialConfig.setResize(resolution[0], resolution[1]) + # manip.initialConfig.setResize(resolution[0], resolution[1]) manip.initialConfig.setFrameType(dai.ImgFrame.Type.NV12) return manip @@ -256,7 +256,7 @@ def _set_undistort_maps(self) -> None: self.cam_config.set_undistort_maps(mapXL, mapYL, mapXR, mapYR) # Takes in the output of multical calibration - def flash(self, calib_json_file: str) -> None: + def flash(self, calib_json_file: str, tof=False) -> None: """Flashes the calibration to the camera. The calibration is read from the calib_json_file and flashed into the camera's eeprom. @@ -289,10 +289,46 @@ def flash(self, calib_json_file: str) -> None: self._logger.info("Setting camera type to fisheye ...") ch.setCameraType(cam_socket, dai.CameraModel.Fisheye) + if tof: + K = np.eye(3) + # Piffed, need to find the real values + K[0][0] = 550 # fx + K[1][1] = 550 # fy + K[0][2] = 320 # cx + K[1][2] = 240 # cy + tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) + ch.setCameraIntrinsics(tof_socket, K.tolist(), (640, 480)) + self._logger.info("Setting extrinsics ...") left_socket = get_socket_from_name("left", self.cam_config.name_to_socket) right_socket = get_socket_from_name("right", self.cam_config.name_to_socket) + if tof: + # right->left + # left->tof + # tof->-1 + print("FLASHING TOF EXTRINSICS") + tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) + print("TOF SOCKET", tof_socket) + T_tof_to_left = np.eye(4) + T_tof_to_left[:3, 3] = [-3.2509, 0, 0] + T_left_to_tof = np.linalg.inv(T_tof_to_left) + ch.setCameraExtrinsics( + tof_socket, + left_socket, + T_left_to_tof[:3, :3].tolist(), + T_left_to_tof[:3, 3].tolist(), + specTranslation=T_left_to_tof[:3, 3].tolist(), + ) + + ch.setCameraExtrinsics( + left_socket, + tof_socket, + T_left_to_tof[:3, :3].tolist(), + T_left_to_tof[:3, 3].tolist(), + specTranslation=T_left_to_tof[:3, 3].tolist(), + ) + right_to_left = camera_poses["right_to_left"] R_right_to_left = np.array(right_to_left["R"]) T_right_to_left = np.array(right_to_left["T"]) @@ -300,24 +336,53 @@ def flash(self, calib_json_file: str) -> None: R_left_to_right, T_left_to_right = get_inv_R_T(R_right_to_left, T_right_to_left) + # ch.setCameraExtrinsics( + # right_socket, + # left_socket, + # R_right_to_left, + # T_right_to_left, + # specTranslation=T_right_to_left, + # ) + ch.setCameraExtrinsics( left_socket, right_socket, - R_right_to_left.tolist(), - T_right_to_left.tolist(), - specTranslation=T_right_to_left.tolist(), - ) - ch.setCameraExtrinsics( - right_socket, - left_socket, - R_left_to_right, - T_left_to_right, - specTranslation=T_left_to_right, + R_left_to_right.tolist(), + T_left_to_right.tolist(), + specTranslation=T_left_to_right.tolist(), ) + # if not tof: + # ch.setCameraExtrinsics( + # left_socket, + # right_socket, + # R_right_to_left.tolist(), + # T_right_to_left.tolist(), + # specTranslation=T_right_to_left.tolist(), + # ) ch.setStereoLeft(left_socket, np.eye(3).tolist()) ch.setStereoRight(right_socket, R_right_to_left.tolist()) + # print( + # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_C, dai.CameraBoardSocket.CAM_B) + # ) # extrinsics left camera <-> ToF OK + # print( + # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_C) + # ) # extrinsics left camera <-> ToF OK + # print( + # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_D) + # ) # extrinsics left camera <-> ToF OK + # print( + # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_D, dai.CameraBoardSocket.CAM_B) + # ) # extrinsics left camera <-> ToF OK + # print( + # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_C, dai.CameraBoardSocket.CAM_D) + # ) # extrinsics left camera <-> ToF OK + # print( + # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_D, dai.CameraBoardSocket.CAM_C) + # ) # extrinsics left camera <-> ToF OK + + ch.eepromToJsonFile("saved_calib.json") self._logger.info("Flashing ...") try: self._device.flashCalibration2(ch) diff --git a/pollen_vision/pollen_vision/perception/utils/pcl_visualizer.py b/pollen_vision/pollen_vision/perception/utils/pcl_visualizer.py index 45f4a00..7dd5400 100644 --- a/pollen_vision/pollen_vision/perception/utils/pcl_visualizer.py +++ b/pollen_vision/pollen_vision/perception/utils/pcl_visualizer.py @@ -35,6 +35,10 @@ def create_point_cloud_from_rgbd( pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic) # pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) + # Filter out points that are too close + pcd = pcd.select_by_index(np.where(np.array(pcd.points)[:, 2] > 0.4)[0]) + # pcd = pcd.select_by_index(np.where(np.array(pcd.points)[:, 2] < 1.8)[0]) + return pcd def add_frame(self, name: str, pose: npt.NDArray[np.float64] = np.eye(4)) -> None: diff --git a/scripts/align_example.py b/scripts/align_example.py new file mode 100644 index 0000000..bc0d034 --- /dev/null +++ b/scripts/align_example.py @@ -0,0 +1,169 @@ +import numpy as np +import cv2 +import depthai as dai +import time +from datetime import timedelta + +# This example is intended to run unchanged on an OAK-D-SR-PoE camera +FPS = 30.0 + +RGB_SOCKET = dai.CameraBoardSocket.CAM_C +TOF_SOCKET = dai.CameraBoardSocket.CAM_D +ALIGN_SOCKET = RGB_SOCKET + + +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + + +pipeline = dai.Pipeline() +# Define sources and outputs +camRgb = pipeline.create(dai.node.ColorCamera) +tof = pipeline.create(dai.node.ToF) +camTof = pipeline.create(dai.node.Camera) +sync = pipeline.create(dai.node.Sync) +align = pipeline.create(dai.node.ImageAlign) +out = pipeline.create(dai.node.XLinkOut) + +tofConfig = tof.initialConfig.get() +tofConfig.enableFPPNCorrection = True +tofConfig.enableOpticalCorrection = False +tofConfig.enablePhaseShuffleTemporalFilter = False +tofConfig.phaseUnwrappingLevel = 1 +tofConfig.phaseUnwrapErrorThreshold = 300 +tofConfig.enableTemperatureCorrection = False # Not yet supported +tofConfig.enableWiggleCorrection = False +tofConfig.median = dai.MedianFilter.KERNEL_3x3 +tof.initialConfig.set(tofConfig) + +# ToF settings +camTof.setFps(FPS) +# camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) +camTof.setBoardSocket(TOF_SOCKET) + +# rgb settings +camRgb.setBoardSocket(RGB_SOCKET) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) +camRgb.setFps(FPS) +camRgb.setIspScale(1, 2) + +out.setStreamName("out") + +sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) + +# Linking +camRgb.isp.link(sync.inputs["rgb"]) +camTof.raw.link(tof.input) +tof.depth.link(align.input) +align.outputAligned.link(sync.inputs["depth_aligned"]) +sync.inputs["rgb"].setBlocking(False) +camRgb.isp.link(align.inputAlignTo) +sync.out.link(out.input) + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb): + """ + Update the rgb and depth weights used to blend depth/rgb image + + @param[in] percent_rgb The rgb weight expressed as a percentage (0..100) + """ + global depthWeight + global rgbWeight + rgbWeight = float(percentRgb) / 100.0 + depthWeight = 1.0 - rgbWeight + + +# Connect to device and start pipeline +with dai.Device(pipeline) as device: + queue = device.getOutputQueue("out", 8, False) + + # Configure windows; trackbar adjusts blending ratio of rgb/depth + rgbDepthWindowName = "rgb-depth" + + cv2.namedWindow(rgbDepthWindowName) + cv2.createTrackbar( + "RGB Weight %", + rgbDepthWindowName, + int(rgbWeight * 100), + 100, + updateBlendWeights, + ) + fpsCounter = FPSCounter() + while True: + messageGroup = queue.get() + fpsCounter.tick() + assert isinstance(messageGroup, dai.MessageGroup) + frameRgb = messageGroup["rgb"] + assert isinstance(frameRgb, dai.ImgFrame) + frameDepth = messageGroup["depth_aligned"] + assert isinstance(frameDepth, dai.ImgFrame) + + sizeRgb = frameRgb.getData().size + sizeDepth = frameDepth.getData().size + # Blend when both received + if frameDepth is not None: + cvFrame = frameRgb.getCvFrame() + # Colorize the aligned depth + alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) + # Resize depth to match the rgb frame + cv2.putText( + alignedDepthColorized, + f"FPS: {fpsCounter.getFps():.2f}", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 2, + ) + cv2.imshow("depth", alignedDepthColorized) + + blended = cv2.addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) + cv2.imshow(rgbDepthWindowName, blended) + + key = cv2.waitKey(1) + if key == ord("q"): + break diff --git a/scripts/align_example2.py b/scripts/align_example2.py new file mode 100644 index 0000000..bc0d034 --- /dev/null +++ b/scripts/align_example2.py @@ -0,0 +1,169 @@ +import numpy as np +import cv2 +import depthai as dai +import time +from datetime import timedelta + +# This example is intended to run unchanged on an OAK-D-SR-PoE camera +FPS = 30.0 + +RGB_SOCKET = dai.CameraBoardSocket.CAM_C +TOF_SOCKET = dai.CameraBoardSocket.CAM_D +ALIGN_SOCKET = RGB_SOCKET + + +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + + +pipeline = dai.Pipeline() +# Define sources and outputs +camRgb = pipeline.create(dai.node.ColorCamera) +tof = pipeline.create(dai.node.ToF) +camTof = pipeline.create(dai.node.Camera) +sync = pipeline.create(dai.node.Sync) +align = pipeline.create(dai.node.ImageAlign) +out = pipeline.create(dai.node.XLinkOut) + +tofConfig = tof.initialConfig.get() +tofConfig.enableFPPNCorrection = True +tofConfig.enableOpticalCorrection = False +tofConfig.enablePhaseShuffleTemporalFilter = False +tofConfig.phaseUnwrappingLevel = 1 +tofConfig.phaseUnwrapErrorThreshold = 300 +tofConfig.enableTemperatureCorrection = False # Not yet supported +tofConfig.enableWiggleCorrection = False +tofConfig.median = dai.MedianFilter.KERNEL_3x3 +tof.initialConfig.set(tofConfig) + +# ToF settings +camTof.setFps(FPS) +# camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) +camTof.setBoardSocket(TOF_SOCKET) + +# rgb settings +camRgb.setBoardSocket(RGB_SOCKET) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) +camRgb.setFps(FPS) +camRgb.setIspScale(1, 2) + +out.setStreamName("out") + +sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) + +# Linking +camRgb.isp.link(sync.inputs["rgb"]) +camTof.raw.link(tof.input) +tof.depth.link(align.input) +align.outputAligned.link(sync.inputs["depth_aligned"]) +sync.inputs["rgb"].setBlocking(False) +camRgb.isp.link(align.inputAlignTo) +sync.out.link(out.input) + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb): + """ + Update the rgb and depth weights used to blend depth/rgb image + + @param[in] percent_rgb The rgb weight expressed as a percentage (0..100) + """ + global depthWeight + global rgbWeight + rgbWeight = float(percentRgb) / 100.0 + depthWeight = 1.0 - rgbWeight + + +# Connect to device and start pipeline +with dai.Device(pipeline) as device: + queue = device.getOutputQueue("out", 8, False) + + # Configure windows; trackbar adjusts blending ratio of rgb/depth + rgbDepthWindowName = "rgb-depth" + + cv2.namedWindow(rgbDepthWindowName) + cv2.createTrackbar( + "RGB Weight %", + rgbDepthWindowName, + int(rgbWeight * 100), + 100, + updateBlendWeights, + ) + fpsCounter = FPSCounter() + while True: + messageGroup = queue.get() + fpsCounter.tick() + assert isinstance(messageGroup, dai.MessageGroup) + frameRgb = messageGroup["rgb"] + assert isinstance(frameRgb, dai.ImgFrame) + frameDepth = messageGroup["depth_aligned"] + assert isinstance(frameDepth, dai.ImgFrame) + + sizeRgb = frameRgb.getData().size + sizeDepth = frameDepth.getData().size + # Blend when both received + if frameDepth is not None: + cvFrame = frameRgb.getCvFrame() + # Colorize the aligned depth + alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) + # Resize depth to match the rgb frame + cv2.putText( + alignedDepthColorized, + f"FPS: {fpsCounter.getFps():.2f}", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 2, + ) + cv2.imshow("depth", alignedDepthColorized) + + blended = cv2.addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) + cv2.imshow(rgbDepthWindowName, blended) + + key = cv2.waitKey(1) + if key == ord("q"): + break diff --git a/scripts/align_rgb_on_depth_test.py b/scripts/align_rgb_on_depth_test.py new file mode 100644 index 0000000..1a0862e --- /dev/null +++ b/scripts/align_rgb_on_depth_test.py @@ -0,0 +1,60 @@ +import numpy as np +import cv2 +import depthai as dai +from datetime import timedelta + +FPS = 30 + +pipeline = dai.Pipeline() +left = pipeline.create(dai.node.ColorCamera) +cam_tof = pipeline.create(dai.node.Camera) +tof = pipeline.create(dai.node.ToF) +sync = pipeline.create(dai.node.Sync) +align = pipeline.create(dai.node.ImageAlign) +out = pipeline.create(dai.node.XLinkOut) +out.setStreamName("out") + +tofConfig = tof.initialConfig.get() +tofConfig.enableFPPNCorrection = True +tofConfig.enableOpticalCorrection = False +tofConfig.enablePhaseShuffleTemporalFilter = False +tofConfig.phaseUnwrappingLevel = 1 +tofConfig.phaseUnwrapErrorThreshold = 300 +tofConfig.enableTemperatureCorrection = False # Not yet supported +tofConfig.enableWiggleCorrection = False +tofConfig.median = dai.MedianFilter.KERNEL_3x3 +tof.initialConfig.set(tofConfig) + +cam_tof.setFps(FPS) +cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) + +left.setBoardSocket(dai.CameraBoardSocket.CAM_C) +left.setFps(FPS) +left.setIspScale(2, 3) + +sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) + +# left.isp.link(sync.inputs["left"]) +tof.depth.link(sync.inputs["depth"]) +cam_tof.raw.link(tof.input) +tof.depth.link(align.inputAlignTo) +left.isp.link(align.input) +sync.inputs["left"].setBlocking(False) +align.outputAligned.link(sync.inputs["left_aligned"]) +sync.out.link(out.input) + +device = dai.Device(pipeline) + +queue = device.getOutputQueue(name="out", maxSize=8, blocking=False) + +while True: + messageGroup = queue.get() + frameDepth = messageGroup["depth"] + frameLeft = messageGroup["left_aligned"] + + left_aligned = frameLeft.getCvFrame() + depth = frameDepth.getFrame() + + cv2.imshow("left_aligned", left_aligned) + cv2.imshow("depth", depth) + cv2.waitKey(1) diff --git a/scripts/chatgpt.py b/scripts/chatgpt.py new file mode 100644 index 0000000..e8e963e --- /dev/null +++ b/scripts/chatgpt.py @@ -0,0 +1,94 @@ +import depthai as dai +import cv2 +import numpy as np +import time + + +# Define a function to colorize the depth map +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor).astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + depthFrameColor[invalidMask] = 0 + except IndexError: + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +# Create a pipeline +pipeline = dai.Pipeline() + +# Create RGB and ToF camera nodes +camRgb = pipeline.create(dai.node.ColorCamera) +camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_C) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1200_P) +camRgb.setFps(30) + +camTof = pipeline.create(dai.node.Camera) +camTof.setFps(30) +camTof.setBoardSocket(dai.CameraBoardSocket.CAM_D) + +# Create ToF node +tof = pipeline.create(dai.node.ToF) +camTof.raw.link(tof.input) + +# Create align node to align ToF depth to RGB +align = pipeline.create(dai.node.ImageAlign) +tof.depth.link(align.input) +camRgb.isp.link(align.inputAlignTo) + +# Create outputs +rgbOut = pipeline.create(dai.node.XLinkOut) +rgbOut.setStreamName("rgb") +alignOut = pipeline.create(dai.node.XLinkOut) +alignOut.setStreamName("depth_aligned") + +camRgb.isp.link(rgbOut.input) +align.outputAligned.link(alignOut.input) + +# Function to update blend weights for depth and RGB overlay +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb): + global depthWeight, rgbWeight + rgbWeight = float(percentRgb) / 100.0 + depthWeight = 1.0 - rgbWeight + + +# Start the pipeline +with dai.Device(pipeline) as device: + qRgb = device.getOutputQueue("rgb", 8, False) + qDepthAligned = device.getOutputQueue("depth_aligned", 8, False) + + cv2.namedWindow("RGB-Depth") + cv2.createTrackbar("RGB Weight %", "RGB-Depth", int(rgbWeight * 100), 100, updateBlendWeights) + + while True: + inRgb = qRgb.get() + inDepthAligned = qDepthAligned.get() + + frameRgb = inRgb.getCvFrame() + frameDepthAligned = inDepthAligned.getCvFrame() + + depthColorized = colorizeDepth(frameDepthAligned) + + # Blend the RGB image with the colorized depth image + blended = cv2.addWeighted(frameRgb, rgbWeight, depthColorized, depthWeight, 0) + cv2.imshow("RGB-Depth", blended) + + key = cv2.waitKey(1) + if key == ord("q"): + break diff --git a/scripts/minimal_tof_test.py b/scripts/minimal_tof_test.py new file mode 100644 index 0000000..bd489f2 --- /dev/null +++ b/scripts/minimal_tof_test.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 + +import time +import cv2 +import depthai as dai +import numpy as np + +print(dai.__version__) + +cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) +cvColorMap[0] = [0, 0, 0] + + +def create_pipeline(): + pipeline = dai.Pipeline() + + tof = pipeline.create(dai.node.ToF) + + # Configure the ToF node + tofConfig = tof.initialConfig.get() + + # Optional. Best accuracy, but adds motion blur. + # see ToF node docs on how to reduce/eliminate motion blur. + tofConfig.enableOpticalCorrection = False + tofConfig.enablePhaseShuffleTemporalFilter = True + tofConfig.phaseUnwrappingLevel = 4 + tofConfig.phaseUnwrapErrorThreshold = 300 + tofConfig.enableTemperatureCorrection = False # Not yet supported + + xinTofConfig = pipeline.create(dai.node.XLinkIn) + xinTofConfig.setStreamName("tofConfig") + xinTofConfig.out.link(tof.inputConfig) + + tof.initialConfig.set(tofConfig) + + cam_tof = pipeline.create(dai.node.Camera) + cam_tof.setFps(30) # ToF node will produce depth frames at /2 of this rate + cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) + cam_tof.raw.link(tof.input) + + xout = pipeline.create(dai.node.XLinkOut) + xout.setStreamName("depth") + tof.depth.link(xout.input) + + tofConfig = tof.initialConfig.get() + + left = pipeline.create(dai.node.ColorCamera) + left.setBoardSocket(dai.CameraBoardSocket.CAM_B) + left.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) + left.setFps(30) + left.setIspScale(2, 3) + + left_xout = pipeline.create(dai.node.XLinkOut) + left_xout.setStreamName("left") + left.video.link(left_xout.input) + + return pipeline, tofConfig + + +if __name__ == "__main__": + pipeline, tofConfig = create_pipeline() + + with dai.Device(pipeline) as device: + print("Connected cameras:", device.getConnectedCameraFeatures()) + + # qDepth = device.getOutputQueue(name="depth") + qDepth = device.getOutputQueue(name="depth", maxSize=8, blocking=False) + + # left_q = device.getOutputQueue(name="left") + left_q = device.getOutputQueue(name="left", maxSize=8, blocking=False) + + while True: + start = time.time() + + imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived + depth_map = imgFrame.getFrame() + + max_depth = (tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. + depth_raw = depth_map / max_depth + depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8) + depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap) + + # If I comment that (3 next lines), the tof works fine + in_left = left_q.get() + left_im = in_left.getCvFrame() + cv2.imshow("left", left_im) + # Until that + + cv2.imshow("Colorized depth", depth_colorized) + cv2.imshow("depth raw", depth_raw) + key = cv2.waitKey(1) + + device.close() diff --git a/scripts/read_calib.json b/scripts/read_calib.json new file mode 100644 index 0000000..404dad8 --- /dev/null +++ b/scripts/read_calib.json @@ -0,0 +1,247 @@ +{ + "batchName": "", + "batchTime": 0, + "boardConf": "", + "boardCustom": "", + "boardName": "", + "boardOptions": 0, + "boardRev": "", + "cameraData": [ + [ + 2, + { + "cameraType": 1, + "distortionCoeff": [ + -0.00550349336117506, + 0.010187107138335705, + -0.011511093005537987, + 0.002370026195421815, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "extrinsics": { + "rotationMatrix": [ + [ + 0.9998169541358948, + 0.019127150997519493, + 0.0004626706358976662 + ], + [ + -0.019127311185002327, + 0.9998170137405396, + 0.0003426657058298588 + ], + [ + -0.0004560317611321807, + -0.00035145264700986445, + 0.9999998211860657 + ] + ], + "specTranslation": { + "x": -6.519075393676758, + "y": 0.21584674715995789, + "z": 0.004252283368259668 + }, + "toCameraSocket": 1, + "translation": { + "x": -6.519075393676758, + "y": 0.21584674715995789, + "z": 0.004252283368259668 + } + }, + "height": 1200, + "intrinsicMatrix": [ + [ + 613.396484375, + 0.0, + 865.1143188476563 + ], + [ + 0.0, + 612.13525390625, + 522.9628295898438 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "lensPosition": 0, + "specHfovDeg": 0.0, + "width": 1920 + } + ], + [ + 1, + { + "cameraType": 1, + "distortionCoeff": [ + 0.0007732227677479386, + -0.00818662904202938, + 0.002664593979716301, + -0.0010734152747318149, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "extrinsics": { + "rotationMatrix": [ + [ + 0.9998169541358948, + -0.019127311185002327, + -0.0004560317611321807 + ], + [ + 0.019127150997519493, + 0.9998170137405396, + -0.00035145264700986445 + ], + [ + 0.0004626706358976662, + 0.0003426657058298588, + 0.9999998211860657 + ] + ], + "specTranslation": { + "x": 6.522012710571289, + "y": -0.09111440926790237, + "z": -0.0013100611977279186 + }, + "toCameraSocket": 2, + "translation": { + "x": 6.522012710571289, + "y": -0.09111440926790237, + "z": -0.0013100611977279186 + } + }, + "height": 1200, + "intrinsicMatrix": [ + [ + 613.5316772460938, + 0.0, + 1061.9139404296875 + ], + [ + 0.0, + 611.7612915039063, + 590.1260986328125 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "lensPosition": 0, + "specHfovDeg": 0.0, + "width": 1920 + } + ] + ], + "deviceName": "", + "hardwareConf": "", + "housingExtrinsics": { + "rotationMatrix": [], + "specTranslation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "toCameraSocket": -1, + "translation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + }, + "imuExtrinsics": { + "rotationMatrix": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ], + "specTranslation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "toCameraSocket": -1, + "translation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + }, + "miscellaneousData": [], + "productName": "", + "stereoEnableDistortionCorrection": false, + "stereoRectificationData": { + "leftCameraSocket": 2, + "rectifiedRotationLeft": [ + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "rectifiedRotationRight": [ + [ + 0.9998169541358948, + 0.019127150997519493, + 0.0004626706358976662 + ], + [ + -0.019127311185002327, + 0.9998170137405396, + 0.0003426657058298588 + ], + [ + -0.0004560317611321807, + -0.00035145264700986445, + 0.9999998211860657 + ] + ], + "rightCameraSocket": 1 + }, + "stereoUseSpecTranslation": true, + "version": 7, + "verticalCameraSocket": -1 +} diff --git a/scripts/test.py b/scripts/test.py new file mode 100644 index 0000000..10e1ca6 --- /dev/null +++ b/scripts/test.py @@ -0,0 +1,7 @@ +import depthai as dai + +device = dai.Device() +calib = device.readCalibration() +calib.eepromToJsonFile("read_calib.json") +# print(calib) +print(calib.getCameraExtrinsics(dai.CameraBoardSocket.CAM_D, dai.CameraBoardSocket.CAM_C)) diff --git a/scripts/tof_and_encoding_test.py b/scripts/tof_and_encoding_test.py new file mode 100644 index 0000000..a63a521 --- /dev/null +++ b/scripts/tof_and_encoding_test.py @@ -0,0 +1,206 @@ +import numpy as np +import cv2 +import depthai as dai +from datetime import timedelta +from pollen_vision.camera_wrappers.depthai.calibration.undistort import compute_undistort_maps, get_mesh +from pollen_vision.camera_wrappers.depthai.cam_config import CamConfig +from pollen_vision.camera_wrappers.depthai.utils import ( + get_config_file_path, + get_config_files_names, +) +import subprocess as sp + +FPS = 60 +pipeline = dai.Pipeline() +left = pipeline.create(dai.node.ColorCamera) +left.setFps(FPS) +left.setBoardSocket(dai.CameraBoardSocket.CAM_C) +left.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) +left.setSensorCrop(208 / 1440, 28 / 1080) +left.setVideoSize(1024, 1024) +# left.setIspScale(2, 3) + +right = pipeline.create(dai.node.ColorCamera) +right.setFps(FPS) +right.setBoardSocket(dai.CameraBoardSocket.CAM_B) +right.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) +right.setSensorCrop(208 / 1440, 28 / 1080) +right.setVideoSize(1024, 1024) +# right.setIspScale(2, 3) + +# cam_tof = pipeline.create(dai.node.Camera) +# cam_tof.setFps(30) +# cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) +# tof = pipeline.create(dai.node.ToF) + + +xout_left = pipeline.createXLinkOut() +xout_left.setStreamName("left") + +xout_right = pipeline.createXLinkOut() +xout_right.setStreamName("right") + +# xout_depth = pipeline.createXLinkOut() +# xout_depth.setStreamName("depth") + +# tofConfig = tof.initialConfig.get() +# tofConfig.enableFPPNCorrection = True +# tofConfig.enableOpticalCorrection = False +# tofConfig.enablePhaseShuffleTemporalFilter = False +# tofConfig.phaseUnwrappingLevel = 1 +# tofConfig.phaseUnwrapErrorThreshold = 300 +# tofConfig.enableTemperatureCorrection = False # Not yet supported +# tofConfig.enableWiggleCorrection = False +# tofConfig.median = dai.MedianFilter.KERNEL_3x3 +# tof.initialConfig.set(tofConfig) + +# Create encoders +profile = dai.VideoEncoderProperties.Profile.H264_BASELINE +bitrate = 4000 +numBFrames = 0 # gstreamer recommends 0 B frames +left_encoder = pipeline.create(dai.node.VideoEncoder) +left_encoder.setDefaultProfilePreset(FPS, profile) +left_encoder.setKeyframeFrequency(FPS) # every 1s +left_encoder.setNumBFrames(numBFrames) +left_encoder.setBitrateKbps(bitrate) + +right_encoder = pipeline.create(dai.node.VideoEncoder) +right_encoder.setDefaultProfilePreset(60, profile) +right_encoder.setKeyframeFrequency(60) # every 1s +right_encoder.setNumBFrames(numBFrames) +right_encoder.setBitrateKbps(bitrate) + +# Create manip +# resolution = (960, 720) +resolution = (1024, 1024) +config_json = get_config_file_path("CONFIG_IMX296_TOF") +cam_config = CamConfig( + cam_config_json=config_json, + fps=FPS, + resize=resolution, + exposure_params=None, + mx_id=None, + # isp_scale=(2, 3), + rectify=True, +) +device = dai.Device() +cam_config.set_sensor_resolution(tuple(resolution)) +width_undistort_resolution = int(1024 * (cam_config.isp_scale[0] / cam_config.isp_scale[1])) +height_unistort_resolution = int(1024 * (cam_config.isp_scale[0] / cam_config.isp_scale[1])) +cam_config.set_undistort_resolution((width_undistort_resolution, height_unistort_resolution)) +cam_config.set_calib(device.readCalibration()) +device.close() +mapXL, mapYL, mapXR, mapYR = compute_undistort_maps(cam_config) +cam_config.set_undistort_maps(mapXL, mapYL, mapXR, mapYR) + + +left_manip = pipeline.createImageManip() +# try: +# mesh, meshWidth, meshHeight = get_mesh(cam_config, "left") +# left_manip.setWarpMesh(mesh, meshWidth, meshHeight) +# except Exception as e: +# print(e) +# exit() +left_manip.setMaxOutputFrameSize(resolution[0] * resolution[1] * 3) +left_manip.initialConfig.setKeepAspectRatio(True) +left_manip.initialConfig.setResize(resolution[0], resolution[1]) +left_manip.initialConfig.setFrameType(dai.ImgFrame.Type.NV12) + + +right_manip = pipeline.createImageManip() +# try: +# mesh, meshWidth, meshHeight = get_mesh(cam_config, "right") +# right_manip.setWarpMesh(mesh, meshWidth, meshHeight) +# except Exception as e: +# print(e) +# exit() +right_manip.setMaxOutputFrameSize(resolution[0] * resolution[1] * 3) +right_manip.initialConfig.setKeepAspectRatio(True) +right_manip.initialConfig.setResize(resolution[0], resolution[1]) +right_manip.initialConfig.setFrameType(dai.ImgFrame.Type.NV12) + +# left.video.link(left_manip.inputImage) +# left_manip.out.link(left_encoder.input) +left.video.link(xout_left.input) + + +# right.video.link(right_manip.inputImage) +# right_manip.out.link(right_encoder.input) +right.video.out.link(xout_right.input) + +# left_encoder.bitstream.link(xout_left.input) +# right_encoder.bitstream.link(xout_right.input) + +# cam_tof.raw.link(tof.input) +# tof.depth.link(xout_depth.input) + +device = dai.Device(pipeline) + +left_queue = device.getOutputQueue(name="left", maxSize=1, blocking=False) +right_queue = device.getOutputQueue(name="right", maxSize=1, blocking=False) +# depth_queue = device.getOutputQueue(name="depth", maxSize=4, blocking=False) + + +def spawn_procs(names): # type: ignore [type-arg] + # width, height = 1280, 720 + # width, height = 1440, 1080 + width, height = 1024, 1024 + command = [ + "ffplay", + "-i", + "-", + "-x", + str(width), + "-y", + str(height), + "-framerate", + str(FPS), + "-fflags", + "nobuffer", + "-flags", + "low_delay", + "-framedrop", + "-strict", + "experimental", + ] + + procs = {} + try: + for name in names: + procs[name] = sp.Popen(command, stdin=sp.PIPE) # Start the ffplay process + except Exception: + exit("Error: cannot run ffplay!\nTry running: sudo apt install ffmpeg") + + return procs + + +latencies_left = [] +latencies_right = [] +# procs = spawn_procs(["left", "right"]) +while True: + latency = {} + + left_frame = left_queue.get() + latency["left"] = dai.Clock.now() - left_frame.getTimestamp() + right_frame = right_queue.get() + latency["right"] = dai.Clock.now() - right_frame.getTimestamp() + # depth_frame = depth_queue.get() + # latency["depth"] = dai.Clock.now() - depth_frame.getTimestamp() + + # io = procs["left"].stdin + # io.write(left_frame.getData()) + + # io = procs["right"].stdin + # io.write(right_frame.getData()) + + cv2.imshow("left", left_frame.getCvFrame()) + cv2.imshow("right", right_frame.getCvFrame()) + # cv2.imshow("depth", depth_frame.getFrame()) + # print(latency) + latencies_left.append(latency["left"]) + latencies_right.append(latency["right"]) + + print(np.mean(latencies_left[-50:]), np.mean(latencies_right[-50:])) + + if cv2.waitKey(1) == ord("q"): + break diff --git a/scripts/tof_pcl_test.py b/scripts/tof_pcl_test.py new file mode 100644 index 0000000..1f83e86 --- /dev/null +++ b/scripts/tof_pcl_test.py @@ -0,0 +1,66 @@ +from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer +import time + +import cv2 +import depthai as dai +import numpy as np + + +def create_pipeline(): + pipeline = dai.Pipeline() + + tof = pipeline.create(dai.node.ToF) + + # Configure the ToF node + tofConfig = tof.initialConfig.get() + + # Optional. Best accuracy, but adds motion blur. + # see ToF node docs on how to reduce/eliminate motion blur. + tofConfig.enableOpticalCorrection = False + tofConfig.enablePhaseShuffleTemporalFilter = True + tofConfig.phaseUnwrappingLevel = 4 + tofConfig.phaseUnwrapErrorThreshold = 300 + + tofConfig.enableTemperatureCorrection = False # Not yet supported + + xinTofConfig = pipeline.create(dai.node.XLinkIn) + xinTofConfig.setStreamName("tofConfig") + xinTofConfig.out.link(tof.inputConfig) + + tof.initialConfig.set(tofConfig) + + cam_tof = pipeline.create(dai.node.Camera) + cam_tof.setFps(60) # ToF node will produce depth frames at /2 of this rate + cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_A) + cam_tof.raw.link(tof.input) + + xout = pipeline.create(dai.node.XLinkOut) + xout.setStreamName("depth") + tof.depth.link(xout.input) + + tofConfig = tof.initialConfig.get() + + return pipeline, tofConfig + + +K = np.eye(3) +K[0][0] = 100 +K[1][1] = 100 +K[0][2] = 320 +K[1][2] = 240 +P = PCLVisualizer(K) + +pipeline, tofConfig = create_pipeline() +rgb = np.zeros((480, 640, 3), dtype=np.uint8) +rgb[:, :, 0] = 255 +with dai.Device(pipeline) as device: + qDepth = device.getOutputQueue(name="depth") + while True: + imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived + depth_map = imgFrame.getFrame() + depth_map = depth_map * 0.001 + print("min", min(depth_map.reshape(-1)), "max", max(depth_map.reshape(-1))) + P.update(rgb, depth_map.astype(np.float32)) + P.tick() + cv2.imshow("depth", depth_map) + cv2.waitKey(1) diff --git a/scripts/tof_test_align.py b/scripts/tof_test_align.py new file mode 100644 index 0000000..0772c0e --- /dev/null +++ b/scripts/tof_test_align.py @@ -0,0 +1,27 @@ +import time +import cv2 +import depthai as dai +import numpy as np + +pipeline = dai.Pipeline() + +tof = pipeline.create(dai.node.ToF) + +tofConfig = tof.initialConfig.get() +tofConfig.enableOpticalCorrection = False +tofConfig.enablePhaseShuffleTemporalFilter = True +tofConfig.phaseUnwrappingLevel = 4 +tofConfig.phaseUnwrapErrorThreshold = 300 +tofConfig.enableTemperatureCorrection = False # Not yet supported + +cam_tof = pipeline.create(dai.node.Camera) +cam_tof.setFps(30) +cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) +cam_tof.raw.link(tof.input) + +cam_left = pipeline.create(dai.node.ColorCamera) +cam_left.setBoardSocket(dai.CameraBoardSocket.CAM_C) + +undistort_manip = pipeline.createImageManip() +mesh, meshWidth, meshHeight = get_mesh(self.cam_config, cam_name) +manip.setWarpMesh(mesh, meshWidth, meshHeight) diff --git a/scripts/vanilla_tof_test.py b/scripts/vanilla_tof_test.py new file mode 100644 index 0000000..f734e3c --- /dev/null +++ b/scripts/vanilla_tof_test.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +import time +import cv2 +import depthai as dai +import numpy as np + +print(dai.__version__) + +cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) +cvColorMap[0] = [0, 0, 0] + + +def create_pipeline(): + pipeline = dai.Pipeline() + + tof = pipeline.create(dai.node.ToF) + # print(dir(tof.properties.initialConfig)) + # print(tof.properties.initialConfig.data) + # exit() + # Configure the ToF node + tofConfig = tof.initialConfig.get() + + # Optional. Best accuracy, but adds motion blur. + # see ToF node docs on how to reduce/eliminate motion blur. + tofConfig.enableFPPNCorrection = True + tofConfig.enableOpticalCorrection = False + tofConfig.enablePhaseShuffleTemporalFilter = False + tofConfig.phaseUnwrappingLevel = 1 + tofConfig.phaseUnwrapErrorThreshold = 300 + tofConfig.enableTemperatureCorrection = False # Not yet supported + tofConfig.enableWiggleCorrection = False + tofConfig.median = dai.MedianFilter.KERNEL_3x3 + + xinTofConfig = pipeline.create(dai.node.XLinkIn) + xinTofConfig.setStreamName("tofConfig") + xinTofConfig.out.link(tof.inputConfig) + + tof.initialConfig.set(tofConfig) + + cam_tof = pipeline.create(dai.node.Camera) + cam_tof.setFps(30) # ToF node will produce depth frames at /2 of this rate + cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) + cam_tof.raw.link(tof.input) + + xout = pipeline.create(dai.node.XLinkOut) + xout.setStreamName("depth") + tof.depth.link(xout.input) + + tofConfig = tof.initialConfig.get() + + right = pipeline.create(dai.node.ColorCamera) + right.setBoardSocket(dai.CameraBoardSocket.CAM_B) + right.setIspScale(2, 3) + left = pipeline.create(dai.node.ColorCamera) + left.setBoardSocket(dai.CameraBoardSocket.CAM_C) + left.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) + left.setFps(30) + left.setIspScale(2, 3) + + left_xout = pipeline.create(dai.node.XLinkOut) + left_xout.setStreamName("left") + left.video.link(left_xout.input) + + right_xout = pipeline.create(dai.node.XLinkOut) + right_xout.setStreamName("right") + right.video.link(right_xout.input) + + return pipeline, tofConfig + + +mouse_x, mouse_y = 0, 0 + + +# mouse callback function +def cb(event, x, y, flags, param): + global mouse_x, mouse_y + mouse_x = x + mouse_y = y + + +if __name__ == "__main__": + # global mouse_x, mouse_y + pipeline, tofConfig = create_pipeline() + cv2.namedWindow("Colorized depth") + cv2.setMouseCallback("Colorized depth", cb) + + with dai.Device(pipeline) as device: + print("Connected cameras:", device.getConnectedCameraFeatures()) + qDepth = device.getOutputQueue(name="depth", maxSize=8, blocking=False) + + left_q = device.getOutputQueue(name="left", maxSize=8, blocking=False) + right_q = device.getOutputQueue(name="right", maxSize=8, blocking=False) + + tofConfigInQueue = device.getInputQueue("tofConfig") + + counter = 0 + while True: + start = time.time() + key = cv2.waitKey(1) + if key == ord("f"): + tofConfig.enableFPPNCorrection = not tofConfig.enableFPPNCorrection + tofConfigInQueue.send(tofConfig) + elif key == ord("o"): + tofConfig.enableOpticalCorrection = not tofConfig.enableOpticalCorrection + tofConfigInQueue.send(tofConfig) + elif key == ord("w"): + tofConfig.enableWiggleCorrection = not tofConfig.enableWiggleCorrection + tofConfigInQueue.send(tofConfig) + elif key == ord("t"): + tofConfig.enableTemperatureCorrection = not tofConfig.enableTemperatureCorrection + tofConfigInQueue.send(tofConfig) + elif key == ord("q"): + break + elif key == ord("0"): + tofConfig.enablePhaseUnwrapping = False + tofConfig.phaseUnwrappingLevel = 0 + tofConfigInQueue.send(tofConfig) + elif key == ord("1"): + tofConfig.enablePhaseUnwrapping = True + tofConfig.phaseUnwrappingLevel = 1 + tofConfigInQueue.send(tofConfig) + elif key == ord("2"): + tofConfig.enablePhaseUnwrapping = True + tofConfig.phaseUnwrappingLevel = 2 + tofConfigInQueue.send(tofConfig) + elif key == ord("3"): + tofConfig.enablePhaseUnwrapping = True + tofConfig.phaseUnwrappingLevel = 3 + tofConfigInQueue.send(tofConfig) + elif key == ord("4"): + tofConfig.enablePhaseUnwrapping = True + tofConfig.phaseUnwrappingLevel = 4 + tofConfigInQueue.send(tofConfig) + elif key == ord("5"): + tofConfig.enablePhaseUnwrapping = True + tofConfig.phaseUnwrappingLevel = 5 + tofConfigInQueue.send(tofConfig) + elif key == ord("m"): + medianSettings = [ + dai.MedianFilter.MEDIAN_OFF, + dai.MedianFilter.KERNEL_3x3, + dai.MedianFilter.KERNEL_5x5, + dai.MedianFilter.KERNEL_7x7, + ] + currentMedian = tofConfig.median + nextMedian = medianSettings[(medianSettings.index(currentMedian) + 1) % len(medianSettings)] + print(f"Changing median to {nextMedian.name} from {currentMedian.name}") + tofConfig.median = nextMedian + tofConfigInQueue.send(tofConfig) + + print("===") + print("enableFPPNCorrection", tofConfig.enableFPPNCorrection) + print("enableOpticalCorrection", tofConfig.enableOpticalCorrection) + print("enableWiggleCorrection", tofConfig.enableWiggleCorrection) + print("enableTemperatureCorrection", tofConfig.enableTemperatureCorrection) + print("enablePhaseUnwrapping", tofConfig.enablePhaseUnwrapping) + print("phaseUnwrappingLevel", tofConfig.phaseUnwrappingLevel) + print("median", tofConfig.median) + print("===") + imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived + depth_map = imgFrame.getFrame() + + max_depth = (tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. + depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8) + depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap) + + depth_at_mouse = depth_map[mouse_y, mouse_x] + print(mouse_x, mouse_y, depth_at_mouse) + depth_colorized = cv2.circle(depth_colorized, (mouse_x, mouse_y), 5, (255, 255, 255), -1) + + in_left = left_q.get() + in_right = right_q.get() + + left_im = in_left.getCvFrame() + right_im = in_right.getCvFrame() + + cv2.imshow("left", left_im) + cv2.imshow("right", right_im) + cv2.imshow("Colorized depth", depth_colorized) + counter += 1 + time.sleep(1 / 30) + + device.close() From ada5d4fcd515c07195bd15c57d165f13d6220b88 Mon Sep 17 00:00:00 2001 From: apirrone Date: Thu, 19 Sep 2024 17:07:33 +0200 Subject: [PATCH 03/17] trying to calibrate the ToF's extrinsics --- .../camera_wrappers/depthai/__init__.py | 1 + .../depthai/calibration/acquire.py | 18 +++++++++-- .../camera_wrappers/depthai/tof.py | 32 ++++++++++--------- .../camera_wrappers/depthai/wrapper.py | 8 ++--- scripts/vanilla_tof_test.py | 11 +++++++ 5 files changed, 48 insertions(+), 22 deletions(-) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/__init__.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/__init__.py index 283494e..b87dc7c 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/__init__.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/__init__.py @@ -1,2 +1,3 @@ from .sdk import SDKWrapper # noqa: F401 from .teleop import TeleopWrapper # noqa: F401 +from .tof import TOFWrapper # noqa: F401 diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py index d75d24b..70cf9be 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py @@ -3,7 +3,7 @@ import cv2 import numpy as np -from pollen_vision.camera_wrappers.depthai import SDKWrapper +from pollen_vision.camera_wrappers.depthai import SDKWrapper, TOFWrapper from pollen_vision.camera_wrappers.depthai.utils import ( get_config_file_path, get_config_files_names, @@ -24,9 +24,14 @@ default="./calib_images/", help="Directory where the acquired images are stored (default ./calib_images/)", ) +argParser.add_argument("--tof", action="store_true", help="Has tof sensor ?") args = argParser.parse_args() -w = SDKWrapper(get_config_file_path(args.config), compute_depth=False, rectify=False) +if not args.tof: + w = SDKWrapper(get_config_file_path(args.config), compute_depth=False, rectify=False) +else: + w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30) + left_path = os.path.join(args.imagesPath, "left") right_path = os.path.join(args.imagesPath, "right") @@ -44,7 +49,14 @@ for name in data.keys(): _data[name] = data[name] - concat = np.hstack((_data["left"], _data["right"])) + if args.tof: + tof_amplitude = _data["tof_amplitude"] + print(_data["left"].shape) + tof_amplitude = cv2.resize(tof_amplitude, _data["left"].shape[:2][::-1]) + tof_amplitude = np.dstack((tof_amplitude, tof_amplitude, tof_amplitude)) + concat = np.hstack((_data["left"], _data["right"], tof_amplitude)) + else: + concat = np.hstack((_data["left"], _data["right"])) cv2.imshow(name, cv2.resize(concat, (0, 0), fx=0.5, fy=0.5)) key = cv2.waitKey(1) if key == 13: diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 676b5af..4fa2ba4 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -16,11 +16,7 @@ class TOFWrapper(DepthaiWrapper): def __init__( - self, - cam_config_json: str, - fps: int = 30, - force_usb2: bool = False, - mx_id: str = "", + self, cam_config_json: str, fps: int = 30, force_usb2: bool = False, mx_id: str = "", crop: bool = False ) -> None: super().__init__( cam_config_json, @@ -35,6 +31,7 @@ def __init__( self.cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) self.cvColorMap[0] = [0, 0, 0] self.cam_config.undistort_resolution = (640, 480) + self.crop = crop def crop_image(self, im, depth): # threshold depth @@ -64,18 +61,20 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], left = messageGroup["left"].getCvFrame() right = messageGroup["right"].getCvFrame() depth = messageGroup["depth_aligned"].getFrame() + tof_amplitude = messageGroup["tof_amplitude"].getCvFrame() # Temporary, not ideal - cropped_left, cropped_depth = self.crop_image(left, depth) - data["left"] = cropped_left - data["right"] = right - data["depth"] = cropped_depth - - # data["left"] = left - # data["right"] = right - # data["depth"] = depth + if self.crop: + cropped_left, cropped_depth = self.crop_image(left, depth) + data["left"] = cropped_left + data["depth"] = cropped_depth + else: + data["left"] = left + data["depth"] = depth + data["right"] = right + data["tof_amplitude"] = tof_amplitude return data, latency, ts def get_K(self) -> npt.NDArray[np.float32]: @@ -124,6 +123,7 @@ def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: self.left.isp.link(self.left_manip.inputImage) self.right.isp.link(self.right_manip.inputImage) + self.tof.amplitude.link(self.sync.inputs["tof_amplitude"]) self.left_manip.out.link(self.sync.inputs["left"]) self.right_manip.out.link(self.sync.inputs["right"]) @@ -164,7 +164,7 @@ def cv_callback(event, x, y, flags, param): if __name__ == "__main__": - t = TOFWrapper(get_config_file_path("CONFIG_AR0234_TOF"), fps=30) + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30) # P = PCLVisualizer(t.get_K()) cv2.namedWindow("depth") @@ -174,14 +174,16 @@ def cv_callback(event, x, y, flags, param): data, _, _ = t.get_data() left = data["left"] right = data["right"] + tof_amplitude = data["tof_amplitude"] # left = cv2.resize(left, (640, 480)) # right = cv2.resize(right, (640, 480)) depth = data["depth"] cv2.imshow("left", left) - cv2.imshow("right", right) + # cv2.imshow("right", right) print(data["depth"][mouse_y, mouse_x]) depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) cv2.imshow("depth", depth) + cv2.imshow("tof_amplitude", tof_amplitude) # P.update(cv2.cvtColor(left, cv2.COLOR_BGR2RGB), depth) # P.tick() cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index 3fbfc09..7722b5d 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -292,10 +292,10 @@ def flash(self, calib_json_file: str, tof=False) -> None: if tof: K = np.eye(3) # Piffed, need to find the real values - K[0][0] = 550 # fx - K[1][1] = 550 # fy - K[0][2] = 320 # cx - K[1][2] = 240 # cy + K[0][0] = 471.8361511230469 # fx + K[1][1] = 471.7205810546875 # fy + K[0][2] = 322.25347900390625 # cx + K[1][2] = 246.209716796875 # cy tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) ch.setCameraIntrinsics(tof_socket, K.tolist(), (640, 480)) diff --git a/scripts/vanilla_tof_test.py b/scripts/vanilla_tof_test.py index f734e3c..583b259 100644 --- a/scripts/vanilla_tof_test.py +++ b/scripts/vanilla_tof_test.py @@ -47,6 +47,10 @@ def create_pipeline(): xout.setStreamName("depth") tof.depth.link(xout.input) + xout_tof_amplitude = pipeline.create(dai.node.XLinkOut) + tof.amplitude.link(xout_tof_amplitude.input) + xout_tof_amplitude.setStreamName("tof_amplitude") + tofConfig = tof.initialConfig.get() right = pipeline.create(dai.node.ColorCamera) @@ -91,6 +95,7 @@ def cb(event, x, y, flags, param): left_q = device.getOutputQueue(name="left", maxSize=8, blocking=False) right_q = device.getOutputQueue(name="right", maxSize=8, blocking=False) + tof_amplitude = device.getOutputQueue(name="tof_amplitude", maxSize=8, blocking=False) tofConfigInQueue = device.getInputQueue("tofConfig") @@ -172,12 +177,18 @@ def cb(event, x, y, flags, param): in_left = left_q.get() in_right = right_q.get() + tof_amplitude_frame = tof_amplitude.get() + left_im = in_left.getCvFrame() right_im = in_right.getCvFrame() + tof_amplitude_img = tof_amplitude_frame.getCvFrame().astype(np.float32) + cv2.imshow("left", left_im) cv2.imshow("right", right_im) cv2.imshow("Colorized depth", depth_colorized) + cv2.imshow("tof_amplitude", tof_amplitude_img / tof_amplitude_img.max() * 255) + counter += 1 time.sleep(1 / 30) From 5d41d334bbbe36b468fba47420eedc79b9f00f86 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 24 Sep 2024 12:38:53 +0200 Subject: [PATCH 04/17] tof --- .../camera_wrappers_examples/live_pcl_tof.py | 15 +++-- .../depthai/calibration/acquire.py | 8 ++- .../camera_wrappers/depthai/tof.py | 58 +++++++++++----- .../camera_wrappers/depthai/wrapper.py | 67 +++++-------------- 4 files changed, 74 insertions(+), 74 deletions(-) diff --git a/examples/camera_wrappers_examples/live_pcl_tof.py b/examples/camera_wrappers_examples/live_pcl_tof.py index 758a7c2..8490cbc 100644 --- a/examples/camera_wrappers_examples/live_pcl_tof.py +++ b/examples/camera_wrappers_examples/live_pcl_tof.py @@ -7,6 +7,7 @@ get_config_file_path, get_config_files_names, ) + from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer valid_configs = get_config_files_names() @@ -24,8 +25,8 @@ w = TOFWrapper(get_config_file_path(args.config)) K = w.get_K() -P = PCLVisualizer(K) -P.add_frame("origin") +# P = PCLVisualizer(K) +# P.add_frame("origin") def colorizeDepth(frameDepth): @@ -64,8 +65,8 @@ def cv_callback(event, x, y, flags, param): mouse_x, mouse_y = x, y -cv2.namedWindow("depth") -cv2.setMouseCallback("depth", cv_callback) +# cv2.namedWindow("depth") +# cv2.setMouseCallback("depth", cv_callback) while True: data, lat, _ = w.get_data() # print(lat["depthNode_left"], lat["depthNode_right"]) @@ -76,7 +77,7 @@ def cv_callback(event, x, y, flags, param): colorized_depth = colorizeDepth(depth) # disparity = data["disparity"] - P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) + # P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) # P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) # disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) @@ -86,7 +87,7 @@ def cv_callback(event, x, y, flags, param): # cv2.imshow("right", data["depthNode_right"]) cv2.imshow("depth", colorized_depth) cv2.imshow("rgb", rgb) - print(depth[mouse_y, mouse_x]) + # print(depth[mouse_y, mouse_x]) key = cv2.waitKey(1) - P.tick() + # P.tick() diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py index 70cf9be..74cf92e 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py @@ -37,6 +37,9 @@ right_path = os.path.join(args.imagesPath, "right") os.makedirs(left_path, exist_ok=True) os.makedirs(right_path, exist_ok=True) +if args.tof: + tof_path = os.path.join(args.imagesPath, "tof") + os.makedirs(tof_path, exist_ok=True) print("Press return to save an image pair.") print("(Keep the focus on the opencv window for the inputs to register.)") @@ -51,17 +54,18 @@ if args.tof: tof_amplitude = _data["tof_amplitude"] - print(_data["left"].shape) tof_amplitude = cv2.resize(tof_amplitude, _data["left"].shape[:2][::-1]) tof_amplitude = np.dstack((tof_amplitude, tof_amplitude, tof_amplitude)) concat = np.hstack((_data["left"], _data["right"], tof_amplitude)) else: concat = np.hstack((_data["left"], _data["right"])) - cv2.imshow(name, cv2.resize(concat, (0, 0), fx=0.5, fy=0.5)) + cv2.imshow("concat", cv2.resize(concat, (0, 0), fx=0.5, fy=0.5)) key = cv2.waitKey(1) if key == 13: cv2.imwrite(os.path.join(left_path, str(i) + ".png"), data["left"]) cv2.imwrite(os.path.join(right_path, str(i) + ".png"), data["right"]) + if args.tof: + cv2.imwrite(os.path.join(tof_path, str(i) + ".png"), data["tof_amplitude"]) print("Saved image pair ", i, "to", left_path, " and ", right_path) i += 1 elif key == 27 or key == ord("q"): diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 4fa2ba4..744d37f 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -23,7 +23,7 @@ def __init__( fps, force_usb2=force_usb2, resize=None, - rectify=False, + rectify=True, exposure_params=None, mx_id=mx_id, # isp_scale=(2, 3), @@ -123,7 +123,7 @@ def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: self.left.isp.link(self.left_manip.inputImage) self.right.isp.link(self.right_manip.inputImage) - self.tof.amplitude.link(self.sync.inputs["tof_amplitude"]) + self.tof.intensity.link(self.sync.inputs["tof_amplitude"]) self.left_manip.out.link(self.sync.inputs["left"]) self.right_manip.out.link(self.sync.inputs["right"]) @@ -163,27 +163,55 @@ def cv_callback(event, x, y, flags, param): mouse_x, mouse_y = x, y +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + if __name__ == "__main__": - t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30) + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False) - # P = PCLVisualizer(t.get_K()) - cv2.namedWindow("depth") - cv2.setMouseCallback("depth", cv_callback) + # cv2.namedWindow("depth") + # cv2.setMouseCallback("depth", cv_callback) print(dai.__version__) while True: data, _, _ = t.get_data() left = data["left"] - right = data["right"] - tof_amplitude = data["tof_amplitude"] - # left = cv2.resize(left, (640, 480)) - # right = cv2.resize(right, (640, 480)) + # right = data["right"] depth = data["depth"] + # tof_amplitude = data["tof_amplitude"] + # # left = cv2.resize(left, (640, 480)) + # # right = cv2.resize(right, (640, 480)) cv2.imshow("left", left) # cv2.imshow("right", right) - print(data["depth"][mouse_y, mouse_x]) - depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) + # print(data["depth"][mouse_y, mouse_x]) + # depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) cv2.imshow("depth", depth) - cv2.imshow("tof_amplitude", tof_amplitude) - # P.update(cv2.cvtColor(left, cv2.COLOR_BGR2RGB), depth) - # P.tick() + # cv2.imshow("tof_amplitude", tof_amplitude) + colorized_depth = colorizeDepth(data["depth"]) + blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0) + cv2.imshow("blended", blended) cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index 7722b5d..0e0ec5c 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -280,25 +280,21 @@ def flash(self, calib_json_file: str, tof=False) -> None: for cam_name, params in cameras.items(): K = np.array(params["K"]) D = np.array(params["dist"]).reshape((-1)) + print(cam_name, K, D) im_size = params["image_size"] cam_socket = get_socket_from_name(cam_name, self.cam_config.name_to_socket) ch.setCameraIntrinsics(cam_socket, K.tolist(), im_size) + if cam_name == "tof": + D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) * 0.01 + # D = np.array([0, 0, 0, 0]) + ch.setDistortionCoefficients(cam_socket, D.tolist()) - if self.cam_config.fisheye: + + if self.cam_config.fisheye and cam_name != "tof": self._logger.info("Setting camera type to fisheye ...") ch.setCameraType(cam_socket, dai.CameraModel.Fisheye) - if tof: - K = np.eye(3) - # Piffed, need to find the real values - K[0][0] = 471.8361511230469 # fx - K[1][1] = 471.7205810546875 # fy - K[0][2] = 322.25347900390625 # cx - K[1][2] = 246.209716796875 # cy - tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) - ch.setCameraIntrinsics(tof_socket, K.tolist(), (640, 480)) - self._logger.info("Setting extrinsics ...") left_socket = get_socket_from_name("left", self.cam_config.name_to_socket) right_socket = get_socket_from_name("right", self.cam_config.name_to_socket) @@ -310,8 +306,14 @@ def flash(self, calib_json_file: str, tof=False) -> None: print("FLASHING TOF EXTRINSICS") tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket) print("TOF SOCKET", tof_socket) + r = np.array(camera_poses["tof_to_left"]["R"]) + t = np.array(camera_poses["tof_to_left"]["T"]) T_tof_to_left = np.eye(4) - T_tof_to_left[:3, 3] = [-3.2509, 0, 0] + T_tof_to_left[:3, :3] = r + T_tof_to_left[:3, 3] = t + + T_tof_to_left[:3, 3] *= 100 # Needs to be in centimeters (?) # TODO test + T_left_to_tof = np.linalg.inv(T_tof_to_left) ch.setCameraExtrinsics( tof_socket, @@ -324,9 +326,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: ch.setCameraExtrinsics( left_socket, tof_socket, - T_left_to_tof[:3, :3].tolist(), - T_left_to_tof[:3, 3].tolist(), - specTranslation=T_left_to_tof[:3, 3].tolist(), + T_tof_to_left[:3, :3].tolist(), + T_tof_to_left[:3, 3].tolist(), + specTranslation=T_tof_to_left[:3, 3].tolist(), ) right_to_left = camera_poses["right_to_left"] @@ -336,14 +338,6 @@ def flash(self, calib_json_file: str, tof=False) -> None: R_left_to_right, T_left_to_right = get_inv_R_T(R_right_to_left, T_right_to_left) - # ch.setCameraExtrinsics( - # right_socket, - # left_socket, - # R_right_to_left, - # T_right_to_left, - # specTranslation=T_right_to_left, - # ) - ch.setCameraExtrinsics( left_socket, right_socket, @@ -351,37 +345,10 @@ def flash(self, calib_json_file: str, tof=False) -> None: T_left_to_right.tolist(), specTranslation=T_left_to_right.tolist(), ) - # if not tof: - # ch.setCameraExtrinsics( - # left_socket, - # right_socket, - # R_right_to_left.tolist(), - # T_right_to_left.tolist(), - # specTranslation=T_right_to_left.tolist(), - # ) ch.setStereoLeft(left_socket, np.eye(3).tolist()) ch.setStereoRight(right_socket, R_right_to_left.tolist()) - # print( - # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_C, dai.CameraBoardSocket.CAM_B) - # ) # extrinsics left camera <-> ToF OK - # print( - # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_C) - # ) # extrinsics left camera <-> ToF OK - # print( - # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_D) - # ) # extrinsics left camera <-> ToF OK - # print( - # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_D, dai.CameraBoardSocket.CAM_B) - # ) # extrinsics left camera <-> ToF OK - # print( - # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_C, dai.CameraBoardSocket.CAM_D) - # ) # extrinsics left camera <-> ToF OK - # print( - # ch.getCameraExtrinsics(dai.CameraBoardSocket.CAM_D, dai.CameraBoardSocket.CAM_C) - # ) # extrinsics left camera <-> ToF OK - ch.eepromToJsonFile("saved_calib.json") self._logger.info("Flashing ...") try: From 01f1901a95f44e052b2953e4453247c6d7aa6b47 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 24 Sep 2024 14:50:51 +0200 Subject: [PATCH 05/17] trying to calibrate the ToF's --- examples/camera_wrappers_examples/live_pcl.py | 2 +- .../camera_wrappers_examples/live_pcl_tof.py | 19 +++++++++---------- .../camera_wrappers/depthai/tof.py | 9 +++++---- .../camera_wrappers/depthai/wrapper.py | 4 +++- 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/examples/camera_wrappers_examples/live_pcl.py b/examples/camera_wrappers_examples/live_pcl.py index 2398254..fde3751 100644 --- a/examples/camera_wrappers_examples/live_pcl.py +++ b/examples/camera_wrappers_examples/live_pcl.py @@ -7,7 +7,7 @@ get_config_file_path, get_config_files_names, ) -from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer +from pollen_vision.utils.pcl_visualizer import PCLVisualizer valid_configs = get_config_files_names() diff --git a/examples/camera_wrappers_examples/live_pcl_tof.py b/examples/camera_wrappers_examples/live_pcl_tof.py index 8490cbc..716dfce 100644 --- a/examples/camera_wrappers_examples/live_pcl_tof.py +++ b/examples/camera_wrappers_examples/live_pcl_tof.py @@ -8,7 +8,7 @@ get_config_files_names, ) -from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer +from pollen_vision.utils.pcl_visualizer import PCLVisualizer valid_configs = get_config_files_names() @@ -22,11 +22,11 @@ ) args = argParser.parse_args() -w = TOFWrapper(get_config_file_path(args.config)) +w = TOFWrapper(get_config_file_path(args.config), crop=False) K = w.get_K() -# P = PCLVisualizer(K) -# P.add_frame("origin") +P = PCLVisualizer(K) +P.add_frame("origin") def colorizeDepth(frameDepth): @@ -74,20 +74,19 @@ def cv_callback(event, x, y, flags, param): depth = data["depth"] rgb = data["left"] - colorized_depth = colorizeDepth(depth) + # colorized_depth = colorizeDepth(depth) # disparity = data["disparity"] - # P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) - # P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) + P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) # disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) # disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET) # cv2.imshow("disparity", disparity) # cv2.imshow("left", data["depthNode_left"]) # cv2.imshow("right", data["depthNode_right"]) - cv2.imshow("depth", colorized_depth) - cv2.imshow("rgb", rgb) + # cv2.imshow("depth", colorized_depth) + # cv2.imshow("rgb", rgb) # print(depth[mouse_y, mouse_x]) key = cv2.waitKey(1) - # P.tick() + P.tick() diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 744d37f..5bb83ad 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -23,7 +23,7 @@ def __init__( fps, force_usb2=force_usb2, resize=None, - rectify=True, + rectify=False, exposure_params=None, mx_id=mx_id, # isp_scale=(2, 3), @@ -94,12 +94,12 @@ def _create_pipeline(self) -> dai.Pipeline: self.tofConfig = self.tof.initialConfig.get() self.tofConfig.enableFPPNCorrection = True self.tofConfig.enableOpticalCorrection = False # True is ok ? - self.tofConfig.enablePhaseShuffleTemporalFilter = True + self.tofConfig.enablePhaseShuffleTemporalFilter = False self.tofConfig.phaseUnwrappingLevel = 1 self.tofConfig.phaseUnwrapErrorThreshold = 300 self.tofConfig.enableTemperatureCorrection = False # Not yet supported self.tofConfig.enableWiggleCorrection = False - self.tofConfig.median = dai.MedianFilter.KERNEL_7x7 + self.tofConfig.median = dai.MedianFilter.KERNEL_3x3 self.tof.initialConfig.set(self.tofConfig) # ========================== @@ -209,9 +209,10 @@ def colorizeDepth(frameDepth): # cv2.imshow("right", right) # print(data["depth"][mouse_y, mouse_x]) # depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) - cv2.imshow("depth", depth) + # cv2.imshow("depth", depth) # cv2.imshow("tof_amplitude", tof_amplitude) colorized_depth = colorizeDepth(data["depth"]) blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0) cv2.imshow("blended", blended) + cv2.imshow("colorized_depth", colorized_depth) cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index 93ac1bf..3b469fb 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -287,7 +287,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: ch.setCameraIntrinsics(cam_socket, K.tolist(), im_size) if cam_name == "tof": - D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) * 0.01 + D = ( + np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) * 0.01 + ) # Values given by luxonis. But not sure if we should use that, and why scale by 0.01 ? # D = np.array([0, 0, 0, 0]) ch.setDistortionCoefficients(cam_socket, D.tolist()) From 7d482291c3dfaecd1a0d500ee8562eaed89ee992 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 8 Oct 2024 15:01:11 +0200 Subject: [PATCH 06/17] update --- .../live_pcl_orbbec.py | 2 +- .../camera_wrappers_examples/live_pcl_tof.py | 2 +- .../depthai/calibration/acquire.py | 12 +- .../depthai/calibration/check_epilines.py | 16 +- .../camera_wrappers/depthai/tof.py | 45 ++--- .../camera_wrappers/depthai/wrapper.py | 22 ++- .../camera_wrappers/orbbec/orbbec_wrapper.py | 34 ++-- scripts/tof_point_cloud_luxonis.py | 160 ++++++++++++++++++ 8 files changed, 239 insertions(+), 54 deletions(-) create mode 100644 scripts/tof_point_cloud_luxonis.py diff --git a/examples/camera_wrappers_examples/live_pcl_orbbec.py b/examples/camera_wrappers_examples/live_pcl_orbbec.py index afab9c2..405eee0 100644 --- a/examples/camera_wrappers_examples/live_pcl_orbbec.py +++ b/examples/camera_wrappers_examples/live_pcl_orbbec.py @@ -1,6 +1,6 @@ import cv2 from pollen_vision.camera_wrappers.orbbec.orbbec_wrapper import OrbbecWrapper -from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer +from pollen_vision.utils.pcl_visualizer import PCLVisualizer w = OrbbecWrapper() diff --git a/examples/camera_wrappers_examples/live_pcl_tof.py b/examples/camera_wrappers_examples/live_pcl_tof.py index 716dfce..83064fc 100644 --- a/examples/camera_wrappers_examples/live_pcl_tof.py +++ b/examples/camera_wrappers_examples/live_pcl_tof.py @@ -22,7 +22,7 @@ ) args = argParser.parse_args() -w = TOFWrapper(get_config_file_path(args.config), crop=False) +w = TOFWrapper(get_config_file_path(args.config), crop=True, fps=10) K = w.get_K() P = PCLVisualizer(K) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py index 74cf92e..3ef3964 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py @@ -30,7 +30,7 @@ if not args.tof: w = SDKWrapper(get_config_file_path(args.config), compute_depth=False, rectify=False) else: - w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30) + w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, noalign=True) left_path = os.path.join(args.imagesPath, "left") @@ -53,10 +53,10 @@ _data[name] = data[name] if args.tof: - tof_amplitude = _data["tof_amplitude"] - tof_amplitude = cv2.resize(tof_amplitude, _data["left"].shape[:2][::-1]) - tof_amplitude = np.dstack((tof_amplitude, tof_amplitude, tof_amplitude)) - concat = np.hstack((_data["left"], _data["right"], tof_amplitude)) + tof_intensity = _data["tof_intensity"] + tof_intensity = cv2.resize(tof_intensity, _data["left"].shape[:2][::-1]) + tof_intensity = np.dstack((tof_intensity, tof_intensity, tof_intensity)) + concat = np.hstack((_data["left"], _data["right"], tof_intensity)) else: concat = np.hstack((_data["left"], _data["right"])) cv2.imshow("concat", cv2.resize(concat, (0, 0), fx=0.5, fy=0.5)) @@ -65,7 +65,7 @@ cv2.imwrite(os.path.join(left_path, str(i) + ".png"), data["left"]) cv2.imwrite(os.path.join(right_path, str(i) + ".png"), data["right"]) if args.tof: - cv2.imwrite(os.path.join(tof_path, str(i) + ".png"), data["tof_amplitude"]) + cv2.imwrite(os.path.join(tof_path, str(i) + ".png"), data["tof_intensity"]) print("Saved image pair ", i, "to", left_path, " and ", right_path) i += 1 elif key == 27 or key == ord("q"): diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py index 54ebd67..f2bed20 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py @@ -1,9 +1,10 @@ import argparse +import numpy as np import logging import cv2 from cv2 import aruco -from pollen_vision.camera_wrappers.depthai import SDKWrapper +from pollen_vision.camera_wrappers.depthai import SDKWrapper, TOFWrapper from pollen_vision.camera_wrappers.depthai.utils import ( drawEpiLines, get_config_file_path, @@ -21,11 +22,15 @@ choices=valid_configs, help=f"Configutation file name : {valid_configs}", ) +argParser.add_argument("--tof", action="store_true", help="Has tof sensor ?") args = argParser.parse_args() ARUCO_DICT = aruco.getPredefinedDictionary(aruco.DICT_4X4_1000) -w = SDKWrapper(get_config_file_path(args.config), rectify=True, resize=(1280, 720), fps=60) +if not args.tof: + w = SDKWrapper(get_config_file_path(args.config), rectify=True, resize=(1280, 720), fps=60) +else: + w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, rectify=True) while True: @@ -37,6 +42,13 @@ epi = drawEpiLines(_data["left"], _data["right"], ARUCO_DICT) epi = cv2.resize(epi, (0, 0), fx=0.5, fy=0.5) cv2.imshow("epi", epi) + + if args.tof: + right_resized = cv2.resize(_data["right"], _data["tof_intensity"].shape[:2][::-1]) + tof_im = _data["tof_intensity"] + tof_im = np.dstack((tof_im, tof_im, tof_im)) + epi_right_tof = drawEpiLines(right_resized, tof_im, ARUCO_DICT) + cv2.imshow("epi_right_tof", epi_right_tof) key = cv2.waitKey(1) if key == 27 or key == ord("q"): diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 5bb83ad..280460f 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -16,14 +16,22 @@ class TOFWrapper(DepthaiWrapper): def __init__( - self, cam_config_json: str, fps: int = 30, force_usb2: bool = False, mx_id: str = "", crop: bool = False + self, + cam_config_json: str, + fps: int = 30, + force_usb2: bool = False, + mx_id: str = "", + crop: bool = False, + noalign=False, + rectify=False, ) -> None: + self.noalign = noalign super().__init__( cam_config_json, fps, force_usb2=force_usb2, resize=None, - rectify=False, + rectify=rectify, exposure_params=None, mx_id=mx_id, # isp_scale=(2, 3), @@ -61,7 +69,7 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], left = messageGroup["left"].getCvFrame() right = messageGroup["right"].getCvFrame() depth = messageGroup["depth_aligned"].getFrame() - tof_amplitude = messageGroup["tof_amplitude"].getCvFrame() + tof_intensity = messageGroup["tof_intensity"].getCvFrame() # Temporary, not ideal @@ -74,7 +82,7 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], data["depth"] = depth data["right"] = right - data["tof_amplitude"] = tof_amplitude + data["tof_intensity"] = tof_intensity return data, latency, ts def get_K(self) -> npt.NDArray[np.float32]: @@ -93,13 +101,13 @@ def _create_pipeline(self) -> dai.Pipeline: # === Tof configuration === self.tofConfig = self.tof.initialConfig.get() self.tofConfig.enableFPPNCorrection = True - self.tofConfig.enableOpticalCorrection = False # True is ok ? - self.tofConfig.enablePhaseShuffleTemporalFilter = False + self.tofConfig.enableOpticalCorrection = True # True is ok ? + self.tofConfig.enablePhaseShuffleTemporalFilter = True self.tofConfig.phaseUnwrappingLevel = 1 self.tofConfig.phaseUnwrapErrorThreshold = 300 self.tofConfig.enableTemperatureCorrection = False # Not yet supported self.tofConfig.enableWiggleCorrection = False - self.tofConfig.median = dai.MedianFilter.KERNEL_3x3 + self.tofConfig.median = dai.MedianFilter.KERNEL_7x7 self.tof.initialConfig.set(self.tofConfig) # ========================== @@ -123,7 +131,7 @@ def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: self.left.isp.link(self.left_manip.inputImage) self.right.isp.link(self.right_manip.inputImage) - self.tof.intensity.link(self.sync.inputs["tof_amplitude"]) + self.tof.intensity.link(self.sync.inputs["tof_intensity"]) self.left_manip.out.link(self.sync.inputs["left"]) self.right_manip.out.link(self.sync.inputs["right"]) @@ -132,15 +140,12 @@ def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: self.cam_tof.raw.link(self.tof.input) - # Align depth on RGB WORKS - self.left_manip.out.link(self.align.inputAlignTo) - self.tof.depth.link(self.align.input) - - # Align RGB on depth - # self.left_manip.out.link(self.align.input) - # self.tof.depth.link(self.align.inputAlignTo) - - self.align.outputAligned.link(self.sync.inputs["depth_aligned"]) + if not self.noalign: + self.left_manip.out.link(self.align.inputAlignTo) + self.tof.depth.link(self.align.input) + self.align.outputAligned.link(self.sync.inputs["depth_aligned"]) + else: + self.tof.depth.link(self.sync.inputs["depth_aligned"]) self.sync.out.link(self.xout_sync.input) @@ -192,7 +197,7 @@ def colorizeDepth(frameDepth): if __name__ == "__main__": - t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False) + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=1, crop=False, rectify=False) # cv2.namedWindow("depth") # cv2.setMouseCallback("depth", cv_callback) @@ -202,7 +207,7 @@ def colorizeDepth(frameDepth): left = data["left"] # right = data["right"] depth = data["depth"] - # tof_amplitude = data["tof_amplitude"] + # tof_intensity = data["tof_intensity"] # # left = cv2.resize(left, (640, 480)) # # right = cv2.resize(right, (640, 480)) cv2.imshow("left", left) @@ -210,7 +215,7 @@ def colorizeDepth(frameDepth): # print(data["depth"][mouse_y, mouse_x]) # depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) # cv2.imshow("depth", depth) - # cv2.imshow("tof_amplitude", tof_amplitude) + # cv2.imshow("tof_intensity", tof_intensity) colorized_depth = colorizeDepth(data["depth"]) blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0) cv2.imshow("blended", blended) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index 3b469fb..c699174 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -285,18 +285,24 @@ def flash(self, calib_json_file: str, tof=False) -> None: im_size = params["image_size"] cam_socket = get_socket_from_name(cam_name, self.cam_config.name_to_socket) + if cam_name == "tof": + K = np.array( + [[471.8361511230469, 0.0, 322.25347900390625], [0.0, 471.7205810546875, 246.209716796875], [0.0, 0.0, 1.0]] + ) + ch.setCameraIntrinsics(cam_socket, K.tolist(), im_size) + if cam_name == "tof": - D = ( - np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) * 0.01 - ) # Values given by luxonis. But not sure if we should use that, and why scale by 0.01 ? - # D = np.array([0, 0, 0, 0]) + D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) + D *= 0.01 ch.setDistortionCoefficients(cam_socket, D.tolist()) if self.cam_config.fisheye and cam_name != "tof": self._logger.info("Setting camera type to fisheye ...") ch.setCameraType(cam_socket, dai.CameraModel.Fisheye) + # else: + # ch.setCameraType(cam_socket, dai.CameraModel.Equirectangular) self._logger.info("Setting extrinsics ...") left_socket = get_socket_from_name("left", self.cam_config.name_to_socket) @@ -315,7 +321,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: T_tof_to_left[:3, :3] = r T_tof_to_left[:3, 3] = t - T_tof_to_left[:3, 3] *= 100 # Needs to be in centimeters (?) # TODO test + T_tof_to_left[ + :3, 3 + ] *= 100 # Needs to be in centimeters https://docs.luxonis.com/software/api/python/#depthai.CalibrationHandler.getCameraExtrinsics T_left_to_tof = np.linalg.inv(T_tof_to_left) ch.setCameraExtrinsics( @@ -337,7 +345,7 @@ def flash(self, calib_json_file: str, tof=False) -> None: right_to_left = camera_poses["right_to_left"] R_right_to_left = np.array(right_to_left["R"]) T_right_to_left = np.array(right_to_left["T"]) - T_right_to_left *= 100 # Needs to be in centimeters (?) # TODO test + T_right_to_left *= 100 # Needs to be in centimeters https://docs.luxonis.com/software/api/python/#depthai.CalibrationHandler.getCameraExtrinsics R_left_to_right, T_left_to_right = get_inv_R_T(R_right_to_left, T_right_to_left) @@ -352,7 +360,7 @@ def flash(self, calib_json_file: str, tof=False) -> None: ch.setStereoLeft(left_socket, np.eye(3).tolist()) ch.setStereoRight(right_socket, R_right_to_left.tolist()) - ch.eepromToJsonFile("saved_calib.json") + # ch.eepromToJsonFile("saved_calib.json") self._logger.info("Flashing ...") try: self._device.flashCalibration2(ch) diff --git a/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py index 6920c51..5d2b6c3 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py @@ -161,20 +161,20 @@ def get_K(self) -> npt.NDArray[np.float32]: # mouse_x, mouse_y = x, y -# if __name__ == "__main__": -# o = OrbbecWrapper() - -# cv2.namedWindow("depth") -# cv2.setMouseCallback("depth", cv2_callback) - -# while True: -# data, _, _ = o.get_data() -# if "depth" in data: -# cv2.imshow("depth", data["depth"]) -# depth_value = data["depth"][mouse_y, mouse_x] -# print(depth_value) -# if "left" in data: -# cv2.imshow("left", data["left"]) - -# cv2.waitKey(1) -# time.sleep(0.01) +if __name__ == "__main__": + o = OrbbecWrapper() + + # cv2.namedWindow("depth") + # cv2.setMouseCallback("depth", cv2_callback) + + while True: + data, _, _ = o.get_data() + # if "depth" in data: + # cv2.imshow("depth", data["depth"]) + # depth_value = data["depth"][mouse_y, mouse_x] + # print(depth_value) + if "left" in data: + cv2.imshow("left", data["left"]) + + cv2.waitKey(1) + # time.sleep(0.01) diff --git a/scripts/tof_point_cloud_luxonis.py b/scripts/tof_point_cloud_luxonis.py new file mode 100644 index 0000000..76601db --- /dev/null +++ b/scripts/tof_point_cloud_luxonis.py @@ -0,0 +1,160 @@ +import depthai as dai +import numpy as np +import cv2 +import time +from datetime import timedelta +import datetime +import os +import sys + +try: + import open3d as o3d +except ImportError: + sys.exit( + "Critical dependency missing: Open3D. Please install it using the command: '{} -m pip install open3d' and then rerun the script.".format( + sys.executable + ) + ) + +FPS = 30 + +RGB_SOCKET = dai.CameraBoardSocket.CAM_C +TOF_SOCKET = dai.CameraBoardSocket.CAM_D +ALIGN_SOCKET = RGB_SOCKET + +pipeline = dai.Pipeline() + +# Define sources and outputs +camRgb = pipeline.create(dai.node.ColorCamera) +tof = pipeline.create(dai.node.ToF) +camTof = pipeline.create(dai.node.Camera) +sync = pipeline.create(dai.node.Sync) +align = pipeline.create(dai.node.ImageAlign) +out = pipeline.create(dai.node.XLinkOut) +pointcloud = pipeline.create(dai.node.PointCloud) + +# ToF settings +camTof.setFps(FPS) +camTof.setBoardSocket(TOF_SOCKET) + +tofConfig = tof.initialConfig.get() +# tofConfig.enableOpticalCorrection = False +tofConfig.phaseUnwrappingLevel = 4 +# choose a median filter or use none - using the median filter improves the pointcloud but causes discretization of the data +# tofConfig.median = dai.MedianFilter.MEDIAN_OFF +tofConfig.median = dai.MedianFilter.KERNEL_3x3 +# tofConfig.median = dai.MedianFilter.KERNEL_5x5 +# tofConfig.median = dai.MedianFilter.KERNEL_7x7 +tof.initialConfig.set(tofConfig) + +# rgb settings +camRgb.setBoardSocket(RGB_SOCKET) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) +camRgb.setFps(FPS) +camRgb.setIspScale(1, 2) + +out.setStreamName("out") + +sync.setSyncThreshold(timedelta(seconds=(1 / FPS))) + +# Linking +camRgb.isp.link(sync.inputs["rgb"]) +camTof.raw.link(tof.input) +tof.depth.link(align.input) +align.outputAligned.link(sync.inputs["depth_aligned"]) +align.outputAligned.link(pointcloud.inputDepth) +sync.inputs["rgb"].setBlocking(False) +camRgb.isp.link(align.inputAlignTo) +pointcloud.outputPointCloud.link(sync.inputs["pcl"]) +sync.out.link(out.input) +out.setStreamName("out") + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +with dai.Device(pipeline) as device: + isRunning = True + + q = device.getOutputQueue(name="out", maxSize=4, blocking=False) + vis = o3d.visualization.VisualizerWithKeyCallback() + vis.create_window() + pcd = o3d.geometry.PointCloud() + coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0, 0, 0]) + # vis.add_geometry(coordinateFrame) + + first = True + + # Create a ViewControl object + view_control = vis.get_view_control() + + while isRunning: + inMessage = q.get() + inColor = inMessage["rgb"] + inPointCloud = inMessage["pcl"] + inDepth = inMessage["depth_aligned"] + + cvColorFrame = inColor.getCvFrame() + # Convert the frame to RGB + cvRGBFrame = cv2.cvtColor(cvColorFrame, cv2.COLOR_BGR2RGB) + cv2.imshow("color", cvColorFrame) + colorized_depth = colorizeDepth(inDepth.getCvFrame()) + cv2.imshow("depth", colorized_depth) + blended = cv2.addWeighted(cvColorFrame, 0.5, colorized_depth, 0.5, 0) + cv2.imshow("blended", blended) + + key = cv2.waitKey(1) + if key == ord("q"): + break + if key == ord("c"): + print("saving...") + current_time = datetime.datetime.now() + formatted_time = current_time.strftime("%Y_%m_%d_%H_%M_%S") + new_output = formatted_time + os.mkdir(new_output) + o3d.io.write_point_cloud(os.path.join(new_output, "tof_pointcloud.ply"), pcd) + cv2.imwrite(os.path.join(new_output, "rgb.png"), cvColorFrame) + print(f"RGB point cloud saved to folder {new_output}") + if inPointCloud: + points = inPointCloud.getPoints().astype(np.float64) + + points[:, 0] = -points[:, 0] # Invert X axis + # points[:, 1] = -points[:, 1] # Invert Y axis + # points[:, 2] = points[:, 2] / 1000.0 # Convert Z axis from mm to meters (if needed) + + pcd.points = o3d.utility.Vector3dVector(points) + colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64) + pcd.colors = o3d.utility.Vector3dVector(colors) + if first: + vis.add_geometry(pcd) + first = False + else: + vis.update_geometry(pcd) + vis.poll_events() + vis.update_renderer() + vis.destroy_window() From da2ea3350473835fb6610d15e96de59dde86cf8f Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 15 Oct 2024 13:27:07 +0200 Subject: [PATCH 07/17] update --- .../depthai/calibration/acquire.py | 2 +- .../depthai/calibration/testtt.py | 32 +++ .../camera_wrappers/depthai/wrapper.py | 8 +- scripts/align_example_corrected.py | 204 ++++++++++++++++++ 4 files changed, 241 insertions(+), 5 deletions(-) create mode 100644 pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py create mode 100644 scripts/align_example_corrected.py diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py index 3ef3964..15dade1 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py @@ -30,7 +30,7 @@ if not args.tof: w = SDKWrapper(get_config_file_path(args.config), compute_depth=False, rectify=False) else: - w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, noalign=True) + w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, noalign=True, rectify=False) left_path = os.path.join(args.imagesPath, "left") diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py new file mode 100644 index 0000000..cbfdfe8 --- /dev/null +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py @@ -0,0 +1,32 @@ +import depthai as dai +import cv2 + +print(dai.__version__) + +pipeline = dai.Pipeline() + +cam_tof = pipeline.create(dai.node.Camera) +cam_tof.setFps(30) + +cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) + +tof = pipeline.create(dai.node.ToF) + +tofConfig = tof.initialConfig.get() +tofConfig.enableDistortionCorrection = False +tof.initialConfig.set(tofConfig) +cam_tof.raw.link(tof.input) + +output = pipeline.create(dai.node.XLinkOut) +output.setStreamName("intensity") + +tof.intensity.link(output.input) + +device = dai.Device(pipeline) + +q = device.getOutputQueue(name="intensity", maxSize=8, blocking=False) + +while True: + data = q.get() + cv2.imshow("intensity", data.getCvFrame()) + cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index c699174..602778d 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -294,7 +294,7 @@ def flash(self, calib_json_file: str, tof=False) -> None: if cam_name == "tof": D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) - D *= 0.01 + # D *= 0.01 ch.setDistortionCoefficients(cam_socket, D.tolist()) @@ -337,9 +337,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: ch.setCameraExtrinsics( left_socket, tof_socket, - T_tof_to_left[:3, :3].tolist(), - T_tof_to_left[:3, 3].tolist(), - specTranslation=T_tof_to_left[:3, 3].tolist(), + T_left_to_tof[:3, :3].tolist(), + T_left_to_tof[:3, 3].tolist(), + specTranslation=T_left_to_tof[:3, 3].tolist(), ) right_to_left = camera_poses["right_to_left"] diff --git a/scripts/align_example_corrected.py b/scripts/align_example_corrected.py new file mode 100644 index 0000000..3bdeb00 --- /dev/null +++ b/scripts/align_example_corrected.py @@ -0,0 +1,204 @@ +import numpy as np +import cv2 +import depthai as dai +import time +from datetime import timedelta + +# This example is intended to run unchanged on an OAK-D-SR-PoE camera +FPS = 30.0 + +RGB_SOCKET = dai.CameraBoardSocket.CAM_C +TOF_SOCKET = dai.CameraBoardSocket.CAM_D +ALIGN_SOCKET = RGB_SOCKET + + +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + + +pipeline = dai.Pipeline() +# Define sources and outputs +camRgb = pipeline.create(dai.node.ColorCamera) +tof = pipeline.create(dai.node.ToF) +camTof = pipeline.create(dai.node.Camera) +sync = pipeline.create(dai.node.Sync) +align = pipeline.create(dai.node.ImageAlign) +out = pipeline.create(dai.node.XLinkOut) + +# ToF settings +camTof.setFps(FPS) +# camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) +camTof.setBoardSocket(TOF_SOCKET) + +# rgb settings +camRgb.setBoardSocket(RGB_SOCKET) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) +camRgb.setFps(FPS) +camRgb.setIspScale(1, 2) + +# depthSize = (1280, 800) # PLEASE SET TO BE SIZE OF THE TOF STREAM +depthSize = (640, 480) # PLEASE SET TO BE SIZE OF THE TOF STREAM +rgbSize = camRgb.getIspSize() + +out.setStreamName("out") + +sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) +rgbSize = camRgb.getIspSize() + +# Linking +camRgb.isp.link(sync.inputs["rgb"]) +camTof.raw.link(tof.input) +tof.depth.link(align.input) +align.outputAligned.link(sync.inputs["depth_aligned"]) +sync.inputs["rgb"].setBlocking(False) +camRgb.isp.link(align.inputAlignTo) +sync.out.link(out.input) + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor + + +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb): + """ + Update the rgb and depth weights used to blend depth/rgb image + + @param[in] percent_rgb The rgb weight expressed as a percentage (0..100) + """ + global depthWeight + global rgbWeight + rgbWeight = float(percentRgb) / 100.0 + depthWeight = 1.0 - rgbWeight + + +# Connect to device and start pipeline +remapping = True +with dai.Device(pipeline) as device: + queue = device.getOutputQueue("out", 8, False) + + # Configure windows; trackbar adjusts blending ratio of rgb/depth + rgbDepthWindowName = "rgb-depth" + + cv2.namedWindow(rgbDepthWindowName) + cv2.createTrackbar( + "RGB Weight %", + rgbDepthWindowName, + int(rgbWeight * 100), + 100, + updateBlendWeights, + ) + try: + calibData = device.readCalibration2() + M1 = np.array(calibData.getCameraIntrinsics(ALIGN_SOCKET, *depthSize)) + D1 = np.array(calibData.getDistortionCoefficients(ALIGN_SOCKET)) + M2 = np.array(calibData.getCameraIntrinsics(RGB_SOCKET, *rgbSize)) + D2 = np.array(calibData.getDistortionCoefficients(RGB_SOCKET)) + + # try: + # T = np.array(calibData.getCameraTranslationVector(ALIGN_SOCKET, RGB_SOCKET, False)) * 10 # to mm for matching the depth + + # T = np.array(calibData.getCameraExtrinsics(ALIGN_SOCKET, RGB_SOCKET, False)) + # print(T) + # exit() + # except RuntimeError: + # print("caca") + # T = np.array([0.0, 0.0, 0.001]) + # try: + R = np.array(calibData.getCameraExtrinsics(ALIGN_SOCKET, TOF_SOCKET, False))[0:3, 0:3] + # except RuntimeError: + # print("caca") + # R = np.eye(3) + TARGET_MATRIX = M1 + + lensPosition = calibData.getLensPosition(RGB_SOCKET) + except: + raise + fpsCounter = FPSCounter() + while True: + messageGroup = queue.get() + fpsCounter.tick() + assert isinstance(messageGroup, dai.MessageGroup) + frameRgb = messageGroup["rgb"] + assert isinstance(frameRgb, dai.ImgFrame) + frameDepth = messageGroup["depth_aligned"] + assert isinstance(frameDepth, dai.ImgFrame) + + sizeRgb = frameRgb.getData().size + sizeDepth = frameDepth.getData().size + # Blend when both received + if frameDepth is not None: + rgbFrame = frameRgb.getCvFrame() + # Colorize the aligned depth + alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) + # Resize depth to match the rgb frame + cv2.putText( + alignedDepthColorized, + f"FPS: {fpsCounter.getFps():.2f}", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 2, + ) + cv2.imshow("depth", alignedDepthColorized) + key = cv2.waitKey(1) + if key == ord("m"): + if remapping: + print("Remap turned OFF.") + remapping = False + else: + print("Remap turned ON.") + remapping = True + + if remapping: + # mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, None, M2, rgbSize, cv2.CV_32FC1) + mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, R, M2, rgbSize, cv2.CV_32FC1) + rgbFrame = cv2.remap(rgbFrame, mapX, mapY, cv2.INTER_LINEAR) + + alignedDepthColorized = cv2.resize(alignedDepthColorized, (rgbFrame.shape[1], rgbFrame.shape[0])) + blended = cv2.addWeighted(rgbFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) + cv2.imshow(rgbDepthWindowName, blended) + + key = cv2.waitKey(1) + if key == ord("q"): + break From 0dd4741de545eaef98f95cbef9d6fe3dd7cacdac Mon Sep 17 00:00:00 2001 From: apirrone Date: Fri, 18 Oct 2024 11:04:02 +0200 Subject: [PATCH 08/17] handle tof distortion correction --- .../camera_wrappers/depthai/calibration/.gitignore | 2 ++ .../pollen_vision/camera_wrappers/depthai/tof.py | 1 + .../camera_wrappers/depthai/wrapper.py | 14 +++++++------- scripts/align_example_corrected.py | 2 +- scripts/tof_point_cloud_luxonis.py | 1 + 5 files changed, 12 insertions(+), 8 deletions(-) create mode 100644 pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/.gitignore diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/.gitignore b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/.gitignore new file mode 100644 index 0000000..99fa8ea --- /dev/null +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/.gitignore @@ -0,0 +1,2 @@ +*.json +calib_images/* diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 280460f..f5b8619 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -108,6 +108,7 @@ def _create_pipeline(self) -> dai.Pipeline: self.tofConfig.enableTemperatureCorrection = False # Not yet supported self.tofConfig.enableWiggleCorrection = False self.tofConfig.median = dai.MedianFilter.KERNEL_7x7 + self.tofConfig.enableDistortionCorrection = True if self.cam_config.rectify else False self.tof.initialConfig.set(self.tofConfig) # ========================== diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index 602778d..651b36f 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -285,16 +285,16 @@ def flash(self, calib_json_file: str, tof=False) -> None: im_size = params["image_size"] cam_socket = get_socket_from_name(cam_name, self.cam_config.name_to_socket) - if cam_name == "tof": - K = np.array( - [[471.8361511230469, 0.0, 322.25347900390625], [0.0, 471.7205810546875, 246.209716796875], [0.0, 0.0, 1.0]] - ) + # if cam_name == "tof": + # K = np.array( + # [[471.8361511230469, 0.0, 322.25347900390625], [0.0, 471.7205810546875, 246.209716796875], [0.0, 0.0, 1.0]] + # ) ch.setCameraIntrinsics(cam_socket, K.tolist(), im_size) - if cam_name == "tof": - D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) - # D *= 0.01 + # if cam_name == "tof": + # # D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) + # D *= 0.01 # still don't know if I should do that or not ch.setDistortionCoefficients(cam_socket, D.tolist()) diff --git a/scripts/align_example_corrected.py b/scripts/align_example_corrected.py index 3bdeb00..7f4ad1f 100644 --- a/scripts/align_example_corrected.py +++ b/scripts/align_example_corrected.py @@ -194,7 +194,7 @@ def updateBlendWeights(percentRgb): # mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, None, M2, rgbSize, cv2.CV_32FC1) mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, R, M2, rgbSize, cv2.CV_32FC1) rgbFrame = cv2.remap(rgbFrame, mapX, mapY, cv2.INTER_LINEAR) - + print(rgbFrame.shape) alignedDepthColorized = cv2.resize(alignedDepthColorized, (rgbFrame.shape[1], rgbFrame.shape[0])) blended = cv2.addWeighted(rgbFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) cv2.imshow(rgbDepthWindowName, blended) diff --git a/scripts/tof_point_cloud_luxonis.py b/scripts/tof_point_cloud_luxonis.py index 76601db..061b542 100644 --- a/scripts/tof_point_cloud_luxonis.py +++ b/scripts/tof_point_cloud_luxonis.py @@ -45,6 +45,7 @@ tofConfig.median = dai.MedianFilter.KERNEL_3x3 # tofConfig.median = dai.MedianFilter.KERNEL_5x5 # tofConfig.median = dai.MedianFilter.KERNEL_7x7 +# tofConfig.enableDistortionCorrection = False tof.initialConfig.set(tofConfig) # rgb settings From 50b07c99b5a5960f3f29b01894fde4e23c8664c8 Mon Sep 17 00:00:00 2001 From: apirrone Date: Fri, 18 Oct 2024 11:50:28 +0200 Subject: [PATCH 09/17] cleaning up --- .../camera_wrappers_examples/tof_example.py | 60 +---- .../camera_wrappers/depthai/tof.py | 56 +---- .../camera_wrappers/depthai/utils.py | 28 +++ scripts/align_example2.py | 169 -------------- scripts/align_rgb_on_depth_test.py | 60 ----- scripts/aze.py | 159 -------------- scripts/minimal_tof_test.py | 93 -------- scripts/test.py | 7 - scripts/tof_and_encoding_test.py | 206 ------------------ scripts/tof_pcl_test.py | 66 ------ scripts/tof_test.py | 135 ------------ scripts/tof_test_align.py | 27 --- 12 files changed, 42 insertions(+), 1024 deletions(-) delete mode 100644 scripts/align_example2.py delete mode 100644 scripts/align_rgb_on_depth_test.py delete mode 100644 scripts/aze.py delete mode 100644 scripts/minimal_tof_test.py delete mode 100644 scripts/test.py delete mode 100644 scripts/tof_and_encoding_test.py delete mode 100644 scripts/tof_pcl_test.py delete mode 100644 scripts/tof_test.py delete mode 100644 scripts/tof_test_align.py diff --git a/examples/camera_wrappers_examples/tof_example.py b/examples/camera_wrappers_examples/tof_example.py index 7fcd37d..970c7ad 100644 --- a/examples/camera_wrappers_examples/tof_example.py +++ b/examples/camera_wrappers_examples/tof_example.py @@ -2,67 +2,23 @@ import numpy as np import depthai as dai from pollen_vision.camera_wrappers import TOFWrapper -from pollen_vision.camera_wrappers.depthai.utils import get_config_file_path +from pollen_vision.camera_wrappers.depthai.utils import get_config_file_path, colorizeDepth -t = TOFWrapper(get_config_file_path("CONFIG_AR0234_TOF"), fps=60) +t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False) - -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - -mouse_x, mouse_y = 0, 0 - - -def cv_callback(event, x, y, flags, param): - global mouse_x, mouse_y - mouse_x, mouse_y = x, y - - -# P = PCLVisualizer(t.get_K()) -cv2.namedWindow("depth") -cv2.setMouseCallback("depth", cv_callback) print(dai.__version__) while True: data, _, _ = t.get_data() left = data["left"] - # right = data["right"] - # left = cv2.resize(left, (640, 480)) - # right = cv2.resize(right, (640, 480)) + depth = data["depth"] - colorized_depth = colorizeDepth(depth) + tof_intensity = data["tof_intensity"] + cv2.imshow("left", left) - # cv2.imshow("right", right) - print(data["depth"][mouse_y, mouse_x]) - # colorized_depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) + cv2.imshow("tof intensity", tof_intensity) + colorized_depth = colorizeDepth(data["depth"]) blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0) cv2.imshow("blended", blended) - cv2.imshow("depth", depth) - # P.update(cv2.cvtColor(left, cv2.COLOR_BGR2RGB), depth) - # P.tick() + cv2.imshow("colorized_depth", colorized_depth) cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index f5b8619..0682426 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -10,8 +10,7 @@ get_socket_from_name, ) from pollen_vision.camera_wrappers.depthai.wrapper import DepthaiWrapper - -# from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer +from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth class TOFWrapper(DepthaiWrapper): @@ -161,62 +160,19 @@ def _create_queues(self) -> Dict[str, dai.DataOutputQueue]: return queues -mouse_x, mouse_y = 0, 0 - - -def cv_callback(event, x, y, flags, param): - global mouse_x, mouse_y - mouse_x, mouse_y = x, y - - -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - if __name__ == "__main__": - t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=1, crop=False, rectify=False) + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False, rectify=False) - # cv2.namedWindow("depth") - # cv2.setMouseCallback("depth", cv_callback) print(dai.__version__) while True: data, _, _ = t.get_data() left = data["left"] - # right = data["right"] + depth = data["depth"] - # tof_intensity = data["tof_intensity"] - # # left = cv2.resize(left, (640, 480)) - # # right = cv2.resize(right, (640, 480)) + tof_intensity = data["tof_intensity"] + cv2.imshow("left", left) - # cv2.imshow("right", right) - # print(data["depth"][mouse_y, mouse_x]) - # depth = cv2.circle(depth, (mouse_x, mouse_y), 5, (0, 255, 0), -1) - # cv2.imshow("depth", depth) - # cv2.imshow("tof_intensity", tof_intensity) + cv2.imshow("tof intensity", tof_intensity) colorized_depth = colorizeDepth(data["depth"]) blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0) cv2.imshow("blended", blended) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py index abef1ce..c588f31 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py @@ -129,3 +129,31 @@ def get_config_file_path(name: str) -> Any: if file.stem == name: return file.resolve() return None + + +def colorizeDepth(frameDepth): + invalidMask = frameDepth == 0 + # Log the depth, minDepth and maxDepth + try: + minDepth = np.percentile(frameDepth[frameDepth != 0], 3) + maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) + logDepth = np.log(frameDepth, where=frameDepth != 0) + logMinDepth = np.log(minDepth) + logMaxDepth = np.log(maxDepth) + np.nan_to_num(logDepth, copy=False, nan=logMinDepth) + # Clip the values to be in the 0-255 range + logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) + + # Interpolate only valid logDepth values, setting the rest based on the mask + depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) + depthFrameColor = np.nan_to_num(depthFrameColor) + depthFrameColor = depthFrameColor.astype(np.uint8) + depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) + # Set invalid depth pixels to black + depthFrameColor[invalidMask] = 0 + except IndexError: + # Frame is likely empty + depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) + except Exception as e: + raise e + return depthFrameColor diff --git a/scripts/align_example2.py b/scripts/align_example2.py deleted file mode 100644 index bc0d034..0000000 --- a/scripts/align_example2.py +++ /dev/null @@ -1,169 +0,0 @@ -import numpy as np -import cv2 -import depthai as dai -import time -from datetime import timedelta - -# This example is intended to run unchanged on an OAK-D-SR-PoE camera -FPS = 30.0 - -RGB_SOCKET = dai.CameraBoardSocket.CAM_C -TOF_SOCKET = dai.CameraBoardSocket.CAM_D -ALIGN_SOCKET = RGB_SOCKET - - -class FPSCounter: - def __init__(self): - self.frameTimes = [] - - def tick(self): - now = time.time() - self.frameTimes.append(now) - self.frameTimes = self.frameTimes[-100:] - - def getFps(self): - if len(self.frameTimes) <= 1: - return 0 - # Calculate the FPS - return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) - - -pipeline = dai.Pipeline() -# Define sources and outputs -camRgb = pipeline.create(dai.node.ColorCamera) -tof = pipeline.create(dai.node.ToF) -camTof = pipeline.create(dai.node.Camera) -sync = pipeline.create(dai.node.Sync) -align = pipeline.create(dai.node.ImageAlign) -out = pipeline.create(dai.node.XLinkOut) - -tofConfig = tof.initialConfig.get() -tofConfig.enableFPPNCorrection = True -tofConfig.enableOpticalCorrection = False -tofConfig.enablePhaseShuffleTemporalFilter = False -tofConfig.phaseUnwrappingLevel = 1 -tofConfig.phaseUnwrapErrorThreshold = 300 -tofConfig.enableTemperatureCorrection = False # Not yet supported -tofConfig.enableWiggleCorrection = False -tofConfig.median = dai.MedianFilter.KERNEL_3x3 -tof.initialConfig.set(tofConfig) - -# ToF settings -camTof.setFps(FPS) -# camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) -camTof.setBoardSocket(TOF_SOCKET) - -# rgb settings -camRgb.setBoardSocket(RGB_SOCKET) -camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) -camRgb.setFps(FPS) -camRgb.setIspScale(1, 2) - -out.setStreamName("out") - -sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) - -# Linking -camRgb.isp.link(sync.inputs["rgb"]) -camTof.raw.link(tof.input) -tof.depth.link(align.input) -align.outputAligned.link(sync.inputs["depth_aligned"]) -sync.inputs["rgb"].setBlocking(False) -camRgb.isp.link(align.inputAlignTo) -sync.out.link(out.input) - - -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - -rgbWeight = 0.4 -depthWeight = 0.6 - - -def updateBlendWeights(percentRgb): - """ - Update the rgb and depth weights used to blend depth/rgb image - - @param[in] percent_rgb The rgb weight expressed as a percentage (0..100) - """ - global depthWeight - global rgbWeight - rgbWeight = float(percentRgb) / 100.0 - depthWeight = 1.0 - rgbWeight - - -# Connect to device and start pipeline -with dai.Device(pipeline) as device: - queue = device.getOutputQueue("out", 8, False) - - # Configure windows; trackbar adjusts blending ratio of rgb/depth - rgbDepthWindowName = "rgb-depth" - - cv2.namedWindow(rgbDepthWindowName) - cv2.createTrackbar( - "RGB Weight %", - rgbDepthWindowName, - int(rgbWeight * 100), - 100, - updateBlendWeights, - ) - fpsCounter = FPSCounter() - while True: - messageGroup = queue.get() - fpsCounter.tick() - assert isinstance(messageGroup, dai.MessageGroup) - frameRgb = messageGroup["rgb"] - assert isinstance(frameRgb, dai.ImgFrame) - frameDepth = messageGroup["depth_aligned"] - assert isinstance(frameDepth, dai.ImgFrame) - - sizeRgb = frameRgb.getData().size - sizeDepth = frameDepth.getData().size - # Blend when both received - if frameDepth is not None: - cvFrame = frameRgb.getCvFrame() - # Colorize the aligned depth - alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) - # Resize depth to match the rgb frame - cv2.putText( - alignedDepthColorized, - f"FPS: {fpsCounter.getFps():.2f}", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (255, 255, 255), - 2, - ) - cv2.imshow("depth", alignedDepthColorized) - - blended = cv2.addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) - cv2.imshow(rgbDepthWindowName, blended) - - key = cv2.waitKey(1) - if key == ord("q"): - break diff --git a/scripts/align_rgb_on_depth_test.py b/scripts/align_rgb_on_depth_test.py deleted file mode 100644 index 1a0862e..0000000 --- a/scripts/align_rgb_on_depth_test.py +++ /dev/null @@ -1,60 +0,0 @@ -import numpy as np -import cv2 -import depthai as dai -from datetime import timedelta - -FPS = 30 - -pipeline = dai.Pipeline() -left = pipeline.create(dai.node.ColorCamera) -cam_tof = pipeline.create(dai.node.Camera) -tof = pipeline.create(dai.node.ToF) -sync = pipeline.create(dai.node.Sync) -align = pipeline.create(dai.node.ImageAlign) -out = pipeline.create(dai.node.XLinkOut) -out.setStreamName("out") - -tofConfig = tof.initialConfig.get() -tofConfig.enableFPPNCorrection = True -tofConfig.enableOpticalCorrection = False -tofConfig.enablePhaseShuffleTemporalFilter = False -tofConfig.phaseUnwrappingLevel = 1 -tofConfig.phaseUnwrapErrorThreshold = 300 -tofConfig.enableTemperatureCorrection = False # Not yet supported -tofConfig.enableWiggleCorrection = False -tofConfig.median = dai.MedianFilter.KERNEL_3x3 -tof.initialConfig.set(tofConfig) - -cam_tof.setFps(FPS) -cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) - -left.setBoardSocket(dai.CameraBoardSocket.CAM_C) -left.setFps(FPS) -left.setIspScale(2, 3) - -sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) - -# left.isp.link(sync.inputs["left"]) -tof.depth.link(sync.inputs["depth"]) -cam_tof.raw.link(tof.input) -tof.depth.link(align.inputAlignTo) -left.isp.link(align.input) -sync.inputs["left"].setBlocking(False) -align.outputAligned.link(sync.inputs["left_aligned"]) -sync.out.link(out.input) - -device = dai.Device(pipeline) - -queue = device.getOutputQueue(name="out", maxSize=8, blocking=False) - -while True: - messageGroup = queue.get() - frameDepth = messageGroup["depth"] - frameLeft = messageGroup["left_aligned"] - - left_aligned = frameLeft.getCvFrame() - depth = frameDepth.getFrame() - - cv2.imshow("left_aligned", left_aligned) - cv2.imshow("depth", depth) - cv2.waitKey(1) diff --git a/scripts/aze.py b/scripts/aze.py deleted file mode 100644 index 86fdcdf..0000000 --- a/scripts/aze.py +++ /dev/null @@ -1,159 +0,0 @@ -import time -from datetime import timedelta - -import cv2 -import depthai as dai -import numpy as np - -# This example is intended to run unchanged on an OAK-D-SR-PoE camera -FPS = 30.0 - -RGB_SOCKET = dai.CameraBoardSocket.CAM_C -TOF_SOCKET = dai.CameraBoardSocket.CAM_A -ALIGN_SOCKET = RGB_SOCKET - - -class FPSCounter: - def __init__(self): - self.frameTimes = [] - - def tick(self): - now = time.time() - self.frameTimes.append(now) - self.frameTimes = self.frameTimes[-100:] - - def getFps(self): - if len(self.frameTimes) <= 1: - return 0 - # Calculate the FPS - return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) - - -pipeline = dai.Pipeline() -# Define sources and outputs -camRgb = pipeline.create(dai.node.ColorCamera) -tof = pipeline.create(dai.node.ToF) -camTof = pipeline.create(dai.node.Camera) -sync = pipeline.create(dai.node.Sync) -align = pipeline.create(dai.node.ImageAlign) -out = pipeline.create(dai.node.XLinkOut) - -# ToF settings -camTof.setFps(FPS) -camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) -camTof.setBoardSocket(TOF_SOCKET) - -# rgb settings -camRgb.setBoardSocket(RGB_SOCKET) -camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) -camRgb.setFps(FPS) -camRgb.setIspScale(1, 2) - -out.setStreamName("out") - -sync.setSyncThreshold(timedelta(seconds=(1 / FPS))) - -# Linking -camRgb.isp.link(sync.inputs["rgb"]) -camTof.raw.link(tof.input) -tof.depth.link(align.input) -align.outputAligned.link(sync.inputs["depth_aligned"]) -sync.inputs["rgb"].setBlocking(False) -camRgb.isp.link(align.inputAlignTo) -sync.out.link(out.input) - - -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - -rgbWeight = 0.4 -depthWeight = 0.6 - - -def updateBlendWeights(percentRgb): - """ - Update the rgb and depth weights used to blend depth/rgb image - - @param[in] percent_rgb The rgb weight expressed as a percentage (0..100) - """ - global depthWeight - global rgbWeight - rgbWeight = float(percentRgb) / 100.0 - depthWeight = 1.0 - rgbWeight - - -# Connect to device and start pipeline -with dai.Device(pipeline) as device: - queue = device.getOutputQueue("out", 8, False) - - # Configure windows; trackbar adjusts blending ratio of rgb/depth - rgbDepthWindowName = "rgb-depth" - - cv2.namedWindow(rgbDepthWindowName) - cv2.createTrackbar( - "RGB Weight %", - rgbDepthWindowName, - int(rgbWeight * 100), - 100, - updateBlendWeights, - ) - fpsCounter = FPSCounter() - while True: - messageGroup = queue.get() - fpsCounter.tick() - assert isinstance(messageGroup, dai.MessageGroup) - frameRgb = messageGroup["rgb"] - assert isinstance(frameRgb, dai.ImgFrame) - frameDepth = messageGroup["depth_aligned"] - assert isinstance(frameDepth, dai.ImgFrame) - - sizeRgb = frameRgb.getData().size - sizeDepth = frameDepth.getData().size - # Blend when both received - if frameDepth is not None: - cvFrame = frameRgb.getCvFrame() - # Colorize the aligned depth - alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) - # Resize depth to match the rgb frame - cv2.putText( - alignedDepthColorized, - f"FPS: {fpsCounter.getFps():.2f}", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (255, 255, 255), - 2, - ) - cv2.imshow("depth", alignedDepthColorized) - - blended = cv2.addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) - cv2.imshow(rgbDepthWindowName, blended) - - key = cv2.waitKey(1) - if key == ord("q"): - break diff --git a/scripts/minimal_tof_test.py b/scripts/minimal_tof_test.py deleted file mode 100644 index bd489f2..0000000 --- a/scripts/minimal_tof_test.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python3 - -import time -import cv2 -import depthai as dai -import numpy as np - -print(dai.__version__) - -cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) -cvColorMap[0] = [0, 0, 0] - - -def create_pipeline(): - pipeline = dai.Pipeline() - - tof = pipeline.create(dai.node.ToF) - - # Configure the ToF node - tofConfig = tof.initialConfig.get() - - # Optional. Best accuracy, but adds motion blur. - # see ToF node docs on how to reduce/eliminate motion blur. - tofConfig.enableOpticalCorrection = False - tofConfig.enablePhaseShuffleTemporalFilter = True - tofConfig.phaseUnwrappingLevel = 4 - tofConfig.phaseUnwrapErrorThreshold = 300 - tofConfig.enableTemperatureCorrection = False # Not yet supported - - xinTofConfig = pipeline.create(dai.node.XLinkIn) - xinTofConfig.setStreamName("tofConfig") - xinTofConfig.out.link(tof.inputConfig) - - tof.initialConfig.set(tofConfig) - - cam_tof = pipeline.create(dai.node.Camera) - cam_tof.setFps(30) # ToF node will produce depth frames at /2 of this rate - cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) - cam_tof.raw.link(tof.input) - - xout = pipeline.create(dai.node.XLinkOut) - xout.setStreamName("depth") - tof.depth.link(xout.input) - - tofConfig = tof.initialConfig.get() - - left = pipeline.create(dai.node.ColorCamera) - left.setBoardSocket(dai.CameraBoardSocket.CAM_B) - left.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) - left.setFps(30) - left.setIspScale(2, 3) - - left_xout = pipeline.create(dai.node.XLinkOut) - left_xout.setStreamName("left") - left.video.link(left_xout.input) - - return pipeline, tofConfig - - -if __name__ == "__main__": - pipeline, tofConfig = create_pipeline() - - with dai.Device(pipeline) as device: - print("Connected cameras:", device.getConnectedCameraFeatures()) - - # qDepth = device.getOutputQueue(name="depth") - qDepth = device.getOutputQueue(name="depth", maxSize=8, blocking=False) - - # left_q = device.getOutputQueue(name="left") - left_q = device.getOutputQueue(name="left", maxSize=8, blocking=False) - - while True: - start = time.time() - - imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived - depth_map = imgFrame.getFrame() - - max_depth = (tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. - depth_raw = depth_map / max_depth - depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8) - depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap) - - # If I comment that (3 next lines), the tof works fine - in_left = left_q.get() - left_im = in_left.getCvFrame() - cv2.imshow("left", left_im) - # Until that - - cv2.imshow("Colorized depth", depth_colorized) - cv2.imshow("depth raw", depth_raw) - key = cv2.waitKey(1) - - device.close() diff --git a/scripts/test.py b/scripts/test.py deleted file mode 100644 index 10e1ca6..0000000 --- a/scripts/test.py +++ /dev/null @@ -1,7 +0,0 @@ -import depthai as dai - -device = dai.Device() -calib = device.readCalibration() -calib.eepromToJsonFile("read_calib.json") -# print(calib) -print(calib.getCameraExtrinsics(dai.CameraBoardSocket.CAM_D, dai.CameraBoardSocket.CAM_C)) diff --git a/scripts/tof_and_encoding_test.py b/scripts/tof_and_encoding_test.py deleted file mode 100644 index a63a521..0000000 --- a/scripts/tof_and_encoding_test.py +++ /dev/null @@ -1,206 +0,0 @@ -import numpy as np -import cv2 -import depthai as dai -from datetime import timedelta -from pollen_vision.camera_wrappers.depthai.calibration.undistort import compute_undistort_maps, get_mesh -from pollen_vision.camera_wrappers.depthai.cam_config import CamConfig -from pollen_vision.camera_wrappers.depthai.utils import ( - get_config_file_path, - get_config_files_names, -) -import subprocess as sp - -FPS = 60 -pipeline = dai.Pipeline() -left = pipeline.create(dai.node.ColorCamera) -left.setFps(FPS) -left.setBoardSocket(dai.CameraBoardSocket.CAM_C) -left.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) -left.setSensorCrop(208 / 1440, 28 / 1080) -left.setVideoSize(1024, 1024) -# left.setIspScale(2, 3) - -right = pipeline.create(dai.node.ColorCamera) -right.setFps(FPS) -right.setBoardSocket(dai.CameraBoardSocket.CAM_B) -right.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1440X1080) -right.setSensorCrop(208 / 1440, 28 / 1080) -right.setVideoSize(1024, 1024) -# right.setIspScale(2, 3) - -# cam_tof = pipeline.create(dai.node.Camera) -# cam_tof.setFps(30) -# cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) -# tof = pipeline.create(dai.node.ToF) - - -xout_left = pipeline.createXLinkOut() -xout_left.setStreamName("left") - -xout_right = pipeline.createXLinkOut() -xout_right.setStreamName("right") - -# xout_depth = pipeline.createXLinkOut() -# xout_depth.setStreamName("depth") - -# tofConfig = tof.initialConfig.get() -# tofConfig.enableFPPNCorrection = True -# tofConfig.enableOpticalCorrection = False -# tofConfig.enablePhaseShuffleTemporalFilter = False -# tofConfig.phaseUnwrappingLevel = 1 -# tofConfig.phaseUnwrapErrorThreshold = 300 -# tofConfig.enableTemperatureCorrection = False # Not yet supported -# tofConfig.enableWiggleCorrection = False -# tofConfig.median = dai.MedianFilter.KERNEL_3x3 -# tof.initialConfig.set(tofConfig) - -# Create encoders -profile = dai.VideoEncoderProperties.Profile.H264_BASELINE -bitrate = 4000 -numBFrames = 0 # gstreamer recommends 0 B frames -left_encoder = pipeline.create(dai.node.VideoEncoder) -left_encoder.setDefaultProfilePreset(FPS, profile) -left_encoder.setKeyframeFrequency(FPS) # every 1s -left_encoder.setNumBFrames(numBFrames) -left_encoder.setBitrateKbps(bitrate) - -right_encoder = pipeline.create(dai.node.VideoEncoder) -right_encoder.setDefaultProfilePreset(60, profile) -right_encoder.setKeyframeFrequency(60) # every 1s -right_encoder.setNumBFrames(numBFrames) -right_encoder.setBitrateKbps(bitrate) - -# Create manip -# resolution = (960, 720) -resolution = (1024, 1024) -config_json = get_config_file_path("CONFIG_IMX296_TOF") -cam_config = CamConfig( - cam_config_json=config_json, - fps=FPS, - resize=resolution, - exposure_params=None, - mx_id=None, - # isp_scale=(2, 3), - rectify=True, -) -device = dai.Device() -cam_config.set_sensor_resolution(tuple(resolution)) -width_undistort_resolution = int(1024 * (cam_config.isp_scale[0] / cam_config.isp_scale[1])) -height_unistort_resolution = int(1024 * (cam_config.isp_scale[0] / cam_config.isp_scale[1])) -cam_config.set_undistort_resolution((width_undistort_resolution, height_unistort_resolution)) -cam_config.set_calib(device.readCalibration()) -device.close() -mapXL, mapYL, mapXR, mapYR = compute_undistort_maps(cam_config) -cam_config.set_undistort_maps(mapXL, mapYL, mapXR, mapYR) - - -left_manip = pipeline.createImageManip() -# try: -# mesh, meshWidth, meshHeight = get_mesh(cam_config, "left") -# left_manip.setWarpMesh(mesh, meshWidth, meshHeight) -# except Exception as e: -# print(e) -# exit() -left_manip.setMaxOutputFrameSize(resolution[0] * resolution[1] * 3) -left_manip.initialConfig.setKeepAspectRatio(True) -left_manip.initialConfig.setResize(resolution[0], resolution[1]) -left_manip.initialConfig.setFrameType(dai.ImgFrame.Type.NV12) - - -right_manip = pipeline.createImageManip() -# try: -# mesh, meshWidth, meshHeight = get_mesh(cam_config, "right") -# right_manip.setWarpMesh(mesh, meshWidth, meshHeight) -# except Exception as e: -# print(e) -# exit() -right_manip.setMaxOutputFrameSize(resolution[0] * resolution[1] * 3) -right_manip.initialConfig.setKeepAspectRatio(True) -right_manip.initialConfig.setResize(resolution[0], resolution[1]) -right_manip.initialConfig.setFrameType(dai.ImgFrame.Type.NV12) - -# left.video.link(left_manip.inputImage) -# left_manip.out.link(left_encoder.input) -left.video.link(xout_left.input) - - -# right.video.link(right_manip.inputImage) -# right_manip.out.link(right_encoder.input) -right.video.out.link(xout_right.input) - -# left_encoder.bitstream.link(xout_left.input) -# right_encoder.bitstream.link(xout_right.input) - -# cam_tof.raw.link(tof.input) -# tof.depth.link(xout_depth.input) - -device = dai.Device(pipeline) - -left_queue = device.getOutputQueue(name="left", maxSize=1, blocking=False) -right_queue = device.getOutputQueue(name="right", maxSize=1, blocking=False) -# depth_queue = device.getOutputQueue(name="depth", maxSize=4, blocking=False) - - -def spawn_procs(names): # type: ignore [type-arg] - # width, height = 1280, 720 - # width, height = 1440, 1080 - width, height = 1024, 1024 - command = [ - "ffplay", - "-i", - "-", - "-x", - str(width), - "-y", - str(height), - "-framerate", - str(FPS), - "-fflags", - "nobuffer", - "-flags", - "low_delay", - "-framedrop", - "-strict", - "experimental", - ] - - procs = {} - try: - for name in names: - procs[name] = sp.Popen(command, stdin=sp.PIPE) # Start the ffplay process - except Exception: - exit("Error: cannot run ffplay!\nTry running: sudo apt install ffmpeg") - - return procs - - -latencies_left = [] -latencies_right = [] -# procs = spawn_procs(["left", "right"]) -while True: - latency = {} - - left_frame = left_queue.get() - latency["left"] = dai.Clock.now() - left_frame.getTimestamp() - right_frame = right_queue.get() - latency["right"] = dai.Clock.now() - right_frame.getTimestamp() - # depth_frame = depth_queue.get() - # latency["depth"] = dai.Clock.now() - depth_frame.getTimestamp() - - # io = procs["left"].stdin - # io.write(left_frame.getData()) - - # io = procs["right"].stdin - # io.write(right_frame.getData()) - - cv2.imshow("left", left_frame.getCvFrame()) - cv2.imshow("right", right_frame.getCvFrame()) - # cv2.imshow("depth", depth_frame.getFrame()) - # print(latency) - latencies_left.append(latency["left"]) - latencies_right.append(latency["right"]) - - print(np.mean(latencies_left[-50:]), np.mean(latencies_right[-50:])) - - if cv2.waitKey(1) == ord("q"): - break diff --git a/scripts/tof_pcl_test.py b/scripts/tof_pcl_test.py deleted file mode 100644 index 1f83e86..0000000 --- a/scripts/tof_pcl_test.py +++ /dev/null @@ -1,66 +0,0 @@ -from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer -import time - -import cv2 -import depthai as dai -import numpy as np - - -def create_pipeline(): - pipeline = dai.Pipeline() - - tof = pipeline.create(dai.node.ToF) - - # Configure the ToF node - tofConfig = tof.initialConfig.get() - - # Optional. Best accuracy, but adds motion blur. - # see ToF node docs on how to reduce/eliminate motion blur. - tofConfig.enableOpticalCorrection = False - tofConfig.enablePhaseShuffleTemporalFilter = True - tofConfig.phaseUnwrappingLevel = 4 - tofConfig.phaseUnwrapErrorThreshold = 300 - - tofConfig.enableTemperatureCorrection = False # Not yet supported - - xinTofConfig = pipeline.create(dai.node.XLinkIn) - xinTofConfig.setStreamName("tofConfig") - xinTofConfig.out.link(tof.inputConfig) - - tof.initialConfig.set(tofConfig) - - cam_tof = pipeline.create(dai.node.Camera) - cam_tof.setFps(60) # ToF node will produce depth frames at /2 of this rate - cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_A) - cam_tof.raw.link(tof.input) - - xout = pipeline.create(dai.node.XLinkOut) - xout.setStreamName("depth") - tof.depth.link(xout.input) - - tofConfig = tof.initialConfig.get() - - return pipeline, tofConfig - - -K = np.eye(3) -K[0][0] = 100 -K[1][1] = 100 -K[0][2] = 320 -K[1][2] = 240 -P = PCLVisualizer(K) - -pipeline, tofConfig = create_pipeline() -rgb = np.zeros((480, 640, 3), dtype=np.uint8) -rgb[:, :, 0] = 255 -with dai.Device(pipeline) as device: - qDepth = device.getOutputQueue(name="depth") - while True: - imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived - depth_map = imgFrame.getFrame() - depth_map = depth_map * 0.001 - print("min", min(depth_map.reshape(-1)), "max", max(depth_map.reshape(-1))) - P.update(rgb, depth_map.astype(np.float32)) - P.tick() - cv2.imshow("depth", depth_map) - cv2.waitKey(1) diff --git a/scripts/tof_test.py b/scripts/tof_test.py deleted file mode 100644 index 7182756..0000000 --- a/scripts/tof_test.py +++ /dev/null @@ -1,135 +0,0 @@ -#!/usr/bin/env python3 - -import time - -import cv2 -import depthai as dai -import numpy as np - -print(dai.__version__) - -cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) -cvColorMap[0] = [0, 0, 0] - - -def create_pipeline(): - pipeline = dai.Pipeline() - - tof = pipeline.create(dai.node.ToF) - - # Configure the ToF node - tofConfig = tof.initialConfig.get() - - # Optional. Best accuracy, but adds motion blur. - # see ToF node docs on how to reduce/eliminate motion blur. - tofConfig.enableOpticalCorrection = True - tofConfig.enablePhaseShuffleTemporalFilter = True - tofConfig.phaseUnwrappingLevel = 4 - tofConfig.phaseUnwrapErrorThreshold = 300 - - tofConfig.enableTemperatureCorrection = False # Not yet supported - - xinTofConfig = pipeline.create(dai.node.XLinkIn) - xinTofConfig.setStreamName("tofConfig") - xinTofConfig.out.link(tof.inputConfig) - - tof.initialConfig.set(tofConfig) - - cam_tof = pipeline.create(dai.node.Camera) - cam_tof.setFps(100) # ToF node will produce depth frames at /2 of this rate - cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_A) - cam_tof.raw.link(tof.input) - - xout = pipeline.create(dai.node.XLinkOut) - xout.setStreamName("depth") - tof.depth.link(xout.input) - - tofConfig = tof.initialConfig.get() - - return pipeline, tofConfig - - -if __name__ == "__main__": - pipeline, tofConfig = create_pipeline() - - with dai.Device(pipeline) as device: - print("Connected cameras:", device.getConnectedCameraFeatures()) - qDepth = device.getOutputQueue(name="depth") - - tofConfigInQueue = device.getInputQueue("tofConfig") - - counter = 0 - while True: - start = time.time() - key = cv2.waitKey(1) - if key == ord("f"): - tofConfig.enableFPPNCorrection = not tofConfig.enableFPPNCorrection - tofConfigInQueue.send(tofConfig) - elif key == ord("o"): - tofConfig.enableOpticalCorrection = not tofConfig.enableOpticalCorrection - tofConfigInQueue.send(tofConfig) - elif key == ord("w"): - tofConfig.enableWiggleCorrection = not tofConfig.enableWiggleCorrection - tofConfigInQueue.send(tofConfig) - elif key == ord("t"): - tofConfig.enableTemperatureCorrection = not tofConfig.enableTemperatureCorrection - tofConfigInQueue.send(tofConfig) - elif key == ord("q"): - break - elif key == ord("0"): - tofConfig.enablePhaseUnwrapping = False - tofConfig.phaseUnwrappingLevel = 0 - tofConfigInQueue.send(tofConfig) - elif key == ord("1"): - tofConfig.enablePhaseUnwrapping = True - tofConfig.phaseUnwrappingLevel = 1 - tofConfigInQueue.send(tofConfig) - elif key == ord("2"): - tofConfig.enablePhaseUnwrapping = True - tofConfig.phaseUnwrappingLevel = 2 - tofConfigInQueue.send(tofConfig) - elif key == ord("3"): - tofConfig.enablePhaseUnwrapping = True - tofConfig.phaseUnwrappingLevel = 3 - tofConfigInQueue.send(tofConfig) - elif key == ord("4"): - tofConfig.enablePhaseUnwrapping = True - tofConfig.phaseUnwrappingLevel = 4 - tofConfigInQueue.send(tofConfig) - elif key == ord("5"): - tofConfig.enablePhaseUnwrapping = True - tofConfig.phaseUnwrappingLevel = 5 - tofConfigInQueue.send(tofConfig) - elif key == ord("m"): - medianSettings = [ - dai.MedianFilter.MEDIAN_OFF, - dai.MedianFilter.KERNEL_3x3, - dai.MedianFilter.KERNEL_5x5, - dai.MedianFilter.KERNEL_7x7, - ] - currentMedian = tofConfig.median - nextMedian = medianSettings[(medianSettings.index(currentMedian) + 1) % len(medianSettings)] - # print(f"Changing median to {nextMedian.name} from {currentMedian.name}") - tofConfig.median = nextMedian - tofConfigInQueue.send(tofConfig) - - imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived - depth_map = imgFrame.getFrame() - max_depth = (tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. - depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8) - depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap) - - print("===") - print("tofConfig.enableOpticalCorrection", tofConfig.enableOpticalCorrection) - print("tofConfig.enablePhaseShuffleTemporalFilter", tofConfig.enablePhaseShuffleTemporalFilter) - print("tofConfig.phaseUnwrappingLevel", tofConfig.phaseUnwrappingLevel) - print("tofConfig.phaseUnwrapErrorThreshold", tofConfig.phaseUnwrapErrorThreshold) - print("tofConfig.enableFPPNCorrection", tofConfig.enableFPPNCorrection) - print("tofConfig.median", tofConfig.median) - print("tofConfig.enableTemperatureCorrection", tofConfig.enableTemperatureCorrection) - print("===") - - cv2.imshow("Colorized depth", depth_colorized) - counter += 1 - - device.close() diff --git a/scripts/tof_test_align.py b/scripts/tof_test_align.py deleted file mode 100644 index 0772c0e..0000000 --- a/scripts/tof_test_align.py +++ /dev/null @@ -1,27 +0,0 @@ -import time -import cv2 -import depthai as dai -import numpy as np - -pipeline = dai.Pipeline() - -tof = pipeline.create(dai.node.ToF) - -tofConfig = tof.initialConfig.get() -tofConfig.enableOpticalCorrection = False -tofConfig.enablePhaseShuffleTemporalFilter = True -tofConfig.phaseUnwrappingLevel = 4 -tofConfig.phaseUnwrapErrorThreshold = 300 -tofConfig.enableTemperatureCorrection = False # Not yet supported - -cam_tof = pipeline.create(dai.node.Camera) -cam_tof.setFps(30) -cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) -cam_tof.raw.link(tof.input) - -cam_left = pipeline.create(dai.node.ColorCamera) -cam_left.setBoardSocket(dai.CameraBoardSocket.CAM_C) - -undistort_manip = pipeline.createImageManip() -mesh, meshWidth, meshHeight = get_mesh(self.cam_config, cam_name) -manip.setWarpMesh(mesh, meshWidth, meshHeight) From 46c5755a8f9a31163d153217b2f3b957aab4ae03 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 22 Oct 2024 12:40:54 +0200 Subject: [PATCH 10/17] tof point cloud mode and other shenanigans --- examples/camera_wrappers_examples/live_pcl.py | 20 -- .../camera_wrappers_examples/live_pcl_tof.py | 61 +---- .../camera_wrappers_examples/tof_example.py | 7 +- .../depthai/calibration/check_epilines.py | 2 +- .../depthai/calibration/test_flash.py | 28 -- .../depthai/calibration/testtt.py | 32 --- .../camera_wrappers/depthai/tof.py | 50 +++- .../camera_wrappers/depthai/wrapper.py | 34 ++- pollen_vision/pollen_vision/utils/__init__.py | 1 + .../pollen_vision/utils/pcl_visualizer.py | 37 ++- .../pollen_vision/utils}/recorder.py | 0 scripts/RECORD_SR.py | 2 +- scripts/chatgpt.py | 94 ------- scripts/demo_webcam.py | 0 scripts/read_calib.json | 247 ------------------ ...{align_example.py => tof_align_example.py} | 7 +- ...cted.py => tof_align_example_corrected.py} | 23 +- scripts/tof_point_cloud_luxonis.py | 30 +-- scripts/vanilla_tof_test.py | 30 +-- 19 files changed, 126 insertions(+), 579 deletions(-) delete mode 100644 pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py delete mode 100644 pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py rename {scripts => pollen_vision/pollen_vision/utils}/recorder.py (100%) delete mode 100644 scripts/chatgpt.py delete mode 100644 scripts/demo_webcam.py delete mode 100644 scripts/read_calib.json rename scripts/{align_example.py => tof_align_example.py} (99%) rename scripts/{align_example_corrected.py => tof_align_example_corrected.py} (92%) diff --git a/examples/camera_wrappers_examples/live_pcl.py b/examples/camera_wrappers_examples/live_pcl.py index fde3751..699ee09 100644 --- a/examples/camera_wrappers_examples/live_pcl.py +++ b/examples/camera_wrappers_examples/live_pcl.py @@ -1,7 +1,6 @@ import argparse import cv2 -import numpy as np from pollen_vision.camera_wrappers import SDKWrapper from pollen_vision.camera_wrappers.depthai.utils import ( get_config_file_path, @@ -26,35 +25,16 @@ K = w.get_K() P = PCLVisualizer(K) - -mouse_x, mouse_y = 0, 0 - - -def cv_callback(event, x, y, flags, param): - global mouse_x, mouse_y - mouse_x, mouse_y = x, y - - -cv2.namedWindow("depth") -cv2.setMouseCallback("depth", cv_callback) while True: data, lat, _ = w.get_data() - # print(lat["depthNode_left"], lat["depthNode_right"]) depth = data["depth"] rgb = data["left"] - # disparity = data["disparity"] P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) - # disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) - # disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET) - # cv2.imshow("disparity", disparity) - # cv2.imshow("left", data["depthNode_left"]) - # cv2.imshow("right", data["depthNode_right"]) cv2.imshow("depth", depth) cv2.imshow("rgb", rgb) - print(depth[mouse_y, mouse_x]) key = cv2.waitKey(1) P.tick() diff --git a/examples/camera_wrappers_examples/live_pcl_tof.py b/examples/camera_wrappers_examples/live_pcl_tof.py index 83064fc..2759f77 100644 --- a/examples/camera_wrappers_examples/live_pcl_tof.py +++ b/examples/camera_wrappers_examples/live_pcl_tof.py @@ -1,13 +1,11 @@ import argparse import cv2 -import numpy as np from pollen_vision.camera_wrappers import TOFWrapper from pollen_vision.camera_wrappers.depthai.utils import ( get_config_file_path, get_config_files_names, ) - from pollen_vision.utils.pcl_visualizer import PCLVisualizer valid_configs = get_config_files_names() @@ -22,71 +20,18 @@ ) args = argParser.parse_args() -w = TOFWrapper(get_config_file_path(args.config), crop=True, fps=10) +w = TOFWrapper(get_config_file_path(args.config), crop=False, fps=30, create_pointcloud=True) -K = w.get_K() -P = PCLVisualizer(K) +P = PCLVisualizer() P.add_frame("origin") -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - -mouse_x, mouse_y = 0, 0 - - -def cv_callback(event, x, y, flags, param): - global mouse_x, mouse_y - mouse_x, mouse_y = x, y - - -# cv2.namedWindow("depth") -# cv2.setMouseCallback("depth", cv_callback) while True: data, lat, _ = w.get_data() - # print(lat["depthNode_left"], lat["depthNode_right"]) - depth = data["depth"] rgb = data["left"] - # colorized_depth = colorizeDepth(depth) - # disparity = data["disparity"] - - P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth) - - # disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8) - # disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET) - # cv2.imshow("disparity", disparity) - # cv2.imshow("left", data["depthNode_left"]) - # cv2.imshow("right", data["depthNode_right"]) - # cv2.imshow("depth", colorized_depth) - # cv2.imshow("rgb", rgb) - # print(depth[mouse_y, mouse_x]) + P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), points=data["pointcloud"]) key = cv2.waitKey(1) P.tick() diff --git a/examples/camera_wrappers_examples/tof_example.py b/examples/camera_wrappers_examples/tof_example.py index 970c7ad..c64de0f 100644 --- a/examples/camera_wrappers_examples/tof_example.py +++ b/examples/camera_wrappers_examples/tof_example.py @@ -1,9 +1,10 @@ import cv2 -import numpy as np import depthai as dai from pollen_vision.camera_wrappers import TOFWrapper -from pollen_vision.camera_wrappers.depthai.utils import get_config_file_path, colorizeDepth - +from pollen_vision.camera_wrappers.depthai.utils import ( + colorizeDepth, + get_config_file_path, +) t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py index f2bed20..1891ebd 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py @@ -1,8 +1,8 @@ import argparse -import numpy as np import logging import cv2 +import numpy as np from cv2 import aruco from pollen_vision.camera_wrappers.depthai import SDKWrapper, TOFWrapper from pollen_vision.camera_wrappers.depthai.utils import ( diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py deleted file mode 100644 index 8175a61..0000000 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/test_flash.py +++ /dev/null @@ -1,28 +0,0 @@ -# left -> right -# left -> tof -# tof -> left - -ch.setCameraExtrinsics( - left_socket, - tof_socket, - R_left_to_tof, - T_left_to_tof, - specTranslation=T_left_to_tof, -) - -ch.setCameraExtrinsics( - right_socket, - left_socket, - R_right_to_left, - T_right_to_left, - specTranslation=T_right_to_left, -) - -# Should I do this ? -ch.setCameraExtrinsics( - tof_socket, - tof_socket, - [0, 0, 0], - np.eye(3).tolist(), - specTranslation=[0, 0, 0], -) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py deleted file mode 100644 index cbfdfe8..0000000 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/testtt.py +++ /dev/null @@ -1,32 +0,0 @@ -import depthai as dai -import cv2 - -print(dai.__version__) - -pipeline = dai.Pipeline() - -cam_tof = pipeline.create(dai.node.Camera) -cam_tof.setFps(30) - -cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D) - -tof = pipeline.create(dai.node.ToF) - -tofConfig = tof.initialConfig.get() -tofConfig.enableDistortionCorrection = False -tof.initialConfig.set(tofConfig) -cam_tof.raw.link(tof.input) - -output = pipeline.create(dai.node.XLinkOut) -output.setStreamName("intensity") - -tof.intensity.link(output.input) - -device = dai.Device(pipeline) - -q = device.getOutputQueue(name="intensity", maxSize=8, blocking=False) - -while True: - data = q.get() - cv2.imshow("intensity", data.getCvFrame()) - cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index 0682426..ae5452a 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -6,11 +6,11 @@ import numpy as np import numpy.typing as npt from pollen_vision.camera_wrappers.depthai.utils import ( + colorizeDepth, get_config_file_path, get_socket_from_name, ) from pollen_vision.camera_wrappers.depthai.wrapper import DepthaiWrapper -from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth class TOFWrapper(DepthaiWrapper): @@ -23,8 +23,13 @@ def __init__( crop: bool = False, noalign=False, rectify=False, + create_pointcloud=False, ) -> None: + """ + Using create_pointcloud mode, we can't undistort at the same time (not enough resources on the device) + """ self.noalign = noalign + self.create_pointcloud = create_pointcloud super().__init__( cam_config_json, fps, @@ -69,6 +74,10 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], right = messageGroup["right"].getCvFrame() depth = messageGroup["depth_aligned"].getFrame() tof_intensity = messageGroup["tof_intensity"].getCvFrame() + if self.create_pointcloud: + inPointCloud = messageGroup["pcl"] + points = inPointCloud.getPoints().astype(np.float64) + points[:, 0] = -points[:, 0] # Invert X axis # Temporary, not ideal @@ -82,13 +91,16 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], data["right"] = right data["tof_intensity"] = tof_intensity + + if self.create_pointcloud: + data["pointcloud"] = points return data, latency, ts def get_K(self) -> npt.NDArray[np.float32]: return super().get_K(left=True) # type: ignore def _create_pipeline(self) -> dai.Pipeline: - pipeline = self._pipeline_basis() + pipeline = self._pipeline_basis(create_imagemanip=not self.create_pointcloud) self.cam_tof = pipeline.create(dai.node.Camera) self.cam_tof.setFps(self.cam_config.fps) @@ -97,6 +109,9 @@ def _create_pipeline(self) -> dai.Pipeline: self.tof = pipeline.create(dai.node.ToF) + if self.create_pointcloud: + self.pointcloud = pipeline.create(dai.node.PointCloud) + # === Tof configuration === self.tofConfig = self.tof.initialConfig.get() self.tofConfig.enableFPPNCorrection = True @@ -129,23 +144,36 @@ def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: return pipeline def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: - self.left.isp.link(self.left_manip.inputImage) - self.right.isp.link(self.right_manip.inputImage) + if not self.create_pointcloud: + self.left.isp.link(self.left_manip.inputImage) + self.right.isp.link(self.right_manip.inputImage) self.tof.intensity.link(self.sync.inputs["tof_intensity"]) - self.left_manip.out.link(self.sync.inputs["left"]) - self.right_manip.out.link(self.sync.inputs["right"]) + if not self.create_pointcloud: + self.left_manip.out.link(self.sync.inputs["left"]) + self.right_manip.out.link(self.sync.inputs["right"]) + else: + self.left.isp.link(self.sync.inputs["left"]) + self.right.isp.link(self.sync.inputs["right"]) + self.sync.inputs["left"].setBlocking(False) self.sync.inputs["right"].setBlocking(False) self.cam_tof.raw.link(self.tof.input) - if not self.noalign: - self.left_manip.out.link(self.align.inputAlignTo) + if self.noalign: + self.tof.depth.link(self.sync.inputs["depth_aligned"]) + else: + if not self.create_pointcloud: + self.left_manip.out.link(self.align.inputAlignTo) + else: + self.left.isp.link(self.align.inputAlignTo) self.tof.depth.link(self.align.input) self.align.outputAligned.link(self.sync.inputs["depth_aligned"]) - else: - self.tof.depth.link(self.sync.inputs["depth_aligned"]) + + if self.create_pointcloud: + self.align.outputAligned.link(self.pointcloud.inputDepth) + self.pointcloud.outputPointCloud.link(self.sync.inputs["pcl"]) self.sync.out.link(self.xout_sync.input) @@ -161,7 +189,7 @@ def _create_queues(self) -> Dict[str, dai.DataOutputQueue]: if __name__ == "__main__": - t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False, rectify=False) + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False, rectify=False, create_pointcloud=True) print(dai.__version__) while True: diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index fa8a21a..b2c7aab 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -153,7 +153,7 @@ def _create_pipeline(self) -> dai.Pipeline: """Abstract method that is implemented by the subclasses.""" pass - def _pipeline_basis(self) -> dai.Pipeline: + def _pipeline_basis(self, create_imagemanip=True) -> dai.Pipeline: """Creates and configures the left and right cameras and the image manip nodes. This method is used (and/or extended) by the subclasses to create the basis pipeline. @@ -186,12 +186,13 @@ def _pipeline_basis(self) -> dai.Pipeline: self.left.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) self.right.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) - self.left_manip = self._create_imageManip( - pipeline, "left", self.cam_config.undistort_resolution, self.cam_config.rectify - ) - self.right_manip = self._create_imageManip( - pipeline, "right", self.cam_config.undistort_resolution, self.cam_config.rectify - ) + if create_imagemanip: + self.left_manip = self._create_imageManip( + pipeline, "left", self.cam_config.undistort_resolution, self.cam_config.rectify + ) + self.right_manip = self._create_imageManip( + pipeline, "right", self.cam_config.undistort_resolution, self.cam_config.rectify + ) return pipeline @@ -232,7 +233,6 @@ def _create_imageManip( rectify: bool = True, ) -> dai.node.ImageManip: """Resize and optionally rectify an image""" - manip = pipeline.createImageManip() if rectify: @@ -256,7 +256,7 @@ def _set_undistort_maps(self) -> None: self.cam_config.set_undistort_maps(mapXL, mapYL, mapXR, mapYR) # Takes in the output of multical calibration - def flash(self, calib_json_file: str, tof=False) -> None: + def flash(self, calib_json_file: str, tof: bool = False) -> None: """Flashes the calibration to the camera. The calibration is read from the calib_json_file and flashed into the camera's eeprom. @@ -284,15 +284,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: im_size = params["image_size"] cam_socket = get_socket_from_name(cam_name, self.cam_config.name_to_socket) - # if cam_name == "tof": - # K = np.array( - # [[471.8361511230469, 0.0, 322.25347900390625], [0.0, 471.7205810546875, 246.209716796875], [0.0, 0.0, 1.0]] - # ) - ch.setCameraIntrinsics(cam_socket, K.tolist(), im_size) # if cam_name == "tof": - # # D = np.array([-9.466925621032715, 30.354965209960938, 0.0001632508501643315, -0.00029841947252862155]) # D *= 0.01 # still don't know if I should do that or not ch.setDistortionCoefficients(cam_socket, D.tolist()) @@ -320,9 +314,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: T_tof_to_left[:3, :3] = r T_tof_to_left[:3, 3] = t - T_tof_to_left[ - :3, 3 - ] *= 100 # Needs to be in centimeters https://docs.luxonis.com/software/api/python/#depthai.CalibrationHandler.getCameraExtrinsics + # Needs to be in centimeters + # https://docs.luxonis.com/software/api/python/#depthai.CalibrationHandler.getCameraExtrinsics + T_tof_to_left[:3, 3] *= 100 T_left_to_tof = np.linalg.inv(T_tof_to_left) ch.setCameraExtrinsics( @@ -344,7 +338,9 @@ def flash(self, calib_json_file: str, tof=False) -> None: right_to_left = camera_poses["right_to_left"] R_right_to_left = np.array(right_to_left["R"]) T_right_to_left = np.array(right_to_left["T"]) - T_right_to_left *= 100 # Needs to be in centimeters https://docs.luxonis.com/software/api/python/#depthai.CalibrationHandler.getCameraExtrinsics + # Needs to be in centimeters + # https://docs.luxonis.com/software/api/python/#depthai.CalibrationHandler.getCameraExtrinsics + T_right_to_left *= 100 R_left_to_right, T_left_to_right = get_inv_R_T(R_right_to_left, T_right_to_left) diff --git a/pollen_vision/pollen_vision/utils/__init__.py b/pollen_vision/pollen_vision/utils/__init__.py index c2fc41b..860da6c 100644 --- a/pollen_vision/pollen_vision/utils/__init__.py +++ b/pollen_vision/pollen_vision/utils/__init__.py @@ -1,6 +1,7 @@ from .annotator import Annotator # noqa from .labels import Labels # noqa from .objects_filter import ObjectsFilter # noqa +from .recorder import FPS, Recorder # noqa # from .pcl_visualizer import PCLVisualizer # noqa from .utils import ( # noqa diff --git a/pollen_vision/pollen_vision/utils/pcl_visualizer.py b/pollen_vision/pollen_vision/utils/pcl_visualizer.py index 7dd5400..531d527 100644 --- a/pollen_vision/pollen_vision/utils/pcl_visualizer.py +++ b/pollen_vision/pollen_vision/utils/pcl_visualizer.py @@ -1,3 +1,5 @@ +from typing import Optional + import numpy as np import numpy.typing as npt @@ -9,7 +11,7 @@ class PCLVisualizer: - def __init__(self, K: npt.NDArray[np.float64], name: str = "PCLVisualizer") -> None: + def __init__(self, K: npt.NDArray[np.float64] = np.eye(3), name: str = "PCLVisualizer") -> None: self.vis = o3d.visualization.Visualizer() self.vis.create_window(name) self.pcd = o3d.geometry.PointCloud() @@ -36,7 +38,7 @@ def create_point_cloud_from_rgbd( # pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) # Filter out points that are too close - pcd = pcd.select_by_index(np.where(np.array(pcd.points)[:, 2] > 0.4)[0]) + # pcd = pcd.select_by_index(np.where(np.array(pcd.points)[:, 2] > 0.4)[0]) # pcd = pcd.select_by_index(np.where(np.array(pcd.points)[:, 2] < 1.8)[0]) return pcd @@ -50,10 +52,33 @@ def add_frame(self, name: str, pose: npt.NDArray[np.float64] = np.eye(4)) -> Non self.frames[name] = {"mesh": mesh, "pose": pose} - def update(self, rgb: npt.NDArray[np.uint8], depth: npt.NDArray[np.float32]) -> None: - new_pcd = self.create_point_cloud_from_rgbd(rgb, depth) - self.pcd.points = new_pcd.points - self.pcd.colors = new_pcd.colors + def update( + self, + rgb: npt.NDArray[np.uint8], + depth: Optional[npt.NDArray[np.float32]] = None, + points: Optional[npt.NDArray[np.float64]] = None, + ) -> None: + """ + Can either take in depth image or points list directly (shape (N, 3))) + Please provide either depth or points, not both, not neithers + """ + if depth is None and points is None: + print("No depth or points provided") + print("Please provide either depth or points") + return + if depth is not None and points is not None: + print("Both depth and points provided") + print("Please provide either depth or points") + return + + if depth is not None: + new_pcd = self.create_point_cloud_from_rgbd(rgb, depth) + self.pcd.points = new_pcd.points + self.pcd.colors = new_pcd.colors + else: + self.pcd.points = o3d.utility.Vector3dVector(points) + colors = (rgb.reshape(-1, 3) / 255.0).astype(np.float64) + self.pcd.colors = o3d.utility.Vector3dVector(colors) if not self.set_geometry: self.vis.add_geometry(self.pcd) diff --git a/scripts/recorder.py b/pollen_vision/pollen_vision/utils/recorder.py similarity index 100% rename from scripts/recorder.py rename to pollen_vision/pollen_vision/utils/recorder.py diff --git a/scripts/RECORD_SR.py b/scripts/RECORD_SR.py index 9c91fcd..d524d95 100644 --- a/scripts/RECORD_SR.py +++ b/scripts/RECORD_SR.py @@ -7,7 +7,7 @@ import numpy as np from pollen_vision.camera_wrappers.depthai import SDKWrapper from pollen_vision.camera_wrappers.depthai.utils import get_config_file_path -from recorder import FPS, Recorder +from pollen_vision.utils import FPS, Recorder argParser = argparse.ArgumentParser(description="record sr") argParser.add_argument( diff --git a/scripts/chatgpt.py b/scripts/chatgpt.py deleted file mode 100644 index e8e963e..0000000 --- a/scripts/chatgpt.py +++ /dev/null @@ -1,94 +0,0 @@ -import depthai as dai -import cv2 -import numpy as np -import time - - -# Define a function to colorize the depth map -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor).astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - depthFrameColor[invalidMask] = 0 - except IndexError: - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - -# Create a pipeline -pipeline = dai.Pipeline() - -# Create RGB and ToF camera nodes -camRgb = pipeline.create(dai.node.ColorCamera) -camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_C) -camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1200_P) -camRgb.setFps(30) - -camTof = pipeline.create(dai.node.Camera) -camTof.setFps(30) -camTof.setBoardSocket(dai.CameraBoardSocket.CAM_D) - -# Create ToF node -tof = pipeline.create(dai.node.ToF) -camTof.raw.link(tof.input) - -# Create align node to align ToF depth to RGB -align = pipeline.create(dai.node.ImageAlign) -tof.depth.link(align.input) -camRgb.isp.link(align.inputAlignTo) - -# Create outputs -rgbOut = pipeline.create(dai.node.XLinkOut) -rgbOut.setStreamName("rgb") -alignOut = pipeline.create(dai.node.XLinkOut) -alignOut.setStreamName("depth_aligned") - -camRgb.isp.link(rgbOut.input) -align.outputAligned.link(alignOut.input) - -# Function to update blend weights for depth and RGB overlay -rgbWeight = 0.4 -depthWeight = 0.6 - - -def updateBlendWeights(percentRgb): - global depthWeight, rgbWeight - rgbWeight = float(percentRgb) / 100.0 - depthWeight = 1.0 - rgbWeight - - -# Start the pipeline -with dai.Device(pipeline) as device: - qRgb = device.getOutputQueue("rgb", 8, False) - qDepthAligned = device.getOutputQueue("depth_aligned", 8, False) - - cv2.namedWindow("RGB-Depth") - cv2.createTrackbar("RGB Weight %", "RGB-Depth", int(rgbWeight * 100), 100, updateBlendWeights) - - while True: - inRgb = qRgb.get() - inDepthAligned = qDepthAligned.get() - - frameRgb = inRgb.getCvFrame() - frameDepthAligned = inDepthAligned.getCvFrame() - - depthColorized = colorizeDepth(frameDepthAligned) - - # Blend the RGB image with the colorized depth image - blended = cv2.addWeighted(frameRgb, rgbWeight, depthColorized, depthWeight, 0) - cv2.imshow("RGB-Depth", blended) - - key = cv2.waitKey(1) - if key == ord("q"): - break diff --git a/scripts/demo_webcam.py b/scripts/demo_webcam.py deleted file mode 100644 index e69de29..0000000 diff --git a/scripts/read_calib.json b/scripts/read_calib.json deleted file mode 100644 index 404dad8..0000000 --- a/scripts/read_calib.json +++ /dev/null @@ -1,247 +0,0 @@ -{ - "batchName": "", - "batchTime": 0, - "boardConf": "", - "boardCustom": "", - "boardName": "", - "boardOptions": 0, - "boardRev": "", - "cameraData": [ - [ - 2, - { - "cameraType": 1, - "distortionCoeff": [ - -0.00550349336117506, - 0.010187107138335705, - -0.011511093005537987, - 0.002370026195421815, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "extrinsics": { - "rotationMatrix": [ - [ - 0.9998169541358948, - 0.019127150997519493, - 0.0004626706358976662 - ], - [ - -0.019127311185002327, - 0.9998170137405396, - 0.0003426657058298588 - ], - [ - -0.0004560317611321807, - -0.00035145264700986445, - 0.9999998211860657 - ] - ], - "specTranslation": { - "x": -6.519075393676758, - "y": 0.21584674715995789, - "z": 0.004252283368259668 - }, - "toCameraSocket": 1, - "translation": { - "x": -6.519075393676758, - "y": 0.21584674715995789, - "z": 0.004252283368259668 - } - }, - "height": 1200, - "intrinsicMatrix": [ - [ - 613.396484375, - 0.0, - 865.1143188476563 - ], - [ - 0.0, - 612.13525390625, - 522.9628295898438 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "lensPosition": 0, - "specHfovDeg": 0.0, - "width": 1920 - } - ], - [ - 1, - { - "cameraType": 1, - "distortionCoeff": [ - 0.0007732227677479386, - -0.00818662904202938, - 0.002664593979716301, - -0.0010734152747318149, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "extrinsics": { - "rotationMatrix": [ - [ - 0.9998169541358948, - -0.019127311185002327, - -0.0004560317611321807 - ], - [ - 0.019127150997519493, - 0.9998170137405396, - -0.00035145264700986445 - ], - [ - 0.0004626706358976662, - 0.0003426657058298588, - 0.9999998211860657 - ] - ], - "specTranslation": { - "x": 6.522012710571289, - "y": -0.09111440926790237, - "z": -0.0013100611977279186 - }, - "toCameraSocket": 2, - "translation": { - "x": 6.522012710571289, - "y": -0.09111440926790237, - "z": -0.0013100611977279186 - } - }, - "height": 1200, - "intrinsicMatrix": [ - [ - 613.5316772460938, - 0.0, - 1061.9139404296875 - ], - [ - 0.0, - 611.7612915039063, - 590.1260986328125 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "lensPosition": 0, - "specHfovDeg": 0.0, - "width": 1920 - } - ] - ], - "deviceName": "", - "hardwareConf": "", - "housingExtrinsics": { - "rotationMatrix": [], - "specTranslation": { - "x": 0.0, - "y": 0.0, - "z": 0.0 - }, - "toCameraSocket": -1, - "translation": { - "x": 0.0, - "y": 0.0, - "z": 0.0 - } - }, - "imuExtrinsics": { - "rotationMatrix": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "specTranslation": { - "x": 0.0, - "y": 0.0, - "z": 0.0 - }, - "toCameraSocket": -1, - "translation": { - "x": 0.0, - "y": 0.0, - "z": 0.0 - } - }, - "miscellaneousData": [], - "productName": "", - "stereoEnableDistortionCorrection": false, - "stereoRectificationData": { - "leftCameraSocket": 2, - "rectifiedRotationLeft": [ - [ - 1.0, - 0.0, - 0.0 - ], - [ - 0.0, - 1.0, - 0.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "rectifiedRotationRight": [ - [ - 0.9998169541358948, - 0.019127150997519493, - 0.0004626706358976662 - ], - [ - -0.019127311185002327, - 0.9998170137405396, - 0.0003426657058298588 - ], - [ - -0.0004560317611321807, - -0.00035145264700986445, - 0.9999998211860657 - ] - ], - "rightCameraSocket": 1 - }, - "stereoUseSpecTranslation": true, - "version": 7, - "verticalCameraSocket": -1 -} diff --git a/scripts/align_example.py b/scripts/tof_align_example.py similarity index 99% rename from scripts/align_example.py rename to scripts/tof_align_example.py index bc0d034..ba2f903 100644 --- a/scripts/align_example.py +++ b/scripts/tof_align_example.py @@ -1,9 +1,10 @@ -import numpy as np -import cv2 -import depthai as dai import time from datetime import timedelta +import cv2 +import depthai as dai +import numpy as np + # This example is intended to run unchanged on an OAK-D-SR-PoE camera FPS = 30.0 diff --git a/scripts/align_example_corrected.py b/scripts/tof_align_example_corrected.py similarity index 92% rename from scripts/align_example_corrected.py rename to scripts/tof_align_example_corrected.py index 7f4ad1f..d8254a3 100644 --- a/scripts/align_example_corrected.py +++ b/scripts/tof_align_example_corrected.py @@ -1,9 +1,10 @@ -import numpy as np -import cv2 -import depthai as dai import time from datetime import timedelta +import cv2 +import depthai as dai +import numpy as np + # This example is intended to run unchanged on an OAK-D-SR-PoE camera FPS = 30.0 @@ -134,24 +135,12 @@ def updateBlendWeights(percentRgb): M2 = np.array(calibData.getCameraIntrinsics(RGB_SOCKET, *rgbSize)) D2 = np.array(calibData.getDistortionCoefficients(RGB_SOCKET)) - # try: - # T = np.array(calibData.getCameraTranslationVector(ALIGN_SOCKET, RGB_SOCKET, False)) * 10 # to mm for matching the depth - - # T = np.array(calibData.getCameraExtrinsics(ALIGN_SOCKET, RGB_SOCKET, False)) - # print(T) - # exit() - # except RuntimeError: - # print("caca") - # T = np.array([0.0, 0.0, 0.001]) - # try: R = np.array(calibData.getCameraExtrinsics(ALIGN_SOCKET, TOF_SOCKET, False))[0:3, 0:3] - # except RuntimeError: - # print("caca") - # R = np.eye(3) + TARGET_MATRIX = M1 lensPosition = calibData.getLensPosition(RGB_SOCKET) - except: + except Exception: raise fpsCounter = FPSCounter() while True: diff --git a/scripts/tof_point_cloud_luxonis.py b/scripts/tof_point_cloud_luxonis.py index 061b542..79714cd 100644 --- a/scripts/tof_point_cloud_luxonis.py +++ b/scripts/tof_point_cloud_luxonis.py @@ -1,20 +1,17 @@ -import depthai as dai -import numpy as np -import cv2 -import time -from datetime import timedelta import datetime import os import sys +from datetime import timedelta + +import cv2 +import depthai as dai +import numpy as np +import numpy.typing as npt try: import open3d as o3d except ImportError: - sys.exit( - "Critical dependency missing: Open3D. Please install it using the command: '{} -m pip install open3d' and then rerun the script.".format( - sys.executable - ) - ) + sys.exit("Open3D missing. Install it using the command: '{} -m pip install open3d'".format(sys.executable)) FPS = 30 @@ -71,7 +68,7 @@ out.setStreamName("out") -def colorizeDepth(frameDepth): +def colorizeDepth(frameDepth: npt.NDArray[np.float32]) -> npt.NDArray[np.uint8]: invalidMask = frameDepth == 0 # Log the depth, minDepth and maxDepth try: @@ -96,7 +93,7 @@ def colorizeDepth(frameDepth): depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) except Exception as e: raise e - return depthFrameColor + return depthFrameColor # type: ignore with dai.Device(pipeline) as device: @@ -115,10 +112,10 @@ def colorizeDepth(frameDepth): view_control = vis.get_view_control() while isRunning: - inMessage = q.get() - inColor = inMessage["rgb"] - inPointCloud = inMessage["pcl"] - inDepth = inMessage["depth_aligned"] + inMessage = q.get() # type: ignore + inColor = inMessage["rgb"] # type: ignore + inPointCloud = inMessage["pcl"] # type: ignore + inDepth = inMessage["depth_aligned"] # type: ignore cvColorFrame = inColor.getCvFrame() # Convert the frame to RGB @@ -143,7 +140,6 @@ def colorizeDepth(frameDepth): print(f"RGB point cloud saved to folder {new_output}") if inPointCloud: points = inPointCloud.getPoints().astype(np.float64) - points[:, 0] = -points[:, 0] # Invert X axis # points[:, 1] = -points[:, 1] # Invert Y axis # points[:, 2] = points[:, 2] / 1000.0 # Convert Z axis from mm to meters (if needed) diff --git a/scripts/vanilla_tof_test.py b/scripts/vanilla_tof_test.py index 583b259..d93534a 100644 --- a/scripts/vanilla_tof_test.py +++ b/scripts/vanilla_tof_test.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import time +from typing import Tuple + import cv2 import depthai as dai import numpy as np @@ -11,7 +13,7 @@ cvColorMap[0] = [0, 0, 0] -def create_pipeline(): +def create_pipeline() -> Tuple[dai.Pipeline, dai.RawToFConfig]: pipeline = dai.Pipeline() tof = pipeline.create(dai.node.ToF) @@ -73,21 +75,9 @@ def create_pipeline(): return pipeline, tofConfig -mouse_x, mouse_y = 0, 0 - - -# mouse callback function -def cb(event, x, y, flags, param): - global mouse_x, mouse_y - mouse_x = x - mouse_y = y - - -if __name__ == "__main__": +if __name__ == "__main__": # noqa # global mouse_x, mouse_y pipeline, tofConfig = create_pipeline() - cv2.namedWindow("Colorized depth") - cv2.setMouseCallback("Colorized depth", cb) with dai.Device(pipeline) as device: print("Connected cameras:", device.getConnectedCameraFeatures()) @@ -164,25 +154,21 @@ def cb(event, x, y, flags, param): print("median", tofConfig.median) print("===") imgFrame = qDepth.get() # blocking call, will wait until a new data has arrived - depth_map = imgFrame.getFrame() + depth_map = imgFrame.getFrame() # type: ignore max_depth = (tofConfig.phaseUnwrappingLevel + 1) * 1500 # 100MHz modulation freq. depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8) depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap) - depth_at_mouse = depth_map[mouse_y, mouse_x] - print(mouse_x, mouse_y, depth_at_mouse) - depth_colorized = cv2.circle(depth_colorized, (mouse_x, mouse_y), 5, (255, 255, 255), -1) - in_left = left_q.get() in_right = right_q.get() tof_amplitude_frame = tof_amplitude.get() - left_im = in_left.getCvFrame() - right_im = in_right.getCvFrame() + left_im = in_left.getCvFrame() # type: ignore + right_im = in_right.getCvFrame() # type: ignore - tof_amplitude_img = tof_amplitude_frame.getCvFrame().astype(np.float32) + tof_amplitude_img = tof_amplitude_frame.getCvFrame().astype(np.float32) # type: ignore cv2.imshow("left", left_im) cv2.imshow("right", right_im) From 2b66107aabeca0b468d5c51cb7429431fda20fc4 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 22 Oct 2024 16:00:41 +0200 Subject: [PATCH 11/17] CI --- scripts/tof_align_example_corrected.py | 28 +------------------------- 1 file changed, 1 insertion(+), 27 deletions(-) diff --git a/scripts/tof_align_example_corrected.py b/scripts/tof_align_example_corrected.py index d8254a3..24291cb 100644 --- a/scripts/tof_align_example_corrected.py +++ b/scripts/tof_align_example_corrected.py @@ -13,22 +13,6 @@ ALIGN_SOCKET = RGB_SOCKET -class FPSCounter: - def __init__(self): - self.frameTimes = [] - - def tick(self): - now = time.time() - self.frameTimes.append(now) - self.frameTimes = self.frameTimes[-100:] - - def getFps(self): - if len(self.frameTimes) <= 1: - return 0 - # Calculate the FPS - return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) - - pipeline = dai.Pipeline() # Define sources and outputs camRgb = pipeline.create(dai.node.ColorCamera) @@ -142,10 +126,8 @@ def updateBlendWeights(percentRgb): lensPosition = calibData.getLensPosition(RGB_SOCKET) except Exception: raise - fpsCounter = FPSCounter() while True: messageGroup = queue.get() - fpsCounter.tick() assert isinstance(messageGroup, dai.MessageGroup) frameRgb = messageGroup["rgb"] assert isinstance(frameRgb, dai.ImgFrame) @@ -160,15 +142,7 @@ def updateBlendWeights(percentRgb): # Colorize the aligned depth alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) # Resize depth to match the rgb frame - cv2.putText( - alignedDepthColorized, - f"FPS: {fpsCounter.getFps():.2f}", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (255, 255, 255), - 2, - ) + cv2.imshow("depth", alignedDepthColorized) key = cv2.waitKey(1) if key == ord("m"): From 2a23506e583327f25871ff0a14279ab721f2c863 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 22 Oct 2024 16:13:51 +0200 Subject: [PATCH 12/17] CI --- scripts/tof_align_example_corrected.py | 1 + 1 file changed, 1 insertion(+) diff --git a/scripts/tof_align_example_corrected.py b/scripts/tof_align_example_corrected.py index 24291cb..a883d7c 100644 --- a/scripts/tof_align_example_corrected.py +++ b/scripts/tof_align_example_corrected.py @@ -31,6 +31,7 @@ camRgb.setBoardSocket(RGB_SOCKET) camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) camRgb.setFps(FPS) + camRgb.setIspScale(1, 2) # depthSize = (1280, 800) # PLEASE SET TO BE SIZE OF THE TOF STREAM From c157dc1d1219ba5d619735571a2ac087a3f5d7f1 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 22 Oct 2024 16:15:27 +0200 Subject: [PATCH 13/17] CI --- .../camera_wrappers/depthai/tof.py | 16 ++--- .../camera_wrappers/depthai/utils.py | 4 +- .../camera_wrappers/depthai/wrapper.py | 6 +- .../camera_wrappers/orbbec/orbbec_wrapper.py | 33 +--------- scripts/tof_align_example.py | 63 +------------------ scripts/tof_align_example_corrected.py | 32 +--------- scripts/tof_point_cloud_luxonis.py | 33 +--------- 7 files changed, 23 insertions(+), 164 deletions(-) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index ae5452a..e7e6cfc 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -13,7 +13,7 @@ from pollen_vision.camera_wrappers.depthai.wrapper import DepthaiWrapper -class TOFWrapper(DepthaiWrapper): +class TOFWrapper(DepthaiWrapper): # type: ignore def __init__( self, cam_config_json: str, @@ -21,9 +21,9 @@ def __init__( force_usb2: bool = False, mx_id: str = "", crop: bool = False, - noalign=False, - rectify=False, - create_pointcloud=False, + noalign: bool = False, + rectify: bool = False, + create_pointcloud: bool = False, ) -> None: """ Using create_pointcloud mode, we can't undistort at the same time (not enough resources on the device) @@ -45,7 +45,9 @@ def __init__( self.cam_config.undistort_resolution = (640, 480) self.crop = crop - def crop_image(self, im, depth): + def crop_image( + self, im: npt.NDArray[np.uint8], depth: npt.NDArray[np.float32] + ) -> Tuple[npt.NDArray[np.uint8], npt.NDArray[np.float32]]: # threshold depth thresholded_depth = np.where(depth > 0, 1, 0).astype(np.uint8) depth_mask_bounding_box = cv2.boundingRect(thresholded_depth) @@ -84,7 +86,7 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], if self.crop: cropped_left, cropped_depth = self.crop_image(left, depth) data["left"] = cropped_left - data["depth"] = cropped_depth + data["depth"] = cropped_depth # type: ignore else: data["left"] = left data["depth"] = depth @@ -193,7 +195,7 @@ def _create_queues(self) -> Dict[str, dai.DataOutputQueue]: print(dai.__version__) while True: - data, _, _ = t.get_data() + data, _, _ = t.get_data() # type: ignore left = data["left"] depth = data["depth"] diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py index c588f31..e76b3eb 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py @@ -131,7 +131,7 @@ def get_config_file_path(name: str) -> Any: return None -def colorizeDepth(frameDepth): +def colorizeDepth(frameDepth: npt.NDArray[np.float32]) -> npt.NDArray[np.uint8]: invalidMask = frameDepth == 0 # Log the depth, minDepth and maxDepth try: @@ -156,4 +156,4 @@ def colorizeDepth(frameDepth): depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) except Exception as e: raise e - return depthFrameColor + return depthFrameColor # type: ignore[return-value] diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py index b2c7aab..a6c56b1 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py @@ -88,8 +88,8 @@ def _prepare(self) -> None: tmp = cam break - width = tmp.width - height = tmp.height + width = tmp.width # type: ignore + height = tmp.height # type: ignore # # Assuming both cameras are the same # width = connected_cameras_features[-1].width @@ -153,7 +153,7 @@ def _create_pipeline(self) -> dai.Pipeline: """Abstract method that is implemented by the subclasses.""" pass - def _pipeline_basis(self, create_imagemanip=True) -> dai.Pipeline: + def _pipeline_basis(self, create_imagemanip: bool = True) -> dai.Pipeline: """Creates and configures the left and right cameras and the image manip nodes. This method is used (and/or extended) by the subclasses to create the basis pipeline. diff --git a/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py b/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py index 3548f66..3886e65 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py +++ b/pollen_vision/pollen_vision/camera_wrappers/orbbec/orbbec_wrapper.py @@ -5,6 +5,7 @@ import numpy as np import numpy.typing as npt from pollen_vision.camera_wrappers import CameraWrapper +from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth from pyorbbecsdk import ( AlignFilter, Config, @@ -170,40 +171,10 @@ def get_D(self) -> npt.NDArray[np.float32]: o = OrbbecWrapper() import time - def colorizeDepth(frameDepth: npt.NDArray[np.float32]) -> npt.NDArray[np.float64]: - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - # cv2.namedWindow("depth") - # cv2.setMouseCallback("depth", cv2_callback) - while True: data, _, _ = o.get_data() # type: ignore if "depth" in data: - cv2.imshow("depth", colorizeDepth(data["depth"])) # type: ignore + cv2.imshow("depth", colorizeDepth(data["depth"])) depth_value = data["depth"][mouse_y, mouse_x] print(depth_value) if "left" in data: diff --git a/scripts/tof_align_example.py b/scripts/tof_align_example.py index ba2f903..0afbd04 100644 --- a/scripts/tof_align_example.py +++ b/scripts/tof_align_example.py @@ -1,9 +1,8 @@ -import time from datetime import timedelta import cv2 import depthai as dai -import numpy as np +from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth # This example is intended to run unchanged on an OAK-D-SR-PoE camera FPS = 30.0 @@ -12,23 +11,6 @@ TOF_SOCKET = dai.CameraBoardSocket.CAM_D ALIGN_SOCKET = RGB_SOCKET - -class FPSCounter: - def __init__(self): - self.frameTimes = [] - - def tick(self): - now = time.time() - self.frameTimes.append(now) - self.frameTimes = self.frameTimes[-100:] - - def getFps(self): - if len(self.frameTimes) <= 1: - return 0 - # Calculate the FPS - return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) - - pipeline = dai.Pipeline() # Define sources and outputs camRgb = pipeline.create(dai.node.ColorCamera) @@ -73,40 +55,11 @@ def getFps(self): camRgb.isp.link(align.inputAlignTo) sync.out.link(out.input) - -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - rgbWeight = 0.4 depthWeight = 0.6 -def updateBlendWeights(percentRgb): +def updateBlendWeights(percentRgb: float) -> None: """ Update the rgb and depth weights used to blend depth/rgb image @@ -133,10 +86,8 @@ def updateBlendWeights(percentRgb): 100, updateBlendWeights, ) - fpsCounter = FPSCounter() while True: messageGroup = queue.get() - fpsCounter.tick() assert isinstance(messageGroup, dai.MessageGroup) frameRgb = messageGroup["rgb"] assert isinstance(frameRgb, dai.ImgFrame) @@ -151,15 +102,7 @@ def updateBlendWeights(percentRgb): # Colorize the aligned depth alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) # Resize depth to match the rgb frame - cv2.putText( - alignedDepthColorized, - f"FPS: {fpsCounter.getFps():.2f}", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (255, 255, 255), - 2, - ) + cv2.imshow("depth", alignedDepthColorized) blended = cv2.addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0) diff --git a/scripts/tof_align_example_corrected.py b/scripts/tof_align_example_corrected.py index a883d7c..48af785 100644 --- a/scripts/tof_align_example_corrected.py +++ b/scripts/tof_align_example_corrected.py @@ -1,9 +1,9 @@ -import time from datetime import timedelta import cv2 import depthai as dai import numpy as np +from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth # This example is intended to run unchanged on an OAK-D-SR-PoE camera FPS = 30.0 @@ -53,39 +53,11 @@ sync.out.link(out.input) -def colorizeDepth(frameDepth): - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor - - rgbWeight = 0.4 depthWeight = 0.6 -def updateBlendWeights(percentRgb): +def updateBlendWeights(percentRgb: float) -> None: """ Update the rgb and depth weights used to blend depth/rgb image diff --git a/scripts/tof_point_cloud_luxonis.py b/scripts/tof_point_cloud_luxonis.py index 79714cd..124fb91 100644 --- a/scripts/tof_point_cloud_luxonis.py +++ b/scripts/tof_point_cloud_luxonis.py @@ -6,7 +6,7 @@ import cv2 import depthai as dai import numpy as np -import numpy.typing as npt +from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth try: import open3d as o3d @@ -67,35 +67,6 @@ sync.out.link(out.input) out.setStreamName("out") - -def colorizeDepth(frameDepth: npt.NDArray[np.float32]) -> npt.NDArray[np.uint8]: - invalidMask = frameDepth == 0 - # Log the depth, minDepth and maxDepth - try: - minDepth = np.percentile(frameDepth[frameDepth != 0], 3) - maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) - logMinDepth = np.log(minDepth) - logMaxDepth = np.log(maxDepth) - np.nan_to_num(logDepth, copy=False, nan=logMinDepth) - # Clip the values to be in the 0-255 range - logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) - - # Interpolate only valid logDepth values, setting the rest based on the mask - depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) - depthFrameColor = np.nan_to_num(depthFrameColor) - depthFrameColor = depthFrameColor.astype(np.uint8) - depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) - # Set invalid depth pixels to black - depthFrameColor[invalidMask] = 0 - except IndexError: - # Frame is likely empty - depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) - except Exception as e: - raise e - return depthFrameColor # type: ignore - - with dai.Device(pipeline) as device: isRunning = True @@ -112,7 +83,7 @@ def colorizeDepth(frameDepth: npt.NDArray[np.float32]) -> npt.NDArray[np.uint8]: view_control = vis.get_view_control() while isRunning: - inMessage = q.get() # type: ignore + inMessage = q.get() inColor = inMessage["rgb"] # type: ignore inPointCloud = inMessage["pcl"] # type: ignore inDepth = inMessage["depth_aligned"] # type: ignore From 504512bfbc53fe3ecdf3db8b0b89dfd6c194fe34 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 22 Oct 2024 17:21:33 +0200 Subject: [PATCH 14/17] CI --- scripts/tof_align_example.py | 1 + 1 file changed, 1 insertion(+) diff --git a/scripts/tof_align_example.py b/scripts/tof_align_example.py index 0afbd04..cd246e8 100644 --- a/scripts/tof_align_example.py +++ b/scripts/tof_align_example.py @@ -79,6 +79,7 @@ def updateBlendWeights(percentRgb: float) -> None: rgbDepthWindowName = "rgb-depth" cv2.namedWindow(rgbDepthWindowName) + cv2.createTrackbar( "RGB Weight %", rgbDepthWindowName, From 4ad55643d450f4ad5bf992181b06ae8f256113e8 Mon Sep 17 00:00:00 2001 From: apirrone Date: Tue, 22 Oct 2024 17:22:12 +0200 Subject: [PATCH 15/17] CI --- scripts/tof_align_example.py | 2 +- scripts/tof_align_example_corrected.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/scripts/tof_align_example.py b/scripts/tof_align_example.py index cd246e8..809ec81 100644 --- a/scripts/tof_align_example.py +++ b/scripts/tof_align_example.py @@ -80,7 +80,7 @@ def updateBlendWeights(percentRgb: float) -> None: cv2.namedWindow(rgbDepthWindowName) - cv2.createTrackbar( + cv2.createTrackbar( # type: ignore[attr-defined] "RGB Weight %", rgbDepthWindowName, int(rgbWeight * 100), diff --git a/scripts/tof_align_example_corrected.py b/scripts/tof_align_example_corrected.py index 48af785..b5b016d 100644 --- a/scripts/tof_align_example_corrected.py +++ b/scripts/tof_align_example_corrected.py @@ -78,7 +78,7 @@ def updateBlendWeights(percentRgb: float) -> None: rgbDepthWindowName = "rgb-depth" cv2.namedWindow(rgbDepthWindowName) - cv2.createTrackbar( + cv2.createTrackbar( # type: ignore[attr-defined] "RGB Weight %", rgbDepthWindowName, int(rgbWeight * 100), @@ -128,7 +128,7 @@ def updateBlendWeights(percentRgb: float) -> None: if remapping: # mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, None, M2, rgbSize, cv2.CV_32FC1) - mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, R, M2, rgbSize, cv2.CV_32FC1) + mapX, mapY = cv2.fisheye.initUndistortRectifyMap(M2, D2, R, M2, rgbSize, cv2.CV_32FC1) # type: ignore rgbFrame = cv2.remap(rgbFrame, mapX, mapY, cv2.INTER_LINEAR) print(rgbFrame.shape) alignedDepthColorized = cv2.resize(alignedDepthColorized, (rgbFrame.shape[1], rgbFrame.shape[0])) From 3bf8d26a5d9c436191bca07f2e85626084b1fdec Mon Sep 17 00:00:00 2001 From: apirrone Date: Wed, 23 Oct 2024 09:41:15 +0200 Subject: [PATCH 16/17] removing useless function in tof.py --- pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py index e7e6cfc..7c87ccc 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -98,9 +98,6 @@ def get_data(self) -> Tuple[Dict[str, npt.NDArray[np.uint8]], Dict[str, float], data["pointcloud"] = points return data, latency, ts - def get_K(self) -> npt.NDArray[np.float32]: - return super().get_K(left=True) # type: ignore - def _create_pipeline(self) -> dai.Pipeline: pipeline = self._pipeline_basis(create_imagemanip=not self.create_pointcloud) From 2d4c76252e51c52043a0712f399fe46a9dc29b9a Mon Sep 17 00:00:00 2001 From: apirrone Date: Wed, 8 Oct 2025 10:40:05 +0200 Subject: [PATCH 17/17] show detected aruco tags in acquire.py --- .../depthai/calibration/acquire.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py index 15dade1..2abaa95 100644 --- a/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py @@ -2,6 +2,7 @@ import os import cv2 +from cv2 import aruco import numpy as np from pollen_vision.camera_wrappers.depthai import SDKWrapper, TOFWrapper from pollen_vision.camera_wrappers.depthai.utils import ( @@ -41,6 +42,8 @@ tof_path = os.path.join(args.imagesPath, "tof") os.makedirs(tof_path, exist_ok=True) +ARUCO_DICT = aruco.getPredefinedDictionary(aruco.DICT_4X4_1000) + print("Press return to save an image pair.") print("(Keep the focus on the opencv window for the inputs to register.)") print("Press esc or q to exit.") @@ -52,13 +55,25 @@ for name in data.keys(): _data[name] = data[name] + display_l, display_r = _data["left"].copy(), _data["right"].copy() + + lcorners, lids, _ = aruco.detectMarkers(image=display_l, dictionary=ARUCO_DICT) + rcorners, rids, _ = aruco.detectMarkers(image=display_r, dictionary=ARUCO_DICT) + + # draw detected markers + if len(lcorners) > 0: + # thick lines for better visibility + aruco.drawDetectedMarkers(display_l, lcorners, lids, borderColor=(0, 0, 255)) + if len(rcorners) > 0: + aruco.drawDetectedMarkers(display_r, rcorners, rids, borderColor=(0, 0, 255)) + if args.tof: tof_intensity = _data["tof_intensity"] tof_intensity = cv2.resize(tof_intensity, _data["left"].shape[:2][::-1]) tof_intensity = np.dstack((tof_intensity, tof_intensity, tof_intensity)) - concat = np.hstack((_data["left"], _data["right"], tof_intensity)) + concat = np.hstack((display_l, display_r, tof_intensity)) else: - concat = np.hstack((_data["left"], _data["right"])) + concat = np.hstack((display_l, display_r)) cv2.imshow("concat", cv2.resize(concat, (0, 0), fx=0.5, fy=0.5)) key = cv2.waitKey(1) if key == 13: