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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
use uuid::Uuid;
use crate::projection::project_to_ground;
use crate::proto::{Detection, Frame, MapObject, SceneMap};
/// Tunable thresholds for association and map maintenance.
#[derive(Debug, Clone, Copy)]
pub struct EngineConfig {
/// Detections farther than this from an existing object (in meters)
/// are treated as a new object rather than a repeat observation of it.
///
/// 0.3m comes from the ground-plane projection's noise sensitivity: for
/// this v0.1 monocular ground-plane method, a fixed pixel error in the
/// detector's bbox translates to a ground-position error that grows
/// sharply with distance/viewing angle (near-grazing rays amplify small
/// pixel noise into large position swings). At the indoor near-to-medium
/// range this v0.1 engine targets (up to ~4m), +/-8-10px of detector
/// localization error — representative of a moderate real-time
/// detector — stays under ~0.26m of ground jitter, so 0.3m has margin
/// without being so loose that distinct nearby objects merge. Beyond
/// ~5-8m or at shallow viewing angles this margin erodes fast; that's
/// an inherent limitation of ground-plane-only projection, not
/// something this constant can fix. Validated against real camera
/// motion with synthetic +/-8px detector noise in
/// tests/replay_tum_fixture.rs / tests/fixtures/tum_freiburg1_xyz.json.
pub association_radius_meters: f32,
/// An object not reinforced by a new observation within this many
/// seconds (frame-timestamp time, not wall-clock) is dropped. One
/// rule covers all of map maintenance: a one-off false detection
/// never gets reinforced and ages out; a genuinely stale object ages
/// out; an object that moved ages out at its old position while a
/// new one forms at the new position.
///
/// 2.0s is a reasoned assumption, not a measured one: object detection
/// is usually the expensive stage of a mobile perception pipeline and
/// commonly throttled well below camera framerate, so assume ~5Hz
/// detections and tolerate ~10 consecutive misses (occlusion, motion
/// blur) before treating an object as gone. There's no real detector
/// dropout data behind this yet — revisit once a live frontend exists.
pub stale_timeout_seconds: f64,
}
impl Default for EngineConfig {
fn default() -> Self {
Self { association_radius_meters: 0.3, stale_timeout_seconds: 2.0 }
}
}
pub struct Engine {
scene_map: SceneMap,
config: EngineConfig,
}
impl Engine {
pub fn new() -> Self {
Self::with_config(EngineConfig::default())
}
pub fn with_config(config: EngineConfig) -> Self {
Self { scene_map: SceneMap::default(), config }
}
pub fn ingest_frame(&mut self, frame: Frame) -> &SceneMap {
let (Some(pose), Some(intrinsics)) = (frame.pose.as_ref(), frame.intrinsics.as_ref())
else {
return &self.scene_map;
};
for detection in &frame.detections {
if let Some((x, y)) = project_to_ground(pose, intrinsics, detection) {
self.associate_and_fuse(detection, x, y, pose.timestamp);
}
}
self.prune_stale(pose.timestamp);
self.scene_map.updated_at = pose.timestamp;
&self.scene_map
}
fn associate_and_fuse(&mut self, detection: &Detection, x: f32, y: f32, timestamp: f64) {
let radius = self.config.association_radius_meters;
let nearest = self
.scene_map
.objects
.iter_mut()
.filter(|obj| obj.label == detection.label)
.filter_map(|obj| {
let dist = ((obj.x - x).powi(2) + (obj.y - y).powi(2)).sqrt();
(dist <= radius).then_some((dist, obj))
})
.min_by(|a, b| a.0.total_cmp(&b.0))
.map(|(_, obj)| obj);
match nearest {
Some(obj) => fuse(obj, detection, x, y, timestamp),
None => self.scene_map.objects.push(new_object(detection, x, y, timestamp)),
}
}
fn prune_stale(&mut self, now: f64) {
let stale_timeout = self.config.stale_timeout_seconds;
self.scene_map
.objects
.retain(|obj| now - obj.last_seen <= stale_timeout);
}
}
impl Default for Engine {
fn default() -> Self {
Self::new()
}
}
fn new_object(detection: &Detection, x: f32, y: f32, timestamp: f64) -> MapObject {
MapObject {
id: Uuid::new_v4().to_string(),
label: detection.label.clone(),
x,
y,
confidence: detection.confidence,
observation_count: 1,
first_seen: timestamp,
last_seen: timestamp,
}
}
fn fuse(obj: &mut MapObject, detection: &Detection, x: f32, y: f32, timestamp: f64) {
let n = obj.observation_count as f32;
obj.x = (obj.x * n + x) / (n + 1.0);
obj.y = (obj.y * n + y) / (n + 1.0);
// Combine as independent evidence so confidence grows toward 1 with
// repeated observations, instead of just averaging toward the mean.
obj.confidence = 1.0 - (1.0 - obj.confidence) * (1.0 - detection.confidence);
obj.observation_count += 1;
obj.last_seen = timestamp;
}
#[cfg(test)]
mod tests {
use super::*;
use crate::proto::{CameraIntrinsics, CameraPose};
fn frame_at(timestamp: f64, x: f32, y: f32, z: f32, detections: Vec<Detection>) -> Frame {
Frame {
pose: Some(CameraPose {
timestamp,
x,
y,
z,
qx: 1.0,
qy: 0.0,
qz: 0.0,
qw: 0.0,
}),
intrinsics: Some(CameraIntrinsics { fx: 500.0, fy: 500.0, cx: 320.0, cy: 240.0 }),
detections,
}
}
fn detection(label: &str, confidence: f32) -> Detection {
Detection {
label: label.to_string(),
confidence,
bbox_x: 0.5,
bbox_y: 0.5,
bbox_w: 0.0,
bbox_h: 0.0,
}
}
#[test]
fn repeated_observations_fuse_into_one_object_with_growing_confidence() {
let mut engine = Engine::new();
engine.ingest_frame(frame_at(0.0, 0.0, 0.0, 2.0, vec![detection("chair", 0.5)]));
let map = engine.ingest_frame(frame_at(0.5, 0.0, 0.0, 2.0, vec![detection("chair", 0.5)]));
assert_eq!(map.objects.len(), 1);
let obj = &map.objects[0];
assert_eq!(obj.observation_count, 2);
assert!(obj.confidence > 0.5, "confidence should grow past a single observation");
}
#[test]
fn different_labels_at_same_spot_stay_separate() {
let mut engine = Engine::new();
let map = engine.ingest_frame(frame_at(
0.0,
0.0,
0.0,
2.0,
vec![detection("chair", 0.5), detection("door", 0.5)],
));
assert_eq!(map.objects.len(), 2);
}
#[test]
fn objects_not_reinforced_go_stale_and_get_pruned() {
let mut engine = Engine::new();
engine.ingest_frame(frame_at(0.0, 0.0, 0.0, 2.0, vec![detection("chair", 0.5)]));
let map = engine.ingest_frame(frame_at(10.0, 0.0, 0.0, 2.0, vec![]));
assert!(map.objects.is_empty());
}
}