use crate::kernels::geometry::js_hypot;
use crate::kernels::motion::{detect_scale, MatchPoints};
pub fn uncertainty(confidence: f64, ambiguous: bool, weak_step: bool) -> bool {
confidence < 0.60 || ambiguous || weak_step
}
pub struct MatchInfo {
pub ambiguous: bool,
pub confidence: f64,
}
pub fn relocalize_verdict(m: Option<MatchInfo>, zoom_change: bool) -> bool {
match m {
Some(m) => !m.ambiguous && m.confidence > 0.6 && !zoom_change,
None => false,
}
}
#[derive(PartialEq, Eq, Debug)]
pub enum FragmentCauseGate {
ZoomChange,
ProbeScale,
None,
}
pub fn fragment_cause_gate(
zoom_change: bool,
has_previous_gray: bool,
blind: bool,
) -> FragmentCauseGate {
if zoom_change {
FragmentCauseGate::ZoomChange
} else if has_previous_gray && !blind {
FragmentCauseGate::ProbeScale
} else {
FragmentCauseGate::None
}
}
pub fn occlusion_eligible(has_previous: bool, kind_moving: bool, decision: u8) -> bool {
has_previous && kind_moving && (decision == 0 || decision == 1)
}
pub fn target_pose(keyframe: (f64, f64), offset: (f64, f64), shift: (f64, f64)) -> (f64, f64) {
(
keyframe.0 + offset.0 + shift.0,
keyframe.1 + offset.1 + shift.1,
)
}
pub struct GlobalMatch {
pub keyframe: (f64, f64),
pub offset: (f64, f64),
pub ambiguous: bool,
pub confidence: f64,
}
pub fn attach_verdict(
global: Option<GlobalMatch>,
resolved_target_eq_canvas: bool,
shift: (f64, f64),
) -> Option<(f64, f64)> {
let g = global?;
if resolved_target_eq_canvas
|| g.ambiguous
|| g.confidence.partial_cmp(&0.72) != Some(std::cmp::Ordering::Greater)
{
return None;
}
Some(target_pose(g.keyframe, g.offset, shift))
}
pub fn odometry_weight(weak_step: bool) -> f64 {
if weak_step {
0.05
} else {
1.0
}
}
pub fn thin_overlap_eligible(
weak_step: bool,
weak: bool,
ambiguous: bool,
confidence: f64,
error: f64,
) -> bool {
(weak_step || weak) && !ambiguous && confidence > 0.72 && error < 8.0
}
pub fn thin_overlap_correction(
canonical_keyframe: (f64, f64),
offset: (f64, f64),
pose: (f64, f64),
) -> Option<((f64, f64), f64)> {
let target = (
canonical_keyframe.0 + offset.0,
canonical_keyframe.1 + offset.1,
);
let discrepancy = js_hypot(target.0 - pose.0, target.1 - pose.1);
if discrepancy >= 16.0 {
Some((target, discrepancy))
} else {
None
}
}
#[derive(PartialEq, Eq, Debug)]
pub enum LoopVerdict {
Closure,
Inconsistent,
Ambiguous,
None,
}
pub fn loop_closure_verdict(
global_keyframe: (f64, f64),
global_offset: (f64, f64),
global_ambiguous: bool,
global_confidence: f64,
shift: (f64, f64),
pose: (f64, f64),
) -> (LoopVerdict, f64) {
let discrepancy = js_hypot(
global_keyframe.0 + shift.0 + global_offset.0 - pose.0,
global_keyframe.1 + shift.1 + global_offset.1 - pose.1,
);
if !global_ambiguous && global_confidence > 0.72 && discrepancy < 16.0 {
(LoopVerdict::Closure, discrepancy)
} else if discrepancy >= 16.0 {
(LoopVerdict::Inconsistent, discrepancy)
} else if global_ambiguous {
(LoopVerdict::Ambiguous, discrepancy)
} else {
(LoopVerdict::None, discrepancy)
}
}
#[allow(clippy::too_many_arguments)]
pub fn needs_keyframe(
kind_moving: bool,
anchor: Option<(f64, f64)>,
pose: (f64, f64),
last_node_frame: Option<i32>,
rect: (f64, f64),
frame_index: i32,
field_difference: f64,
) -> bool {
let Some(last_node_frame) = last_node_frame else {
return true;
};
if !kind_moving {
return false;
}
let anchor_distance = match anchor {
Some(a) => js_hypot(pose.0 - a.0, pose.1 - a.1),
None => f64::INFINITY,
};
let frames_since_last_node = (frame_index - last_node_frame) as f64;
anchor_distance > f64::max(48.0, f64::min(rect.0, rect.1) * 0.30)
|| (frames_since_last_node > 90.0 && field_difference > 0.2)
}
pub fn zoom_changed(region_zoom: Option<f64>, field_zoom: f64) -> bool {
match region_zoom {
Some(rz) => (rz - 1.0).abs() > 0.04,
None => (field_zoom - 1.0).abs() > 0.04,
}
}
pub fn region_zoom(kind_moving: bool, matches: &[MatchPoints]) -> Option<f64> {
if !kind_moving {
return None;
}
let unique_count = matches.iter().filter(|m| m.unique).count();
if unique_count >= 8 {
Some(detect_scale(matches))
} else {
None
}
}