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{ (double)a->get_frame_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL),
(double)b->get_frame_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL) };
}
}
void composite_matcher::dispatch(frame_holder f, syncronization_environment env)
{
std::stringstream s;
s <<"DISPATCH "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<<std::fixed<< f->get_frame_timestamp()<<"\n";
LOG_DEBUG(s.str());
clean_inactive_streams(f);
auto matcher = find_matcher(f);
update_last_arrived(f, matcher.get());
matcher->dispatch(std::move(f), env);
}
void composite_matcher::sync(frame_holder f, syncronization_environment env)
{
std::stringstream s;
s <<"SYNC "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<<std::fixed<< f->get_frame_timestamp()<<"\n";
LOG_DEBUG(s.str());
update_next_expected(f);
auto matcher = find_matcher(f);
_frames_queue[matcher.get()].enqueue(std::move(f));
std::vector<frame_holder*> frames_arrived;
std::vector<librealsense::matcher*> frames_arrived_matchers;
std::vector<librealsense::matcher*> synced_frames;
std::vector<librealsense::matcher*> missing_streams;
do
{
auto old_frames = false;
synced_frames.clear();
missing_streams.clear();
frames_arrived_matchers.clear();
frames_arrived.clear();
for (auto s = _frames_queue.begin(); s != _frames_queue.end(); s++)
{
frame_holder* f;
if (s->second.peek(&f))
{
frames_arrived.push_back(f);
frames_arrived_matchers.push_back(s->first);
}
else
{
missing_streams.push_back(s->first);
}
}
if (frames_arrived.size() == 0)
break;
frame_holder* curr_sync;
if (frames_arrived.size() > 0)
{
curr_sync = frames_arrived[0];
synced_frames.push_back(frames_arrived_matchers[0]);
}
for (auto i = 1; i < frames_arrived.size(); i++)
{
if (are_equivalent(*curr_sync, *frames_arrived[i]))
{
synced_frames.push_back(frames_arrived_matchers[i]);
}
else if (is_smaller_than(*frames_arrived[i], *curr_sync))
{
old_frames = true;
synced_frames.clear();
synced_frames.push_back(frames_arrived_matchers[i]);
curr_sync = frames_arrived[i];
}
else
{
old_frames = true;
}
}
if (!old_frames)
{
for (auto i : missing_streams)
{
if (!skip_missing_stream(synced_frames, i))
{
synced_frames.clear();
break;
}
}
}
if (synced_frames.size())
{
std::vector<frame_holder> match;
match.reserve(synced_frames.size());
for (auto index : synced_frames)
{
frame_holder frame;
_frames_queue[index].dequeue(&frame);
match.push_back(std::move(frame));
}
std::sort(match.begin(), match.end(), [](frame_holder& f1, frame_holder& f2)
{
return f1->get_stream()->get_unique_id()> f2->get_stream()->get_unique_id();
});
std::stringstream s;
s<<"MATCHED: ";
for(auto&& f: match)
{
auto composite = dynamic_cast<composite_frame*>(f.frame);
if(composite)
{
for (int i = 0; i < composite->get_embedded_frames_count(); i++)
{
auto matched = composite->get_frame(i);
s << matched->get_stream()->get_stream_type()<<" "<<matched->get_frame_timestamp()<<" ";
}
}
else {
s<<f->get_stream()->get_stream_type()<<" "<<(double)f->get_frame_timestamp()<<" ";
}
}
s<<"\n";
LOG_DEBUG(s.str());
frame_holder composite = env.source->allocate_composite_frame(std::move(match));
if (composite.frame)
{
s <<"SYNCED "<<_name<<"--> "<< composite->get_stream()->get_stream_type() << " " << composite->get_frame_number() << ", "<<std::fixed<< composite->get_frame_timestamp()<<"\n";
auto cb = begin_callback();
_callback(std::move(composite), env);
}
}
} while (synced_frames.size() > 0);
}
void identity_matcher::dispatch(frame_holder f, syncronization_environment env)
{
std::stringstream s;
s <<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<<std::fixed<< f->get_frame_timestamp()<<"\n";
LOG_DEBUG(s.str());
sync(std::move(f), env);
}
