use crate::error::Error;
use crate::features::Corner;
use crate::image::RasterImage;
use crate::pixel::SingleChannel;
use crate::{Coordinate, CoordinateF64};
use super::peaks::{NmsRadius, pixel_site};
const MAX_RECENTRES: usize = 3;
const RANK_EPSILON: f64 = 1e-12;
pub fn refine_corners<IX, IY, P>(
corners: &mut [Corner],
gx: &IX,
gy: &IY,
radius: NmsRadius,
) -> Result<usize, Error>
where
IX: RasterImage<Pixel = P>,
IY: RasterImage<Pixel = P>,
P: SingleChannel,
f64: From<P::Channel>,
{
if gx.size() != gy.size() {
return Err(Error::SizeMismatch {
expected: gx.size(),
actual: gy.size(),
});
}
let mut refined = 0;
for corner in corners {
if let Some(at) = refine_one(gx, gy, corner.at, radius.get()) {
corner.at = at;
refined += 1;
}
}
Ok(refined)
}
fn refine_one<IX, IY, P>(
gx: &IX,
gy: &IY,
at: CoordinateF64,
radius: usize,
) -> Option<CoordinateF64>
where
IX: RasterImage<Pixel = P>,
IY: RasterImage<Pixel = P>,
P: SingleChannel,
f64: From<P::Channel>,
{
let mut centre = pixel_site(at)?;
for _ in 0..=MAX_RECENTRES {
let solved = solve_window(gx, gy, centre, radius)?;
let nearest = pixel_site(solved)?;
if nearest == centre {
return Some(solved);
}
centre = nearest;
}
None
}
fn solve_window<IX, IY, P>(
gx: &IX,
gy: &IY,
centre: Coordinate,
radius: usize,
) -> Option<CoordinateF64>
where
IX: RasterImage<Pixel = P>,
IY: RasterImage<Pixel = P>,
P: SingleChannel,
f64: From<P::Channel>,
{
if centre.x < radius || centre.y < radius {
return None;
}
let hi_x = centre.x.checked_add(radius)?;
let hi_y = centre.y.checked_add(radius)?;
if hi_x >= gx.width() || hi_y >= gx.height() {
return None;
}
let (mut sxx, mut sxy, mut syy) = (0.0f64, 0.0f64, 0.0f64);
let (mut bx, mut by) = (0.0f64, 0.0f64);
for y in centre.y - radius..=hi_y {
let row_x = gx.row(y);
let row_y = gy.row(y);
let dy = y as f64 - centre.y as f64;
for x in centre.x - radius..=hi_x {
let gxv = f64::from(row_x[x].channel(0));
let gyv = f64::from(row_y[x].channel(0));
let dx = x as f64 - centre.x as f64;
let (xx, xy, yy) = (gxv * gxv, gxv * gyv, gyv * gyv);
sxx += xx;
sxy += xy;
syy += yy;
bx += xx * dx + xy * dy;
by += xy * dx + yy * dy;
}
}
let determinant = sxx * syy - sxy * sxy;
let trace = sxx + syy;
let two_edge_directions = determinant > RANK_EPSILON * trace * trace;
if !two_edge_directions {
return None;
}
let dx = (syy * bx - sxy * by) / determinant;
let dy = (sxx * by - sxy * bx) / determinant;
let reach = radius as f64;
let inside_the_window = dx.abs() <= reach && dy.abs() <= reach;
if !inside_the_window {
return None;
}
Some(CoordinateF64::new(
centre.x as f64 + dx,
centre.y as f64 + dy,
))
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Sigma;
use crate::border::{Clamp, Skip};
use crate::features::detect::{
CornerParams, FastParams, SegmentTest, ShiTomasi, corner_response_map, detect_corners,
fast, interpolate_corners,
};
use crate::features::{HasPosition, HasResponse};
use crate::image::{Image, ImageView, ImageViewMut};
use crate::pixel::{MonoF32, MonoF64};
use crate::sigma;
use crate::transform::{sobel_x, sobel_y};
fn square(n: usize, lo: usize, hi: usize) -> Image<MonoF32> {
Image::generate(n, n, |x, y| {
let inside = (lo..hi).contains(&x) && (lo..hi).contains(&y);
MonoF32::new(if inside { 1.0 } else { 0.0 })
})
}
fn square_corners(lo: usize, hi: usize) -> [(f64, f64); 4] {
let (a, b) = ((lo as f64) - 0.5, (hi as f64) - 0.5);
[(a, a), (b, a), (a, b), (b, b)]
}
fn nearest_truth(truth: &[(f64, f64); 4], p: CoordinateF64) -> (f64, f64) {
*truth
.iter()
.min_by(|a, b| {
let da = (p.x - a.0).powi(2) + (p.y - a.1).powi(2);
let db = (p.x - b.0).powi(2) + (p.y - b.1).powi(2);
da.total_cmp(&db)
})
.unwrap()
}
fn max_response(map: &Image<MonoF32>) -> f32 {
(0..map.height())
.flat_map(|y| (0..map.width()).map(move |x| (x, y)))
.map(|(x, y)| map.pixel_at(x, y).value())
.fold(f32::NEG_INFINITY, f32::max)
}
fn crossing_gradients() -> (Image<MonoF32>, Image<MonoF32>) {
let gx = Image::generate(9, 9, |x, y| {
let on = (x == 3 || x == 4) && (y == 3 || y == 4);
MonoF32::new(if on { 1.0 } else { 0.0 })
});
let gy = Image::generate(9, 9, |_, y| {
MonoF32::new(match y {
5 => 2.0,
6 => 1.0,
_ => 0.0,
})
});
(gx, gy)
}
#[test]
fn the_windows_inward_drift_is_removed_for_every_sigma() {
let image = square(24, 8, 16);
let gx = sobel_x(&image, &Clamp);
let gy = sobel_y(&image, &Clamp);
let truth = square_corners(8, 16);
let mut per_radius: std::collections::BTreeMap<usize, f64> =
std::collections::BTreeMap::new();
for sigma in [0.8f32, 1.0, 1.2, 1.6, 2.0] {
let window = Sigma::new(sigma).unwrap();
let map: Image<MonoF32> = corner_response_map(&image, ShiTomasi, window);
let params =
CornerParams::try_new(window, 0.3 * max_response(&map), NmsRadius::new(3).unwrap())
.unwrap();
let mut corners = detect_corners(&image, ShiTomasi, params);
assert_eq!(corners.len(), 4, "sigma {sigma}: {corners:?}");
let radius = (2.0 * f64::from(sigma)).ceil() as usize;
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(radius).unwrap()).unwrap(),
4,
"sigma {sigma}"
);
for corner in &corners {
let p = corner.position();
let (tx, ty) = nearest_truth(&truth, p);
assert!(
(p.x - tx).abs() <= 0.05 && (p.y - ty).abs() <= 0.05,
"sigma {sigma}, radius {radius}: refined to {p:?}, corner at ({tx}, {ty})",
);
let residual = (p.x - tx).abs().max((p.y - ty).abs());
let worst = per_radius.entry(radius).or_insert(0.0f64);
if residual > *worst {
*worst = residual;
}
}
}
for (radius, lo, hi) in [(2usize, 0.02, 0.045), (3, 0.015, 0.035), (4, 0.01, 0.03)] {
let worst = per_radius[&radius];
assert!(
(lo..=hi).contains(&worst),
"radius {radius}: worst residual {worst} left the band {lo}..={hi}",
);
}
}
#[test]
fn interpolation_locates_the_drifted_peak_and_refinement_the_corner() {
let image = square(24, 8, 16);
let window = sigma!(1.6);
let map: Image<MonoF32> = corner_response_map(&image, ShiTomasi, window);
let params =
CornerParams::try_new(window, 0.3 * max_response(&map), NmsRadius::new(3).unwrap())
.unwrap();
let corners = detect_corners(&image, ShiTomasi, params);
assert_eq!(corners[0].position(), CoordinateF64::new(9.0, 9.0));
let distance = |p: CoordinateF64| ((p.x - 7.5).powi(2) + (p.y - 7.5).powi(2)).sqrt();
let mut interpolated = corners.clone();
assert!(interpolate_corners(&mut interpolated, &map) >= 1);
assert!(
distance(interpolated[0].position()) > 1.0,
"the drift survives interpolation: {:?}",
interpolated[0]
);
let mut refined = corners;
let gx = sobel_x(&image, &Clamp);
let gy = sobel_y(&image, &Clamp);
assert_eq!(
refine_corners(&mut refined, &gx, &gy, NmsRadius::new(4).unwrap()).unwrap(),
4
);
assert!(
distance(refined[0].position()) <= 0.06,
"refinement removes it: {:?}",
refined[0]
);
}
#[test]
fn a_segment_test_corner_refines_onto_the_geometric_corner() {
let image = square(24, 8, 16);
let params = FastParams::new(
SegmentTest::new(0.5, 9).unwrap(),
NmsRadius::new(2).unwrap(),
);
let mut corners = fast(&image, params, &Skip);
assert_eq!(corners.len(), 4, "{corners:?}");
let truth = square_corners(8, 16);
let bias = corners
.iter()
.map(|c| {
let p = c.position();
let (tx, ty) = nearest_truth(&truth, p);
((p.x - tx).powi(2) + (p.y - ty).powi(2)).sqrt()
})
.fold(0.0f64, f64::max);
assert!(
bias > 2.0,
"the detection bias being corrected: {corners:?}"
);
let gx = sobel_x(&image, &Clamp);
let gy = sobel_y(&image, &Clamp);
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(3).unwrap()).unwrap(),
4
);
for corner in &corners {
let p = corner.position();
let (tx, ty) = nearest_truth(&truth, p);
assert!(
(p.x - tx).abs() <= 0.05 && (p.y - ty).abs() <= 0.05,
"refined to {p:?}, corner at ({tx}, {ty})",
);
}
}
#[test]
fn the_solve_is_exact_when_no_pixel_sees_both_edges() {
let (gx, gy) = crossing_gradients();
let mut corners = vec![Corner::new(CoordinateF64::new(4.0, 5.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
1
);
let p = corners[0].position();
assert!((p.x - 3.5).abs() < 1e-9, "{p:?}");
assert!((p.y - 5.2).abs() < 1e-9, "{p:?}");
}
#[test]
fn the_window_recentres_onto_the_corner() {
let (gx, gy) = crossing_gradients();
let mut corners = vec![Corner::new(CoordinateF64::new(6.0, 6.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
1
);
let p = corners[0].position();
assert!((p.x - 3.5).abs() < 1e-9, "{p:?}");
assert!((p.y - 5.2).abs() < 1e-9, "{p:?}");
}
#[test]
fn a_window_leaving_the_frame_is_refused() {
let (gx, gy) = crossing_gradients();
let mut corners = vec![Corner::new(CoordinateF64::new(4.0, 5.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(5).unwrap()).unwrap(),
0
);
assert_eq!(corners[0].position(), CoordinateF64::new(4.0, 5.0));
}
#[test]
fn a_window_seeing_one_straight_edge_is_refused() {
let image = square(24, 8, 16);
let gx = sobel_x(&image, &Clamp);
let gy = sobel_y(&image, &Clamp);
let mut corners = vec![Corner::new(CoordinateF64::new(8.0, 12.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
0
);
assert_eq!(corners[0].position(), CoordinateF64::new(8.0, 12.0));
}
#[test]
fn a_flat_window_is_refused() {
let gx: Image<MonoF32> = Image::zero(9, 9);
let gy: Image<MonoF32> = Image::zero(9, 9);
let mut corners = vec![Corner::new(CoordinateF64::new(4.0, 4.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
0
);
assert_eq!(corners[0].position(), CoordinateF64::new(4.0, 4.0));
}
#[test]
fn a_nan_gradient_refuses_the_fit() {
let (gx, mut gy) = crossing_gradients();
*gy.pixel_at_mut(4, 5) = MonoF32::new(f32::NAN);
let mut corners = vec![Corner::new(CoordinateF64::new(4.0, 5.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
0
);
assert_eq!(corners[0].position(), CoordinateF64::new(4.0, 5.0));
}
#[test]
fn a_position_off_the_map_is_left_alone() {
let (gx, gy) = crossing_gradients();
let mut corners = vec![
Corner::new(CoordinateF64::new(40.0, 40.0), 0.9),
Corner::new(CoordinateF64::new(-3.0, 2.0), 0.9),
Corner::new(CoordinateF64::new(f64::NAN, 2.0), 0.9),
];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
0
);
assert_eq!(corners[0].position(), CoordinateF64::new(40.0, 40.0));
assert_eq!(corners[1].position(), CoordinateF64::new(-3.0, 2.0));
}
#[test]
fn mismatched_gradient_sizes_are_reported() {
let gx: Image<MonoF32> = Image::zero(9, 9);
let gy: Image<MonoF32> = Image::zero(9, 8);
let err = refine_corners(&mut [], &gx, &gy, NmsRadius::new(2).unwrap()).unwrap_err();
assert!(matches!(err, Error::SizeMismatch { .. }), "{err:?}");
}
#[test]
fn the_count_reports_only_the_fits_that_succeeded() {
let (gx, gy) = crossing_gradients();
let mut corners = vec![
Corner::new(CoordinateF64::new(0.0, 0.0), 0.5), Corner::new(CoordinateF64::new(4.0, 5.0), 0.5), ];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
1
);
assert_eq!(corners[0].position(), CoordinateF64::new(0.0, 0.0));
assert!((corners[1].position().x - 3.5).abs() < 1e-9);
}
#[test]
fn the_response_is_left_alone() {
let (gx, gy) = crossing_gradients();
let mut corners = vec![Corner::new(CoordinateF64::new(4.0, 5.0), 0.75)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
1
);
assert_eq!(
corners[0].response(),
0.75,
"the refined position is not a place the detector scored",
);
}
#[test]
fn refining_no_corners_is_no_work() {
let (gx, gy) = crossing_gradients();
assert_eq!(
refine_corners(&mut [], &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
0
);
}
#[test]
fn f64_gradients_are_accepted() {
let gx: Image<MonoF64> = Image::generate(9, 9, |x, y| {
let on = (x == 3 || x == 4) && (y == 3 || y == 4);
MonoF64::new(if on { 1.0 } else { 0.0 })
});
let gy: Image<MonoF64> = Image::generate(9, 9, |_, y| {
MonoF64::new(match y {
5 => 2.0,
6 => 1.0,
_ => 0.0,
})
});
let mut corners = vec![Corner::new(CoordinateF64::new(4.0, 5.0), 1.0)];
assert_eq!(
refine_corners(&mut corners, &gx, &gy, NmsRadius::new(2).unwrap()).unwrap(),
1
);
assert!((corners[0].position().y - 5.2).abs() < 1e-9);
}
}