diff --git a/examples/camera_wrappers_examples/live_pcl.py b/examples/camera_wrappers_examples/live_pcl.py index 2058c03..699ee09 100644 --- a/examples/camera_wrappers_examples/live_pcl.py +++ b/examples/camera_wrappers_examples/live_pcl.py @@ -1,13 +1,12 @@ import argparse 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, ) -from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer +from pollen_vision.utils.pcl_visualizer import PCLVisualizer valid_configs = get_config_files_names() @@ -23,23 +22,19 @@ 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) 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) key = cv2.waitKey(1) P.tick() 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 new file mode 100644 index 0000000..2759f77 --- /dev/null +++ b/examples/camera_wrappers_examples/live_pcl_tof.py @@ -0,0 +1,37 @@ +import argparse + +import cv2 +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() + +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), crop=False, fps=30, create_pointcloud=True) + +P = PCLVisualizer() +P.add_frame("origin") + + +while True: + data, lat, _ = w.get_data() + + rgb = data["left"] + + 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 new file mode 100644 index 0000000..c64de0f --- /dev/null +++ b/examples/camera_wrappers_examples/tof_example.py @@ -0,0 +1,25 @@ +import cv2 +import depthai as dai +from pollen_vision.camera_wrappers import TOFWrapper +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) + +print(dai.__version__) +while True: + data, _, _ = t.get_data() + left = data["left"] + + depth = data["depth"] + tof_intensity = data["tof_intensity"] + + cv2.imshow("left", left) + 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("colorized_depth", colorized_depth) + 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 new file mode 100644 index 0000000..74487a5 --- /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_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/__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/.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/calibration/acquire.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py index d75d24b..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,8 +2,9 @@ import os import cv2 +from cv2 import aruco 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,14 +25,24 @@ 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, noalign=True, rectify=False) + left_path = os.path.join(args.imagesPath, "left") 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) + +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.)") @@ -44,12 +55,32 @@ for name in data.keys(): _data[name] = data[name] - concat = np.hstack((_data["left"], _data["right"])) - cv2.imshow(name, cv2.resize(concat, (0, 0), fx=0.5, fy=0.5)) + 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((display_l, display_r, tof_intensity)) + else: + 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: 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_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 03567dc..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 @@ -2,8 +2,9 @@ import logging import cv2 +import numpy as np 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: @@ -35,8 +40,15 @@ 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) + + 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/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/tof.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py new file mode 100644 index 0000000..7c87ccc --- /dev/null +++ b/pollen_vision/pollen_vision/camera_wrappers/depthai/tof.py @@ -0,0 +1,207 @@ +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 ( + colorizeDepth, + get_config_file_path, + get_socket_from_name, +) +from pollen_vision.camera_wrappers.depthai.wrapper import DepthaiWrapper + + +class TOFWrapper(DepthaiWrapper): # type: ignore + def __init__( + self, + cam_config_json: str, + fps: int = 30, + force_usb2: bool = False, + mx_id: str = "", + crop: bool = 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) + """ + self.noalign = noalign + self.create_pointcloud = create_pointcloud + super().__init__( + cam_config_json, + fps, + force_usb2=force_usb2, + resize=None, + rectify=rectify, + 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] + self.cam_config.undistort_resolution = (640, 480) + self.crop = crop + + 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) + 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]]: + 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() + 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 + + if self.crop: + cropped_left, cropped_depth = self.crop_image(left, depth) + data["left"] = cropped_left + data["depth"] = cropped_depth # type: ignore + else: + data["left"] = left + data["depth"] = depth + + data["right"] = right + data["tof_intensity"] = tof_intensity + + if self.create_pointcloud: + data["pointcloud"] = points + return data, latency, ts + + def _create_pipeline(self) -> dai.Pipeline: + 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) + 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) + + if self.create_pointcloud: + self.pointcloud = pipeline.create(dai.node.PointCloud) + + # === Tof configuration === + self.tofConfig = self.tof.initialConfig.get() + self.tofConfig.enableFPPNCorrection = True + 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_7x7 + self.tofConfig.enableDistortionCorrection = True if self.cam_config.rectify else False + self.tof.initialConfig.set(self.tofConfig) + # ========================== + + 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) + + return self._link_pipeline(pipeline) + + def _create_output_streams(self, pipeline: dai.Pipeline) -> dai.Pipeline: + pipeline = super()._create_output_streams(pipeline) + + self.xout_sync = pipeline.create(dai.node.XLinkOut) + self.xout_sync.setStreamName("sync_out") + + return pipeline + + def _link_pipeline(self, pipeline: dai.Pipeline) -> dai.Pipeline: + 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"]) + + 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 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"]) + + 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) + + return pipeline + + def _create_queues(self) -> Dict[str, dai.DataOutputQueue]: + # queues: Dict[str, dai.DataOutputQueue] = super()._create_queues() + + queues: Dict[str, dai.DataOutputQueue] = {} + queues["sync_out"] = self._device.getOutputQueue("sync_out", maxSize=8, blocking=False) + + return queues + + +if __name__ == "__main__": + t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False, rectify=False, create_pointcloud=True) + + print(dai.__version__) + while True: + data, _, _ = t.get_data() # type: ignore + left = data["left"] + + depth = data["depth"] + tof_intensity = data["tof_intensity"] + + cv2.imshow("left", left) + 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("colorized_depth", colorized_depth) + cv2.waitKey(1) diff --git a/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py b/pollen_vision/pollen_vision/camera_wrappers/depthai/utils.py index abef1ce..e76b3eb 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: 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[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 0914008..a6c56b1 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 # type: ignore + height = tmp.height # type: ignore + + # # 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)) @@ -143,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: 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. @@ -176,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 @@ -222,7 +233,6 @@ def _create_imageManip( rectify: bool = True, ) -> dai.node.ImageManip: """Resize and optionally rectify an image""" - manip = pipeline.createImageManip() if rectify: @@ -235,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 @@ -246,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: 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. @@ -270,44 +280,82 @@ def flash(self, calib_json_file: str) -> 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 *= 0.01 # still don't know if I should do that or not + 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) + # 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) 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) + 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] = r + T_tof_to_left[:3, 3] = t + + # 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( + 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"]) - T_right_to_left *= 100 # Needs to be in centimeters (?) # TODO test + # 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) 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(), ) ch.setStereoLeft(left_socket, np.eye(3).tolist()) ch.setStereoRight(right_socket, R_right_to_left.tolist()) + # 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 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/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 45f4a00..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() @@ -35,6 +37,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: @@ -46,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/demo_webcam.py b/scripts/demo_webcam.py deleted file mode 100644 index e69de29..0000000 diff --git a/scripts/tof_align_example.py b/scripts/tof_align_example.py new file mode 100644 index 0000000..809ec81 --- /dev/null +++ b/scripts/tof_align_example.py @@ -0,0 +1,114 @@ +from datetime import timedelta + +import cv2 +import depthai as dai +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 + +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) + +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) + +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb: float) -> None: + """ + 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( # type: ignore[attr-defined] + "RGB Weight %", + rgbDepthWindowName, + int(rgbWeight * 100), + 100, + updateBlendWeights, + ) + while True: + messageGroup = queue.get() + 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.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_align_example_corrected.py b/scripts/tof_align_example_corrected.py new file mode 100644 index 0000000..b5b016d --- /dev/null +++ b/scripts/tof_align_example_corrected.py @@ -0,0 +1,140 @@ +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 + +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) + +# 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) + + +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percentRgb: float) -> None: + """ + 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( # type: ignore[attr-defined] + "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)) + + R = np.array(calibData.getCameraExtrinsics(ALIGN_SOCKET, TOF_SOCKET, False))[0:3, 0:3] + + TARGET_MATRIX = M1 + + lensPosition = calibData.getLensPosition(RGB_SOCKET) + except Exception: + raise + while True: + messageGroup = queue.get() + 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.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) # type: ignore + 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) + + key = cv2.waitKey(1) + if key == ord("q"): + break diff --git a/scripts/tof_point_cloud_luxonis.py b/scripts/tof_point_cloud_luxonis.py new file mode 100644 index 0000000..124fb91 --- /dev/null +++ b/scripts/tof_point_cloud_luxonis.py @@ -0,0 +1,128 @@ +import datetime +import os +import sys +from datetime import timedelta + +import cv2 +import depthai as dai +import numpy as np +from pollen_vision.camera_wrappers.depthai.utils import colorizeDepth + +try: + import open3d as o3d +except ImportError: + sys.exit("Open3D missing. Install it using the command: '{} -m pip install open3d'".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 +# tofConfig.enableDistortionCorrection = False +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") + +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"] # type: ignore + inPointCloud = inMessage["pcl"] # type: ignore + inDepth = inMessage["depth_aligned"] # type: ignore + + 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() diff --git a/scripts/vanilla_tof_test.py b/scripts/vanilla_tof_test.py new file mode 100644 index 0000000..d93534a --- /dev/null +++ b/scripts/vanilla_tof_test.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python3 + +import time +from typing import Tuple + +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() -> Tuple[dai.Pipeline, dai.RawToFConfig]: + 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) + + 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) + 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 + + +if __name__ == "__main__": # noqa + # global mouse_x, mouse_y + pipeline, tofConfig = create_pipeline() + + 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) + tof_amplitude = device.getOutputQueue(name="tof_amplitude", 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() # 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) + + in_left = left_q.get() + in_right = right_q.get() + + tof_amplitude_frame = tof_amplitude.get() + + left_im = in_left.getCvFrame() # type: ignore + right_im = in_right.getCvFrame() # type: ignore + + tof_amplitude_img = tof_amplitude_frame.getCvFrame().astype(np.float32) # type: ignore + + 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) + + device.close()