awe/radar_system
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rework radar system

Browse Source
This commit is contained in:
Ayzen
2026-10-06 12:32:26 +03:00
parent 552cdc688a
commit 5bb86b7165
299 changed files with 13537 additions and 27763 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/processors/include/object_approach_filter.hpp
+14 -24
View File
@@ -10,8 +10,7 @@
namespace radar::processing {
// Temporal "approach" filter for coherent-BP detections — a port of the objects-only
// filter in Horns_motion_3libre's demonstrate notebook.
// Temporal "approach" filter for coherent-BP detections.
//
// It keeps only objects that persist as a *motion-consistent track* across at least
// `min_frames` consecutive frames: as the radar moves, a real target reappears at a
@@ -30,12 +29,8 @@ namespace radar::processing {
// inter-frame gap (`kMaxFrameGapSeconds`), so a stale history from a previous run cannot
// spuriously confirm objects.
//
// IDEMPOTENT under reprocessing: the data_processor re-runs the last collection (or
// replays the whole window) whenever live settings or the socket speed change, with no
// new sweep. The history is therefore keyed by `frame_id` (the strictly increasing
// collection id): the same id replaces its entry (reprocess of the current frame), a
// smaller id rebuilds from scratch (a replay restart), so repeated reprocessing never
// duplicates frames or falsely confirms a track.
// Reprocessing replaces the same (collection_id, capture_start_ns) entry. Earlier
// capture times rebuild the history when the processor replays its retained window.
class ObjectApproachFilter {
public:
struct Point {
@@ -43,20 +38,16 @@ class ObjectApproachFilter {
double z_m;
};
// Record `objects` as frame `frame_id` and return, per object, whether it is
// confirmed (ends a >= `min_frames` motion-consistent track). `frame_id` is the
// collection id (identity/order, survives reprocessing); `frame_time_seconds` is the
// frame's wall-clock timestamp (drives the inter-frame interval); `speed_m_s`/
// `look_angle_deg` are the live motion estimate. `min_frames <= 1` disables filtering.
// Acquisition time drives both ordering and motion intervals. min_frames <= 1 disables filtering.
[[nodiscard]] auto confirm(
const std::vector<Point>& objects,
std::uint64_t frame_id,
double frame_time_seconds,
std::uint64_t capture_start_ns,
double speed_m_s,
double look_angle_deg,
std::size_t min_frames
) -> std::vector<bool> {
record_frame(Frame{frame_id, frame_time_seconds, objects});
record_frame(Frame{frame_id, capture_start_ns, objects});
const std::size_t history_depth = std::max<std::size_t>(min_frames, 1U);
while (history_.size() > history_depth) {
history_.pop_front();
@@ -79,17 +70,14 @@ class ObjectApproachFilter {
private:
struct Frame {
std::uint64_t id;
double time_seconds;
std::uint64_t capture_start_ns;
std::vector<Point> objects;
};
// Append a genuinely new frame, replace the current one on reprocessing (same id),
// or rebuild from scratch when the id steps backward (a replay restart). This keeps
// the history one entry per distinct collection no matter how often settings change.
void record_frame(Frame frame) {
if (history_.empty() || frame.id > history_.back().id) {
if (history_.empty() || frame.capture_start_ns > history_.back().capture_start_ns) {
history_.push_back(std::move(frame));
} else if (frame.id == history_.back().id) {
} else if (frame.id == history_.back().id && frame.capture_start_ns == history_.back().capture_start_ns) {
history_.back() = std::move(frame);
} else {
history_.clear();
@@ -97,7 +85,7 @@ class ObjectApproachFilter {
}
}
// Fixed matching tolerances (Horns_motion notebook 0.2 block). Range decreases as the
// Fixed matching tolerances. Range decreases as the
// radar approaches the target, hence the negative Z sign.
static constexpr double kXToleranceM = 0.45;
static constexpr double kRangeToleranceFraction = 0.85;
@@ -122,8 +110,10 @@ class ObjectApproachFilter {
for (std::size_t step = 1U; step < min_frames; ++step) {
const std::size_t earlier_index = newest - step;
const Frame& earlier = history_[earlier_index];
const double dt = history_[earlier_index + 1U].time_seconds - earlier.time_seconds;
if (!(dt > 0.0) || dt > kMaxFrameGapSeconds) {
const auto later_ns = history_[earlier_index + 1U].capture_start_ns;
if (later_ns <= earlier.capture_start_ns) { return false; }
const double dt = static_cast<double>(later_ns - earlier.capture_start_ns) * 1e-9;
if (dt > kMaxFrameGapSeconds) {
return false; // non-monotonic time, or a gap that breaks the track
}
const double expected_range_shift = range_step_per_second * dt;