From 4cb74a953aaf04bff517c5d758fe0c4f2d6cf011 Mon Sep 17 00:00:00 2001 From: Marco Paladini Date: Mon, 8 Aug 2022 16:16:58 +0100 Subject: [PATCH 1/2] Demo program to save point cloud and rectified image data. --- gen2-pointcloud/save_point_cloud/README.md | 28 +++ gen2-pointcloud/save_point_cloud/main.py | 225 ++++++++++++++++++ .../save_point_cloud/o3d_view_pcd.py | 13 + 3 files changed, 266 insertions(+) create mode 100644 gen2-pointcloud/save_point_cloud/README.md create mode 100644 gen2-pointcloud/save_point_cloud/main.py create mode 100644 gen2-pointcloud/save_point_cloud/o3d_view_pcd.py diff --git a/gen2-pointcloud/save_point_cloud/README.md b/gen2-pointcloud/save_point_cloud/README.md new file mode 100644 index 000000000..1b9e42b9a --- /dev/null +++ b/gen2-pointcloud/save_point_cloud/README.md @@ -0,0 +1,28 @@ +## Save colored point cloud demo + +Capture and display color and depth aligned, plus the rectified stereo images. +Press 'q' on any image window to exit. + +Before terminating, the program will save: +- point cloud file in .pcd format +- depth image as 16-bit .png +- rgb image as .png +- rectified Left stereo image as .png +- rectified Right stereo image as .png +- intrinsics used to compute the point cloud as .json +- full camera calibration info as .json + +## Usage: + +```bash +python main.py +``` + +data will be saved only when quitting the viewer. + +## Optional, view the point cloud with open3d: + +```bash +python o3d_view_pcd.py *.pcd +``` + diff --git a/gen2-pointcloud/save_point_cloud/main.py b/gen2-pointcloud/save_point_cloud/main.py new file mode 100644 index 000000000..97b98bfa8 --- /dev/null +++ b/gen2-pointcloud/save_point_cloud/main.py @@ -0,0 +1,225 @@ +"""Display depth+rgb aligned and save to point cloud file. +Adapted from https://github.com/marco-paladini/depthai-python/blob/main/examples/StereoDepth/rgb_depth_aligned.py""" +#!/usr/bin/env python3 + +import json +import time + +import cv2 +import depthai as dai +import numpy as np +import open3d as o3d + +# Weights to use when blending depth/rgb image (should equal 1.0) +rgbWeight = 0.4 +depthWeight = 0.6 + + +def updateBlendWeights(percent_rgb): + """ + 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(percent_rgb) / 100.0 + depthWeight = 1.0 - rgbWeight + + +# Optional. If set (True), the ColorCamera is downscaled from 1080p to 720p. +# Otherwise (False), the aligned depth is automatically upscaled to 1080p +downscaleColor = True +fps = 30 +# The disparity is computed at this resolution, then upscaled to RGB resolution +monoResolution = dai.MonoCameraProperties.SensorResolution.THE_720_P +# widhth and height that match `monoResolution` above +width = 1280 +height = 720 +# max depth (only used for visualisation) in millimeters +visualisation_max_depth = 4000 + +# Create pipeline +pipeline = dai.Pipeline() +device = dai.Device() +queueNames = [] + +# Define sources and outputs +camRgb = pipeline.create(dai.node.ColorCamera) +left = pipeline.create(dai.node.MonoCamera) +right = pipeline.create(dai.node.MonoCamera) +stereo = pipeline.create(dai.node.StereoDepth) + +rgbOut = pipeline.create(dai.node.XLinkOut) +depthOut = pipeline.create(dai.node.XLinkOut) + + +rgbOut.setStreamName("rgb") +queueNames.append("rgb") +depthOut.setStreamName("depth") +queueNames.append("depth") + +# Properties +camRgb.setBoardSocket(dai.CameraBoardSocket.RGB) +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) +camRgb.setFps(fps) +if downscaleColor: + camRgb.setIspScale(2, 3) +# For now, RGB needs fixed focus to properly align with depth. +# This value was used during calibration +try: + calibData = device.readCalibration2() + lensPosition = calibData.getLensPosition(dai.CameraBoardSocket.RGB) + if lensPosition: + camRgb.initialControl.setManualFocus(lensPosition) +except: + raise +left.setResolution(monoResolution) +left.setBoardSocket(dai.CameraBoardSocket.LEFT) +left.setFps(fps) +right.setResolution(monoResolution) +right.setBoardSocket(dai.CameraBoardSocket.RIGHT) +right.setFps(fps) + +stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) +# LR-check is required for depth alignment +stereo.setLeftRightCheck(True) +stereo.setDepthAlign(dai.CameraBoardSocket.RGB) + +# Linking +camRgb.isp.link(rgbOut.input) +left.out.link(stereo.left) +right.out.link(stereo.right) +stereo.depth.link(depthOut.input) +# rectified stereo +xoutRectifLeft = pipeline.create(dai.node.XLinkOut) +xoutRectifRight = pipeline.create(dai.node.XLinkOut) +xoutRectifLeft.setStreamName("rectifiedLeft") +xoutRectifRight.setStreamName("rectifiedRight") +stereo.rectifiedLeft.link(xoutRectifLeft.input) +stereo.rectifiedRight.link(xoutRectifRight.input) + +# Connect to device and start pipeline +with device: + serial_number = device.getMxId() + timestamp = str(int(time.time())) + calibFile = f"calibration_{serial_number}_{timestamp}.json" + calibData = device.readCalibration() + calibData.eepromToJsonFile(calibFile) + print("wrote", calibFile) + + M_rgb = np.array( + calibData.getCameraIntrinsics(dai.CameraBoardSocket.RGB, width, height) + ) + + device.startPipeline(pipeline) + + frameRgb = None + frameDepth = None + + # Configure windows; trackbar adjusts blending ratio of rgb/depth + rgbWindowName = "rgb" + depthWindowName = "depth" + blendedWindowName = "rgb-depth" + cv2.namedWindow(rgbWindowName) + cv2.namedWindow(depthWindowName) + cv2.namedWindow(blendedWindowName) + cv2.createTrackbar( + "RGB Weight %", blendedWindowName, int(rgbWeight * 100), 100, updateBlendWeights + ) + + while True: + latestPacket = { + "rgb": None, + "depth": None, + "rectifiedLeft": None, + "rectifiedRight": None, + } + + queueEvents = device.getQueueEvents( + ("rgb", "depth", "rectifiedLeft", "rectifiedRight") + ) + for queueName in queueEvents: + packets = device.getOutputQueue(queueName).tryGetAll() + if len(packets) > 0: + latestPacket[queueName] = packets[-1] + + if latestPacket["rgb"] is not None: + frameRgb = latestPacket["rgb"].getCvFrame() + raw_rgb = frameRgb.copy() + cv2.imshow(rgbWindowName, frameRgb) + + if latestPacket["rectifiedLeft"] is not None: + raw_left = latestPacket["rectifiedLeft"].getCvFrame().copy() + cv2.imshow("rectifiedLeft", raw_left) + + if latestPacket["rectifiedRight"] is not None: + raw_right = latestPacket["rectifiedRight"].getCvFrame().copy() + cv2.imshow("rectifiedRight", raw_right) + + if latestPacket["depth"] is not None: + frameDepth = latestPacket["depth"].getFrame() + raw_depth = frameDepth.copy().astype(np.uint16) + frameDepth = (frameDepth * 255.0 / visualisation_max_depth).astype(np.uint8) + frameDepth = cv2.applyColorMap(frameDepth, cv2.COLORMAP_HOT) + frameDepth = np.ascontiguousarray(frameDepth) + cv2.imshow(depthWindowName, frameDepth) + + # Blend when both received + if frameRgb is not None and frameDepth is not None: + # Need to have both frames in BGR format before blending + if len(frameDepth.shape) < 3: + frameDepth = cv2.cvtColor(frameDepth, cv2.COLOR_GRAY2BGR) + blended = cv2.addWeighted(frameRgb, rgbWeight, frameDepth, depthWeight, 0) + cv2.imshow(blendedWindowName, blended) + frameRgb = None + frameDepth = None + + if cv2.waitKey(1) == ord("q"): + # save all images + if cv2.imwrite(f"rgb_{serial_number}_{timestamp}.png", raw_rgb): + print("wrote", f"rgb_{serial_number}_{timestamp}.png") + if cv2.imwrite(f"depth_{serial_number}_{timestamp}.png", raw_depth): + print("wrote", f"depth_{serial_number}_{timestamp}.png") + if cv2.imwrite(f"rectifiedLeft_{serial_number}_{timestamp}.png", raw_left): + print("wrote", f"rectifiedLeft_{serial_number}_{timestamp}.png") + if cv2.imwrite( + f"rectifiedRight_{serial_number}_{timestamp}.png", raw_right + ): + print("wrote", f"rectifiedRight_{serial_number}_{timestamp}.png") + # compute point cloud + pcd = o3d.geometry.PointCloud.create_from_rgbd_image( + image=o3d.geometry.RGBDImage.create_from_color_and_depth( + o3d.geometry.Image(cv2.cvtColor(raw_rgb, cv2.COLOR_BGR2RGB)), + o3d.geometry.Image(raw_depth), + ), + intrinsic=o3d.camera.PinholeCameraIntrinsic( + width=width, + height=height, + fx=M_rgb[0][0], + fy=M_rgb[1][1], + cx=M_rgb[0][2], + cy=M_rgb[1, 2], + ), + ) + # save point cloud + if o3d.io.write_point_cloud( + f"{serial_number}_{timestamp}.pcd", pcd, compressed=True + ): + print("wrote", f"{serial_number}_{timestamp}.pcd") + # save intrinsics + with open( + f"{serial_number}_{timestamp}_metadata.json", "w", encoding="utf-8" + ) as outfile: + json.dump( + { + "serial": serial_number, + "fx": M_rgb[0][0], + "fy": M_rgb[1][1], + "cx": M_rgb[0][2], + "cy": M_rgb[1, 2], + }, + outfile, + ) + print("wrote", f"{serial_number}_{timestamp}_metadata.json") + break diff --git a/gen2-pointcloud/save_point_cloud/o3d_view_pcd.py b/gen2-pointcloud/save_point_cloud/o3d_view_pcd.py new file mode 100644 index 000000000..57368eec3 --- /dev/null +++ b/gen2-pointcloud/save_point_cloud/o3d_view_pcd.py @@ -0,0 +1,13 @@ +"""minimal point cloud viewer using open3d""" +import argparse + +import open3d as o3d + +parser = argparse.ArgumentParser(description="minimal point cloud viewer using open3d") +parser.add_argument("pcd", help="the .pcd point cloud file") +args = parser.parse_args() + +pcd = o3d.io.read_point_cloud(args.pcd) +print("loaded ", args.pcd) +print("press 'q' to exit") +o3d.visualization.draw_geometries([pcd]) From 7e1d96792f1f301a1367cfd93d751823e19731fa Mon Sep 17 00:00:00 2001 From: Marco Paladini Date: Tue, 9 Aug 2022 16:24:23 +0100 Subject: [PATCH 2/2] enable pattern projector, fix filenames --- gen2-pointcloud/save_point_cloud/main.py | 25 +++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/gen2-pointcloud/save_point_cloud/main.py b/gen2-pointcloud/save_point_cloud/main.py index 97b98bfa8..6f1bac143 100644 --- a/gen2-pointcloud/save_point_cloud/main.py +++ b/gen2-pointcloud/save_point_cloud/main.py @@ -39,6 +39,11 @@ def updateBlendWeights(percent_rgb): # max depth (only used for visualisation) in millimeters visualisation_max_depth = 4000 +serials = [] +for device in dai.Device.getAllAvailableDevices(): + serials.append(device.getMxId()) + print(f"{len(serials)}: {device.getMxId()} {device.state}") + # Create pipeline pipeline = dai.Pipeline() device = dai.Device() @@ -74,6 +79,7 @@ def updateBlendWeights(percent_rgb): camRgb.initialControl.setManualFocus(lensPosition) except: raise + left.setResolution(monoResolution) left.setBoardSocket(dai.CameraBoardSocket.LEFT) left.setFps(fps) @@ -101,9 +107,10 @@ def updateBlendWeights(percent_rgb): # Connect to device and start pipeline with device: + device.setIrLaserDotProjectorBrightness(1200) serial_number = device.getMxId() timestamp = str(int(time.time())) - calibFile = f"calibration_{serial_number}_{timestamp}.json" + calibFile = f"{serial_number}_{timestamp}_calibration.json" calibData = device.readCalibration() calibData.eepromToJsonFile(calibFile) print("wrote", calibFile) @@ -177,16 +184,16 @@ def updateBlendWeights(percent_rgb): if cv2.waitKey(1) == ord("q"): # save all images - if cv2.imwrite(f"rgb_{serial_number}_{timestamp}.png", raw_rgb): - print("wrote", f"rgb_{serial_number}_{timestamp}.png") - if cv2.imwrite(f"depth_{serial_number}_{timestamp}.png", raw_depth): - print("wrote", f"depth_{serial_number}_{timestamp}.png") - if cv2.imwrite(f"rectifiedLeft_{serial_number}_{timestamp}.png", raw_left): - print("wrote", f"rectifiedLeft_{serial_number}_{timestamp}.png") + if cv2.imwrite(f"{serial_number}_{timestamp}_rgb.png", raw_rgb): + print("wrote", f"{serial_number}_{timestamp}_rgb.png") + if cv2.imwrite(f"{serial_number}_{timestamp}_depth.png", raw_depth): + print("wrote", f"{serial_number}_{timestamp}_depth.png") + if cv2.imwrite(f"{serial_number}_{timestamp}_rectifiedLeft.png", raw_left): + print("wrote", f"{serial_number}_{timestamp}_rectifiedLeft.png") if cv2.imwrite( - f"rectifiedRight_{serial_number}_{timestamp}.png", raw_right + f"{serial_number}_{timestamp}_rectifiedRight.png", raw_right ): - print("wrote", f"rectifiedRight_{serial_number}_{timestamp}.png") + print("wrote", f"{serial_number}_{timestamp}_rectifiedRight.png") # compute point cloud pcd = o3d.geometry.PointCloud.create_from_rgbd_image( image=o3d.geometry.RGBDImage.create_from_color_and_depth(