# PointCloud Visualization

This example demonstrates how to visualize an on-device created point cloud using DepthAI and Open3D. It captures color and depth
data from an OAK device, combines them to create a colored point cloud, and displays it in real-time. The FPS counter is included
to monitor performance.

## Key Concepts

 * Capturing synchronized color and depth frames using the DepthAI API.
 * Creating a point cloud with color data for visualization.
 * Real-time visualization of the point cloud using Open3D.
 * Implementing an FPS counter to monitor the application's performance.

## Demo

## Setup

If you are using Python, ensure you have DepthAI and Open3D installed in your Python environment:

```bash
python3 -m pip install depthai open3d
```

If Open3D is not already installed, the script will prompt you to install it.

## Source code

The example initializes the DepthAI pipeline with color and mono cameras and a stereo depth node to generate depth information. It
then creates a point cloud node and configures it for sparse point cloud generation. The script visualizes this point cloud using
Open3D, with each point's color corresponding to the color image captured by the RGB camera.

#### Python

```python
#!/usr/bin/env python3

import depthai as dai
from time import sleep
import numpy as np
import cv2
import time
import sys
try:
    import open3d as o3d
except ImportError:
    sys.exit("Critical dependency missing: Open3D. Please install it using the command: '{} -m pip install open3d' and then rerun the script.".format(sys.executable))

FPS = 30
class FPSCounter:
    def __init__(self):
        self.frameCount = 0
        self.fps = 0
        self.startTime = time.time()

    def tick(self):
        self.frameCount += 1
        if self.frameCount % 10 == 0:
            elapsedTime = time.time() - self.startTime
            self.fps = self.frameCount / elapsedTime
            self.frameCount = 0
            self.startTime = time.time()
        return self.fps

pipeline = dai.Pipeline()
camRgb = pipeline.create(dai.node.ColorCamera)
monoLeft = pipeline.create(dai.node.MonoCamera)
monoRight = pipeline.create(dai.node.MonoCamera)
depth = pipeline.create(dai.node.StereoDepth)
pointcloud = pipeline.create(dai.node.PointCloud)
sync = pipeline.create(dai.node.Sync)
xOut = pipeline.create(dai.node.XLinkOut)
xOut.input.setBlocking(False)

camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_A)
camRgb.setIspScale(1,3)
camRgb.setFps(FPS)

monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoLeft.setCamera("left")
monoLeft.setFps(FPS)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoRight.setCamera("right")
monoRight.setFps(FPS)

depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
depth.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7)
depth.setLeftRightCheck(True)
depth.setExtendedDisparity(False)
depth.setSubpixel(True)
depth.setDepthAlign(dai.CameraBoardSocket.CAM_A)

monoLeft.out.link(depth.left)
monoRight.out.link(depth.right)
depth.depth.link(pointcloud.inputDepth)
camRgb.isp.link(sync.inputs["rgb"])
pointcloud.outputPointCloud.link(sync.inputs["pcl"])
sync.out.link(xOut.input)
xOut.setStreamName("out")

with dai.Device(pipeline) as device:
    isRunning = True
    def key_callback(vis, action, mods):
        global isRunning
        if action == 0:
            isRunning = False

    q = device.getOutputQueue(name="out", maxSize=4, blocking=False)
    vis = o3d.visualization.VisualizerWithKeyCallback()
    vis.create_window()
    vis.register_key_action_callback(81, key_callback)
    pcd = o3d.geometry.PointCloud()
    coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0,0,0])
    vis.add_geometry(coordinateFrame)

    first = True
    fpsCounter = FPSCounter()
    while isRunning:
        inMessage = q.get()
        inColor = inMessage["rgb"]
        inPointCloud = inMessage["pcl"]
        cvColorFrame = inColor.getCvFrame()
        # Convert the frame to RGB
        cvRGBFrame = cv2.cvtColor(cvColorFrame, cv2.COLOR_BGR2RGB)
        fps = fpsCounter.tick()
        # Display the FPS on the frame
        cv2.putText(cvColorFrame, f"FPS: {fps:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
        cv2.imshow("color", cvColorFrame)
        key = cv2.waitKey(1)
        if key == ord('q'):
            break
        if inPointCloud:
            t_before = time.time()
            points = inPointCloud.getPoints().astype(np.float64)
            pcd.points = o3d.utility.Vector3dVector(points)
            colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64)
            pcd.colors = o3d.utility.Vector3dVector(colors)
            if first:
                vis.add_geometry(pcd)
                first = False
            else:
                vis.update_geometry(pcd)
        vis.poll_events()
        vis.update_renderer()
    vis.destroy_window()
```

#### C++

```cpp
#include <iostream>
#include <opencv2/opencv.hpp>

#include "depthai/depthai.hpp"

int main() {
    auto pipeline = dai::Pipeline();
    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
    auto monoRight = pipeline.create<dai::node::MonoCamera>();
    auto depth = pipeline.create<dai::node::StereoDepth>();
    auto pointcloud = pipeline.create<dai::node::PointCloud>();
    auto xout = pipeline.create<dai::node::XLinkOut>();
    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();

    monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoLeft->setCamera("left");
    monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    monoRight->setCamera("right");

    // Create a node that will produce the depth map (using disparity output as
    // it's easier to visualize depth this way)
    depth->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
    // Options: MEDIAN_OFF, KERNEL_3x3, KERNEL_5x5, KERNEL_7x7 (default)
    depth->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
    depth->setLeftRightCheck(true);
    depth->setExtendedDisparity(false);
    depth->setSubpixel(true);

    xout->setStreamName("out");
    xoutDepth->setStreamName("depth");

    monoLeft->out.link(depth->left);
    monoRight->out.link(depth->right);
    depth->depth.link(pointcloud->inputDepth);
    depth->disparity.link(xoutDepth->input);
    pointcloud->outputPointCloud.link(xout->input);
    pointcloud->initialConfig.setSparse(true);

    auto viewer = std::make_unique<pcl::visualization::PCLVisualizer>("Cloud Viewer");
    bool first = true;

    dai::Device device(pipeline);

    auto q = device.getOutputQueue("out", 8, false);
    auto qDepth = device.getOutputQueue("depth", 8, false);
    long counter = 0;
    while(true) {
        std::cout << "Waiting for data" << std::endl;
        auto depthImg = qDepth->get<dai::ImgFrame>();
        auto pclMsg = q->get<dai::PointCloudData>();
        std::cout << "Got data" << std::endl;
        if(!pclMsg) {
            std::cout << "No data" << std::endl;
            continue;
        }

        auto frame = depthImg->getCvFrame();
        frame.convertTo(frame, CV_8UC1, 255 / depth->initialConfig.getMaxDisparity());
        cv::imshow("depth", frame);
        cv::waitKey(1);

        if(pclMsg->getPoints().empty()) {
            std::cout << "Empty point cloud" << std::endl;
            continue;
        }
        std::cout << "Number of points: " << pclMsg->getPoints().size() << std::endl;
        std::cout << "Min x: " << pclMsg->getMinX() << std::endl;
        std::cout << "Min y: " << pclMsg->getMinY() << std::endl;
        std::cout << "Min z: " << pclMsg->getMinZ() << std::endl;
        std::cout << "Max x: " << pclMsg->getMaxX() << std::endl;
        std::cout << "Max y: " << pclMsg->getMaxY() << std::endl;
        std::cout << "Max z: " << pclMsg->getMaxZ() << std::endl;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pclMsg->getPclData();
        if(first) {
            viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");
            first = false;
        } else {
            viewer->updatePointCloud(cloud, "cloud");
        }

        viewer->spinOnce(10);

        if(viewer->wasStopped()) {
            break;
        }
    }

    return 0;
}
```

### Need assistance?

Head over to [Discussion Forum](https://discuss.luxonis.com/) for technical support or any other questions you might have.