How to create a point cloud with color data for visualization in C++

I was referring to the example which demonstrates how to visualize an on-device created point cloud using DepthAI in C++ and was able to visualize the point cloud (without color data) in PCL Visualizer.

Now, I want to create and visualize the point cloud with color data in C++. I modified the original example script to add a color camera pipeline and synced the the color and pointcloud stream

auto pipeline = dai::Pipeline();

auto camrgb = pipeline.create<dai::node::ColorCamera>();

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 sync = pipeline.create<dai::node::Sync>();

auto xout = pipeline.create<dai::node::XLinkOut>();

xout->input.setBlocking(false);

auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
camrgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P);

camrgb->setBoardSocket(dai::CameraBoardSocket::CAM_A);
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);

depth->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);

depth->setLeftRightCheck(true); depth->setExtendedDisparity(false);

depth->setSubpixel(true); depth->setDepthAlign(dai::CameraBoardSocket::CAM_A);

xoutDepth->setStreamName("depth");
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");

When I try to get the output point cloud data by doing the following, I get the output as "No data" which means that its not able to get the point cloud with color data from the device.

auto q = device.getOutputQueue("out", 8, false);

auto pclMsg = q->get<dai::PointCloudData>();

std::cout << "Got data" << std::endl;

if(!pclMsg)

{

std::cout << "No data" << std::endl;

continue;

}

Also, if the above problem is resolved and I get the point cloud with color data, how do I visualize the point cloud data in PCL visualizer since the getPclData() method only contains points with x,y and z information?

I tried doing both but its still giving me the same output of "No data".

The following is the source code that I am using (pcd_visualizer.cpp):

#include <iostream>
#include <opencv2/opencv.hpp>
#include "depthai/depthai.hpp"

int FPS = 60;

int main() {
    auto pipeline = dai::Pipeline();
    auto camrgb = pipeline.create<dai::node::ColorCamera>();
    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 sync = pipeline.create<dai::node::Sync>();
    auto xout = pipeline.create<dai::node::XLinkOut>();
    xout->input.setBlocking(false);
    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();

    camrgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_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);


    // 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);
    depth->setDepthAlign(dai::CameraBoardSocket::CAM_A);
    sync->setSyncAttempts(0);

    xoutDepth->setStreamName("depth");

    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");


    depth->disparity.link(xoutDepth->input);
    pointcloud->initialConfig.setSparse(true);

    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;
        }

        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;
    }

    return 0;
}

CMakeList.txt:

cmake_minimum_required(VERSION 3.4)

# Add depthai-core dependency
set(depthai_DIR "/path/to/depthai-core/build/")

set(TARGET_NAME pcd_visualizer)
project(${TARGET_NAME})

find_package(depthai CONFIG REQUIRED)
# Dependencies (optional, only used for example)
find_package(OpenCV REQUIRED)

add_executable("${TARGET_NAME}"
    src/pcd_visualizer.cpp
)
# Link with libraries
target_link_libraries(${TARGET_NAME}
    PRIVATE
        depthai::core
        depthai::opencv
)

# Set some errors
if(NOT MSVC)
    target_compile_options(${TARGET_NAME} PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Werror=return-type>)
endif()

# Set compiler features (c++14)
set_property(TARGET ${TARGET_NAME} PROPERTY CXX_STANDARD 14)

Build the project and run it:
./pcd_visualizer