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

[matlabbe]

---

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]

Any updates !!

[matlabbe]

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]

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

[roelofvandijk]

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]

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