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);
}
int rs_get_frame_timestamp(const rs_device * device, rs_stream stream, rs_error ** error) try
{
VALIDATE_NOT_NULL(device);
VALIDATE_ENUM(stream);
return device->get_frame_timestamp(stream);
}
syncer_proccess_unit::syncer_proccess_unit()
: _matcher((new timestamp_composite_matcher({})))
{
_matcher->set_callback([this](frame_holder f, syncronization_environment env)
{
std::stringstream ss;
ss << "SYNCED: ";
auto composite = dynamic_cast<composite_frame*>(f.frame);
for (int i = 0; i < composite->get_embedded_frames_count(); i++)
{
auto matched = composite->get_frame(i);
ss << matched->get_stream()->get_stream_type() << " " << matched->get_frame_number() << ", "<<std::fixed<< matched->get_frame_timestamp()<<"\n";
}
LOG_DEBUG(ss.str());
env.matches.enqueue(std::move(f));
});
auto f = [&](frame_holder frame, synthetic_source_interface* source)
{
single_consumer_queue<frame_holder> matches;
{
std::lock_guard<std::mutex> lock(_mutex);
_matcher->dispatch(std::move(frame), { source, matches });
}
frame_holder f;
while (matches.try_dequeue(&f))
{
get_source().frame_ready(std::move(f));
}
};
set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(
new internal_frame_processor_callback<decltype(f)>(f)));
}
