点云展示

1#!/usr/bin/env python3
2"""
3PointCloud Node Showcase
4
5Demonstrates filtered/organized output, camera-to-camera transforms,
6housing coordinate system transforms, and custom 4x4 matrix transforms.
7
8See examples/cpp/PointCloud/README.md for full documentation.
9"""
10
11import time
12import depthai as dai
13
14
15# ---------------------------------------------------------------------------
16# Print helpers
17# ---------------------------------------------------------------------------
18def printHeader(title: str) -> None:
19    print("\n╔══════════════════════════════════════════════╗")
20    print(f"║  {title:<44s}║")
21    print("╚══════════════════════════════════════════════╝")
22
23
24def printPointCloudInfo(pcd: dai.PointCloudData, frameNum: int) -> None:
25    points = pcd.getPoints()
26    print(f"\n--- Frame {frameNum} ---")
27    print(f"  Points       : {len(points)}")
28    print(f"  Width×Height : {pcd.getWidth()} × {pcd.getHeight()}")
29    print(f"  Organized    : {'yes' if pcd.isOrganized() else 'no'}")
30    print(f"  Color        : {'yes' if pcd.isColor() else 'no'}")
31    print(f"  Bounding box :"
32          f"  X [{pcd.getMinX()}, {pcd.getMaxX()}]"
33          f"  Y [{pcd.getMinY()}, {pcd.getMaxY()}]"
34          f"  Z [{pcd.getMinZ()}, {pcd.getMaxZ()}]")
35
36
37# ===========================================================================
38NUM_FRAMES = 3
39
40
41def main() -> None:
42    print("PointCloud Node Showcase")
43    print("========================")
44    print("Connecting to device...")
45
46    device = dai.Device()
47    print(f"Device: {device.getDeviceName()}  (ID: {device.getDeviceId()})\n")
48
49    # ------------------------------------------------------------------
50    # Single pipeline – shared Camera + StereoDepth, multiple PointCloud
51    # nodes configured differently.
52    # ------------------------------------------------------------------
53    with dai.Pipeline(device) as pipeline:
54        colorSockets = device.getConnectedCameras(dai.CameraSensorType.COLOR)
55        colorSocket = colorSockets[0] if colorSockets else dai.CameraBoardSocket.CAM_A
56        color = pipeline.create(dai.node.Camera).build(colorSocket)
57        colorOut = color.requestOutput(
58            (640, 400), type=dai.ImgFrame.Type.RGB888i,
59            resizeMode=dai.ImgResizeMode.CROP, enableUndistortion=True,
60        )
61
62        depth = pipeline.create(dai.node.Depth).build(dai.node.Depth.Algorithm.AUTO, None, (640, 400))
63        depth.setAlignTo(colorOut)
64
65        # ── 1. Filtered point cloud  (METER) ────
66        pcSparse = pipeline.create(dai.node.PointCloud)
67        pcSparse.setRunOnHost(True)
68        pcSparse.initialConfig.setLengthUnit(dai.LengthUnit.METER)
69        depth.depth.link(pcSparse.inputDepth)
70        qSparse = pcSparse.outputPointCloud.createOutputQueue(maxSize=4, blocking=False)
71
72        # ── 2. Organized point cloud  (MILLIMETER) ───────
73        pcOrganized = pipeline.create(dai.node.PointCloud)
74        pcOrganized.setRunOnHost(True)
75        pcOrganized.initialConfig.setLengthUnit(dai.LengthUnit.MILLIMETER)
76        pcOrganized.initialConfig.setOrganized(True)
77        depth.depth.link(pcOrganized.inputDepth)
78        qOrganized = pcOrganized.outputPointCloud.createOutputQueue(maxSize=4, blocking=False)
79
80        # ── 3. Transform pointcloud into another device's coordinate system ───
81        pcCam = pipeline.create(dai.node.PointCloud)
82        pcCam.setRunOnHost(True)
83        pcCam.initialConfig.setLengthUnit(dai.LengthUnit.MILLIMETER)
84        pcCam.initialConfig.setTargetCoordinateSystem(dai.CameraBoardSocket.CAM_A)
85        # Or transform to a housing coordinate system instead, e.g.:
86        # pcCam.initialConfig.setTargetCoordinateSystem(dai.HousingCoordinateSystem.VESA_A)
87        depth.depth.link(pcCam.inputDepth)
88        qCam = pcCam.outputPointCloud.createOutputQueue(maxSize=4, blocking=False)
89
90        # ── 4. Custom 4×4 transform  (90° Z rotation) + passthrough ──────
91        pcCustom = pipeline.create(dai.node.PointCloud)
92        pcCustom.setRunOnHost(True)
93        pcCustom.initialConfig.setLengthUnit(dai.LengthUnit.MILLIMETER)
94        pcCustom.useCPU()
95        transform = [
96            [0.0, -1.0, 0.0, 0.0],
97            [1.0,  0.0, 0.0, 0.0],
98            [0.0,  0.0, 1.0, 0.0],
99            [0.0,  0.0, 0.0, 1.0],
100        ]
101        pcCustom.initialConfig.setTransformationMatrix(transform)
102        depth.depth.link(pcCustom.inputDepth)
103        qCustom = pcCustom.outputPointCloud.createOutputQueue(maxSize=4, blocking=False)
104        qDepth = pcCustom.passthroughDepth.createOutputQueue(maxSize=4, blocking=False)
105
106        # ── 5. Colorized point cloud (aligned RGB from color camera) ─────
107        pcColorized = pipeline.create(dai.node.PointCloud)
108        pcColorized.setRunOnHost(True)
109        pcColorized.initialConfig.setLengthUnit(dai.LengthUnit.METER)
110        depth.depth.link(pcColorized.inputDepth)
111        colorOut.link(pcColorized.inputColor)
112        qColorized = pcColorized.outputPointCloud.createOutputQueue(maxSize=4, blocking=False)
113
114        # Note: Housing coordinate system transform is also available, e.g.:
115        #   pc.initialConfig.setTargetCoordinateSystem(dai.HousingCoordinateSystem.VESA_A)
116        # See the docstring at the top of this file for all available
117        # CameraBoardSocket and HousingCoordinateSystem values.
118
119        sparseFrames = []
120        organizedFrames = []
121        camFrames = []
122        customFrames = []
123        depthFrames = []
124        colorizedFrames = []
125
126        pipeline.start()
127
128        # Wait for auto-exposure to settle and stereo depth to stabilize
129        print("Waiting for auto-exposure to settle...")
130        time.sleep(1)
131
132        # Drain stale frames that arrived during warm-up
133        qSparse.tryGetAll()
134        qOrganized.tryGetAll()
135        qCam.tryGetAll()
136        qCustom.tryGetAll()
137        qDepth.tryGetAll()
138        qColorized.tryGetAll()
139
140        for _ in range(NUM_FRAMES):
141            sparseFrames.append(qSparse.get())
142            organizedFrames.append(qOrganized.get())
143            camFrames.append(qCam.get())
144            customFrames.append(qCustom.get())
145            depthFrames.append(qDepth.get())
146            colorizedFrames.append(qColorized.get())
147
148    # ------------------------------------------------------------------
149    # Display results grouped by feature
150    # ------------------------------------------------------------------
151
152    # 1 ── Sparse point cloud
153    printHeader("1. Basic sparse point cloud")
154    print("  Config: METER")
155    for i, pcd in enumerate(sparseFrames):
156        printPointCloudInfo(pcd, i)
157
158    # 2 ── Organized point cloud
159    printHeader("2. Organized point cloud")
160    print("  Config: MILLIMETER, initialConfig.setOrganized(True)")
161    for i, pcd in enumerate(organizedFrames):
162        printPointCloudInfo(pcd, i)
163
164    # 3 ── Transform pointcloud into another device's coordinate system
165    printHeader("3. Camera-to-camera transform")
166    print("  Config: setTargetCoordinateSystem(CAM_A)")
167    for i, pcd in enumerate(camFrames):
168        printPointCloudInfo(pcd, i)
169
170    # 4 ── Custom transform + passthrough depth
171    printHeader("4. Custom transform matrix + passthrough")
172    print("  Config: 90° Z rotation via initialConfig")
173    if len(customFrames) != len(depthFrames):
174        raise RuntimeError("customFrames and depthFrames must have the same length")
175    for i, (pcd, depth) in enumerate(zip(customFrames, depthFrames)):
176        printPointCloudInfo(pcd, i)
177        print(f"  Depth frame  : {depth.getWidth()} × {depth.getHeight()}")
178
179    # 5 ── Colorized point cloud
180    printHeader("5. Colorized point cloud (RGB)")
181    print("  Config: METER, aligned color camera linked to inputColor")
182    for i, pcd in enumerate(colorizedFrames):
183        printPointCloudInfo(pcd, i)
184
185    print("\nAll demos completed.")
186
187
188if __name__ == "__main__":
189    main()

1/**
2 * PointCloud Node Showcase
3 *
4 * Demonstrates filtered/organized output, camera-to-camera transforms,
5 * housing coordinate system transforms, and custom 4×4 matrix transforms.
6 *
7 * See README.md in this directory for full documentation.
8 */
9
10#include <chrono>
11#include <iomanip>
12#include <iostream>
13#include <memory>
14#include <thread>
15#include <vector>
16
17#include "depthai/depthai.hpp"
18
19// ---------------------------------------------------------------------------
20// Print helpers
21// ---------------------------------------------------------------------------
22static void printHeader(const std::string& title) {
23    std::cout << "\n╔══════════════════════════════════════════════╗\n";
24    std::cout << "║  " << std::left << std::setw(44) << title << "║\n";
25    std::cout << "╚══════════════════════════════════════════════╝\n";
26}
27
28static void printPointCloudInfo(const dai::PointCloudData& pcd, int frameNum) {
29    auto points = pcd.getPoints();
30
31    std::cout << "\n--- Frame " << frameNum << " ---\n";
32    std::cout << "  Points       : " << points.size() << "\n";
33    std::cout << "  Width×Height : " << pcd.getWidth() << " × " << pcd.getHeight() << "\n";
34    std::cout << "  Organized    : " << (pcd.isOrganized() ? "yes" : "no") << "\n";
35    std::cout << "  Color        : " << (pcd.isColor() ? "yes" : "no") << "\n";
36    std::cout << "  Bounding box :" << "  X [" << pcd.getMinX() << ", " << pcd.getMaxX() << "]" << "  Y [" << pcd.getMinY() << ", " << pcd.getMaxY() << "]"
37              << "  Z [" << pcd.getMinZ() << ", " << pcd.getMaxZ() << "]\n";
38}
39
40// ---------------------------------------------------------------------------
41// Main – single pipeline with multiple PointCloud nodes
42// ---------------------------------------------------------------------------
43static constexpr int NUM_FRAMES = 3;
44
45int main() {
46    std::cout << "PointCloud Node Showcase\n"
47              << "========================\n"
48              << "Connecting to device...\n";
49
50    auto device = std::make_shared<dai::Device>();
51    std::cout << "Device: " << device->getDeviceName() << "  (ID: " << device->getDeviceId() << ")\n";
52
53    try {
54        // ==============================================================
55        // Single pipeline – shared Camera + StereoDepth, multiple
56        // PointCloud nodes configured differently.
57        // ==============================================================
58        dai::Pipeline pipeline(device);
59
60        auto colorSockets = device->getConnectedCameras(dai::CameraSensorType::COLOR);
61        auto colorSocket = colorSockets.empty() ? dai::CameraBoardSocket::CAM_A : colorSockets.front();
62        auto color = pipeline.create<dai::node::Camera>()->build(colorSocket);
63        auto* colorOut = color->requestOutput(std::make_pair(640, 400), dai::ImgFrame::Type::RGB888i, dai::ImgResizeMode::CROP, std::nullopt, true);
64        auto depth = pipeline.create<dai::node::Depth>();
65        depth->build(dai::node::Depth::Algorithm::AUTO, std::nullopt, std::make_pair(640u, 400u));
66        depth->setAlignTo(*colorOut);
67
68        // ── 1. Filtered point cloud (METER)
69        auto pcSparse = pipeline.create<dai::node::PointCloud>();
70        pcSparse->setRunOnHost(true);
71        pcSparse->initialConfig->setLengthUnit(dai::LengthUnit::METER);
72        depth->depth().link(pcSparse->inputDepth);
73        auto qSparse = pcSparse->outputPointCloud.createOutputQueue();
74
75        // ── 2. Organized point cloud (MILLIMETER)
76        auto pcOrganized = pipeline.create<dai::node::PointCloud>();
77        pcOrganized->setRunOnHost(true);
78        pcOrganized->initialConfig->setLengthUnit(dai::LengthUnit::MILLIMETER);
79        pcOrganized->initialConfig->setOrganized(true);
80        depth->depth().link(pcOrganized->inputDepth);
81        auto qOrganized = pcOrganized->outputPointCloud.createOutputQueue();
82
83        // ── 3. Transform pointcloud into another camera's coordinate system
84        auto pcCam = pipeline.create<dai::node::PointCloud>();
85        pcCam->setRunOnHost(true);
86        pcCam->initialConfig->setLengthUnit(dai::LengthUnit::MILLIMETER);
87        pcCam->initialConfig->setTargetCoordinateSystem(dai::CameraBoardSocket::CAM_A);
88        // Or transform to a housing coordinate system instead, e.g.:
89        // pcCam->initialConfig->setTargetCoordinateSystem(dai::HousingCoordinateSystem::VESA_A);
90        depth->depth().link(pcCam->inputDepth);
91        auto qCam = pcCam->outputPointCloud.createOutputQueue();
92
93        // ── 4. Custom 4×4 transform (90° Z rotation) + passthrough
94        auto pcCustom = pipeline.create<dai::node::PointCloud>();
95        pcCustom->setRunOnHost(true);
96        pcCustom->initialConfig->setLengthUnit(dai::LengthUnit::MILLIMETER);
97        pcCustom->useCPU();
98        std::array<std::array<float, 4>, 4> transform = {{{{0.f, -1.f, 0.f, 0.f}}, {{1.f, 0.f, 0.f, 0.f}}, {{0.f, 0.f, 1.f, 0.f}}, {{0.f, 0.f, 0.f, 1.f}}}};
99        pcCustom->initialConfig->setTransformationMatrix(transform);
100        depth->depth().link(pcCustom->inputDepth);
101        auto qCustom = pcCustom->outputPointCloud.createOutputQueue();
102        auto qDepth = pcCustom->passthroughDepth.createOutputQueue();
103
104        // ── 5. Colorized point cloud (aligned RGB from color camera)
105        auto pcColor = pipeline.create<dai::node::PointCloud>();
106        pcColor->setRunOnHost(true);
107        pcColor->initialConfig->setLengthUnit(dai::LengthUnit::METER);
108        depth->depth().link(pcColor->inputDepth);
109        colorOut->link(pcColor->getColorInput());
110        auto qColor = pcColor->outputPointCloud.createOutputQueue();
111
112        // ==============================================================
113        // Collect frames – drain all queues evenly to avoid back-pressure
114        // ==============================================================
115        struct TestCase {
116            std::shared_ptr<dai::MessageQueue> queue;
117            std::string title;
118            std::string config;
119            std::vector<std::shared_ptr<dai::PointCloudData>> frames;
120        };
121
122        std::vector<TestCase> testCases = {
123            {qSparse, "1. Basic sparse point cloud", "METER", {}},
124            {qOrganized, "2. Organized point cloud", "MILLIMETER, initialConfig->setOrganized(true)", {}},
125            {qCam, "3. Camera-to-camera transform", "setTargetCoordinateSystem(CAM_A)", {}},
126            {qCustom, "4. Custom transform matrix + passthrough", "90° Z rotation via initialConfig", {}},
127            {qColor, "5. Colorized point cloud (RGB)", "METER, aligned color camera linked to inputColor", {}},
128        };
129
130        std::vector<std::shared_ptr<dai::ImgFrame>> depthFrames;
131
132        pipeline.start();
133
134        // Wait for auto-exposure to settle and stereo depth to stabilize
135        std::cout << "Waiting for auto-exposure to settle...\n";
136        std::this_thread::sleep_for(std::chrono::seconds(1));
137
138        // Drain stale frames that arrived during warm-up
139        for(auto& tc : testCases) {
140            tc.queue->tryGetAll<dai::PointCloudData>();
141        }
142        qDepth->tryGetAll<dai::ImgFrame>();
143
144        for(int i = 0; i < NUM_FRAMES; ++i) {
145            for(auto& tc : testCases) {
146                tc.frames.push_back(tc.queue->get<dai::PointCloudData>());
147            }
148            depthFrames.push_back(qDepth->get<dai::ImgFrame>());
149        }
150        pipeline.stop();
151
152        // ==============================================================
153        // Display results grouped by feature
154        // ==============================================================
155        for(const auto& tc : testCases) {
156            printHeader(tc.title);
157            std::cout << "  Config: " << tc.config << "\n";
158            for(int i = 0; i < NUM_FRAMES; ++i) {
159                printPointCloudInfo(*tc.frames[i], i);
160                // Show depth passthrough info for the custom transform case
161                if(tc.title.find("Custom") != std::string::npos) {
162                    std::cout << "  Depth frame  : " << depthFrames[i]->getWidth() << " × " << depthFrames[i]->getHeight() << "\n";
163                }
164            }
165        }
166
167    } catch(const std::exception& e) {
168        std::cerr << "\nError: " << e.what() << std::endl;
169        return 1;
170    }
171
172    std::cout << "\nAll demos completed.\n";
173    return 0;
174}