PointCloud - TODO port to v3

View as Markdown

1pipeline = dai.Pipeline()
2pointCloud = pipeline.create(dai.node.PointCloud)

1dai::Pipeline pipeline;
2auto pointCloud = pipeline.create<dai::node::PointCloud>();

1import open3d as o3d
2import numpy as np
3import depthai as dai
4
5pcd = o3d.geometry.PointCloud()
6vis = o3d.visualization.VisualizerWithKeyCallback()
7vis.create_window()
8
9with dai.Device(pipeline) as device:
10    coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0,0,0])
11    vis.add_geometry(coordinateFrame)
12
13    while device.isPipelineRunning():
14        inMessage = q.get()
15        inColor = inMessage["rgb"]
16        inPointCloud = inMessage["pcl"]
17        cvColorFrame = inColor.getCvFrame()
18
19        if inPointCloud:
20            points = inPointCloud.getPoints().astype(np.float64)
21            pcd.points = o3d.utility.Vector3dVector(points)
22            colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64)
23            pcd.colors = o3d.utility.Vector3dVector(colors)
24            vis.update_geometry(pcd)
25
26        vis.poll_events()
27        vis.update_renderer()
28
29    vis.destroy_window()

1#include <iostream>
2#include <open3d/Open3D.h>
3#include <depthai/depthai.hpp>
4
5int main() {
6    auto viewer = std::make_unique<pcl::visualization::PCLVisualizer>("Cloud Viewer");
7    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");
8
9    dai::Device device(pipeline);
10
11    auto q = device.getOutputQueue("out", 8, false);
12    auto qDepth = device.getOutputQueue("depth", 8, false);
13
14    while(true) {
15        std::cout << "Waiting for data" << std::endl;
16        auto depthImg = qDepth->get<dai::ImgFrame>();
17        auto pclMsg = q->get<dai::PointCloudData>();
18
19        if(!pclMsg) {
20            std::cout << "No data" << std::endl;
21            continue;
22        }
23
24        auto frame = depthImg->getCvFrame();
25        frame.convertTo(frame, CV_8UC1, 255 / depth->initialConfig.getMaxDisparity());
26
27        if(pclMsg->getPoints().empty()) {
28            std::cout << "Empty point cloud" << std::endl;
29            continue;
30        }
31
32        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pclMsg->getPclData();
33        viewer->updatePointCloud(cloud, "cloud");
34
35        viewer->spinOnce(10);
36
37        if(viewer->wasStopped()) {
38            break;
39        }
40    }
41}