1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
|
#include "mediapipe/calculators/ims/time_sync_calculator.h"
namespace mediapipe {
absl::Status TimeSyncCalculator::GetContract(CalculatorContract* cc) { cc->Inputs().Tag("IR_IMAGE").Set<cv::Mat>(); cc->Inputs().Tag("OMS_IMAGE").Set<cv::Mat>(); cc->Inputs().Tag("RADAR_DATA").Set<RadarData>(); cc->Inputs().Tag("TIMESTAMP").Set<int64_t>(); cc->Outputs().Tag("SYNCED_IR_IMAGE").Set<cv::Mat>(); cc->Outputs().Tag("SYNCED_OMS_IMAGE").Set<cv::Mat>(); cc->Outputs().Tag("SYNCED_RADAR_DATA").Set<RadarData>(); return absl::OkStatus(); }
absl::Status TimeSyncCalculator::Process(CalculatorContext* cc) { int64_t target_timestamp = cc->InputTimestamp().Value(); if (!cc->Inputs().Tag("IR_IMAGE").IsEmpty()) { const auto& ir_image = cc->Inputs().Tag("IR_IMAGE").Get<cv::Mat>(); image_buffers_["IR"].push_back({ir_image, target_timestamp, 1.0f}); } if (!cc->Inputs().Tag("OMS_IMAGE").IsEmpty()) { const auto& oms_image = cc->Inputs().Tag("OMS_IMAGE").Get<cv::Mat>(); image_buffers_["OMS"].push_back({oms_image, target_timestamp, 1.0f}); } if (!cc->Inputs().Tag("RADAR_DATA").IsEmpty()) { const auto& radar = cc->Inputs().Tag("RADAR_DATA").Get<RadarData>(); radar_buffers_["RADAR"].push_back({radar, target_timestamp, 1.0f}); } for (auto& [name, buffer] : image_buffers_) { while (!buffer.empty() && target_timestamp - buffer.front().timestamp > max_buffer_size_ms_ * 1000) { buffer.pop_front(); } } auto synced_ir = GetNearestPacket(image_buffers_["IR"], target_timestamp); auto synced_oms = GetNearestPacket(image_buffers_["OMS"], target_timestamp); auto synced_radar = GetNearestPacket(radar_buffers_["RADAR"], target_timestamp); bool ir_valid = std::abs(synced_ir.timestamp - target_timestamp) < sync_tolerance_ms_ * 1000; bool oms_valid = std::abs(synced_oms.timestamp - target_timestamp) < sync_tolerance_ms_ * 1000; bool radar_valid = std::abs(synced_radar.timestamp - target_timestamp) < sync_tolerance_ms_ * 1000; if (ir_valid) { cc->Outputs().Tag("SYNCED_IR_IMAGE").AddPacket( MakePacket<cv::Mat>(synced_ir.data).At(cc->InputTimestamp())); } if (oms_valid) { cc->Outputs().Tag("SYNCED_OMS_IMAGE").AddPacket( MakePacket<cv::Mat>(synced_oms.data).At(cc->InputTimestamp())); } if (radar_valid) { cc->Outputs().Tag("SYNCED_RADAR_DATA").AddPacket( MakePacket<RadarData>(synced_radar.data).At(cc->InputTimestamp())); } return absl::OkStatus(); }
template<typename T> SyncedPacket<T> TimeSyncCalculator::GetNearestPacket( const std::deque<SyncedPacket<T>>& buffer, int64_t target_timestamp) { if (buffer.empty()) { return {T(), 0, 0.0f}; } auto it = std::lower_bound(buffer.begin(), buffer.end(), target_timestamp, [](const SyncedPacket<T>& p, int64_t ts) { return p.timestamp < ts; }); if (it == buffer.end()) { return buffer.back(); } if (it == buffer.begin()) { return *it; } auto prev = it - 1; return InterpolatePacket(*prev, *it, target_timestamp); }
REGISTER_CALCULATOR(TimeSyncCalculator);
}
|