|
| 1 | +PointCloud |
| 2 | +========== |
| 3 | + |
| 4 | +The PointCloud node enables on-device point cloud generation from depth map. |
| 5 | + |
| 6 | +How to place it |
| 7 | +############### |
| 8 | + |
| 9 | +.. tabs:: |
| 10 | + |
| 11 | + .. code-tab:: py |
| 12 | + |
| 13 | + pipeline = dai.Pipeline() |
| 14 | + pointCloud = pipeline.create(dai.node.PointCloud) |
| 15 | + |
| 16 | + .. code-tab:: c++ |
| 17 | + |
| 18 | + dai::Pipeline pipeline; |
| 19 | + auto pointCloud = pipeline.create<dai::node::PointCloud>(); |
| 20 | + |
| 21 | + |
| 22 | +Inputs and Outputs |
| 23 | +################## |
| 24 | + |
| 25 | +.. code-block:: |
| 26 | +
|
| 27 | + ┌─────────────────────┐ |
| 28 | + │ │ |
| 29 | + inputConfig │ PointCloud │ |
| 30 | + ──────────────►│ │ outputPointCloud |
| 31 | + │ ├────────────────► |
| 32 | + inputDepth │ │ passthroughDepth |
| 33 | + ──────────────►│---------------------├────────────────► |
| 34 | + └─────────────────────┘ |
| 35 | +
|
| 36 | +**Message types** |
| 37 | + |
| 38 | +- :code:`inputDepth` - :ref:`ImgFrame` |
| 39 | +- :code:`inputConfig` - :ref:`PointCloudConfig` |
| 40 | +- :code:`outputPointCloud` - :ref:`PointCloudData` |
| 41 | +- :code:`passthroughDepth` - :ref:`ImgFrame` (passthrough input depth map) |
| 42 | + |
| 43 | + |
| 44 | +Example visualization with Open3D |
| 45 | +################################# |
| 46 | + |
| 47 | +.. tabs:: |
| 48 | + |
| 49 | + .. code-tab:: py |
| 50 | + |
| 51 | + import open3d as o3d |
| 52 | + import numpy as np |
| 53 | + import depthai as dai |
| 54 | + |
| 55 | + pcd = o3d.geometry.PointCloud() |
| 56 | + vis = o3d.visualization.VisualizerWithKeyCallback() |
| 57 | + vis.create_window() |
| 58 | + |
| 59 | + with dai.Device(pipeline) as device: |
| 60 | + coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0,0,0]) |
| 61 | + vis.add_geometry(coordinateFrame) |
| 62 | + |
| 63 | + while device.isPipelineRunning(): |
| 64 | + inMessage = q.get() |
| 65 | + inColor = inMessage["rgb"] |
| 66 | + inPointCloud = inMessage["pcl"] |
| 67 | + cvColorFrame = inColor.getCvFrame() |
| 68 | + |
| 69 | + if inPointCloud: |
| 70 | + points = inPointCloud.getPoints().astype(np.float64) |
| 71 | + pcd.points = o3d.utility.Vector3dVector(points) |
| 72 | + colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64) |
| 73 | + pcd.colors = o3d.utility.Vector3dVector(colors) |
| 74 | + vis.update_geometry(pcd) |
| 75 | + |
| 76 | + vis.poll_events() |
| 77 | + vis.update_renderer() |
| 78 | + |
| 79 | + vis.destroy_window() |
| 80 | + |
| 81 | + |
| 82 | + .. code-tab:: c++ |
| 83 | + |
| 84 | + #include <iostream> |
| 85 | + #include <open3d/Open3D.h> |
| 86 | + #include <depthai/depthai.hpp> |
| 87 | + |
| 88 | + int main() { |
| 89 | + auto viewer = std::make_unique<pcl::visualization::PCLVisualizer>("Cloud Viewer"); |
| 90 | + viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud"); |
| 91 | + |
| 92 | + dai::Device device(pipeline); |
| 93 | + |
| 94 | + auto q = device.getOutputQueue("out", 8, false); |
| 95 | + auto qDepth = device.getOutputQueue("depth", 8, false); |
| 96 | + |
| 97 | + while(true) { |
| 98 | + std::cout << "Waiting for data" << std::endl; |
| 99 | + auto depthImg = qDepth->get<dai::ImgFrame>(); |
| 100 | + auto pclMsg = q->get<dai::PointCloudData>(); |
| 101 | + |
| 102 | + if(!pclMsg) { |
| 103 | + std::cout << "No data" << std::endl; |
| 104 | + continue; |
| 105 | + } |
| 106 | + |
| 107 | + auto frame = depthImg->getCvFrame(); |
| 108 | + frame.convertTo(frame, CV_8UC1, 255 / depth->initialConfig.getMaxDisparity()); |
| 109 | + |
| 110 | + if(pclMsg->getPoints().empty()) { |
| 111 | + std::cout << "Empty point cloud" << std::endl; |
| 112 | + continue; |
| 113 | + } |
| 114 | + |
| 115 | + pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pclMsg->getPclData(); |
| 116 | + viewer->updatePointCloud(cloud, "cloud"); |
| 117 | + |
| 118 | + viewer->spinOnce(10); |
| 119 | + |
| 120 | + if(viewer->wasStopped()) { |
| 121 | + break; |
| 122 | + } |
| 123 | + } |
| 124 | + |
| 125 | + |
| 126 | +Examples using PointCloud |
| 127 | +######################### |
| 128 | + |
| 129 | +- :ref:`PointCloud Visualization` |
| 130 | +- :ref:`PointCloud Control` |
| 131 | + |
| 132 | + |
| 133 | +Reference |
| 134 | +######### |
| 135 | + |
| 136 | +.. tabs:: |
| 137 | + |
| 138 | + .. tab:: Python |
| 139 | + |
| 140 | + .. autoclass:: depthai.node.PointCloud |
| 141 | + :members: |
| 142 | + :inherited-members: |
| 143 | + :noindex: |
| 144 | + |
| 145 | + .. tab:: C++ |
| 146 | + |
| 147 | + .. doxygenclass:: dai::node::PointCloud |
| 148 | + :project: depthai-core |
| 149 | + :members: |
| 150 | + :private-members: |
| 151 | + :undoc-members: |
| 152 | + |
| 153 | + |
| 154 | +.. include:: ../../includes/footer-short.rst |
0 commit comments