点云可视化器

1#!/usr/bin/env python3
2"""
3PointCloud Visualizer
4
5Live 3D visualization of stereo-depth point clouds using Open3D.
6Press Q in the Open3D window (or Ctrl-C in the terminal) to quit.
7"""
8
9import time
10
11import cv2
12import numpy as np
13
14try:
15    import open3d as o3d
16except ImportError:
17    raise SystemExit("Please install open3d: pip install open3d")
18
19import depthai as dai
20
21
22def main() -> None:
23    print("PointCloud Visualizer")
24    print("=====================")
25    print("Connecting to device...")
26
27    device = dai.Device()
28    print(f"Device: {device.getDeviceName()}  (ID: {device.getDeviceId()})\n")
29
30    with dai.Pipeline(device) as pipeline:
31        # ── Camera + Depth ─────────────────────────────────────
32        colorSockets = device.getConnectedCameras(dai.CameraSensorType.COLOR)
33        colorSocket = colorSockets[0] if colorSockets else dai.CameraBoardSocket.CAM_A
34        color = pipeline.create(dai.node.Camera).build(colorSocket)
35        colorOut = color.requestOutput(
36            (640, 400), type=dai.ImgFrame.Type.RGB888i,
37            resizeMode=dai.ImgResizeMode.CROP, enableUndistortion=True,
38        )
39
40        depth = pipeline.create(dai.node.Depth).build(dai.node.Depth.Algorithm.AUTO, None, (640, 400))
41        depth.setAlignTo(colorOut)
42
43        # ── PointCloud node ───────────────────────────────────────────
44        pc = pipeline.create(dai.node.PointCloud)
45        pc.setRunOnHost(True)
46        pc.initialConfig.setLengthUnit(dai.LengthUnit.METER)
47        depth.depth.link(pc.inputDepth)
48        colorOut.link(pc.inputColor)
49
50        queue = pc.outputPointCloud.createOutputQueue(maxSize=4, blocking=False)
51        qDepth = pc.passthroughDepth.createOutputQueue(maxSize=4, blocking=False)
52
53        # ── Open3D setup ──────────────────────────────────────────────
54        vis = o3d.visualization.Visualizer()
55        vis.create_window("PointCloud Visualizer")
56        pcd = o3d.geometry.PointCloud()
57        coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1)
58        vis.add_geometry(coord)
59        first = True
60
61        # ── Start pipeline & wait for auto-exposure ───────────────────
62        pipeline.start()
63        print("Waiting for auto-exposure to settle...")
64        time.sleep(1)
65        queue.tryGetAll()  # drain stale frames
66        qDepth.tryGetAll()
67
68        print("Streaming... Press Q in the Open3D window to quit.")
69
70        try:
71            while True:
72                pclData = queue.tryGet()
73
74                if pclData is not None:
75                    if pclData.isColor():
76                        points, colors = pclData.getPointsRGB()
77                        if len(points) > 0:
78                            pcd.points = o3d.utility.Vector3dVector(points.astype(np.float64))
79                            pcd.colors = o3d.utility.Vector3dVector(
80                                np.delete(colors / 255.0, 3, 1).astype(np.float64)
81                            )
82                    else:
83                        points = pclData.getPoints()
84                        if len(points) > 0:
85                            pcd.points = o3d.utility.Vector3dVector(points)
86                            pcd.colors = o3d.utility.Vector3dVector()
87
88                    if len(pcd.points) > 0:
89                        if first:
90                            vis.add_geometry(pcd)
91                            ctr = vis.get_view_control()
92                            ctr.set_front([0, 0, -1])
93                            ctr.set_up([0, -1, 0])
94                            ctr.set_lookat([0, 0, 1])
95                            ctr.set_zoom(0.3)
96                            first = False
97                        else:
98                            vis.update_geometry(pcd)
99
100                # Show colorized depth in an OpenCV window
101                depthMsg = qDepth.tryGet()
102                if depthMsg is not None:
103                    cv2.imshow("Depth", dai.utility.colorizeDepthFrame(depthMsg, 300, 12000, cv2.COLORMAP_HOT, useLog=True).getCvFrame())
104
105                if cv2.waitKey(1) == ord("q"):
106                    break
107                if not vis.poll_events():
108                    break
109                vis.update_renderer()
110
111        except KeyboardInterrupt:
112            print("\nStopping...")
113
114        vis.destroy_window()
115        cv2.destroyAllWindows()
116
117    print("Done.")
118
119
120if __name__ == "__main__":
121    main()

1/**
2 * PointCloud Visualizer
3 *
4 * Live 3D visualization of colorized stereo-depth point clouds using the
5 * built-in RemoteConnection (foxglove) visualizer.
6 *
7 * Run the example, then open the visualizer in a browser (default
8 * http://localhost:8082) to see the live point cloud.
9 * Press 'q' in the viewer or Ctrl-C in the terminal to quit.
10 */
11
12#include <csignal>
13#include <iostream>
14
15#include "depthai/depthai.hpp"
16#include "depthai/remote_connection/RemoteConnection.hpp"
17
18// ---------------------------------------------------------------------------
19static volatile std::sig_atomic_t isRunning{1};
20void signalHandler(int) {
21    isRunning = 0;
22}
23
24// ---------------------------------------------------------------------------
25int main() {
26    std::signal(SIGINT, signalHandler);
27
28    std::cout << "PointCloud Visualizer\n"
29              << "=====================\n"
30              << "Connecting to device...\n";
31
32    dai::RemoteConnection remoteConnector;
33    dai::Pipeline pipeline;
34
35    auto device = pipeline.getDefaultDevice();
36    std::cout << "Device: " << device->getDeviceName() << "  (ID: " << device->getDeviceId() << ")\n\n";
37
38    const auto size = std::make_pair(640, 400);
39
40    // ── Cameras ──────────────────────────────────────────────────────
41    auto colorSockets = device->getConnectedCameras(dai::CameraSensorType::COLOR);
42    auto colorSocket = colorSockets.empty() ? dai::CameraBoardSocket::CAM_A : colorSockets.front();
43    auto color = pipeline.create<dai::node::Camera>();
44    color->build(colorSocket);
45
46    // ── Align depth to color camera ──────────────────────────────────
47    auto colorOut = color->requestOutput(size, dai::ImgFrame::Type::RGB888i, dai::ImgResizeMode::CROP, std::nullopt, true);
48
49    auto depth = pipeline.create<dai::node::Depth>();
50    depth->build(dai::node::Depth::Algorithm::AUTO, std::nullopt, std::make_pair(640u, 400u));
51    depth->setAlignTo(*colorOut);
52
53    // ── PointCloud node ──────────────────────────────────────────────
54    auto pc = pipeline.create<dai::node::PointCloud>();
55    pc->setRunOnHost(true);
56    depth->depth().link(pc->inputDepth);
57    colorOut->link(pc->getColorInput());
58
59    // Publish the point cloud to the remote visualizer
60    remoteConnector.addTopic("pcl", pc->outputPointCloud);
61
62    pipeline.start();
63    remoteConnector.registerPipeline(pipeline);
64
65    device->setIrLaserDotProjectorIntensity(0.7);
66
67    std::cout << "Pipeline started.\n"
68              << "Open the visualizer at http://localhost:8082 to see the point cloud.\n"
69              << "Press 'q' in the viewer or Ctrl-C to quit.\n";
70
71    while(isRunning != 0 && pipeline.isRunning()) {
72        int key = remoteConnector.waitKey(1);
73        if(key == 'q') {
74            std::cout << "Got 'q' key from the remote connection.\n";
75            break;
76        }
77    }
78
79    std::cout << "Done.\n";
80    return 0;
81}