O3R Multithreaded Data Capture Issue

[robinbansal08]

In our use case, we require to capture 3D data from multiple camera heads(O3R252) connected to a single VPU(OVP810) from separate threads as shown below in the figure.

We want data capture process for each camera head to be independent of each other and can be initiated at different point of time, that is why we are handling them in different threads. In our use case it might be possible that data is captured at the same time (2 or 3 heads are running simultaneously). Due to noisy point cloud we have to run temporal filter for few seconds to improve the point cloud quality. In our test we tried to run data capture for each camera head in separate thread, first thread was able to get 3D data successfully but later, other threads failed with framegrabber timeout error.

void IfmO3rPcdGrabber::getPCDFromSensor(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz) {
    std::cout << "Getting point cloud from sensor: " << sensor_ip << " " << port_3d << std::endl;
    auto t1 = std::chrono::high_resolution_clock::now();

    auto cam = std::make_shared<ifm3d::O3R>(sensor_ip);

    //enable temporal filter and set to run mode
    std::cout << "Enabling filter to sensor: " << sensor_ip << " " << port_3d << std::endl;
    auto parameter = cam->ToJSON();
    parameter["ports"][port_3d]["state"] = "RUN";
    parameter["ports"][port_3d]["processing"]["diParam"]["enableTemporalFilter"] = true;
    cam->FromJSON(parameter);

    //wait for temporal filter
    std::this_thread::sleep_for(std::chrono::milliseconds(4000));

    const auto PCIC_PORT = cam->Port(port_3d).pcic_port;
    auto fg = std::make_shared<ifm3d::FrameGrabber>(cam, PCIC_PORT);
    fg->Start({ifm3d::buffer_id::XYZ});
    auto future = fg->WaitForFrame();
    if (future.wait_for(10s) != std::future_status::ready){
        throw std::runtime_error("Timeout waiting for camera!");
    }
    std::cout << "Got frame from sensor: " << sensor_ip << " " << port_3d << std::endl;
    auto frame = future.get();
    auto xyz = frame->GetBuffer(ifm3d::buffer_id::XYZ);

    convertToPointCloud(xyz, cloud_xyz);
    fg->Stop();

    // disable temporal filter, and set to idle mode
    std::cout << "Disabling filter to sensor: " << sensor_ip << " " << port_3d << std::endl;
    parameter["ports"][port_3d]["state"] = "IDLE";
    parameter["ports"][port_3d]["processing"]["diParam"]["enableTemporalFilter"] = false;
    cam->FromJSON(parameter);
}

void testParallelization() {
    //create threads, one for each camera
    std::thread t1([](){
        IfmO3rPcdGrabber sensor("192.168.0.69", "port0");
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
        sensor.getPCDFromSensor(cloud_xyz);
    });
    std::thread t2([](){
        IfmO3rPcdGrabber sensor("192.168.0.69", "port1");
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
        sensor.getPCDFromSensor(cloud_xyz);
    });
    std::thread t3([](){
        IfmO3rPcdGrabber sensor("192.168.0.69", "port2");
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
        sensor.getPCDFromSensor(cloud_xyz);
    });
    t1.join();
    t2.join();
    t3.join();
}

[dekhanra]

Hello @robinbansal08 , I will take a look and get back to you.

[lola-masson]

Hey @robinbansal08, I tried your code (I guessed at the missing parts), and I was able to reproduce the timeouts. I am not exactly sure why this happens. Probably something to do with the way ifm3d handles the wait for frame, I will check.

Have you tried using the callback instead? This way, you would not need to handle threading yourself. The simplified example below gets you the same behavior (in this case the callback will be called every time a frame is received, which will depend on the frame rate):

#include <chrono>
#include <ifm3d/common/json_impl.hpp>
#include <ifm3d/device/err.h>
#include <ifm3d/device/o3r.h>
#include <ifm3d/fg.h>
#include <iostream>
#include <string>
#include <thread>
#include <pcl-1.12/pcl/common/common.h>
#include <pcl-1.12/pcl/pcl_base.h>

using namespace std::chrono_literals;
using namespace ifm3d::literals;

void convertToPointCloud(const ifm3d::Buffer& buffer, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // Assuming the buffer contains XYZ data in a specific format
    // Adjust the following code according to the actual format of the buffer
    std::cout << "Converting buffer to PCL format." << std::endl;
    int width = buffer.width();
    int height = buffer.height();
    cloud->width = width;
    cloud->height = height;
    cloud->is_dense = false;
    cloud->points.resize(width * height);
    std::cout << "Width: " << width << std::endl;
    std::cout << "Height: " << height << std::endl;
    ifm3d::Buffer_< ifm3d::Point3D_32F>  xyz_data = buffer; // no copy of data 
    for (const auto& point : xyz_data)
    {
        cloud->push_back(pcl::PointXYZ(point.val[0], point.val[1], point.val[2])); 

    }
    std::cout << "Finished converting the buffer" << std::endl;
}

int main(){
    auto IP = "192.168.0.69";
    auto o3r = std::make_shared<ifm3d::O3R>(IP);
    auto ports = {"port0", "port1", "port2"};
    o3r->Set(ifm3d::json::parse(R"({"ports":{"port0":{"state":"RUN", "processing": {"diParam":{"enableTemporalFilter": true}}}, "port1":{"state":"RUN", "processing": {"diParam":{"enableTemporalFilter": true}}}, "port2":{"state":"RUN","processing": {"diParam":{"enableTemporalFilter": true}}}}})"));

    std::vector<ifm3d::FrameGrabber::Ptr> fgs;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
    for (auto port: ports){
        std::cout << "Setting up callback for port " << port << std::endl;
        auto fg = std::make_shared<ifm3d::FrameGrabber>(o3r, o3r->Port(port).pcic_port);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
        fg->OnNewFrame([port, cloud_xyz](ifm3d::Frame::Ptr frame){
            std::cout << "Handling frame from " << port << std::endl;
            auto xyz = frame->GetBuffer(ifm3d::buffer_id::XYZ);
            convertToPointCloud(xyz, cloud_xyz);
            });
        fg->Start({ifm3d::buffer_id::XYZ});
        fgs.push_back(fg);
        clouds.push_back(cloud_xyz);
        //Do something with clouds.
    }
    std::this_thread::sleep_for(0.2s);

    for (auto fg: fgs){
        fg->Stop();
    }
    o3r->Set(ifm3d::json::parse(R"({"ports":{"port0":{"state":"IDLE", "processing": {"diParam":{"enableTemporalFilter": false}}}, "port1":{"state":"IDLE", "processing": {"diParam":{"enableTemporalFilter": false}}}, "port2":{"state":"IDLE","processing": {"diParam":{"enableTemporalFilter": false}}}}})"));

    return 0;
}