Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

rs2::syncer T265 fails to synchronize fisheye streams (since v2.43.0) #9030

Open
matlabbe opened this issue May 14, 2021 · 5 comments
Open

Comments

@matlabbe
Copy link

matlabbe commented May 14, 2021


Required Info
Camera Model T265
Firmware Version 0.2.0.951
Operating System & Version Linux (Ubuntu 20.04)
Kernel Version (Linux Only) 5.8.0-53-generic
Platform PC
SDK Version 2.42.0 vs 2.43.0 and later
Language C
Segment Robot

Issue Description

This issue is related to this issue introlab/rtabmap#716 where rs2:syncer API cannot synchronize T265 frames correctly with realsense SDK >= v2.43.0

I tried to make a minimal example to reproduce the problem using the rs2::syncer api. Here is a modified version of pose-and-image example:

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#include <librealsense2/rs.hpp>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <thread>
#include <mutex>

int main(int argc, char * argv[]) try
{
    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    // Create a configuration for configuring the pipeline with a non default profile
    rs2::config cfg;
    // Enable both image streams
    // Note: It is not currently possible to enable only one
    cfg.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
    cfg.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);

    // Define frame callback

    // The callback is executed on a sensor thread and can be called simultaneously from multiple sensors
    // Therefore any modification to common memory should be done under lock
    std::mutex data_mutex;
    rs2::syncer syncer;
    auto callback = [&](const rs2::frame& frame)
    {
        std::lock_guard<std::mutex> lock(data_mutex);
        
        if (auto fs = frame.as<rs2::frameset>())
        {
            for(size_t i=0; i<fs.size(); ++i) {
                auto video_profile = fs[i].get_profile().as<rs2::video_stream_profile>();
                printf("Received: Format=%s %dx%d fps=%d index=%d stream=%s stamp=%f\n", rs2_format_to_string(video_profile.format()),
                video_profile.width(),
                video_profile.height(),
                video_profile.fps(),
                video_profile.stream_index(),
                video_profile.stream_name().c_str(),
                frame.get_timestamp());
                
                syncer(fs[i]);
            }
        }
    };

    // Start streaming through the callback
    rs2::pipeline_profile profiles = pipe.start(cfg, callback);

    // Sleep this thread until we are done
    while(true) {
        auto frameset = syncer.wait_for_frames(5000);
        
        printf("Synchronized %d frames!\n", (int)frameset.size());
        for(size_t i=0; i<frameset.size(); ++i) {
            auto video_profile = frameset[i].get_profile().as<rs2::video_stream_profile>();
            printf("Sync: Format=%s %dx%d fps=%d index=%d stream=%s stamp=%f\n", rs2_format_to_string(video_profile.format()),
                video_profile.width(),
                video_profile.height(),
                video_profile.fps(),
                video_profile.stream_index(),
                video_profile.stream_name().c_str(),
                frameset[i].get_timestamp());
        }
            
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

Expected output (v2.42.0)

Received: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621012448488.187744
Received: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621012448488.187744
Synchronized 2 frames!
Sync: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621012448488.187744
Sync: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621012448488.187744
Received: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621012448521.520996
Received: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621012448521.520996
Synchronized 2 frames!
Sync: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621012448521.520996
Sync: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621012448521.520996
Received: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621012448554.854492
Received: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621012448554.854492
Synchronized 2 frames!
Sync: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621012448554.854492
Sync: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621012448554.854492

Note that the 2 frames are synchronized.

Current master version (d5e775b):

Received: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621011972221.884277
Received: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621011972221.884277
Synchronized 1 frames!
Sync: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621011972221.884277
Received: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621011972255.217773
Received: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621011972255.217773
Synchronized 1 frames!
Sync: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621011972255.217773
Received: Format=Y8 848x800 fps=30 index=1 stream=Fisheye 1 stamp=1621011972255.217773
Received: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621011972255.217773
Synchronized 1 frames!
Sync: Format=Y8 848x800 fps=30 index=2 stream=Fisheye 2 stamp=1621011972255.217773

The current master version of rs2::syncer cannot synchronize T265 images correctly, returning randomly either the image of the first or second camera. The problem seems appeared since this commit: a49d873 (@aangerma)

@sumitsarkar1
Copy link

Any updates !!

@matlabbe
Copy link
Author

matlabbe commented Nov 5, 2021

Found a fix:

diff --git a/src/proc/syncer-processing-block.cpp b/src/proc/syncer-processing-block.cpp
index 34ebcb492..020b88eb7 100644
--- a/src/proc/syncer-processing-block.cpp
+++ b/src/proc/syncer-processing-block.cpp
@@ -11,7 +11,7 @@
 namespace librealsense
 {
     syncer_process_unit::syncer_process_unit(std::initializer_list< bool_option::ptr > enable_opts, bool log)
-        : processing_block("syncer"), _matcher((new composite_identity_matcher({})))
+        : processing_block("syncer"), _matcher((new timestamp_composite_matcher({})))
         , _enable_opts(enable_opts.begin(), enable_opts.end())
     {
         _matcher->set_callback( [this]( frame_holder f, syncronization_environment env ) {

It seems that the matcher type was timestamp_composite_matcher before v2.43.0, then changed to composite_identity_matcher (which seems not compatible with T265 stream). I tested with D435i and L515, and timestamp_composite_matcher is working for all sensors.

EDIT: Patch here https://gist.github.com/matlabbe/8d66bbf3c690aa689c73da1a8708306b

@sumitsarkar1
Copy link

the above solution is working with T265 and L515 ...thanx @matlabbe

@roelofvandijk
Copy link

I am also facing a similar problem, a syncer that used to sync all 8 frames from 2 D415s perfectly on 2.38, but does not anymore on 2.50.
Is there any timeline for reverting to timestamp_composite_matcher?

@sumitsarkar1
Copy link

@roelofvandijk did you try processing_block("syncer"), _matcher((new timestamp_composite_matcher({}))) for 2.50

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants