Unable to retrieve working pointcloud from ToF node

SoumayaMans

Hello,

I have been attempting to generate a pointcloud through the ToF node using the OAK ToF camera unsuccessfully. My bare bones script primarily uses visualize_pointcloud.cpp and tof_depth.cpp as reference.

I confirmed that the stereo depth version of the pointcloud example works and that the depth images can be retrieved from seperate examples. However, the output pointcloud from my script is populating but isn't displayed in the viewer and the pointcloud data is suspicious. Here is a sample from the script output:

Waiting for data
[19443010011B7F5A00] [10.140.5.80] [5.996] [system] [warning] Device configured to generate sync, but no camera with sync output capability was started
[19443010011B7F5A00] [10.140.5.80] [11.223] [PointCloud(2)] [warning] TODO: implement... Using intrinsics from calib
[19443010011B7F5A00] [10.140.5.80] [11.280] [PointCloud(2)] [warning] TODO: implement... Using intrinsics from calib
Got data
Number of points: 306523
Min x: -inf
Min y: -inf
Min z: 1
Max x: 0
Max y: 0
Max z: 65535
[19443010011B7F5A00] [10.140.5.80] [11.345] [PointCloud(2)] [warning] TODO: implement... Using intrinsics from calib
Waiting for data
Got data
Number of points: 307200
Min x: -inf
Min y: -0
Min z: 53
Max x: 0
Max y: 2.00707
Max z: 5848
[19443010011B7F5A00] [10.140.5.80] [11.402] [PointCloud(2)] [warning] TODO: implement... Using intrinsics from calib
Waiting for data
Got data
Number of points: 307200
Min x: -inf
Min y: -0
Min z: 40
Max x: 0
Max y: 2.05078
Max z: 5846
Waiting for data
Got data
[19443010011B7F5A00] [10.140.5.80] [11.460] [PointCloud(2)] [warning] TODO: implement... Using intrinsics from calib
Number of points: 307200
Min x: -inf
Min y: -0
Min z: 40
Max x: 0
Max y: 1.89606
Max z: 5845

The following is my script. Am I handling the pipeline incorrectly?

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

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "depthai/depthai.hpp"
#include <depthai/pipeline/datatype/PointCloudData.hpp>

int main() {
    auto pipeline = dai::Pipeline();
    auto tof = pipeline.create<dai::node::ToF>();
    auto camTof = pipeline.create<dai::node::Camera>();
    
    auto pointcloud = pipeline.create<dai::node::PointCloud>();
    auto xout = pipeline.create<dai::node::XLinkOut>();
    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();

    // Configure the ToF node
    auto tofConfig = tof->initialConfig.get();

    tofConfig.enableOpticalCorrection = true;
    tofConfig.enablePhaseShuffleTemporalFilter = true;
    tofConfig.phaseUnwrappingLevel = 4;
    tofConfig.phaseUnwrapErrorThreshold = 300;
    
    tof->initialConfig.set(tofConfig);

    camTof->setFps(30);
    camTof->setBoardSocket(dai::CameraBoardSocket::CAM_A);

    pointcloud->initialConfig.setSparse(true);

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

    camTof->raw.link(tof->input);
    tof->depth.link(xoutDepth->input);
    tof->depth.link(pointcloud->inputDepth);
    pointcloud->outputPointCloud.link(xout->input);

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

    dai::Device device(pipeline);

    auto q = device.getOutputQueue("pcl", 8, false);
    auto qDepth = device.getOutputQueue("depth", 8, false);

    while(true) {
        std::cout << "Waiting for data" << std::endl;
        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;

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

OskarSonc

Hey SoumayaMans ToF node on DepthAi v2 is incomplete, the warning
PointCloud ... TODO: implement... Using intrinsics from calib indicates the ToF->PointCloud path is falling back to calibration intrinsics, and on v2 this can produce invalid point coordinates (-inf) so PCL shows nothing.

I would strongly recommend moving to DepthAi v3 where this should work.
We just updated the docs on v3 ToF and also we have this example for just what you are trying to do:
luxonis/oak-examplestree/main/depth-measurement/3d-measurement/tof-pointcloud

Thanks,
Oskar

SoumayaMans

Hey Oskar,

I had doing attempted ToF -> Pointcloud similar to this on Depthai v3 at first but still got that same warning:
PointCloud ... TODO: implement... Using intrinsics from calib and a non-existent pointcloud on PCL.
Here is the Depthai v3 code I had attempted with similar results:

#include <iostream>
#include <opencv2/opencv.hpp>
#include "depthai/depthai.hpp"
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    auto viewer = std::make_unique<pcl::visualization::PCLVisualizer>("Cloud Viewer");
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");

    dai::Pipeline pipeline;

    // ToF node
    dai::CameraBoardSocket socket = dai::CameraBoardSocket::AUTO;
    dai::ImageFiltersPresetMode presetMode = dai::ImageFiltersPresetMode::TOF_MID_RANGE;
    std::shared_ptr<dai::node::ToF> tof = pipeline.create<dai::node::ToF>()->build(socket, presetMode);
    
    // Pointcloud Node
    std::shared_ptr<dai::node::PointCloud> pointcloud = pipeline.create<dai::node::PointCloud>();
    pointcloud->initialConfig->setSparse(true);

    std::shared_ptr<dai::MessageQueue> pclQueue = pointcloud->outputPointCloud.createOutputQueue();
    std::shared_ptr<dai::PointCloudData> pclMsg;

    tof->depth.link(pointcloud->inputDepth);

    pipeline.start();
    while(true) {
        std::cout << "Waiting for data" << std::endl;

        pclMsg = pclQueue->get<dai::PointCloudData>();
        
        if(!pclMsg) {
            std::cout << "No data" << std::endl;
            continue;
        }

        if(pclMsg->getPoints().empty()) {
            std::cout << "Empty point cloud" << std::endl;
            continue;
        }

        cloud = pclMsg->getPclData();
        viewer->updatePointCloud(cloud, "cloud");
        viewer->spinOnce(10);

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

I was hoping to handle computing the pointclouds onboard the camera, the example you linked, example, seems to do that differently no?

OskarSonc

SoumayaMans - oh I see. Yes you are correct.
The tof-pointcloud example computes XYZ on the host from ToF depth + calibration intrinsics, not through dai::node::PointCloud on-device.
Looks like this is a limitation, will ask the team if there is any workaround and get back to you asap.

Thanks,
Oskar

OskarSonc

SoumayaMans I asked the team and currently the only working way is like it is done in the example, but there are plans to add this for on device also in future.
Thanks

SoumayaMans

OskarSonc Alright that makes sense, I'll use that example as a reference then for now. Thank you!