long_screen/kernels/track/
verdicts.rs1use crate::kernels::geometry::js_hypot;
10use crate::kernels::motion::{detect_scale, MatchPoints};
11
12pub fn uncertainty(confidence: f64, ambiguous: bool, weak_step: bool) -> bool {
14 confidence < 0.60 || ambiguous || weak_step
15}
16
17pub struct MatchInfo {
20 pub ambiguous: bool,
21 pub confidence: f64,
22}
23
24pub fn relocalize_verdict(m: Option<MatchInfo>, zoom_change: bool) -> bool {
26 match m {
27 Some(m) => !m.ambiguous && m.confidence > 0.6 && !zoom_change,
28 None => false,
29 }
30}
31
32#[derive(PartialEq, Eq, Debug)]
37pub enum FragmentCauseGate {
38 ZoomChange,
39 ProbeScale,
40 None,
41}
42
43pub fn fragment_cause_gate(
45 zoom_change: bool,
46 has_previous_gray: bool,
47 blind: bool,
48) -> FragmentCauseGate {
49 if zoom_change {
50 FragmentCauseGate::ZoomChange
51 } else if has_previous_gray && !blind {
52 FragmentCauseGate::ProbeScale
53 } else {
54 FragmentCauseGate::None
55 }
56}
57
58pub fn occlusion_eligible(has_previous: bool, kind_moving: bool, decision: u8) -> bool {
61 has_previous && kind_moving && (decision == 0 || decision == 1)
62}
63
64pub fn target_pose(keyframe: (f64, f64), offset: (f64, f64), shift: (f64, f64)) -> (f64, f64) {
66 (
67 keyframe.0 + offset.0 + shift.0,
68 keyframe.1 + offset.1 + shift.1,
69 )
70}
71
72pub struct GlobalMatch {
74 pub keyframe: (f64, f64),
75 pub offset: (f64, f64),
76 pub ambiguous: bool,
77 pub confidence: f64,
78}
79
80pub fn attach_verdict(
84 global: Option<GlobalMatch>,
85 resolved_target_eq_canvas: bool,
86 shift: (f64, f64),
87) -> Option<(f64, f64)> {
88 let g = global?;
89 if resolved_target_eq_canvas
94 || g.ambiguous
95 || g.confidence.partial_cmp(&0.72) != Some(std::cmp::Ordering::Greater)
96 {
97 return None;
98 }
99 Some(target_pose(g.keyframe, g.offset, shift))
100}
101
102pub fn odometry_weight(weak_step: bool) -> f64 {
104 if weak_step {
105 0.05
106 } else {
107 1.0
108 }
109}
110
111pub fn thin_overlap_eligible(
113 weak_step: bool,
114 weak: bool,
115 ambiguous: bool,
116 confidence: f64,
117 error: f64,
118) -> bool {
119 (weak_step || weak) && !ambiguous && confidence > 0.72 && error < 8.0
120}
121
122pub fn thin_overlap_correction(
124 canonical_keyframe: (f64, f64),
125 offset: (f64, f64),
126 pose: (f64, f64),
127) -> Option<((f64, f64), f64)> {
128 let target = (
129 canonical_keyframe.0 + offset.0,
130 canonical_keyframe.1 + offset.1,
131 );
132 let discrepancy = js_hypot(target.0 - pose.0, target.1 - pose.1);
133 if discrepancy >= 16.0 {
134 Some((target, discrepancy))
135 } else {
136 None
137 }
138}
139
140#[derive(PartialEq, Eq, Debug)]
142pub enum LoopVerdict {
143 Closure,
144 Inconsistent,
145 Ambiguous,
146 None,
147}
148
149pub fn loop_closure_verdict(
151 global_keyframe: (f64, f64),
152 global_offset: (f64, f64),
153 global_ambiguous: bool,
154 global_confidence: f64,
155 shift: (f64, f64),
156 pose: (f64, f64),
157) -> (LoopVerdict, f64) {
158 let discrepancy = js_hypot(
159 global_keyframe.0 + shift.0 + global_offset.0 - pose.0,
160 global_keyframe.1 + shift.1 + global_offset.1 - pose.1,
161 );
162 if !global_ambiguous && global_confidence > 0.72 && discrepancy < 16.0 {
163 (LoopVerdict::Closure, discrepancy)
164 } else if discrepancy >= 16.0 {
165 (LoopVerdict::Inconsistent, discrepancy)
166 } else if global_ambiguous {
167 (LoopVerdict::Ambiguous, discrepancy)
168 } else {
169 (LoopVerdict::None, discrepancy)
170 }
171}
172
173#[allow(clippy::too_many_arguments)]
180pub fn needs_keyframe(
181 kind_moving: bool,
182 anchor: Option<(f64, f64)>,
183 pose: (f64, f64),
184 last_node_frame: Option<i32>,
185 rect: (f64, f64),
186 frame_index: i32,
187 field_difference: f64,
188) -> bool {
189 let Some(last_node_frame) = last_node_frame else {
190 return true;
191 };
192 if !kind_moving {
193 return false;
194 }
195 let anchor_distance = match anchor {
196 Some(a) => js_hypot(pose.0 - a.0, pose.1 - a.1),
197 None => f64::INFINITY,
198 };
199 let frames_since_last_node = (frame_index - last_node_frame) as f64;
200 anchor_distance > f64::max(48.0, f64::min(rect.0, rect.1) * 0.30)
201 || (frames_since_last_node > 90.0 && field_difference > 0.2)
202}
203
204pub fn zoom_changed(region_zoom: Option<f64>, field_zoom: f64) -> bool {
206 match region_zoom {
207 Some(rz) => (rz - 1.0).abs() > 0.04,
208 None => (field_zoom - 1.0).abs() > 0.04,
209 }
210}
211
212pub fn region_zoom(kind_moving: bool, matches: &[MatchPoints]) -> Option<f64> {
216 if !kind_moving {
217 return None;
218 }
219 let unique_count = matches.iter().filter(|m| m.unique).count();
220 if unique_count >= 8 {
221 Some(detect_scale(matches))
222 } else {
223 None
224 }
225}