1use serde::{Deserialize, Serialize};
43
44use crate::detect::radon::primitives::{box_blur_inplace, fit_peak_frac, ANGLES, DIR_COS, DIR_SIN};
45use crate::imageview::ImageView;
46use crate::refine::{CornerRefiner, RefineContext, RefineResult, RefineStatus};
47
48pub use crate::detect::radon::primitives::PeakFitMode;
49
50#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
52#[serde(default)]
53#[non_exhaustive]
54pub struct RadonPeakConfig {
55 pub ray_radius: u32,
61 pub patch_radius: u32,
65 pub image_upsample: u32,
70 pub response_blur_radius: u32,
73 pub peak_fit: PeakFitMode,
75 pub min_response: f32,
79 pub max_offset: f32,
83}
84
85impl Default for RadonPeakConfig {
86 fn default() -> Self {
87 Self {
88 ray_radius: 2,
89 patch_radius: 3,
90 image_upsample: 2,
91 response_blur_radius: 1,
92 peak_fit: PeakFitMode::Gaussian,
93 min_response: 0.0,
94 max_offset: 1.5,
95 }
96 }
97}
98
99impl RadonPeakConfig {
100 #[inline]
101 fn image_upsample_clamped(&self) -> u32 {
102 self.image_upsample.max(1)
103 }
104
105 #[inline]
106 fn side(&self) -> usize {
107 (2 * self.patch_radius as usize * self.image_upsample_clamped() as usize) + 1
108 }
109}
110
111#[derive(Debug)]
115pub struct RadonPeakRefiner {
116 cfg: RadonPeakConfig,
117 resp: Vec<f32>,
120 blur_scratch: Vec<f32>,
122 side: usize,
123}
124
125impl RadonPeakRefiner {
126 pub fn new(cfg: RadonPeakConfig) -> Self {
129 let side = cfg.side();
130 Self {
131 cfg,
132 resp: vec![0.0; side * side],
133 blur_scratch: vec![0.0; side * side],
134 side,
135 }
136 }
137
138 #[inline]
140 pub fn config(&self) -> &RadonPeakConfig {
141 &self.cfg
142 }
143
144 #[inline]
148 pub fn response(&self) -> (&[f32], usize) {
149 (&self.resp, self.side)
150 }
151
152 #[inline]
155 fn response_at(&self, img: &ImageView<'_>, x: f32, y: f32, step: f32) -> f32 {
156 let samples_per_side =
160 (self.cfg.ray_radius as i32) * (self.cfg.image_upsample_clamped() as i32);
161 let samples_per_side = samples_per_side.max(1);
162 let mut max_r = f32::NEG_INFINITY;
163 let mut min_r = f32::INFINITY;
164 for a in 0..ANGLES {
165 let cx = DIR_COS[a];
166 let sy = DIR_SIN[a];
167 let mut sum = 0.0f32;
168 for k in -samples_per_side..=samples_per_side {
169 let kf = k as f32 * step;
170 sum += img.sample_bilinear(x + kf * cx, y + kf * sy);
171 }
172 if sum > max_r {
173 max_r = sum;
174 }
175 if sum < min_r {
176 min_r = sum;
177 }
178 }
179 let d = max_r - min_r;
180 d * d
181 }
182}
183
184impl CornerRefiner for RadonPeakRefiner {
185 #[inline]
186 fn radius(&self) -> i32 {
187 self.cfg.patch_radius as i32 + self.cfg.ray_radius as i32
191 }
192
193 fn refine(&mut self, seed_xy: [f32; 2], ctx: RefineContext<'_>) -> RefineResult {
194 let img = match ctx.image {
195 Some(view) => view,
196 None => {
197 return RefineResult {
198 x: seed_xy[0],
199 y: seed_xy[1],
200 score: 0.0,
201 status: RefineStatus::Rejected,
202 }
203 }
204 };
205
206 let cx = seed_xy[0].round() as i32;
207 let cy = seed_xy[1].round() as i32;
208 let patch_r = self.cfg.patch_radius as i32;
209 let ray_r = self.cfg.ray_radius as i32;
210
211 if !img.supports_patch(cx, cy, patch_r + ray_r) {
212 return RefineResult {
213 x: seed_xy[0],
214 y: seed_xy[1],
215 score: 0.0,
216 status: RefineStatus::OutOfBounds,
217 };
218 }
219
220 let upsample = self.cfg.image_upsample_clamped() as i32;
221 let step = 1.0f32 / upsample as f32;
222 let side = self.side as i32;
223 debug_assert_eq!(side, 2 * patch_r * upsample + 1);
224 let center_i = patch_r * upsample;
225
226 for iy in 0..side {
227 let dy = (iy - center_i) as f32 * step;
228 for ix in 0..side {
229 let dx = (ix - center_i) as f32 * step;
230 let r = self.response_at(&img, cx as f32 + dx, cy as f32 + dy, step);
231 self.resp[(iy as usize) * self.side + ix as usize] = r;
232 }
233 }
234
235 box_blur_inplace(
236 &mut self.resp,
237 &mut self.blur_scratch,
238 self.side,
239 self.side,
240 self.cfg.response_blur_radius as usize,
241 );
242
243 let mut best = f32::NEG_INFINITY;
244 let mut best_ix = 0i32;
245 let mut best_iy = 0i32;
246 for iy in 0..side {
247 for ix in 0..side {
248 let r = self.resp[(iy as usize) * self.side + ix as usize];
249 if r > best {
250 best = r;
251 best_ix = ix;
252 best_iy = iy;
253 }
254 }
255 }
256
257 if best < self.cfg.min_response {
258 return RefineResult {
259 x: seed_xy[0],
260 y: seed_xy[1],
261 score: best,
262 status: RefineStatus::Rejected,
263 };
264 }
265
266 if best_ix == 0 || best_iy == 0 || best_ix == side - 1 || best_iy == side - 1 {
268 return RefineResult {
269 x: seed_xy[0],
270 y: seed_xy[1],
271 score: best,
272 status: RefineStatus::IllConditioned,
273 };
274 }
275
276 let at = |rx: i32, ry: i32, resp: &[f32], side: usize| -> f32 {
277 resp[(ry as usize) * side + rx as usize]
278 };
279 let r_c = at(best_ix, best_iy, &self.resp, self.side);
280 let r_xm = at(best_ix - 1, best_iy, &self.resp, self.side);
281 let r_xp = at(best_ix + 1, best_iy, &self.resp, self.side);
282 let r_ym = at(best_ix, best_iy - 1, &self.resp, self.side);
283 let r_yp = at(best_ix, best_iy + 1, &self.resp, self.side);
284
285 let fx = fit_peak_frac(r_xm, r_c, r_xp, self.cfg.peak_fit);
286 let fy = fit_peak_frac(r_ym, r_c, r_yp, self.cfg.peak_fit);
287
288 let dx = (best_ix - center_i) as f32 * step + fx * step;
289 let dy = (best_iy - center_i) as f32 * step + fy * step;
290
291 let max_off = self.cfg.max_offset.min(patch_r as f32 + 0.5);
292 if dx.abs() > max_off || dy.abs() > max_off {
293 return RefineResult {
294 x: seed_xy[0],
295 y: seed_xy[1],
296 score: best,
297 status: RefineStatus::Rejected,
298 };
299 }
300
301 let score = best.sqrt();
302 RefineResult::accepted([cx as f32 + dx, cy as f32 + dy], score)
303 }
304}
305
306#[cfg(test)]
307mod tests {
308 use super::*;
309
310 fn synthetic_chessboard(
322 size: usize,
323 cell: usize,
324 offset: (f32, f32),
325 dark: u8,
326 bright: u8,
327 ) -> Vec<u8> {
328 const SUPER: usize = 8;
329 let (ox, oy) = offset;
330 let c = cell as f32;
331 let dark_f = dark as f32;
332 let bright_f = bright as f32;
333 let inv_super2 = 1.0 / (SUPER * SUPER) as f32;
334 let mut img = vec![0u8; size * size];
335 for y in 0..size {
336 for x in 0..size {
337 let mut acc = 0.0f32;
338 for sy in 0..SUPER {
339 let yf = y as f32 + (sy as f32 + 0.5) / SUPER as f32 - 0.5;
340 let cy = ((yf - oy) / c).floor() as i32;
341 for sx in 0..SUPER {
342 let xf = x as f32 + (sx as f32 + 0.5) / SUPER as f32 - 0.5;
343 let cx = ((xf - ox) / c).floor() as i32;
344 let dark_cell = (cx + cy).rem_euclid(2) == 0;
345 acc += if dark_cell { dark_f } else { bright_f };
346 }
347 }
348 img[y * size + x] = (acc * inv_super2).round().clamp(0.0, 255.0) as u8;
349 }
350 }
351 let mut blurred = img.clone();
353 for y in 1..(size - 1) {
354 for x in 1..(size - 1) {
355 let mut acc = 0u32;
356 for ky in -1..=1 {
357 for kx in -1..=1 {
358 acc +=
359 img[(y as i32 + ky) as usize * size + (x as i32 + kx) as usize] as u32;
360 }
361 }
362 blurred[y * size + x] = (acc / 9) as u8;
363 }
364 }
365 blurred
366 }
367
368 fn gaussian_blur(img: &mut [u8], size: usize, sigma: f32) {
370 let radius = ((3.0 * sigma).ceil() as usize).max(1);
371 let klen = 2 * radius + 1;
372 let mut kernel = vec![0f32; klen];
373 let mut sum = 0f32;
374 for (i, k) in kernel.iter_mut().enumerate() {
375 let x = i as f32 - radius as f32;
376 *k = (-(x * x) / (2.0 * sigma * sigma)).exp();
377 sum += *k;
378 }
379 for k in kernel.iter_mut() {
380 *k /= sum;
381 }
382 let mut tmp = vec![0f32; size * size];
383 for y in 0..size {
384 for x in 0..size {
385 let mut acc = 0f32;
386 for (ki, &k) in kernel.iter().enumerate() {
387 let sx =
388 (x as i32 + ki as i32 - radius as i32).clamp(0, size as i32 - 1) as usize;
389 acc += img[y * size + sx] as f32 * k;
390 }
391 tmp[y * size + x] = acc;
392 }
393 }
394 for y in 0..size {
395 for x in 0..size {
396 let mut acc = 0f32;
397 for (ki, &k) in kernel.iter().enumerate() {
398 let sy =
399 (y as i32 + ki as i32 - radius as i32).clamp(0, size as i32 - 1) as usize;
400 acc += tmp[sy * size + x] * k;
401 }
402 img[y * size + x] = acc.round().clamp(0.0, 255.0) as u8;
403 }
404 }
405 }
406
407 fn add_gaussian_noise(img: &mut [u8], sigma: f32, seed: u64) {
409 let mut state = seed ^ 0x9E3779B97F4A7C15;
410 let mut next_u32 = || {
411 state = state
412 .wrapping_mul(6_364_136_223_846_793_005)
413 .wrapping_add(1_442_695_040_888_963_407);
414 (state >> 33) as u32
415 };
416 let mut uniform = || -> f32 { (next_u32() as f32 + 1.0) / (u32::MAX as f32 + 2.0) };
417 for px in img.iter_mut() {
418 let u1 = uniform();
419 let u2 = uniform();
420 let z = (-2.0 * u1.ln()).sqrt() * (2.0 * core::f32::consts::PI * u2).cos();
421 let v = *px as f32 + z * sigma;
422 *px = v.round().clamp(0.0, 255.0) as u8;
423 }
424 }
425
426 fn error([x, y]: [f32; 2], [tx, ty]: [f32; 2]) -> f32 {
427 ((x - tx).powi(2) + (y - ty).powi(2)).sqrt()
428 }
429
430 const TEST_SIZE: usize = 35;
434 const TEST_CELL: usize = 6;
435 const TEST_CENTER: f32 = 17.0;
436
437 #[test]
438 fn compare_clean_accuracy_vs_saddlepoint() {
439 use crate::refine::{SaddlePointConfig, SaddlePointRefiner};
443 let truth = (TEST_CENTER + 0.35, TEST_CENTER + 0.8);
444 let img = synthetic_chessboard(TEST_SIZE, TEST_CELL, truth, 30, 230);
445 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
446 let seed = [truth.0.round(), truth.1.round()];
447 let ctx = RefineContext {
448 image: Some(view),
449 response: None,
450 };
451
452 let mut rp = RadonPeakRefiner::new(RadonPeakConfig::default());
453 let radon = rp.refine(seed, ctx);
454 assert_eq!(radon.status, RefineStatus::Accepted);
455
456 let mut sp = SaddlePointRefiner::new(SaddlePointConfig::default());
457 let saddle = sp.refine(seed, ctx);
458
459 let radon_err = error([radon.x, radon.y], [truth.0, truth.1]);
460 let saddle_err = if saddle.status == RefineStatus::Accepted {
461 error([saddle.x, saddle.y], [truth.0, truth.1])
462 } else {
463 f32::NAN
464 };
465 eprintln!(
466 "clean-data accuracy: radon_peak={:.4} saddle_point={:.4}",
467 radon_err, saddle_err
468 );
469 assert!(radon_err < 0.1, "radon_err {radon_err} exceeds 0.1 px");
470 }
471
472 #[test]
473 fn recovers_ideal_subpixel_offset() {
474 let truth = (TEST_CENTER + 0.35, TEST_CENTER + 0.8);
475 let img = synthetic_chessboard(TEST_SIZE, TEST_CELL, truth, 30, 230);
476 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
477 let mut refiner = RadonPeakRefiner::new(RadonPeakConfig::default());
478 let res = refiner.refine(
479 [truth.0.round(), truth.1.round()],
480 RefineContext {
481 image: Some(view),
482 response: None,
483 },
484 );
485 assert_eq!(res.status, RefineStatus::Accepted);
486 let err = error([res.x, res.y], [truth.0, truth.1]);
487 assert!(err < 0.1, "err={} res=({},{})", err, res.x, res.y);
488 }
489
490 #[test]
491 fn subpixel_sweep_mean_error_bounded() {
492 let mut refiner = RadonPeakRefiner::new(RadonPeakConfig::default());
493 let mut total = 0.0f32;
494 let mut worst = 0.0f32;
495 let mut count = 0.0f32;
496 for k in 0..8 {
497 let off = TEST_CENTER + (k as f32) / 8.0;
498 let img = synthetic_chessboard(TEST_SIZE, TEST_CELL, (off, off), 30, 230);
499 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
500 let res = refiner.refine(
501 [off.round(), off.round()],
502 RefineContext {
503 image: Some(view),
504 response: None,
505 },
506 );
507 assert_eq!(res.status, RefineStatus::Accepted, "k={}", k);
508 let err = error([res.x, res.y], [off, off]);
509 total += err;
510 worst = worst.max(err);
511 count += 1.0;
512 }
513 let mean = total / count;
514 assert!(
515 mean < 0.1 && worst < 0.2,
516 "mean {mean} worst {worst} over 8 offsets"
517 );
518 }
519
520 #[test]
521 fn gaussian_fit_beats_parabolic_on_clean_inputs() {
522 let mut gauss_total = 0.0f32;
525 let mut parab_total = 0.0f32;
526 let cfg_gauss = RadonPeakConfig::default();
527 let cfg_parab = RadonPeakConfig {
528 peak_fit: PeakFitMode::Parabolic,
529 ..cfg_gauss
530 };
531 let mut gauss = RadonPeakRefiner::new(cfg_gauss);
532 let mut parab = RadonPeakRefiner::new(cfg_parab);
533 for k in 0..8 {
534 let off = TEST_CENTER + (k as f32) / 8.0;
535 let img = synthetic_chessboard(TEST_SIZE, TEST_CELL, (off, off), 30, 230);
536 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
537 let seed = [off.round(), off.round()];
538 let ctx = RefineContext {
539 image: Some(view),
540 response: None,
541 };
542 let rg = gauss.refine(seed, ctx);
543 let rp = parab.refine(seed, ctx);
544 assert_eq!(rg.status, RefineStatus::Accepted);
545 assert_eq!(rp.status, RefineStatus::Accepted);
546 gauss_total += error([rg.x, rg.y], [off, off]);
547 parab_total += error([rp.x, rp.y], [off, off]);
548 }
549 eprintln!("gauss_mean={} parab_mean={}", gauss_total, parab_total);
550 assert!(
551 gauss_total <= parab_total + 1e-3,
552 "Gaussian fit regressed vs parabolic: {} > {}",
553 gauss_total,
554 parab_total
555 );
556 }
557
558 #[test]
559 fn refined_beats_seed_under_blur() {
560 let truth = (TEST_CENTER + 0.3, TEST_CENTER + 0.7);
561 for sigma in [1.0f32, 2.0] {
562 let mut img = synthetic_chessboard(TEST_SIZE, TEST_CELL, truth, 30, 230);
563 gaussian_blur(&mut img, TEST_SIZE, sigma);
564 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
565 let mut refiner = RadonPeakRefiner::new(RadonPeakConfig::default());
566 let seed = [truth.0.round(), truth.1.round()];
567 let res = refiner.refine(
568 seed,
569 RefineContext {
570 image: Some(view),
571 response: None,
572 },
573 );
574 assert_eq!(res.status, RefineStatus::Accepted, "sigma={}", sigma);
575 let seed_err = error(seed, [truth.0, truth.1]);
576 let ref_err = error([res.x, res.y], [truth.0, truth.1]);
577 assert!(
578 ref_err <= seed_err,
579 "sigma={}: refined {} not better than seed {}",
580 sigma,
581 ref_err,
582 seed_err
583 );
584 }
585 }
586
587 #[test]
588 fn refined_beats_seed_under_moderate_noise() {
589 let truth = (TEST_CENTER + 0.3, TEST_CENTER + 0.7);
590 let mut img = synthetic_chessboard(TEST_SIZE, TEST_CELL, truth, 30, 230);
591 add_gaussian_noise(&mut img, 5.0, 0xC0FFEE);
592 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
593 let mut refiner = RadonPeakRefiner::new(RadonPeakConfig::default());
594 let seed = [truth.0.round(), truth.1.round()];
595 let res = refiner.refine(
596 seed,
597 RefineContext {
598 image: Some(view),
599 response: None,
600 },
601 );
602 assert_eq!(res.status, RefineStatus::Accepted);
603 let seed_err = error(seed, [truth.0, truth.1]);
604 let ref_err = error([res.x, res.y], [truth.0, truth.1]);
605 assert!(
606 ref_err <= seed_err,
607 "refined {} not better than seed {}",
608 ref_err,
609 seed_err
610 );
611 }
612
613 #[test]
614 fn rejects_flat_region() {
615 let flat = vec![128u8; TEST_SIZE * TEST_SIZE];
616 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &flat).unwrap();
617 let cfg = RadonPeakConfig {
618 min_response: 1.0,
619 ..RadonPeakConfig::default()
620 };
621 let mut refiner = RadonPeakRefiner::new(cfg);
622 let res = refiner.refine(
623 [TEST_CENTER, TEST_CENTER],
624 RefineContext {
625 image: Some(view),
626 response: None,
627 },
628 );
629 assert_ne!(res.status, RefineStatus::Accepted);
630 }
631
632 #[test]
633 fn out_of_bounds_near_image_edge() {
634 let img = synthetic_chessboard(TEST_SIZE, TEST_CELL, (2.0, 2.0), 30, 230);
635 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
636 let mut refiner = RadonPeakRefiner::new(RadonPeakConfig::default());
637 let res = refiner.refine(
638 [1.0, 1.0],
639 RefineContext {
640 image: Some(view),
641 response: None,
642 },
643 );
644 assert_eq!(res.status, RefineStatus::OutOfBounds);
645 }
646
647 #[test]
648 fn deterministic_repeated_calls() {
649 let img = synthetic_chessboard(
650 TEST_SIZE,
651 TEST_CELL,
652 (TEST_CENTER + 0.2, TEST_CENTER + 0.6),
653 30,
654 230,
655 );
656 let view = ImageView::from_u8_slice(TEST_SIZE, TEST_SIZE, &img).unwrap();
657 let mut refiner = RadonPeakRefiner::new(RadonPeakConfig::default());
658 let ctx = RefineContext {
659 image: Some(view),
660 response: None,
661 };
662 let a = refiner.refine([TEST_CENTER, TEST_CENTER + 1.0], ctx);
663 let b = refiner.refine([TEST_CENTER, TEST_CENTER + 1.0], ctx);
664 assert_eq!(a.status, b.status);
665 assert_eq!(a.x.to_bits(), b.x.to_bits());
666 assert_eq!(a.y.to_bits(), b.y.to_bits());
667 assert_eq!(a.score.to_bits(), b.score.to_bits());
668 }
669
670 #[test]
671 fn config_round_trips_through_json() {
672 let cfg = RadonPeakConfig {
673 ray_radius: 5,
674 patch_radius: 3,
675 image_upsample: 2,
676 response_blur_radius: 2,
677 peak_fit: PeakFitMode::Parabolic,
678 min_response: 2.5,
679 max_offset: 1.25,
680 };
681 let json = serde_json::to_string(&cfg).unwrap();
682 let back: RadonPeakConfig = serde_json::from_str(&json).unwrap();
683 assert_eq!(cfg, back);
684 }
685
686 #[test]
687 fn scratch_buffer_sized_correctly() {
688 for up in [1u32, 2, 4] {
689 let cfg = RadonPeakConfig {
690 patch_radius: 3,
691 image_upsample: up,
692 ..RadonPeakConfig::default()
693 };
694 let refiner = RadonPeakRefiner::new(cfg);
695 let expected_side = 2 * 3 * up as usize + 1;
696 assert_eq!(refiner.side, expected_side);
697 assert_eq!(refiner.resp.len(), expected_side * expected_side);
698 assert_eq!(refiner.blur_scratch.len(), expected_side * expected_side);
699 }
700 }
701}