1use serde::{Deserialize, Serialize};
7use std::collections::HashMap;
8use thiserror::Error;
9
10use crate::detect::{validate_request, DetectionParams, DetectionRequest, Evidence};
11use crate::feature::OrientedFeature;
12use crate::lattice::{GridDimensions, LatticeKind};
13use crate::shared::recovery_schedule::SquareAxisProvenance;
14use crate::topological::classify::EdgeClass;
15use crate::topological::square_detector::{
16 detect_square_oriented2_all_observed, SquarePipelineTrace,
17};
18use crate::topological::TopologicalParams;
19
20#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
21#[non_exhaustive]
22pub struct TopologicalCornerTrace {
24 pub index: usize,
26 pub source_index: usize,
28 pub position: [f32; 2],
30 pub axis_angles_rad: [f32; 2],
32 pub axis_sigmas_rad: [Option<f32>; 2],
34 pub usable: bool,
36}
37
38#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
39#[serde(rename_all = "snake_case")]
40#[non_exhaustive]
41pub enum TopologicalEdgeClass {
43 Grid,
45 Diagonal,
47 Spurious,
49}
50
51impl From<EdgeClass> for TopologicalEdgeClass {
52 fn from(value: EdgeClass) -> Self {
53 match value {
54 EdgeClass::Grid => Self::Grid,
55 EdgeClass::Diagonal => Self::Diagonal,
56 EdgeClass::Spurious => Self::Spurious,
57 }
58 }
59}
60
61#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
62#[non_exhaustive]
63pub struct TopologicalEdgeTrace {
65 pub start: usize,
67 pub end: usize,
69 pub class: TopologicalEdgeClass,
71}
72
73#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
74#[non_exhaustive]
75pub struct TopologicalTriangleTrace {
77 pub vertices: [usize; 3],
79 pub edge_classes: [TopologicalEdgeClass; 3],
81}
82
83#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
84#[non_exhaustive]
85pub struct TopologicalQuadTrace {
87 pub vertices: [usize; 4],
89}
90
91#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
92#[non_exhaustive]
93pub struct TopologicalLabelTrace {
95 pub u: i32,
97 pub v: i32,
99 pub feature_index: usize,
101 pub source_index: usize,
103 pub residual_px: Option<f32>,
105}
106
107#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
109pub struct TopologicalFitTrace {
110 pub count: usize,
112 pub mean_px: f32,
114 pub max_px: f32,
116}
117
118#[derive(Clone, Debug, Serialize, Deserialize)]
119#[non_exhaustive]
120pub struct TopologicalComponentTrace {
122 pub index: usize,
124 pub labels: Vec<TopologicalLabelTrace>,
126 pub fit: Option<TopologicalFitTrace>,
128}
129
130#[derive(Clone, Copy, Debug, Default, Serialize, Deserialize)]
131#[non_exhaustive]
132pub struct TopologicalTraceDiagnostics {
134 pub corners_in: usize,
136 pub corners_used: usize,
138 pub triangles: usize,
140 pub raw_quads: usize,
142 pub topology_quads: usize,
144 pub geometry_quads: usize,
146 pub scale_quads: usize,
148 pub walk_components: usize,
150 pub merged_components: usize,
152 pub final_components: usize,
154 pub final_labels: usize,
156}
157
158#[derive(Clone, Debug, Serialize, Deserialize)]
159#[non_exhaustive]
160pub struct TopologicalTrace {
162 pub schema_version: u32,
164 pub params: TopologicalParams,
166 pub corners: Vec<TopologicalCornerTrace>,
168 pub edges: Vec<TopologicalEdgeTrace>,
170 pub triangles: Vec<TopologicalTriangleTrace>,
172 pub raw_quads: Vec<TopologicalQuadTrace>,
174 pub topology_quads: Vec<TopologicalQuadTrace>,
176 pub geometry_quads: Vec<TopologicalQuadTrace>,
178 pub scale_quads: Vec<TopologicalQuadTrace>,
180 pub walk_components: Vec<TopologicalComponentTrace>,
182 pub merged_components: Vec<TopologicalComponentTrace>,
184 pub final_components: Vec<TopologicalComponentTrace>,
186 pub diagnostics: TopologicalTraceDiagnostics,
188}
189
190#[derive(Clone, Debug, Error, PartialEq, Eq)]
191#[non_exhaustive]
192pub enum TopologicalTraceError {
194 #[error("topological detection failed: {message}")]
196 DetectionFailed {
197 message: String,
199 },
200}
201
202pub fn build_grid_topological_trace(
204 features: &[OrientedFeature<2>],
205 dimensions: Option<GridDimensions>,
206 params: DetectionParams,
207) -> Result<TopologicalTrace, TopologicalTraceError> {
208 let request = DetectionRequest::new(LatticeKind::Square, Evidence::Oriented2(features))
209 .with_params(params.clone());
210 let request = match dimensions {
211 Some(dimensions) => request.with_dimensions(dimensions),
212 None => request,
213 };
214 validate_request(&request).map_err(|error| TopologicalTraceError::DetectionFailed {
215 message: error.to_string(),
216 })?;
217 let topological_params = params.tuning().topological;
218 let mut raw = SquarePipelineTrace::default();
219 let solutions = detect_square_oriented2_all_observed(
220 features,
221 dimensions,
222 ¶ms,
223 SquareAxisProvenance::FullyMeasured,
224 Some(&mut raw),
225 )
226 .map_err(|error| TopologicalTraceError::DetectionFailed {
227 message: error.to_string(),
228 })?;
229
230 let corners = features
231 .iter()
232 .enumerate()
233 .map(|(index, feature)| TopologicalCornerTrace {
234 index,
235 source_index: feature.point.source_index,
236 position: [feature.point.position.x, feature.point.position.y],
237 axis_angles_rad: [feature.axes[0].angle_rad, feature.axes[1].angle_rad],
238 axis_sigmas_rad: [feature.axes[0].sigma_rad, feature.axes[1].sigma_rad],
239 usable: raw.usable[index],
240 })
241 .collect();
242 let edges: Vec<TopologicalEdgeTrace> = raw
243 .edges
244 .iter()
245 .map(|&(start, end, class)| TopologicalEdgeTrace {
246 start,
247 end,
248 class: class.into(),
249 })
250 .collect();
251 let triangles = raw
252 .triangles
253 .iter()
254 .enumerate()
255 .map(|(index, &vertices)| TopologicalTriangleTrace {
256 vertices,
257 edge_classes: [
258 edges[3 * index].class,
259 edges[3 * index + 1].class,
260 edges[3 * index + 2].class,
261 ],
262 })
263 .collect();
264 let raw_quads = quads(&raw.raw_quads);
265 let topology_quads = quads(&raw.topology_quads);
266 let geometry_quads = quads(&raw.geometry_quads);
267 let scale_quads = quads(&raw.scale_quads);
268 let walk_components = components(&raw.walk_components, features);
269 let merged_components = components(&raw.merged_components, features);
270 let feature_index_by_source: HashMap<usize, usize> = features
271 .iter()
272 .enumerate()
273 .map(|(index, feature)| (feature.point.source_index, index))
274 .collect();
275 let final_components: Vec<TopologicalComponentTrace> = solutions
276 .iter()
277 .enumerate()
278 .map(|(index, solution)| TopologicalComponentTrace {
279 index,
280 labels: solution
281 .detection
282 .grid()
283 .entries()
284 .iter()
285 .map(|entry| TopologicalLabelTrace {
286 u: entry.coord.u,
287 v: entry.coord.v,
288 feature_index: feature_index_by_source[&entry.source_index],
289 source_index: entry.source_index,
290 residual_px: entry.residual_px,
291 })
292 .collect(),
293 fit: Some(TopologicalFitTrace {
294 count: solution.detection.fit().residuals.count,
295 mean_px: solution.detection.fit().residuals.mean_px,
296 max_px: solution.detection.fit().residuals.max_px,
297 }),
298 })
299 .collect();
300 let diagnostics = TopologicalTraceDiagnostics {
301 corners_in: features.len(),
302 corners_used: raw.usable.iter().filter(|&&usable| usable).count(),
303 triangles: raw.triangles.len(),
304 raw_quads: raw.raw_quads.len(),
305 topology_quads: raw.topology_quads.len(),
306 geometry_quads: raw.geometry_quads.len(),
307 scale_quads: raw.scale_quads.len(),
308 walk_components: raw.walk_components.len(),
309 merged_components: raw.merged_components.len(),
310 final_components: final_components.len(),
311 final_labels: final_components
312 .iter()
313 .map(|component| component.labels.len())
314 .sum(),
315 };
316 Ok(TopologicalTrace {
317 schema_version: 1,
318 params: topological_params,
319 corners,
320 edges,
321 triangles,
322 raw_quads,
323 topology_quads,
324 geometry_quads,
325 scale_quads,
326 walk_components,
327 merged_components,
328 final_components,
329 diagnostics,
330 })
331}
332
333fn quads(items: &[[usize; 4]]) -> Vec<TopologicalQuadTrace> {
334 items
335 .iter()
336 .copied()
337 .map(|vertices| TopologicalQuadTrace { vertices })
338 .collect()
339}
340
341fn components(
342 items: &[Vec<(crate::Coord, usize)>],
343 features: &[OrientedFeature<2>],
344) -> Vec<TopologicalComponentTrace> {
345 items
346 .iter()
347 .enumerate()
348 .map(|(index, labels)| TopologicalComponentTrace {
349 index,
350 labels: labels
351 .iter()
352 .map(|&(coord, feature_index)| TopologicalLabelTrace {
353 u: coord.u,
354 v: coord.v,
355 feature_index,
356 source_index: features[feature_index].point.source_index,
357 residual_px: None,
358 })
359 .collect(),
360 fit: None,
361 })
362 .collect()
363}