You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
788 lines
29 KiB
788 lines
29 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2015 Intel Corporation. All Rights Reserved.
|
|
|
|
#include "sync.h"
|
|
#include "core/frame-holder.h"
|
|
#include "core/synthetic-source-interface.h"
|
|
#include "core/stream-profile-interface.h"
|
|
#include "core/device-interface.h"
|
|
#include "core/sensor-interface.h"
|
|
#include "composite-frame.h"
|
|
#include "core/time-service.h"
|
|
|
|
#include <rsutils/string/from.h>
|
|
|
|
|
|
namespace librealsense
|
|
{
|
|
const int MAX_GAP = 1000;
|
|
|
|
#define LOG_IF_ENABLE( OSTREAM, ENV ) \
|
|
while( ENV.log ) \
|
|
{ \
|
|
LOG_DEBUG( OSTREAM ); \
|
|
break; \
|
|
}
|
|
|
|
|
|
matcher::matcher(std::vector<stream_id> streams_id)
|
|
: _streams_id(streams_id){}
|
|
|
|
void matcher::sync(frame_holder f, const syncronization_environment& env)
|
|
{
|
|
auto cb = begin_callback();
|
|
_callback(std::move(f), env);
|
|
}
|
|
|
|
void matcher::set_callback(sync_callback f)
|
|
{
|
|
_callback = f;
|
|
}
|
|
|
|
callback_invocation_holder matcher::begin_callback()
|
|
{
|
|
return{ _callback_inflight.allocate(), &_callback_inflight };
|
|
}
|
|
|
|
std::string matcher::get_name() const
|
|
{
|
|
return _name;
|
|
}
|
|
|
|
bool matcher::get_active() const
|
|
{
|
|
return _active;
|
|
}
|
|
|
|
void matcher::set_active(const bool active)
|
|
{
|
|
_active = active;
|
|
}
|
|
|
|
const std::vector<stream_id>& matcher::get_streams() const
|
|
{
|
|
return _streams_id;
|
|
}
|
|
|
|
const std::vector<rs2_stream>& matcher::get_streams_types() const
|
|
{
|
|
return _streams_type;
|
|
}
|
|
|
|
matcher::~matcher()
|
|
{
|
|
_callback_inflight.stop_allocation();
|
|
|
|
auto callbacks_inflight = _callback_inflight.get_size();
|
|
if (callbacks_inflight > 0)
|
|
{
|
|
LOG_WARNING(callbacks_inflight << " callbacks are still running on some other threads. Waiting until all callbacks return...");
|
|
}
|
|
// wait until user is done with all the stuff he chose to borrow
|
|
try
|
|
{
|
|
_callback_inflight.wait_until_empty();
|
|
}
|
|
catch( const std::exception & e )
|
|
{
|
|
LOG_DEBUG( "Error while waiting for user callback to finish: " << e.what() );
|
|
}
|
|
}
|
|
|
|
identity_matcher::identity_matcher(stream_id stream, rs2_stream stream_type)
|
|
:matcher({stream})
|
|
{
|
|
_streams_type = {stream_type};
|
|
|
|
std::ostringstream ss;
|
|
ss << get_abbr_string( stream_type );
|
|
ss << stream;
|
|
_name = ss.str();
|
|
}
|
|
|
|
void identity_matcher::dispatch(frame_holder f, const syncronization_environment& env)
|
|
{
|
|
//LOG_IF_ENABLE( "--> identity_matcher: " << _name, env );
|
|
|
|
sync(std::move(f), env);
|
|
}
|
|
|
|
std::string create_composite_name(const std::vector<std::shared_ptr<matcher>>& matchers, const std::string& name)
|
|
{
|
|
std::stringstream s;
|
|
s<<"("<<name;
|
|
|
|
bool first = true;
|
|
for (auto&& matcher : matchers)
|
|
{
|
|
if( first )
|
|
first = false;
|
|
else
|
|
s << ' ';
|
|
s << matcher->get_name();
|
|
}
|
|
s << ")";
|
|
return s.str();
|
|
}
|
|
|
|
composite_matcher::composite_matcher(
|
|
std::vector< std::shared_ptr< matcher > > const & matchers, std::string const & name )
|
|
{
|
|
for (auto&& matcher : matchers)
|
|
{
|
|
for (auto&& stream : matcher->get_streams())
|
|
{
|
|
matcher->set_callback(
|
|
[&]( frame_holder f, const syncronization_environment & env ) {
|
|
LOG_IF_ENABLE( "<-- " << *f.frame << " " << _name, env );
|
|
sync( std::move( f ), env );
|
|
} );
|
|
_matchers[stream] = matcher;
|
|
_streams_id.push_back(stream);
|
|
}
|
|
for (auto&& stream : matcher->get_streams_types())
|
|
{
|
|
_streams_type.push_back(stream);
|
|
}
|
|
}
|
|
|
|
_name = create_composite_name(matchers, name);
|
|
}
|
|
|
|
composite_matcher::matcher_queue::matcher_queue()
|
|
: q( QUEUE_MAX_SIZE,
|
|
[]( frame_holder const & fh )
|
|
{
|
|
// If queues are overrun, we'll get here
|
|
LOG_DEBUG( "DROPPED frame " << fh );
|
|
} )
|
|
{
|
|
}
|
|
|
|
|
|
void composite_matcher::dispatch(frame_holder f, const syncronization_environment& env)
|
|
{
|
|
clean_inactive_streams(f);
|
|
auto matcher = find_matcher(f);
|
|
|
|
//LOG_IF_ENABLE( "--> composite_matcher: " << _name, env );
|
|
|
|
if (matcher)
|
|
{
|
|
update_last_arrived(f, matcher.get());
|
|
matcher->dispatch(std::move(f), env);
|
|
}
|
|
else
|
|
{
|
|
LOG_ERROR( "didn't find any matcher; releasing unsynchronized frame " << *f.frame );
|
|
_callback(std::move(f), env);
|
|
}
|
|
}
|
|
|
|
std::shared_ptr<matcher> composite_matcher::find_matcher(const frame_holder& frame)
|
|
{
|
|
auto stream_profile = frame.frame->get_stream();
|
|
auto stream_id = stream_profile->get_unique_id();
|
|
auto stream_type = stream_profile->get_stream_type();
|
|
|
|
std::shared_ptr<matcher> matcher;
|
|
auto it = _matchers.find( stream_id );
|
|
if( it != _matchers.end() )
|
|
{
|
|
matcher = it->second;
|
|
if( matcher )
|
|
{
|
|
if( ! matcher->get_active() )
|
|
{
|
|
matcher->set_active( true );
|
|
_frames_queue[matcher.get()].q.start();
|
|
}
|
|
return matcher;
|
|
}
|
|
}
|
|
LOG_DEBUG( "no matcher found for " << get_abbr_string( stream_type ) << stream_id
|
|
<< "; creating matcher from device..." );
|
|
|
|
auto sensor = frame.frame->get_sensor().get(); //TODO: Potential deadlock if get_sensor() gets a hold of the last reference of that sensor
|
|
auto dev_exist = false;
|
|
if (sensor)
|
|
{
|
|
const device_interface* dev = nullptr;
|
|
try
|
|
{
|
|
dev = sensor->get_device().shared_from_this().get();
|
|
}
|
|
catch (const std::bad_weak_ptr&)
|
|
{
|
|
LOG_WARNING("Device destroyed");
|
|
}
|
|
if (dev)
|
|
{
|
|
dev_exist = true;
|
|
matcher = dev->create_matcher(frame);
|
|
LOG_DEBUG( "... created " << matcher->get_name() );
|
|
|
|
matcher->set_callback(
|
|
[&]( frame_holder f, syncronization_environment const & env ) {
|
|
LOG_IF_ENABLE( "<-- " << *f.frame << " " << _name, env );
|
|
sync( std::move( f ), env );
|
|
} );
|
|
|
|
for (auto stream : matcher->get_streams())
|
|
{
|
|
if (_matchers[stream])
|
|
{
|
|
_frames_queue.erase(_matchers[stream].get());
|
|
}
|
|
_matchers[stream] = matcher;
|
|
_streams_id.push_back(stream);
|
|
}
|
|
for (auto stream : matcher->get_streams_types())
|
|
{
|
|
_streams_type.push_back(stream);
|
|
}
|
|
|
|
if (std::find(_streams_type.begin(), _streams_type.end(), stream_type) == _streams_type.end())
|
|
{
|
|
LOG_ERROR("Stream matcher not found! stream=" << rs2_stream_to_string(stream_type));
|
|
}
|
|
else
|
|
{
|
|
_name = create_composite_name( { matcher },
|
|
_name.substr( 1, _name.length() - 2 ) ); // Remove the "()" around "(CI: )"
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
LOG_DEBUG("sensor does not exist");
|
|
}
|
|
|
|
if (!dev_exist)
|
|
{
|
|
matcher = _matchers[stream_id];
|
|
// We don't know what device this frame came from, so just store it under device NULL with ID matcher
|
|
if (!matcher)
|
|
{
|
|
_matchers[stream_id] = std::make_shared<identity_matcher>(stream_id, stream_type);
|
|
_streams_id.push_back(stream_id);
|
|
_streams_type.push_back(stream_type);
|
|
matcher = _matchers[stream_id];
|
|
|
|
matcher->set_callback(
|
|
[&]( frame_holder f, syncronization_environment const & env ) {
|
|
LOG_IF_ENABLE( "<-- " << *f.frame << " " << _name, env );
|
|
sync( std::move( f ), env );
|
|
} );
|
|
}
|
|
}
|
|
return matcher;
|
|
}
|
|
|
|
void composite_matcher::stop()
|
|
{
|
|
// We don't want to stop while dispatching!
|
|
std::lock_guard<std::mutex> lock( _mutex );
|
|
|
|
// Mark ourselves inactive, so we don't get new dispatches
|
|
set_active( false );
|
|
|
|
// Stop all our queues to wake up anyone waiting on them
|
|
for( auto & fq : _frames_queue )
|
|
fq.second.q.stop();
|
|
|
|
// Trickle the stop down to any children
|
|
for( auto m : _matchers )
|
|
m.second->stop();
|
|
}
|
|
|
|
std::string
|
|
composite_matcher::frames_to_string( std::vector< frame_holder * > const & frames )
|
|
{
|
|
std::ostringstream os;
|
|
for( auto pfh : frames )
|
|
os << *pfh;
|
|
return os.str();
|
|
}
|
|
|
|
std::string
|
|
composite_matcher::matchers_to_string( std::vector< librealsense::matcher* > const& matchers )
|
|
{
|
|
std::ostringstream os;
|
|
os << '[';
|
|
for( auto m : matchers )
|
|
{
|
|
auto const & q = _frames_queue[m].q;
|
|
q.peek( [&os]( frame_holder const & fh ) {
|
|
os << fh;
|
|
} );
|
|
}
|
|
os << ']';
|
|
return os.str();
|
|
}
|
|
|
|
void composite_matcher::sync(frame_holder f, const syncronization_environment& env)
|
|
{
|
|
auto matcher = find_matcher(f);
|
|
if (!matcher)
|
|
{
|
|
LOG_ERROR("didn't find any matcher for " << f << " will not be synchronized");
|
|
_callback(std::move(f), env);
|
|
return;
|
|
}
|
|
update_next_expected( matcher, f );
|
|
|
|
// We want to keep track of a "last-arrived" frame which is our current equivalent of "now" -- it contains the
|
|
// latest timestamp/frame-number/etc. that we can compare to.
|
|
auto const last_arrived = f->get_header();
|
|
|
|
if( ! _frames_queue[matcher.get()].q.enqueue( std::move( f ) ) )
|
|
// If we get stopped, nothing to do!
|
|
return;
|
|
|
|
// We have a queue for each known stream we want to sync.
|
|
// E.g., for (Depth Color), we need to sync two frames, one from each.
|
|
// If we have a Color frame but not Depth, then Depth is "missing" and needs to be
|
|
// waited-for...
|
|
|
|
std::vector< frame_holder * > frames_arrived;
|
|
std::vector< librealsense::matcher * > frames_arrived_matchers;
|
|
std::vector< int > synced_frames;
|
|
std::vector< int > unsynced_frames;
|
|
std::vector< librealsense::matcher * > missing_streams;
|
|
|
|
while( true )
|
|
{
|
|
missing_streams.clear();
|
|
frames_arrived_matchers.clear();
|
|
frames_arrived.clear();
|
|
|
|
std::vector< frame_holder > match;
|
|
{
|
|
// We don't want to stop while syncing!
|
|
std::lock_guard< std::mutex > lock( _mutex );
|
|
|
|
// We want to release one frame from each matcher. If a matcher has nothing queued, it is "missing" and
|
|
// we need to consider waiting for it:
|
|
for( auto s = _frames_queue.begin(); s != _frames_queue.end(); s++ )
|
|
{
|
|
librealsense::matcher * const m = s->first;
|
|
if( ! s->second.q.peek( [&]( frame_holder & fh ) {
|
|
LOG_IF_ENABLE( "... have " << *fh.frame, env );
|
|
frames_arrived.push_back( &fh );
|
|
frames_arrived_matchers.push_back( m );
|
|
} ) )
|
|
{
|
|
missing_streams.push_back( m );
|
|
}
|
|
}
|
|
if( frames_arrived.empty() )
|
|
{
|
|
// LOG_IF_ENABLE( "... nothing more to do", env );
|
|
break;
|
|
}
|
|
|
|
// From what we collected, we want to release only the frames that are synchronized (based on timestamp,
|
|
// number, etc.) -- anything else we'll leave to the next iteration. The synced frames should be the
|
|
// earliest possible!
|
|
|
|
frame_holder * curr_sync = frames_arrived[0];
|
|
synced_frames.clear();
|
|
synced_frames.push_back( 0 );
|
|
|
|
// Sometimes we have to release newly-arrived frames even before frames we already had previously
|
|
// queued. If we have something like this, 'have_unsynced_frames' will be true:
|
|
unsynced_frames.clear();
|
|
for( auto i = 1; i < frames_arrived.size(); i++ )
|
|
{
|
|
if( are_equivalent( *curr_sync, *frames_arrived[i] ) )
|
|
{
|
|
synced_frames.push_back( i );
|
|
}
|
|
else if( is_smaller_than( *frames_arrived[i], *curr_sync ) )
|
|
{
|
|
unsynced_frames.insert( unsynced_frames.end(), synced_frames.begin(), synced_frames.end() );
|
|
synced_frames.clear();
|
|
synced_frames.push_back( i );
|
|
curr_sync = frames_arrived[i];
|
|
}
|
|
else
|
|
{
|
|
unsynced_frames.push_back( i );
|
|
}
|
|
}
|
|
bool release_synced_frames = ( synced_frames.size() != 0 );
|
|
if( unsynced_frames.empty() )
|
|
{
|
|
// Everything (could be only one!) matches together... but if we also have
|
|
// something missing, we can't release anything yet...
|
|
for( auto i : missing_streams )
|
|
{
|
|
LOG_IF_ENABLE( "... missing " << i->get_name() << ", next expected @"
|
|
<< rsutils::string::from( _next_expected[i].value ) << " (from "
|
|
<< rsutils::string::from( _next_expected[i].fps ) << " fps)",
|
|
env );
|
|
if( skip_missing_stream( *curr_sync, i, last_arrived, env ) )
|
|
{
|
|
LOG_IF_ENABLE( "... cannot be synced; not waiting for it", env );
|
|
continue;
|
|
}
|
|
|
|
LOG_IF_ENABLE( "... waiting for it", env );
|
|
release_synced_frames = false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for( auto i : unsynced_frames )
|
|
{
|
|
LOG_IF_ENABLE( " - " << *frames_arrived[i]->frame << " is not in sync; won't be released", env );
|
|
}
|
|
}
|
|
if( ! release_synced_frames )
|
|
break;
|
|
|
|
match.reserve( synced_frames.size() );
|
|
|
|
for( auto index : synced_frames )
|
|
{
|
|
frame_holder frame;
|
|
int const timeout_ms = 5000;
|
|
librealsense::matcher * m = frames_arrived_matchers[index];
|
|
_frames_queue[m].q.dequeue( &frame, timeout_ms );
|
|
match.push_back( std::move( frame ) );
|
|
}
|
|
}
|
|
|
|
// The frameset should always be with the same order of streams (the first stream carries extra
|
|
// meaning because it decides the frameset properties) -- so we sort them...
|
|
std::sort( match.begin(),
|
|
match.end(),
|
|
[]( const frame_holder & f1, const frame_holder & f2 ) {
|
|
return f1.frame->get_stream()->get_unique_id()
|
|
> f2.frame->get_stream()->get_unique_id();
|
|
} );
|
|
|
|
|
|
frame_holder composite = env.source->allocate_composite_frame(std::move(match));
|
|
if (composite.frame)
|
|
{
|
|
auto cb = begin_callback();
|
|
_callback(std::move(composite), env);
|
|
}
|
|
}
|
|
}
|
|
|
|
frame_number_composite_matcher::frame_number_composite_matcher(
|
|
std::vector< std::shared_ptr< matcher > > const & matchers )
|
|
: composite_matcher( matchers, "FN: " )
|
|
{
|
|
}
|
|
|
|
void frame_number_composite_matcher::update_last_arrived(frame_holder& f, matcher* m)
|
|
{
|
|
_last_arrived[m] =f->get_frame_number();
|
|
}
|
|
|
|
bool frame_number_composite_matcher::are_equivalent(frame_holder& a, frame_holder& b)
|
|
{
|
|
return a->get_frame_number() == b->get_frame_number();
|
|
}
|
|
bool frame_number_composite_matcher::is_smaller_than(frame_holder & a, frame_holder & b)
|
|
{
|
|
return a->get_frame_number() < b->get_frame_number();
|
|
}
|
|
void frame_number_composite_matcher::clean_inactive_streams(frame_holder& f)
|
|
{
|
|
std::vector<stream_id> inactive_matchers;
|
|
for(auto m: _matchers)
|
|
{
|
|
if( _last_arrived[m.second.get()]
|
|
&& ( std::abs( (long long)f->get_frame_number() - (long long)_last_arrived[m.second.get()] ) ) > 5 )
|
|
{
|
|
std::stringstream s;
|
|
s << "clean inactive stream in "<<_name;
|
|
for (auto stream : m.second->get_streams_types())
|
|
{
|
|
s << stream << " ";
|
|
}
|
|
LOG_DEBUG(s.str());
|
|
|
|
inactive_matchers.push_back(m.first);
|
|
m.second->set_active(false);
|
|
}
|
|
}
|
|
|
|
for(auto id: inactive_matchers)
|
|
{
|
|
_frames_queue[_matchers[id].get()].q.clear();
|
|
}
|
|
}
|
|
|
|
bool
|
|
frame_number_composite_matcher::skip_missing_stream( frame_interface const * const synced_frame,
|
|
matcher * missing,
|
|
frame_header const & last_arrived,
|
|
const syncronization_environment & env )
|
|
{
|
|
if(!missing->get_active())
|
|
return true;
|
|
|
|
auto const & next_expected = _next_expected[missing];
|
|
|
|
if( synced_frame->get_frame_number() - next_expected.value > 4
|
|
|| synced_frame->get_frame_number() < next_expected.value )
|
|
{
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void frame_number_composite_matcher::update_next_expected(
|
|
std::shared_ptr< matcher > const & matcher, const frame_holder & f )
|
|
{
|
|
_next_expected[matcher.get()].value = f.frame->get_frame_number()+1.;
|
|
}
|
|
|
|
std::pair<double, double> extract_timestamps(frame_holder & a, frame_holder & b)
|
|
{
|
|
if (a->get_frame_timestamp_domain() == b->get_frame_timestamp_domain())
|
|
return{ a->get_frame_timestamp(), b->get_frame_timestamp() };
|
|
else
|
|
return{ a->get_frame_system_time(), b->get_frame_system_time() };
|
|
}
|
|
|
|
timestamp_composite_matcher::timestamp_composite_matcher(
|
|
std::vector< std::shared_ptr< matcher > > const & matchers )
|
|
: composite_matcher( matchers, "TS: " )
|
|
{
|
|
}
|
|
bool timestamp_composite_matcher::are_equivalent(frame_holder & a, frame_holder & b)
|
|
{
|
|
auto a_fps = get_fps(a);
|
|
auto b_fps = get_fps(b);
|
|
|
|
auto min_fps = std::min(a_fps, b_fps);
|
|
|
|
auto ts = extract_timestamps(a, b);
|
|
|
|
return are_equivalent(ts.first, ts.second, min_fps);
|
|
}
|
|
|
|
bool timestamp_composite_matcher::is_smaller_than(frame_holder & a, frame_holder & b)
|
|
{
|
|
if (!a || !b)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
auto ts = extract_timestamps(a, b);
|
|
|
|
return ts.first < ts.second;
|
|
}
|
|
|
|
void timestamp_composite_matcher::update_last_arrived(frame_holder& f, matcher* m)
|
|
{
|
|
auto const now = time_service::get_time();
|
|
//LOG_DEBUG( _name << ": _last_arrived[" << m->get_name() << "] = " << now );
|
|
_last_arrived[m] = now;
|
|
}
|
|
|
|
double timestamp_composite_matcher::get_fps( frame_interface const * f )
|
|
{
|
|
double fps = 0.;
|
|
rs2_metadata_type fps_md;
|
|
if( f->find_metadata( RS2_FRAME_METADATA_ACTUAL_FPS, &fps_md ) )
|
|
fps = fps_md / 1000.;
|
|
if( fps )
|
|
{
|
|
//LOG_DEBUG( "fps " << rsutils::string::from( fps ) << " from metadata " << *f );
|
|
}
|
|
else
|
|
{
|
|
fps = f->get_stream()->get_framerate();
|
|
//LOG_DEBUG( "fps " << rsutils::string::from( fps ) << " from stream framerate " << *f );
|
|
}
|
|
return fps;
|
|
}
|
|
|
|
void
|
|
timestamp_composite_matcher::update_next_expected( std::shared_ptr< matcher > const & matcher,
|
|
const frame_holder & f )
|
|
{
|
|
auto fps = get_fps( f );
|
|
auto gap = 1000. / fps;
|
|
|
|
auto ts = f.frame->get_frame_timestamp();
|
|
auto ne = ts + gap;
|
|
//LOG_DEBUG( "... next_expected = {timestamp}" << rsutils::string::from( ts ) << " + {gap}(1000/{fps}"
|
|
// << rsutils::string::from( fps )
|
|
// << ") = " << rsutils::string::from( ne ) );
|
|
auto & next_expected = _next_expected[matcher.get()];
|
|
next_expected.value = ne;
|
|
next_expected.fps = fps;
|
|
next_expected.domain = f.frame->get_frame_timestamp_domain();
|
|
}
|
|
|
|
void timestamp_composite_matcher::clean_inactive_streams(frame_holder& f)
|
|
{
|
|
// We let skip_missing_stream clean any inactive missing streams
|
|
}
|
|
|
|
bool timestamp_composite_matcher::skip_missing_stream( frame_interface const * waiting_to_be_released,
|
|
matcher * missing,
|
|
frame_header const & last_arrived,
|
|
const syncronization_environment & env )
|
|
{
|
|
// true : frameset is ready despite the missing stream (no use waiting) -- "skip" it
|
|
// false: the missing stream is relevant and our frameset isn't ready yet!
|
|
|
|
if(!missing->get_active())
|
|
return true;
|
|
|
|
//LOG_IF_ENABLE( "... matcher " << synced[0]->get_name(), env );
|
|
|
|
auto const & next_expected = _next_expected[missing];
|
|
// LOG_IF_ENABLE( "... next " << std::fixed << next_expected, env );
|
|
|
|
if( next_expected.domain != last_arrived.timestamp_domain )
|
|
{
|
|
// LOG_IF_ENABLE( "... not the same domain: frameset not ready!", env );
|
|
// D457 dev - return false removed
|
|
// because IR has no md, so its ts domain is "system time"
|
|
// other streams have md, and their ts domain is "hardware clock"
|
|
//return false;
|
|
}
|
|
|
|
// We want to calculate a cutout for inactive stream detection: if we wait too long past
|
|
// this cutout, then the missing stream is inactive and we no longer wait for it.
|
|
//
|
|
// cutout = next expected + threshold
|
|
// threshold = 7 * gap = 7 * (1000 / FPS)
|
|
//
|
|
//
|
|
// E.g.:
|
|
//
|
|
// D: 100 fps -> 10 ms gap
|
|
// C: 10 fps -> 100 ms gap
|
|
//
|
|
// D C @timestamp
|
|
// -- -- ----------
|
|
// 1 0 -> release (we don't know about a missing stream yet, so don't get here)
|
|
// ...
|
|
// 6 50 -> release (D6); next expected (NE) -> 60
|
|
// 1 0 -> release (C1); NE -> 100 <----- LATENCY
|
|
// 7 ? 60 -> release (D7) (not comparable to NE because we use fps of 100!)
|
|
// ...
|
|
// 11 ? 100 -> wait for C (now comparable); cutout is 100+7*10 = 170
|
|
// 12 ? 110 -> wait (> NE, < cutout)
|
|
// ...
|
|
// 19 ? 180 > 170 -> release (D11); mark C inactive (> cutout)
|
|
// release (D12) ... (D19) (no missing streams, so don't get here)
|
|
// 20 190 -> release (D20) (no missing streams, so don't get here)
|
|
// ...
|
|
//
|
|
// But, if C had arrived before D19:
|
|
//
|
|
// ...
|
|
// 2 100 -> release (D11, C2) (nothing missing); NE -> 200
|
|
// ? release (D12) ... (D18) (C missing but 100 not comparable to NE 200)
|
|
// 19 ? 180 -> release (D19)
|
|
// 20 ? 190 -> release (D20)
|
|
// 21 ? 200 -> wait for C (comparable to NE 200)
|
|
//
|
|
//
|
|
// The threshold is a function of the FPS, but note we can't keep more frames than
|
|
// our queue size and per-stream archive size allow.
|
|
auto const fps = get_fps( waiting_to_be_released );
|
|
|
|
rs2_time_t now = last_arrived.timestamp;
|
|
if( now > next_expected.value )
|
|
{
|
|
// Wait for the missing stream frame to arrive -- up to a cutout: anything more and we
|
|
// let the frameset be ready without it...
|
|
auto gap = 1000. / fps;
|
|
// NOTE: the threshold is a function of the gap; the bigger it is, the more latency
|
|
// between the streams we're willing to live with. Each gap is a frame so we are limited
|
|
// by the number of frames we're willing to keep (which is our queue limit)
|
|
auto threshold = 7 * gap; // really 7+1 because NE is already 1 away
|
|
if( now - next_expected.value < threshold )
|
|
{
|
|
//LOG_IF_ENABLE( "... still below cutout of {NE+7*gap}"
|
|
// << rsutils::string::from( next_expected + threshold ),
|
|
// env );
|
|
return false;
|
|
}
|
|
LOG_IF_ENABLE( "... exceeded cutout of {NE+7*gap}"
|
|
<< rsutils::string::from( next_expected.value + threshold ) << "; deactivating matcher!",
|
|
env );
|
|
|
|
auto const q_it = _frames_queue.find( missing );
|
|
if( q_it != _frames_queue.end() )
|
|
{
|
|
if( q_it->second.q.empty() )
|
|
_frames_queue.erase( q_it );
|
|
}
|
|
missing->set_active( false );
|
|
return true;
|
|
}
|
|
|
|
return ! are_equivalent( waiting_to_be_released->get_frame_timestamp(),
|
|
next_expected.value,
|
|
fps );
|
|
}
|
|
|
|
bool timestamp_composite_matcher::are_equivalent( double a, double b, double fps )
|
|
{
|
|
auto gap = 1000. / fps;
|
|
if( std::abs( a - b ) < ( gap / 2 ) )
|
|
{
|
|
//LOG_DEBUG( "... " << rsutils::string::from( a ) << " == " << rsutils::string::from( b ) << " {diff}"
|
|
// << std::abs( a - b ) << " < " << rsutils::string::from( gap / 2 ) << "{gap/2}" );
|
|
return true;
|
|
}
|
|
|
|
//LOG_DEBUG( "... " << rsutils::string::from( a ) << " != " << rsutils::string::from( b ) << " {diff}"
|
|
// << rsutils::string::from( std::abs( a - b ) ) << " >= " << rsutils::string::from( gap / 2 )
|
|
// << "{gap/2}" );
|
|
return false;
|
|
}
|
|
|
|
composite_identity_matcher::composite_identity_matcher(
|
|
std::vector< std::shared_ptr< matcher > > const & matchers )
|
|
: composite_matcher( matchers, "CI: " )
|
|
{
|
|
}
|
|
|
|
void composite_identity_matcher::sync(frame_holder f, const syncronization_environment& env)
|
|
{
|
|
auto composite = dynamic_cast<const composite_frame *>(f.frame);
|
|
// Syncer have to output composite frame
|
|
if (!composite)
|
|
{
|
|
std::vector<frame_holder> match;
|
|
std::ostringstream frame_string_for_logging;
|
|
frame_string_for_logging << f; // Saving frame holder string before moving frame
|
|
|
|
match.push_back(std::move(f));
|
|
frame_holder composite = env.source->allocate_composite_frame(std::move(match));
|
|
if (composite.frame)
|
|
{
|
|
auto cb = begin_callback();
|
|
LOG_DEBUG( "wrapped with composite: " << *composite.frame );
|
|
_callback(std::move(composite), env);
|
|
}
|
|
else
|
|
{
|
|
LOG_ERROR( "composite_identity_matcher: "
|
|
<< _name << " " << frame_string_for_logging.str()
|
|
<< " faild to create composite_frame, user callback will not be called" );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_callback( std::move( f ), env );
|
|
}
|
|
}
|
|
} // namespace librealsense
|