right = None
pcl_converter = None
while True:
nnet_packets, data_packets = pipeline.get_available_nnet_and_data_packets()
for packet in data_packets:
if packet.stream_name == "right":
right = packet.getData()
cv2.imshow(packet.stream_name, right)
elif packet.stream_name == "depth":
frame = packet.getData()
if right is not None:
if pcl_converter is None:
fd, path = tempfile.mkstemp(suffix='.json')
with os.fdopen(fd, 'w') as tmp:
json.dump({
"width": 1280,
"height": 720,
"intrinsic_matrix": [item for row in device.get_right_intrinsic() for item in row]
}, tmp)
pcl_converter = PointCloudVisualizer(path)
pcd = pcl_converter.rgbd_to_projection(frame, right)
pcl_converter.visualize_pcd()
cv2.imshow(packet.stream_name, frame)
if cv2.waitKey(1) == ord("q"):
break
if pcl_converter is not None:
pcl_converter.close_window()
print(" Subpixel: ", subpixel)
print(" Median filtering: ", median)
# TODO add API to read this from device / calib data
right_intrinsic = [[860.0, 0.0, 640.0], [0.0, 860.0, 360.0], [0.0, 0.0, 1.0]]
pcl_converter = None
if point_cloud:
if out_rectified:
try:
from projector_3d import PointCloudVisualizer
except ImportError as e:
raise ImportError(
f"\033[1;5;31mError occured when importing PCL projector: {e}. Try disabling the point cloud \033[0m "
)
pcl_converter = PointCloudVisualizer(right_intrinsic, 1280, 720)
else:
print("Disabling point-cloud visualizer, as out_rectified is not set")
def create_rgb_cam_pipeline():
print("Creating pipeline: RGB CAM -> XLINK OUT")
pipeline = dai.Pipeline()
cam = pipeline.createColorCamera()
xout_preview = pipeline.createXLinkOut()
xout_video = pipeline.createXLinkOut()
cam.setPreviewSize(540, 540)
cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
cam.setInterleaved(False)
rgb_image_pts = np.matmul(M_RGB, rgb_frame_ref_cloud_normalized)
rgb_image_pts = rgb_image_pts.astype(np.int16)
print("shape is {}".format(rgb_image_pts.shape[1]))
u_v_z = np.vstack((rgb_image_pts, rgb_frame_ref_cloud[2, :]))
lft = np.logical_and(0 <= u_v_z[0], u_v_z[0] < 1280)
rgt = np.logical_and(0 <= u_v_z[1], u_v_z[1] < 720)
idx_bool = np.logical_and(lft, rgt)
u_v_z_sampled = u_v_z[:, np.where(idx_bool)]
y_idx = u_v_z_sampled[1].astype(int)
x_idx = u_v_z_sampled[0].astype(int)
depth_rgb = np.full((720, 1280), 65535, dtype=np.uint16)
depth_rgb[y_idx,x_idx] = u_v_z_sampled[3]*10
frame_rgb = cv2.cvtColor(color, cv2.COLOR_BGR2RGB)
end = time.time()
print('for loop Convertion time')
print(end - start)
if pcl_converter is None:
pcl_converter = PointCloudVisualizer(M_RGB, 1280, 720)
pcd = pcl_converter.rgbd_to_projection(depth_rgb, frame_rgb)
pcl_converter.visualize_pcd()
if cv2.waitKey(1) == ord("q"):
break
if pcl_converter is not None:
pcl_converter.close_window()