mirror of
https://github.com/azaion/autopilot.git
synced 2026-06-21 13:21:10 +00:00
[AZ-662] [AZ-669] Implement ego-motion estimator and primitive graph
AZ-662: movement_detector ego-motion - Add opencv + petgraph to workspace dependencies - internal/zoom_bands: per-band telemetry skew tolerances - internal/telemetry_sync: skew gate (check_skew) - internal/optical_flow: frame→gray, degenerate detection, LK sparse flow + RANSAC homography estimation - internal/ego_motion: EgoMotionEstimator + atomic counters AZ-669: semantic_analyzer primitive graph - internal/primitive_graph: NodeType, PrimitiveNode, PrimitiveGraph, PrimitiveGraphBuilder with proximity-adjacency + BFS connectivity check - internal/scoring/freshness: FreshnessScorer (Laplacian variance, texture stddev, undisturbed-surroundings heuristic) - All ACs covered by unit tests (AC-1/2/3 per task) Note: native OpenCV not installed on macOS; authoritative test is cargo test --workspace on Jetson (ssh jetson-e2e). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -11,5 +11,8 @@ authors.workspace = true
|
||||
shared = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
opencv = { workspace = true }
|
||||
petgraph = { workspace = true }
|
||||
|
||||
# TensorRT / ONNX runtime wiring lands with AZ-670.
|
||||
[dev-dependencies]
|
||||
bytes = { workspace = true }
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
pub mod primitive_graph;
|
||||
pub mod scoring;
|
||||
@@ -0,0 +1,281 @@
|
||||
//! AZ-669 — Build a `PrimitiveGraph` from a `DetectionBatch` inside an ROI,
|
||||
//! then validate connectivity of the path sub-graph.
|
||||
|
||||
use std::sync::atomic::{AtomicU64, Ordering};
|
||||
use std::sync::Arc;
|
||||
|
||||
use shared::models::{detection::DetectionBatch, frame::BoundingBox};
|
||||
|
||||
use super::graph::{NodeType, PrimitiveGraph, PrimitiveNode};
|
||||
|
||||
// ── class-name → NodeType mapping ────────────────────────────────────────────
|
||||
|
||||
fn classify_class_name(name: &str) -> NodeType {
|
||||
let lower = name.to_ascii_lowercase();
|
||||
if lower.contains("path") || lower.contains("road") || lower.contains("footpath") {
|
||||
NodeType::Path
|
||||
} else if lower.contains("branch")
|
||||
|| lower.contains("pile")
|
||||
|| lower.contains("entrance")
|
||||
|| lower.contains("dugout")
|
||||
{
|
||||
NodeType::Endpoint
|
||||
} else {
|
||||
// trees, tree blocks, and unknowns are contextual landmarks
|
||||
NodeType::Context
|
||||
}
|
||||
}
|
||||
|
||||
// ── spatial proximity helpers ─────────────────────────────────────────────────
|
||||
|
||||
/// Centre of a bounding box in normalised image coordinates.
|
||||
fn centre(b: &BoundingBox) -> (f32, f32) {
|
||||
((b.x_min + b.x_max) / 2.0, (b.y_min + b.y_max) / 2.0)
|
||||
}
|
||||
|
||||
/// Euclidean distance between two bbox centres.
|
||||
fn centre_dist(a: &BoundingBox, b: &BoundingBox) -> f32 {
|
||||
let (ax, ay) = centre(a);
|
||||
let (bx, by) = centre(b);
|
||||
((ax - bx).powi(2) + (ay - by).powi(2)).sqrt()
|
||||
}
|
||||
|
||||
/// Maximum dimension of a bounding box (normalised units).
|
||||
fn max_dim(b: &BoundingBox) -> f32 {
|
||||
(b.x_max - b.x_min).max(b.y_max - b.y_min)
|
||||
}
|
||||
|
||||
// ── connectivity (BFS on path nodes) ─────────────────────────────────────────
|
||||
|
||||
/// Returns the number of connected components in the path sub-graph described
|
||||
/// by `edges` over the `path_indices` set.
|
||||
fn count_path_components(
|
||||
path_indices: &[usize],
|
||||
edges: &[(usize, usize)],
|
||||
) -> usize {
|
||||
if path_indices.is_empty() {
|
||||
return 0;
|
||||
}
|
||||
// Map global node index → local index within `path_indices`.
|
||||
let mut local: std::collections::HashMap<usize, usize> =
|
||||
path_indices.iter().enumerate().map(|(l, &g)| (g, l)).collect();
|
||||
let n = path_indices.len();
|
||||
let mut adj: Vec<Vec<usize>> = vec![vec![]; n];
|
||||
for &(a, b) in edges {
|
||||
if let (Some(&la), Some(&lb)) = (local.get(&a), local.get(&b)) {
|
||||
adj[la].push(lb);
|
||||
adj[lb].push(la);
|
||||
}
|
||||
}
|
||||
let mut visited = vec![false; n];
|
||||
let mut components = 0usize;
|
||||
for start in 0..n {
|
||||
if visited[start] {
|
||||
continue;
|
||||
}
|
||||
components += 1;
|
||||
let mut queue = std::collections::VecDeque::new();
|
||||
queue.push_back(start);
|
||||
visited[start] = true;
|
||||
while let Some(cur) = queue.pop_front() {
|
||||
for &nb in &adj[cur] {
|
||||
if !visited[nb] {
|
||||
visited[nb] = true;
|
||||
queue.push_back(nb);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
components
|
||||
}
|
||||
|
||||
// ── builder ───────────────────────────────────────────────────────────────────
|
||||
|
||||
pub struct GraphCounters {
|
||||
pub graphs_built_total: AtomicU64,
|
||||
pub disconnected_graphs_total: AtomicU64,
|
||||
}
|
||||
|
||||
impl GraphCounters {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
graphs_built_total: AtomicU64::new(0),
|
||||
disconnected_graphs_total: AtomicU64::new(0),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for GraphCounters {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
pub struct PrimitiveGraphBuilder {
|
||||
counters: Arc<GraphCounters>,
|
||||
/// Spatial-proximity multiplier: two path nodes are adjacent when their
|
||||
/// centre-to-centre distance ≤ this factor × the larger of their max dims.
|
||||
adjacency_factor: f32,
|
||||
}
|
||||
|
||||
impl PrimitiveGraphBuilder {
|
||||
pub fn new(counters: Arc<GraphCounters>) -> Self {
|
||||
Self { counters, adjacency_factor: 2.5 }
|
||||
}
|
||||
|
||||
pub fn counters(&self) -> &Arc<GraphCounters> {
|
||||
&self.counters
|
||||
}
|
||||
|
||||
/// Build a `PrimitiveGraph` from detections inside `roi`.
|
||||
///
|
||||
/// Only detections whose bbox centre lies inside `roi` are included.
|
||||
/// After construction the path sub-graph is validated for connectivity;
|
||||
/// a disconnected graph is flagged and the counter is incremented.
|
||||
pub fn build(&self, roi: &BoundingBox, batch: &DetectionBatch) -> PrimitiveGraph {
|
||||
let nodes: Vec<PrimitiveNode> = batch
|
||||
.detections
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter(|(_, d)| {
|
||||
let (cx, cy) = centre(&d.bbox_normalized);
|
||||
cx >= roi.x_min
|
||||
&& cx <= roi.x_max
|
||||
&& cy >= roi.y_min
|
||||
&& cy <= roi.y_max
|
||||
})
|
||||
.map(|(i, d)| PrimitiveNode {
|
||||
node_type: classify_class_name(&d.class_name),
|
||||
bbox: d.bbox_normalized,
|
||||
confidence: d.confidence,
|
||||
class_name: d.class_name.clone(),
|
||||
detection_index: i,
|
||||
})
|
||||
.collect();
|
||||
|
||||
// Build proximity edges between path nodes only.
|
||||
let path_idxs: Vec<usize> = nodes
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter(|(_, n)| n.node_type == NodeType::Path)
|
||||
.map(|(i, _)| i)
|
||||
.collect();
|
||||
|
||||
let mut edges: Vec<(usize, usize)> = Vec::new();
|
||||
for i in 0..path_idxs.len() {
|
||||
for j in (i + 1)..path_idxs.len() {
|
||||
let ni = &nodes[path_idxs[i]];
|
||||
let nj = &nodes[path_idxs[j]];
|
||||
let dist = centre_dist(&ni.bbox, &nj.bbox);
|
||||
let threshold = self.adjacency_factor * max_dim(&ni.bbox).max(max_dim(&nj.bbox));
|
||||
if dist <= threshold {
|
||||
edges.push((path_idxs[i], path_idxs[j]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Connectivity validation.
|
||||
let components = count_path_components(&path_idxs, &edges);
|
||||
let disconnected = components > 1;
|
||||
let valid = !disconnected;
|
||||
|
||||
if disconnected {
|
||||
self.counters.disconnected_graphs_total.fetch_add(1, Ordering::Relaxed);
|
||||
tracing::warn!(
|
||||
disconnected_components = components,
|
||||
"primitive graph has disconnected path components"
|
||||
);
|
||||
}
|
||||
self.counters.graphs_built_total.fetch_add(1, Ordering::Relaxed);
|
||||
|
||||
PrimitiveGraph { nodes, edges, valid, disconnected }
|
||||
}
|
||||
}
|
||||
|
||||
// ── Tests ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use shared::models::detection::{Detection, DetectionBatch};
|
||||
use shared::models::frame::BoundingBox;
|
||||
|
||||
fn roi() -> BoundingBox {
|
||||
BoundingBox { x_min: 0.0, y_min: 0.0, x_max: 1.0, y_max: 1.0 }
|
||||
}
|
||||
|
||||
fn det(class_name: &str, x: f32, y: f32) -> Detection {
|
||||
Detection {
|
||||
class_id: 0,
|
||||
class_name: class_name.to_owned(),
|
||||
confidence: 0.9,
|
||||
bbox_normalized: BoundingBox {
|
||||
x_min: x - 0.05,
|
||||
y_min: y - 0.05,
|
||||
x_max: x + 0.05,
|
||||
y_max: y + 0.05,
|
||||
},
|
||||
mask_or_polyline: None,
|
||||
source_frame_seq: 0,
|
||||
}
|
||||
}
|
||||
|
||||
fn batch(dets: Vec<Detection>) -> DetectionBatch {
|
||||
DetectionBatch {
|
||||
frame_seq: 1,
|
||||
detections: dets,
|
||||
latency_ms: 10,
|
||||
model_version: "v1".to_owned(),
|
||||
}
|
||||
}
|
||||
|
||||
// AC-1: correct node counts per detection class.
|
||||
#[test]
|
||||
fn ac1_node_counts_per_class() {
|
||||
let counters = Arc::new(GraphCounters::new());
|
||||
let builder = PrimitiveGraphBuilder::new(Arc::clone(&counters));
|
||||
|
||||
let dets = vec![
|
||||
det("footpath", 0.1, 0.1),
|
||||
det("footpath", 0.2, 0.2),
|
||||
det("footpath", 0.3, 0.3),
|
||||
det("branch_pile", 0.4, 0.4),
|
||||
det("branch_pile", 0.5, 0.5),
|
||||
det("tree", 0.6, 0.1),
|
||||
det("tree", 0.7, 0.2),
|
||||
det("tree", 0.8, 0.3),
|
||||
det("tree", 0.15, 0.6),
|
||||
det("tree_block", 0.25, 0.7),
|
||||
];
|
||||
let b = batch(dets);
|
||||
let graph = builder.build(&roi(), &b);
|
||||
|
||||
let paths = graph.nodes.iter().filter(|n| n.node_type == NodeType::Path).count();
|
||||
let endpoints = graph.nodes.iter().filter(|n| n.node_type == NodeType::Endpoint).count();
|
||||
let contexts = graph.nodes.iter().filter(|n| n.node_type == NodeType::Context).count();
|
||||
|
||||
assert_eq!(paths, 3, "expected 3 path nodes");
|
||||
assert_eq!(endpoints, 2, "expected 2 endpoint nodes");
|
||||
assert_eq!(contexts, 5, "expected 5 context nodes");
|
||||
assert_eq!(counters.graphs_built_total.load(Ordering::Relaxed), 1);
|
||||
}
|
||||
|
||||
// AC-3: disconnected path components are flagged and counter increments.
|
||||
#[test]
|
||||
fn ac3_disconnected_path_graph_flagged() {
|
||||
let counters = Arc::new(GraphCounters::new());
|
||||
// Use a very small adjacency factor so distant nodes don't accidentally connect.
|
||||
let builder = PrimitiveGraphBuilder { counters: Arc::clone(&counters), adjacency_factor: 0.5 };
|
||||
|
||||
// Two isolated path clusters — far apart in the image.
|
||||
let dets = vec![
|
||||
det("footpath", 0.1, 0.1), // cluster A
|
||||
det("footpath", 0.9, 0.9), // cluster B (isolated)
|
||||
];
|
||||
let graph = builder.build(&roi(), &batch(dets));
|
||||
|
||||
assert!(graph.disconnected, "graph should be marked disconnected");
|
||||
assert!(!graph.valid);
|
||||
assert_eq!(counters.disconnected_graphs_total.load(Ordering::Relaxed), 1);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
//! Primitive graph types — path, endpoint, and context nodes.
|
||||
|
||||
use shared::models::frame::BoundingBox;
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum NodeType {
|
||||
/// Footpath, road — the main navigation surface.
|
||||
Path,
|
||||
/// Branch pile, dark entrance, dugout — a decision point or POI endpoint.
|
||||
Endpoint,
|
||||
/// Tree, tree block — contextual landmark.
|
||||
Context,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PrimitiveNode {
|
||||
pub node_type: NodeType,
|
||||
pub bbox: BoundingBox,
|
||||
pub confidence: f32,
|
||||
pub class_name: String,
|
||||
/// Index into the source `DetectionBatch.detections` vec.
|
||||
pub detection_index: usize,
|
||||
}
|
||||
|
||||
/// A small ROI-scoped graph of primitive detections.
|
||||
///
|
||||
/// `edges` encodes spatial-proximity adjacency between path nodes
|
||||
/// (indices into `nodes`). `valid = false` and `disconnected = true`
|
||||
/// when ≥2 separate path components are found.
|
||||
#[derive(Debug, Default)]
|
||||
pub struct PrimitiveGraph {
|
||||
pub nodes: Vec<PrimitiveNode>,
|
||||
/// Undirected adjacency edges between path nodes (node indices).
|
||||
pub edges: Vec<(usize, usize)>,
|
||||
/// False when the path sub-graph has ≥2 connected components.
|
||||
pub valid: bool,
|
||||
pub disconnected: bool,
|
||||
}
|
||||
|
||||
impl PrimitiveGraph {
|
||||
pub fn path_nodes(&self) -> impl Iterator<Item = (usize, &PrimitiveNode)> {
|
||||
self.nodes
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter(|(_, n)| n.node_type == NodeType::Path)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
//! AZ-669 — Primitive graph builder + graph validation.
|
||||
|
||||
pub mod builder;
|
||||
pub mod graph;
|
||||
|
||||
pub use builder::PrimitiveGraphBuilder;
|
||||
pub use graph::{NodeType, PrimitiveGraph, PrimitiveNode};
|
||||
@@ -0,0 +1,263 @@
|
||||
//! AZ-669 — Path-freshness scoring.
|
||||
//!
|
||||
//! Combines three classical CV cues: edge clarity (Laplacian variance),
|
||||
//! texture variance (pixel std-dev), and undisturbed surroundings (border
|
||||
//! region variance). Each sub-score is normalised to [0, 1] and averaged.
|
||||
|
||||
use opencv::{
|
||||
core::{self, Mat, Scalar},
|
||||
imgproc,
|
||||
prelude::*,
|
||||
};
|
||||
|
||||
use shared::models::frame::{BoundingBox, Frame, PixelFormat};
|
||||
|
||||
use super::super::primitive_graph::graph::{NodeType, PrimitiveGraph};
|
||||
|
||||
/// Freshness score for a single path node.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct PathFreshnessScore {
|
||||
/// Index into `PrimitiveGraph::nodes`.
|
||||
pub node_index: usize,
|
||||
/// Normalised score in `[0.0, 1.0]`.
|
||||
pub score: f32,
|
||||
}
|
||||
|
||||
pub struct FreshnessScorer;
|
||||
|
||||
impl FreshnessScorer {
|
||||
/// Score all path nodes in `graph` against the frame crop.
|
||||
/// Every returned `PathFreshnessScore::score` is in `[0.0, 1.0]`.
|
||||
pub fn score(
|
||||
graph: &PrimitiveGraph,
|
||||
frame_crop: &Frame,
|
||||
) -> opencv::Result<Vec<PathFreshnessScore>> {
|
||||
let gray = frame_to_gray_mat(frame_crop)?;
|
||||
let mut scores = Vec::new();
|
||||
for (idx, node) in graph.path_nodes() {
|
||||
let s = score_region(&gray, &node.bbox, frame_crop.width, frame_crop.height)?;
|
||||
scores.push(PathFreshnessScore { node_index: idx, score: s });
|
||||
}
|
||||
Ok(scores)
|
||||
}
|
||||
}
|
||||
|
||||
// ── CV helpers ────────────────────────────────────────────────────────────────
|
||||
|
||||
fn frame_to_gray_mat(frame: &Frame) -> opencv::Result<Mat> {
|
||||
let h = frame.height as i32;
|
||||
let w = frame.width as i32;
|
||||
let data: &[u8] = &frame.pixels;
|
||||
match frame.pix_fmt {
|
||||
PixelFormat::Nv12 | PixelFormat::Yuv420p => {
|
||||
let y_len = (w * h) as usize;
|
||||
let mut mat = Mat::new_rows_cols_with_default(h, w, core::CV_8UC1, Scalar::all(0.0))?;
|
||||
// SAFETY: freshly allocated continuous Mat; no aliasing.
|
||||
let dst = unsafe { std::slice::from_raw_parts_mut(mat.data_mut()?, y_len) };
|
||||
dst.copy_from_slice(&data[..y_len]);
|
||||
Ok(mat)
|
||||
}
|
||||
PixelFormat::Rgb24 => {
|
||||
let rgb_len = (w * h * 3) as usize;
|
||||
let mut rgb =
|
||||
Mat::new_rows_cols_with_default(h, w, core::CV_8UC3, Scalar::all(0.0))?;
|
||||
let dst = unsafe { std::slice::from_raw_parts_mut(rgb.data_mut()?, rgb_len) };
|
||||
dst.copy_from_slice(&data[..rgb_len]);
|
||||
let mut gray = Mat::default();
|
||||
imgproc::cvt_color(&rgb, &mut gray, imgproc::COLOR_RGB2GRAY, 0)?;
|
||||
Ok(gray)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute a freshness score for the bbox region within `gray`.
|
||||
/// Returns a value in [0.0, 1.0].
|
||||
fn score_region(
|
||||
gray: &Mat,
|
||||
bbox: &BoundingBox,
|
||||
frame_w: u32,
|
||||
frame_h: u32,
|
||||
) -> opencv::Result<f32> {
|
||||
let roi_rect = bbox_to_rect(bbox, frame_w, frame_h, gray.cols(), gray.rows());
|
||||
if roi_rect.width <= 0 || roi_rect.height <= 0 {
|
||||
return Ok(0.0);
|
||||
}
|
||||
|
||||
let roi = Mat::roi(gray, roi_rect)?;
|
||||
|
||||
// 1. Edge clarity: Laplacian variance — sharp edges indicate an active path.
|
||||
let mut lap = Mat::default();
|
||||
imgproc::laplacian(&roi, &mut lap, core::CV_64F, 3, 1.0, 0.0, core::BORDER_DEFAULT)?;
|
||||
let edge_var = variance(&lap)? as f32;
|
||||
|
||||
// 2. Texture: std-dev of pixel intensities.
|
||||
let texture_std = stddev_f32(&roi)?;
|
||||
|
||||
// 3. Undisturbed surroundings: low variance in the border region around bbox
|
||||
// signals an untouched environment → higher freshness contribution.
|
||||
let surround_var = surround_variance(gray, roi_rect)? as f32;
|
||||
let undisturbed_score = 1.0 - normalise(surround_var, 3000.0);
|
||||
|
||||
let edge_score = normalise(edge_var, 1500.0);
|
||||
let texture_score = normalise(texture_std, 40.0);
|
||||
|
||||
let freshness = ((edge_score + texture_score + undisturbed_score) / 3.0).clamp(0.0, 1.0);
|
||||
Ok(freshness)
|
||||
}
|
||||
|
||||
fn bbox_to_rect(
|
||||
bbox: &BoundingBox,
|
||||
frame_w: u32,
|
||||
frame_h: u32,
|
||||
mat_w: i32,
|
||||
mat_h: i32,
|
||||
) -> core::Rect {
|
||||
let x = ((bbox.x_min * frame_w as f32) as i32).clamp(0, mat_w - 1);
|
||||
let y = ((bbox.y_min * frame_h as f32) as i32).clamp(0, mat_h - 1);
|
||||
let x2 = ((bbox.x_max * frame_w as f32) as i32).clamp(0, mat_w);
|
||||
let y2 = ((bbox.y_max * frame_h as f32) as i32).clamp(0, mat_h);
|
||||
core::Rect::new(x, y, (x2 - x).max(1), (y2 - y).max(1))
|
||||
}
|
||||
|
||||
/// Compute the variance of all values in a Mat as f64.
|
||||
fn variance(mat: &Mat) -> opencv::Result<f64> {
|
||||
let mut mean_mat = Mat::default();
|
||||
let mut stddev_mat = Mat::default();
|
||||
core::mean_std_dev(mat, &mut mean_mat, &mut stddev_mat, &core::no_array())?;
|
||||
let std = stddev_mat.at::<f64>(0).map(|v| *v).unwrap_or(0.0);
|
||||
Ok(std * std)
|
||||
}
|
||||
|
||||
fn stddev_f32(mat: &Mat) -> opencv::Result<f32> {
|
||||
let mut mean_mat = Mat::default();
|
||||
let mut stddev_mat = Mat::default();
|
||||
core::mean_std_dev(mat, &mut mean_mat, &mut stddev_mat, &core::no_array())?;
|
||||
Ok(stddev_mat.at::<f64>(0).map(|v| *v as f32).unwrap_or(0.0))
|
||||
}
|
||||
|
||||
/// Compute the pixel variance in a ~16 px border region around `rect`.
|
||||
fn surround_variance(gray: &Mat, rect: core::Rect) -> opencv::Result<f64> {
|
||||
let border = 16i32;
|
||||
let x = (rect.x - border).max(0);
|
||||
let y = (rect.y - border).max(0);
|
||||
let x2 = (rect.x + rect.width + border).min(gray.cols());
|
||||
let y2 = (rect.y + rect.height + border).min(gray.rows());
|
||||
let outer_rect = core::Rect::new(x, y, (x2 - x).max(1), (y2 - y).max(1));
|
||||
|
||||
let outer = Mat::roi(gray, outer_rect)?;
|
||||
// Build a mask: 0 inside inner rect, 255 in the border band.
|
||||
let mut mask = Mat::new_rows_cols_with_default(
|
||||
outer_rect.height,
|
||||
outer_rect.width,
|
||||
core::CV_8UC1,
|
||||
Scalar::all(255.0),
|
||||
)?;
|
||||
let inner_x = rect.x - x;
|
||||
let inner_y = rect.y - y;
|
||||
let inner = core::Rect::new(
|
||||
inner_x.clamp(0, outer_rect.width - 1),
|
||||
inner_y.clamp(0, outer_rect.height - 1),
|
||||
rect.width.min(outer_rect.width - inner_x.max(0)),
|
||||
rect.height.min(outer_rect.height - inner_y.max(0)),
|
||||
);
|
||||
if inner.width > 0 && inner.height > 0 {
|
||||
let mut inner_roi = Mat::roi_mut(&mut mask, inner)?;
|
||||
inner_roi.set_to(&Scalar::all(0.0), &core::no_array())?;
|
||||
}
|
||||
|
||||
let mut mean_mat = Mat::default();
|
||||
let mut stddev_mat = Mat::default();
|
||||
core::mean_std_dev(&outer, &mut mean_mat, &mut stddev_mat, &mask)?;
|
||||
let std = stddev_mat.at::<f64>(0).map(|v| *v).unwrap_or(0.0);
|
||||
Ok(std * std)
|
||||
}
|
||||
|
||||
/// Map `value` ∈ [0, ∞) to [0.0, 1.0] by dividing by `scale` and clamping.
|
||||
#[inline]
|
||||
fn normalise(value: f32, scale: f32) -> f32 {
|
||||
(value / scale).clamp(0.0, 1.0)
|
||||
}
|
||||
|
||||
// ── Tests ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use std::sync::Arc;
|
||||
use bytes::Bytes;
|
||||
|
||||
use super::*;
|
||||
use super::super::super::primitive_graph::{builder::{GraphCounters, PrimitiveGraphBuilder}, graph::PrimitiveGraph};
|
||||
use shared::models::{
|
||||
detection::{Detection, DetectionBatch},
|
||||
frame::{BoundingBox, Frame, PixelFormat},
|
||||
};
|
||||
|
||||
fn rgb_frame(w: u32, h: u32, fill: u8, ts: u64) -> Frame {
|
||||
Frame {
|
||||
seq: 0,
|
||||
capture_ts_monotonic_ns: ts,
|
||||
decode_ts_monotonic_ns: ts,
|
||||
pixels: Arc::new(Bytes::from(vec![fill; (w * h * 3) as usize])),
|
||||
width: w,
|
||||
height: h,
|
||||
pix_fmt: PixelFormat::Rgb24,
|
||||
ai_locked: false,
|
||||
}
|
||||
}
|
||||
|
||||
fn noisy_rgb_frame(w: u32, h: u32, ts: u64) -> Frame {
|
||||
let total = (w * h * 3) as usize;
|
||||
let pixels: Vec<u8> = (0..total).map(|i| (i % 256) as u8).collect();
|
||||
Frame {
|
||||
seq: 0,
|
||||
capture_ts_monotonic_ns: ts,
|
||||
decode_ts_monotonic_ns: ts,
|
||||
pixels: Arc::new(Bytes::from(pixels)),
|
||||
width: w,
|
||||
height: h,
|
||||
pix_fmt: PixelFormat::Rgb24,
|
||||
ai_locked: false,
|
||||
}
|
||||
}
|
||||
|
||||
fn single_path_graph() -> PrimitiveGraph {
|
||||
let counters = Arc::new(GraphCounters::new());
|
||||
let builder = PrimitiveGraphBuilder::new(counters);
|
||||
let roi = BoundingBox { x_min: 0.0, y_min: 0.0, x_max: 1.0, y_max: 1.0 };
|
||||
let batch = DetectionBatch {
|
||||
frame_seq: 1,
|
||||
detections: vec![Detection {
|
||||
class_id: 0,
|
||||
class_name: "footpath".to_owned(),
|
||||
confidence: 0.9,
|
||||
bbox_normalized: BoundingBox {
|
||||
x_min: 0.2, y_min: 0.2, x_max: 0.8, y_max: 0.8,
|
||||
},
|
||||
mask_or_polyline: None,
|
||||
source_frame_seq: 1,
|
||||
}],
|
||||
latency_ms: 5,
|
||||
model_version: "v1".to_owned(),
|
||||
};
|
||||
builder.build(&roi, &batch)
|
||||
}
|
||||
|
||||
// AC-2: every freshness score is in [0.0, 1.0] for any valid input.
|
||||
#[test]
|
||||
fn ac2_freshness_score_bounded() -> opencv::Result<()> {
|
||||
let graph = single_path_graph();
|
||||
// Uniform gray frame.
|
||||
let uniform = rgb_frame(64, 64, 128, 0);
|
||||
let scores_uniform = FreshnessScorer::score(&graph, &uniform)?;
|
||||
for s in &scores_uniform {
|
||||
assert!(s.score >= 0.0 && s.score <= 1.0, "score out of range: {}", s.score);
|
||||
}
|
||||
// Noisy textured frame.
|
||||
let noisy = noisy_rgb_frame(64, 64, 0);
|
||||
let scores_noisy = FreshnessScorer::score(&graph, &noisy)?;
|
||||
for s in &scores_noisy {
|
||||
assert!(s.score >= 0.0 && s.score <= 1.0, "score out of range: {}", s.score);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
pub mod freshness;
|
||||
|
||||
pub use freshness::{FreshnessScorer, PathFreshnessScore};
|
||||
@@ -1,46 +1,71 @@
|
||||
//! `semantic_analyzer` — Tier 2 primitive graph + ROI CNN.
|
||||
//! `semantic_analyzer` — primitive graph + freshness scoring.
|
||||
//!
|
||||
//! Real implementation lands in:
|
||||
//! - AZ-669 `semantic_analyzer_primitive_graph`
|
||||
//! - AZ-670 `semantic_analyzer_roi_cnn`
|
||||
//! - AZ-671 `semantic_analyzer_action_policy`
|
||||
//! AZ-669: primitive graph builder + freshness scorer (this batch).
|
||||
//! AZ-670: TensorRT/ONNX scene-embedding classifier.
|
||||
//! AZ-671: output publisher.
|
||||
|
||||
use shared::error::{AutopilotError, Result};
|
||||
use shared::health::ComponentHealth;
|
||||
use shared::models::tier2::Tier2Evidence;
|
||||
use std::sync::Arc;
|
||||
|
||||
use tokio::sync::broadcast;
|
||||
|
||||
use shared::health::{ComponentHealth, HealthLevel};
|
||||
use shared::models::detection::DetectionBatch;
|
||||
|
||||
pub(crate) mod internal;
|
||||
|
||||
use internal::{
|
||||
primitive_graph::builder::{GraphCounters, PrimitiveGraphBuilder},
|
||||
scoring::FreshnessScorer,
|
||||
};
|
||||
|
||||
const NAME: &str = "semantic_analyzer";
|
||||
|
||||
pub struct SemanticAnalyzer;
|
||||
pub struct SemanticAnalyzer {
|
||||
tx: broadcast::Sender<DetectionBatch>,
|
||||
counters: Arc<GraphCounters>,
|
||||
}
|
||||
|
||||
impl SemanticAnalyzer {
|
||||
pub fn new() -> Self {
|
||||
Self
|
||||
pub fn new(channel_capacity: usize) -> Self {
|
||||
let (tx, _) = broadcast::channel(channel_capacity);
|
||||
Self { tx, counters: Arc::new(GraphCounters::new()) }
|
||||
}
|
||||
|
||||
pub fn handle(&self) -> SemanticAnalyzerHandle {
|
||||
SemanticAnalyzerHandle
|
||||
SemanticAnalyzerHandle {
|
||||
tx: self.tx.clone(),
|
||||
counters: Arc::clone(&self.counters),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for SemanticAnalyzer {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
#[derive(Clone)]
|
||||
pub struct SemanticAnalyzerHandle {
|
||||
tx: broadcast::Sender<DetectionBatch>,
|
||||
counters: Arc<GraphCounters>,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
pub struct SemanticAnalyzerHandle;
|
||||
|
||||
impl SemanticAnalyzerHandle {
|
||||
pub async fn analyze(&self, _roi: Vec<u8>) -> Result<Tier2Evidence> {
|
||||
Err(AutopilotError::NotImplemented(
|
||||
"semantic_analyzer::analyze (AZ-669)",
|
||||
))
|
||||
pub fn detections(&self) -> broadcast::Receiver<DetectionBatch> {
|
||||
self.tx.subscribe()
|
||||
}
|
||||
|
||||
pub fn health(&self) -> ComponentHealth {
|
||||
ComponentHealth::disabled(NAME)
|
||||
let disconnected = self.counters.disconnected_graphs_total.load(
|
||||
std::sync::atomic::Ordering::Relaxed,
|
||||
);
|
||||
if disconnected > 0 {
|
||||
ComponentHealth::yellow(
|
||||
NAME,
|
||||
format!("disconnected_graphs_total={disconnected}"),
|
||||
)
|
||||
} else {
|
||||
ComponentHealth {
|
||||
level: HealthLevel::Disabled,
|
||||
component: NAME,
|
||||
detail: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -50,7 +75,10 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn it_compiles() {
|
||||
let h = SemanticAnalyzer::new().handle();
|
||||
assert_eq!(h.health().level, shared::health::HealthLevel::Disabled);
|
||||
let h = SemanticAnalyzer::new(16).handle();
|
||||
assert!(matches!(
|
||||
h.health().level,
|
||||
HealthLevel::Disabled | HealthLevel::Yellow
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user