mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-17 08:40:52 +00:00
insert refactored qr code
code alignment and finding now works *great*. Near real-time performance!
This commit is contained in:
parent
f6d6db566a
commit
16c9b2c7e2
1 changed files with 642 additions and 44 deletions
|
|
@ -1,5 +1,11 @@
|
||||||
// dev code for QR code algorithms
|
// dev code for QR code algorithms
|
||||||
use cramium_hal::minigfx::Point;
|
use core::cell::RefCell;
|
||||||
|
use core::convert::TryFrom;
|
||||||
|
use core::ops::{BitXor, Not};
|
||||||
|
|
||||||
|
use cramium_hal::minigfx::{ColorNative, FrameBuffer, Point};
|
||||||
|
|
||||||
|
use super::gfx;
|
||||||
|
|
||||||
const SEQ_LEN: usize = 5;
|
const SEQ_LEN: usize = 5;
|
||||||
|
|
||||||
|
|
@ -13,6 +19,434 @@ const UPPER_1: usize = 2 << SEQ_FP_SHIFT;
|
||||||
const LOWER_3: usize = 2 << SEQ_FP_SHIFT;
|
const LOWER_3: usize = 2 << SEQ_FP_SHIFT;
|
||||||
const UPPER_3: usize = 4 << SEQ_FP_SHIFT;
|
const UPPER_3: usize = 4 << SEQ_FP_SHIFT;
|
||||||
|
|
||||||
|
pub const STORAGE: usize = 92;
|
||||||
|
|
||||||
|
pub const BW_THRESH: u8 = 128;
|
||||||
|
/// Finder search margin, as defined by expected QR code code widths (so this scales with the effective
|
||||||
|
/// resolution of the code)
|
||||||
|
pub const FINDER_SEARCH_MARGIN: isize = 2;
|
||||||
|
/// How much we want the final QR image to be "pulled in" from the outer edge of the image buffer
|
||||||
|
pub const HOMOGRAPHY_MARGIN: isize = -4;
|
||||||
|
pub const CROSSHAIR_LEN: isize = 3;
|
||||||
|
|
||||||
|
pub fn draw_crosshair(image: &mut dyn FrameBuffer, p: Point) {
|
||||||
|
use cramium_hal::minigfx::Line;
|
||||||
|
gfx::line(
|
||||||
|
image,
|
||||||
|
Line::new(p + Point::new(0, CROSSHAIR_LEN), p - Point::new(0, CROSSHAIR_LEN)),
|
||||||
|
None,
|
||||||
|
true,
|
||||||
|
);
|
||||||
|
gfx::line(
|
||||||
|
image,
|
||||||
|
Line::new(p + Point::new(CROSSHAIR_LEN, 0), p - Point::new(CROSSHAIR_LEN, 0)),
|
||||||
|
None,
|
||||||
|
true,
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn draw_line(image: &mut dyn FrameBuffer, l: &LineDerivation, color: ColorNative) {
|
||||||
|
let axis = l.independent_axis;
|
||||||
|
let (m, b) = l.equation.unwrap();
|
||||||
|
match axis {
|
||||||
|
Axis::X => {
|
||||||
|
for x in 0..image.dimensions().x {
|
||||||
|
let y = (x as f32 * m + b) as isize;
|
||||||
|
if y >= 0 && y < image.dimensions().y as isize {
|
||||||
|
image.put_pixel(Point::new(x, y), color);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
Axis::Y => {
|
||||||
|
for y in 0..image.dimensions().y {
|
||||||
|
let x = (y as f32 * m + b) as isize;
|
||||||
|
if x >= 0 && x < image.dimensions().x as isize {
|
||||||
|
image.put_pixel(Point::new(x, y), color);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
struct DirRange {
|
||||||
|
start: usize,
|
||||||
|
stop_exclusive: usize,
|
||||||
|
is_up: bool,
|
||||||
|
range: DirectionalRange,
|
||||||
|
}
|
||||||
|
impl Iterator for DirRange {
|
||||||
|
type Item = usize;
|
||||||
|
|
||||||
|
fn next(&mut self) -> Option<Self::Item> { self.range.next() }
|
||||||
|
}
|
||||||
|
impl DirRange {
|
||||||
|
pub fn new(start: usize, stop_exclusive: usize, is_up: bool) -> Self {
|
||||||
|
Self {
|
||||||
|
start,
|
||||||
|
stop_exclusive,
|
||||||
|
is_up,
|
||||||
|
range: if is_up {
|
||||||
|
DirectionalRange::Up(start..stop_exclusive)
|
||||||
|
} else {
|
||||||
|
DirectionalRange::Down((start..stop_exclusive).rev())
|
||||||
|
},
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn reset(&mut self) {
|
||||||
|
self.range = if self.is_up {
|
||||||
|
DirectionalRange::Up(self.start..self.stop_exclusive)
|
||||||
|
} else {
|
||||||
|
DirectionalRange::Down((self.start..self.stop_exclusive).rev())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
enum DirectionalRange {
|
||||||
|
Up(core::ops::Range<usize>),
|
||||||
|
Down(core::iter::Rev<core::ops::Range<usize>>),
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Iterator for DirectionalRange {
|
||||||
|
type Item = usize;
|
||||||
|
|
||||||
|
fn next(&mut self) -> Option<Self::Item> {
|
||||||
|
match self {
|
||||||
|
DirectionalRange::Up(iter) => iter.next(),
|
||||||
|
DirectionalRange::Down(iter) => iter.next(),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Copy, Clone, PartialEq, Eq)]
|
||||||
|
pub enum Axis {
|
||||||
|
X,
|
||||||
|
Y,
|
||||||
|
}
|
||||||
|
|
||||||
|
fn adjust_axis(p: Point, axis: Axis) -> (f32, f32) {
|
||||||
|
(
|
||||||
|
match axis {
|
||||||
|
Axis::X => p.x as f32,
|
||||||
|
Axis::Y => p.y as f32,
|
||||||
|
},
|
||||||
|
match axis {
|
||||||
|
Axis::X => p.y as f32,
|
||||||
|
Axis::Y => p.x as f32,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
fn least_squares_fit(points: &[Point], axis: Axis) -> (f32, f32) {
|
||||||
|
let n = points.len() as f32;
|
||||||
|
let mut sum_x = 0.0f32;
|
||||||
|
let mut sum_y = 0.0f32;
|
||||||
|
let mut sum_xx = 0.0f32;
|
||||||
|
let mut sum_xy = 0.0f32;
|
||||||
|
for &point in points.iter() {
|
||||||
|
// flip x/y coordinates based on the independent axis
|
||||||
|
let (x, y) = adjust_axis(point, axis);
|
||||||
|
sum_x += x;
|
||||||
|
sum_y += y;
|
||||||
|
sum_xx += x * x;
|
||||||
|
sum_xy += x * y;
|
||||||
|
}
|
||||||
|
let m = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x);
|
||||||
|
let b = (sum_y - m * sum_x) / n;
|
||||||
|
(m, b)
|
||||||
|
}
|
||||||
|
|
||||||
|
fn point_from_hv_lines(hline: &LineDerivation, vline: &LineDerivation) -> Option<Point> {
|
||||||
|
if let Some((m1, b1)) = hline.equation {
|
||||||
|
if let Some((m2v, b2v)) = vline.equation {
|
||||||
|
crate::println!("h: {}, {} | v: {}, {}", m1, b1, m2v, b2v);
|
||||||
|
if m2v != 0.0 {
|
||||||
|
let m2 = 1.0 / m2v;
|
||||||
|
let b2 = -b2v / m2v;
|
||||||
|
let x = (b2 - b1) / (m1 - m2);
|
||||||
|
let y = m1 * x + b1;
|
||||||
|
Some(Point::new(x as isize, y as isize))
|
||||||
|
} else {
|
||||||
|
let y = m1 * b2v + b1;
|
||||||
|
Some(Point::new(b2v as isize, y as isize))
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Threshold to reject points if they don't fit on the best-fit line
|
||||||
|
const OUTLIER_THRESHOLD: f32 = 1.0;
|
||||||
|
const OUTLIER_ITERS: usize = 5;
|
||||||
|
#[derive(Copy, Clone)]
|
||||||
|
pub struct LineDerivation {
|
||||||
|
pub equation: Option<(f32, f32)>,
|
||||||
|
pub independent_axis: Axis,
|
||||||
|
pub data_points: [Point; STORAGE],
|
||||||
|
pub data_index: usize,
|
||||||
|
}
|
||||||
|
impl LineDerivation {
|
||||||
|
pub fn new(axis: Axis) -> Self {
|
||||||
|
LineDerivation {
|
||||||
|
equation: None,
|
||||||
|
independent_axis: axis,
|
||||||
|
data_points: [Point::new(0, 0); STORAGE],
|
||||||
|
data_index: 0,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn push(&mut self, p: Point) {
|
||||||
|
if self.data_index < STORAGE {
|
||||||
|
self.data_points[self.data_index] = p;
|
||||||
|
self.data_index += 1;
|
||||||
|
} else {
|
||||||
|
assert!(false, "Static storage exceeded");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// This implementation heavily relies on f32, so it is slow on an embedded processor; however,
|
||||||
|
/// we need the precision and the solving should be done only rarely.
|
||||||
|
pub fn solve(&mut self) {
|
||||||
|
let mut points = [Point::default(); STORAGE];
|
||||||
|
let mut filtered_points = [Point::default(); STORAGE];
|
||||||
|
let mut residuals = [0.0f32; STORAGE];
|
||||||
|
let mut sorted_residuals = [0.0f32; STORAGE];
|
||||||
|
let mut filtered_index;
|
||||||
|
let mut count = self.data_index;
|
||||||
|
let mut m_guess: f32 = 0.0;
|
||||||
|
let mut b_guess: f32 = 0.0;
|
||||||
|
|
||||||
|
let mut converged_in = 0;
|
||||||
|
points[..count].copy_from_slice(&self.data_points[..count]);
|
||||||
|
for guesses in 0..OUTLIER_ITERS {
|
||||||
|
converged_in = guesses;
|
||||||
|
// guess a best-fit line
|
||||||
|
(m_guess, b_guess) = least_squares_fit(&points[..count], self.independent_axis);
|
||||||
|
// compute the residuals of the points to the guessed line
|
||||||
|
for (&p, residual) in points[..count].iter().zip(residuals.iter_mut()) {
|
||||||
|
let (x, y) = adjust_axis(p, self.independent_axis);
|
||||||
|
let predicted_y = m_guess * x + b_guess;
|
||||||
|
*residual = y - predicted_y;
|
||||||
|
if *residual < 0.0 {
|
||||||
|
*residual = -*residual;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// extract the median residual
|
||||||
|
sorted_residuals[..count].copy_from_slice(&residuals[..count]);
|
||||||
|
sorted_residuals[..count]
|
||||||
|
.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap_or(core::cmp::Ordering::Greater));
|
||||||
|
let threshold = (sorted_residuals[count / 2] * 1.5).max(OUTLIER_THRESHOLD);
|
||||||
|
|
||||||
|
filtered_index = 0;
|
||||||
|
for (i, &p) in points[..count].iter().enumerate() {
|
||||||
|
if residuals[i] <= threshold {
|
||||||
|
filtered_points[filtered_index] = p;
|
||||||
|
filtered_index += 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if filtered_index == count {
|
||||||
|
break;
|
||||||
|
} else {
|
||||||
|
count = filtered_index;
|
||||||
|
points[..count].copy_from_slice(&filtered_points[..count]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
crate::println!("Solver converged in {} iterations", converged_in);
|
||||||
|
self.equation = Some((m_guess, b_guess))
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct Corner {
|
||||||
|
pub finder_ref: Option<Point>,
|
||||||
|
pub h_line: LineDerivation,
|
||||||
|
pub v_line: LineDerivation,
|
||||||
|
}
|
||||||
|
impl Default for Corner {
|
||||||
|
fn default() -> Self {
|
||||||
|
Self { finder_ref: None, h_line: LineDerivation::new(Axis::X), v_line: LineDerivation::new(Axis::Y) }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Default)]
|
||||||
|
pub struct QrCorners {
|
||||||
|
// Relies on the property that Direction enumerates and iterates consistently as a usize from 0-3,
|
||||||
|
// which covers each of the four corner directions exactly and uniquely.
|
||||||
|
corners: [Corner; 4],
|
||||||
|
width: isize,
|
||||||
|
height: isize,
|
||||||
|
derived_corner: Direction,
|
||||||
|
finder_width: usize,
|
||||||
|
}
|
||||||
|
impl QrCorners {
|
||||||
|
pub fn from_finders(points: &[Point; 3], dimensions: Point, finder_width: usize) -> Option<Self> {
|
||||||
|
let x_half = dimensions.x / 2;
|
||||||
|
let y_half = dimensions.y / 2;
|
||||||
|
|
||||||
|
let mut qrc = QrCorners::default();
|
||||||
|
qrc.width = dimensions.x;
|
||||||
|
qrc.height = dimensions.y;
|
||||||
|
qrc.finder_width = finder_width;
|
||||||
|
|
||||||
|
for &p in points {
|
||||||
|
if p.x < x_half && p.y < y_half {
|
||||||
|
qrc.corners[Direction::SouthWest as usize].finder_ref = Some(p);
|
||||||
|
} else if p.x < x_half && p.y >= y_half {
|
||||||
|
qrc.corners[Direction::NorthWest as usize].finder_ref = Some(p);
|
||||||
|
} else if p.x >= x_half && p.y < y_half {
|
||||||
|
qrc.corners[Direction::SouthEast as usize].finder_ref = Some(p);
|
||||||
|
} else if p.x >= x_half && p.y >= y_half {
|
||||||
|
qrc.corners[Direction::NorthEast as usize].finder_ref = Some(p);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// check that at least three corners are filled
|
||||||
|
if qrc.corners.iter().map(|c| if c.finder_ref.is_some() { 1 } else { 0 }).sum::<usize>() == 3 {
|
||||||
|
for (dir_index, corner) in qrc.corners.iter().enumerate() {
|
||||||
|
if corner.finder_ref.is_none() {
|
||||||
|
qrc.derived_corner = Direction::try_from(dir_index).unwrap();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
Some(qrc)
|
||||||
|
} else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn derived_corner(&self) -> Direction { self.derived_corner }
|
||||||
|
|
||||||
|
pub fn center_point(&self, dir: Direction) -> Option<Point> { self.corners[dir as usize].finder_ref }
|
||||||
|
|
||||||
|
fn outline_search(&mut self, ir: &mut ImageRoi) {
|
||||||
|
for (d, corner) in self.corners.iter_mut().enumerate() {
|
||||||
|
let direction = Direction::try_from(d).unwrap();
|
||||||
|
// this test ensures we automatically skip the "missing" corner
|
||||||
|
if let Some(p) = corner.finder_ref {
|
||||||
|
ir.set_roi(
|
||||||
|
Point::new(
|
||||||
|
(p.x - self.finder_width as isize / 2).max(0),
|
||||||
|
(p.y + self.finder_width as isize / 2).min(ir.height as isize),
|
||||||
|
),
|
||||||
|
Point::new(
|
||||||
|
(p.x + self.finder_width as isize / 2).min(ir.width as isize),
|
||||||
|
(p.y - self.finder_width as isize / 2).max(0),
|
||||||
|
),
|
||||||
|
);
|
||||||
|
let signs: Point = direction.into();
|
||||||
|
|
||||||
|
let mut y_range = if signs.y < 0 {
|
||||||
|
DirRange::new(0, ir.roi_height(), true)
|
||||||
|
} else {
|
||||||
|
DirRange::new(0, ir.roi_height(), false)
|
||||||
|
};
|
||||||
|
let mut x_range = if signs.x < 0 {
|
||||||
|
DirRange::new(0, ir.roi_width(), true)
|
||||||
|
} else {
|
||||||
|
DirRange::new(0, ir.roi_width(), false)
|
||||||
|
};
|
||||||
|
|
||||||
|
loop {
|
||||||
|
if let Some(y) = y_range.next() {
|
||||||
|
x_range.reset();
|
||||||
|
loop {
|
||||||
|
if let Some(x) = x_range.next() {
|
||||||
|
if Color::Black
|
||||||
|
== ir.get_roi_binary_pixel(Point::new(x as isize, y as isize)).unwrap()
|
||||||
|
{
|
||||||
|
corner.v_line.push(
|
||||||
|
ir.roi_to_absolute(Point::new(x as isize, y as isize)).unwrap(),
|
||||||
|
);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
corner.v_line.solve();
|
||||||
|
|
||||||
|
y_range.reset();
|
||||||
|
x_range.reset();
|
||||||
|
|
||||||
|
loop {
|
||||||
|
if let Some(x) = x_range.next() {
|
||||||
|
y_range.reset();
|
||||||
|
loop {
|
||||||
|
if let Some(y) = y_range.next() {
|
||||||
|
if Color::Black
|
||||||
|
== ir.get_roi_binary_pixel(Point::new(x as isize, y as isize)).unwrap()
|
||||||
|
{
|
||||||
|
corner.h_line.push(
|
||||||
|
ir.roi_to_absolute(Point::new(x as isize, y as isize)).unwrap(),
|
||||||
|
);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
corner.h_line.solve();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Returns a (src, dst) tuple of point mappings for homomorphic transformation.
|
||||||
|
/// The destination is the four corners of image shape as specified when the structure
|
||||||
|
/// was made, with a margin added.
|
||||||
|
///
|
||||||
|
/// The margin should be negative for the corners to go in toward the center.
|
||||||
|
pub fn mapping(&mut self, ir: &mut ImageRoi, margin: isize) -> ([Option<Point>; 4], [Option<Point>; 4]) {
|
||||||
|
// first, search for the lines that define the outline of the QR code
|
||||||
|
self.outline_search(ir);
|
||||||
|
|
||||||
|
let mut src = [None; 4];
|
||||||
|
let mut dst = [None; 4];
|
||||||
|
|
||||||
|
for (i, corner) in self.corners.iter().enumerate() {
|
||||||
|
if let Some(_p) = corner.finder_ref {
|
||||||
|
// This is a known finder corner:
|
||||||
|
// derive the corner from the extracted h and v lines along the finder pattern
|
||||||
|
src[i] = point_from_hv_lines(&corner.h_line, &corner.v_line);
|
||||||
|
} else {
|
||||||
|
// This is the unknown corner:
|
||||||
|
// derive the corner from the h and v lines from the nearest finders' lines
|
||||||
|
let h_line = match Direction::try_from(i) {
|
||||||
|
Ok(Direction::NorthWest) => &self.corners[Direction::NorthEast as usize].h_line,
|
||||||
|
Ok(Direction::NorthEast) => &self.corners[Direction::NorthWest as usize].h_line,
|
||||||
|
Ok(Direction::SouthWest) => &self.corners[Direction::SouthEast as usize].h_line,
|
||||||
|
Ok(Direction::SouthEast) => &self.corners[Direction::SouthWest as usize].h_line,
|
||||||
|
_ => panic!("Bad index"),
|
||||||
|
};
|
||||||
|
let v_line = match Direction::try_from(i) {
|
||||||
|
Ok(Direction::NorthWest) => &self.corners[Direction::SouthWest as usize].v_line,
|
||||||
|
Ok(Direction::NorthEast) => &self.corners[Direction::SouthEast as usize].v_line,
|
||||||
|
Ok(Direction::SouthWest) => &self.corners[Direction::NorthWest as usize].v_line,
|
||||||
|
Ok(Direction::SouthEast) => &self.corners[Direction::NorthEast as usize].v_line,
|
||||||
|
_ => panic!("Bad index"),
|
||||||
|
};
|
||||||
|
src[i] = point_from_hv_lines(h_line, v_line);
|
||||||
|
}
|
||||||
|
dst[i] = match Direction::try_from(i) {
|
||||||
|
Ok(Direction::NorthWest) => Some(Point::new(-margin, self.height + margin)),
|
||||||
|
Ok(Direction::NorthEast) => Some(Point::new(self.width + margin, self.height + margin)),
|
||||||
|
Ok(Direction::SouthWest) => Some(Point::new(-margin, -margin)),
|
||||||
|
Ok(Direction::SouthEast) => Some(Point::new(self.width + margin, -margin)),
|
||||||
|
_ => None,
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
(src, dst)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#[derive(Copy, Clone, Default, Debug)]
|
#[derive(Copy, Clone, Default, Debug)]
|
||||||
struct FinderSeq {
|
struct FinderSeq {
|
||||||
/// run length of the pixels leading up to the current position
|
/// run length of the pixels leading up to the current position
|
||||||
|
|
@ -24,7 +458,7 @@ struct FinderSeq {
|
||||||
}
|
}
|
||||||
|
|
||||||
#[derive(Copy, Clone, PartialEq, Eq, Debug)]
|
#[derive(Copy, Clone, PartialEq, Eq, Debug)]
|
||||||
enum Color {
|
pub enum Color {
|
||||||
Black,
|
Black,
|
||||||
White,
|
White,
|
||||||
}
|
}
|
||||||
|
|
@ -33,53 +467,210 @@ impl Color {
|
||||||
if luminance > thresh { Color::White } else { Color::Black }
|
if luminance > thresh { Color::White } else { Color::Black }
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
/// Used for counting pixels
|
||||||
|
impl Into<usize> for Color {
|
||||||
|
fn into(self) -> usize {
|
||||||
|
match self {
|
||||||
|
Color::Black => 0,
|
||||||
|
Color::White => 1,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/// Used for translating pixels into RGB or Luma color spaces
|
||||||
|
impl Into<u8> for Color {
|
||||||
|
fn into(self) -> u8 {
|
||||||
|
match self {
|
||||||
|
Color::Black => 0,
|
||||||
|
Color::White => 255,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
impl Default for Color {
|
impl Default for Color {
|
||||||
fn default() -> Self { Color::Black }
|
fn default() -> Self { Color::Black }
|
||||||
}
|
}
|
||||||
|
|
||||||
const AFF_FP_SHIFT: isize = 16;
|
impl Not for Color {
|
||||||
const AFF_FP_ONE: f32 = (1 << AFF_FP_SHIFT) as f32;
|
type Output = Color;
|
||||||
pub struct AffineTransform {
|
|
||||||
pub a: isize,
|
fn not(self) -> Self::Output {
|
||||||
pub b: isize,
|
match self {
|
||||||
pub c: isize,
|
Color::Black => Color::White,
|
||||||
pub d: isize,
|
Color::White => Color::Black,
|
||||||
pub tx: isize,
|
}
|
||||||
pub ty: isize,
|
}
|
||||||
pub rows: isize,
|
|
||||||
pub cols: isize,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl AffineTransform {
|
impl BitXor for Color {
|
||||||
pub fn from_coordinates(tl: Point, tr: Point, cols: usize, rows: usize) -> Self {
|
type Output = Color;
|
||||||
// crate::println!("{}/{}", tr.y - tl.y, tr.x - tl.x);
|
|
||||||
let angle: f32 = libm::atan2f(tr.y as f32 - tl.y as f32, tr.x as f32 - tl.x as f32);
|
fn bitxor(self, rhs: Self) -> Self::Output { if self == rhs { Color::Black } else { Color::White } }
|
||||||
// crate::println!("angle: {}", angle);
|
}
|
||||||
|
|
||||||
|
#[derive(Copy, Clone, Debug)]
|
||||||
|
/// We use Direction as both a way to encode a meaning and a unique array index.
|
||||||
|
#[repr(usize)]
|
||||||
|
pub enum Direction {
|
||||||
|
NorthWest = 0,
|
||||||
|
NorthEast = 1,
|
||||||
|
SouthWest = 2,
|
||||||
|
SouthEast = 3,
|
||||||
|
North = 4,
|
||||||
|
West = 5,
|
||||||
|
East = 6,
|
||||||
|
South = 7,
|
||||||
|
}
|
||||||
|
impl Default for Direction {
|
||||||
|
fn default() -> Self { Self::NorthWest }
|
||||||
|
}
|
||||||
|
impl Into<Point> for Direction {
|
||||||
|
fn into(self) -> Point {
|
||||||
|
use Direction::*;
|
||||||
|
match self {
|
||||||
|
North => Point::new(0, 1),
|
||||||
|
West => Point::new(-1, 0),
|
||||||
|
East => Point::new(1, 0),
|
||||||
|
South => Point::new(0, -1),
|
||||||
|
NorthWest => Point::new(-1, 1),
|
||||||
|
NorthEast => Point::new(1, 1),
|
||||||
|
SouthWest => Point::new(-1, -1),
|
||||||
|
SouthEast => Point::new(1, -1),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
impl Into<usize> for Direction {
|
||||||
|
fn into(self) -> usize {
|
||||||
|
use Direction::*;
|
||||||
|
match self {
|
||||||
|
NorthWest => 0,
|
||||||
|
NorthEast => 1,
|
||||||
|
SouthWest => 2,
|
||||||
|
SouthEast => 3,
|
||||||
|
North => 4,
|
||||||
|
West => 5,
|
||||||
|
East => 6,
|
||||||
|
South => 7,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
impl TryFrom<usize> for Direction {
|
||||||
|
type Error = &'static str;
|
||||||
|
|
||||||
|
fn try_from(value: usize) -> Result<Self, Self::Error> {
|
||||||
|
use Direction::*;
|
||||||
|
match value {
|
||||||
|
0 => Ok(NorthWest),
|
||||||
|
1 => Ok(NorthEast),
|
||||||
|
2 => Ok(SouthWest),
|
||||||
|
3 => Ok(SouthEast),
|
||||||
|
4 => Ok(North),
|
||||||
|
5 => Ok(West),
|
||||||
|
6 => Ok(East),
|
||||||
|
7 => Ok(South),
|
||||||
|
_ => Err("Invalid direction coding"),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// (0, 0) is at the lower left corner
|
||||||
|
pub struct ImageRoi<'a> {
|
||||||
|
data: &'a mut [u8],
|
||||||
|
pub width: usize,
|
||||||
|
pub height: usize,
|
||||||
|
thresh: u8,
|
||||||
|
// coordinates of a subimage, if set. The ROI includes these points.
|
||||||
|
x0: usize,
|
||||||
|
x1: usize,
|
||||||
|
y0: usize,
|
||||||
|
y1: usize,
|
||||||
|
iter_row: RefCell<usize>,
|
||||||
|
col_iter_start: usize,
|
||||||
|
col_iter_row_index: RefCell<usize>,
|
||||||
|
iter_col: RefCell<usize>,
|
||||||
|
}
|
||||||
|
impl<'a> ImageRoi<'a> {
|
||||||
|
pub fn new(data: &'a mut [u8], dimensions: Point, thresh: u8) -> Self {
|
||||||
|
// ROI is default the entire area
|
||||||
Self {
|
Self {
|
||||||
a: (libm::cosf(angle) * AFF_FP_ONE) as isize,
|
data,
|
||||||
b: (-libm::sinf(angle) * AFF_FP_ONE) as isize,
|
width: dimensions.x as usize,
|
||||||
c: (libm::sinf(angle) * AFF_FP_ONE) as isize,
|
height: dimensions.y as usize,
|
||||||
d: (libm::cosf(angle) * AFF_FP_ONE) as isize,
|
thresh,
|
||||||
tx: 0,
|
x0: 0,
|
||||||
ty: 0,
|
x1: dimensions.x as usize,
|
||||||
cols: cols as isize,
|
y0: 0,
|
||||||
rows: rows as isize,
|
y1: dimensions.y as usize,
|
||||||
|
iter_row: RefCell::new(0),
|
||||||
|
col_iter_row_index: RefCell::new(0),
|
||||||
|
col_iter_start: 0,
|
||||||
|
iter_col: RefCell::new(0),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn transform(&self, src: &[u8], dst: &mut [core::mem::MaybeUninit<u8>]) {
|
pub fn binarize(&self, luma: u8) -> Color { if luma > self.thresh { Color::White } else { Color::Black } }
|
||||||
for y in 0..self.rows as usize {
|
|
||||||
for x in 0..self.cols as usize {
|
pub fn get_pixel(&self, x: usize, y: usize) -> u8 { self.data[x + y * self.width] }
|
||||||
let x_src = (self.a * x as isize + self.b * y as isize + self.tx) >> AFF_FP_SHIFT;
|
|
||||||
let y_src = (self.c * x as isize + self.d * y as isize + self.ty) >> AFF_FP_SHIFT;
|
pub fn get_roi_binary_pixel(&self, roi_point: Point) -> Option<Color> {
|
||||||
dst[y * self.cols as usize + x] = core::mem::MaybeUninit::new(
|
if let Some(abs_p) = self.roi_to_absolute(roi_point) {
|
||||||
if x_src >= 0 && x_src < self.cols && y_src >= 0 && y_src < self.rows {
|
let p = self.get_pixel(abs_p.x as usize, abs_p.y as usize);
|
||||||
src[x_src as usize + y_src as usize * self.cols as usize]
|
Some(self.binarize(p))
|
||||||
} else {
|
} else {
|
||||||
0
|
None
|
||||||
},
|
}
|
||||||
);
|
}
|
||||||
}
|
|
||||||
|
pub fn set_roi(&mut self, tl: Point, br: Point) {
|
||||||
|
assert!(tl.x >= 0);
|
||||||
|
assert!(tl.y >= 0);
|
||||||
|
assert!(br.x >= 0);
|
||||||
|
assert!(br.y >= 0);
|
||||||
|
assert!(br.x >= tl.x);
|
||||||
|
assert!(tl.y >= br.y);
|
||||||
|
self.x0 = tl.x as usize;
|
||||||
|
self.x1 = br.x as usize;
|
||||||
|
self.y0 = br.y as usize;
|
||||||
|
self.y1 = tl.y as usize;
|
||||||
|
*self.iter_row.borrow_mut() = 0;
|
||||||
|
*self.iter_col.borrow_mut() = 0;
|
||||||
|
*self.col_iter_row_index.borrow_mut() = 0;
|
||||||
|
self.col_iter_start = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn roi_width(&self) -> usize { self.x1 - self.x0 }
|
||||||
|
|
||||||
|
pub fn roi_height(&self) -> usize { self.y1 - self.y0 }
|
||||||
|
|
||||||
|
pub fn roi_to_absolute(&self, point: Point) -> Option<Point> {
|
||||||
|
let x = point.x + self.x0 as isize;
|
||||||
|
let y = point.y + self.y0 as isize;
|
||||||
|
if x >= 0 && x < self.width as isize && y >= 0 && y < self.height as isize {
|
||||||
|
Some(Point::new(x, y))
|
||||||
|
} else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn absolute_to_roi(&self, point: Point) -> Option<Point> {
|
||||||
|
let x = point.x - self.x0 as isize;
|
||||||
|
let y = point.y - self.y0 as isize;
|
||||||
|
if x >= 0 && x < self.x1 as isize && y >= 0 && y < self.y1 as isize {
|
||||||
|
Some(Point::new(x, y))
|
||||||
|
} else {
|
||||||
|
None
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a> Iterator for ImageRoi<'_> {
|
||||||
|
type Item = u8;
|
||||||
|
|
||||||
|
fn next(&mut self) -> Option<Self::Item> {
|
||||||
|
let cur_row = *self.col_iter_row_index.borrow();
|
||||||
|
if cur_row < self.y1 - self.y0 {
|
||||||
|
*self.col_iter_row_index.borrow_mut() = cur_row + 1;
|
||||||
|
Some(self.data[(self.y0 + cur_row) * self.width + self.x0 + self.col_iter_start])
|
||||||
|
} else {
|
||||||
|
None
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -134,7 +725,8 @@ impl SeqBuffer {
|
||||||
copied_count
|
copied_count
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn search(&self) -> Option<usize> {
|
// returns a tuple of (center point of the sequence, total length of sequence)
|
||||||
|
pub fn search(&self) -> Option<(usize, usize)> {
|
||||||
let mut ratios = [0usize; SEQ_LEN];
|
let mut ratios = [0usize; SEQ_LEN];
|
||||||
let mut seq: [FinderSeq; SEQ_LEN] = [FinderSeq::default(); SEQ_LEN];
|
let mut seq: [FinderSeq; SEQ_LEN] = [FinderSeq::default(); SEQ_LEN];
|
||||||
if self.retrieve(&mut seq) == SEQ_LEN {
|
if self.retrieve(&mut seq) == SEQ_LEN {
|
||||||
|
|
@ -156,7 +748,7 @@ impl SeqBuffer {
|
||||||
{
|
{
|
||||||
// crate::println!(" seq {:?}", &seq);
|
// crate::println!(" seq {:?}", &seq);
|
||||||
// crate::println!(" ratios {:?}", &ratios);
|
// crate::println!(" ratios {:?}", &ratios);
|
||||||
Some(seq[2].pos - seq[2].run / 2 - 1)
|
Some((seq[2].pos - seq[2].run / 2 - 1, seq.iter().map(|s| s.run).sum()))
|
||||||
} else {
|
} else {
|
||||||
None
|
None
|
||||||
}
|
}
|
||||||
|
|
@ -170,7 +762,8 @@ impl SeqBuffer {
|
||||||
}
|
}
|
||||||
|
|
||||||
const ROW_LIMIT: usize = 128;
|
const ROW_LIMIT: usize = 128;
|
||||||
pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8, stride: usize) {
|
/// Returns the average width of the finder regions found.
|
||||||
|
pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8, stride: usize) -> usize {
|
||||||
let mut row_candidates: [Option<Point>; ROW_LIMIT] = [None; ROW_LIMIT];
|
let mut row_candidates: [Option<Point>; ROW_LIMIT] = [None; ROW_LIMIT];
|
||||||
|
|
||||||
// ideally, the candidates would be a Vec, but we want this to work without allocations
|
// ideally, the candidates would be a Vec, but we want this to work without allocations
|
||||||
|
|
@ -195,7 +788,7 @@ pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8,
|
||||||
run_length = 1;
|
run_length = 1;
|
||||||
|
|
||||||
// manage the sequence index
|
// manage the sequence index
|
||||||
if let Some(pos) = seq_buffer.search() {
|
if let Some((pos, _width)) = seq_buffer.search() {
|
||||||
// crate::println!("row candidate {}, {}", pos, y);
|
// crate::println!("row candidate {}, {}", pos, y);
|
||||||
row_candidates[row_candidate_index] = Some(Point::new(pos as _, y as _));
|
row_candidates[row_candidate_index] = Some(Point::new(pos as _, y as _));
|
||||||
row_candidate_index += 1;
|
row_candidate_index += 1;
|
||||||
|
|
@ -214,11 +807,14 @@ pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8,
|
||||||
|
|
||||||
// crate::println!("CCCCCCCCCCCCCCCC");
|
// crate::println!("CCCCCCCCCCCCCCCC");
|
||||||
let mut candidate_index = 0;
|
let mut candidate_index = 0;
|
||||||
|
let mut candidate_width = 0;
|
||||||
for x in 0..stride {
|
for x in 0..stride {
|
||||||
seq_buffer.clear();
|
seq_buffer.clear();
|
||||||
|
|
||||||
let mut last_color = Color::from(image[x], thresh);
|
let mut last_color = Color::from(image[x], thresh);
|
||||||
let mut run_length = 1;
|
let mut run_length = 1;
|
||||||
|
// could rewrite this to abort the search after more than 3 finders are found, but for now,
|
||||||
|
// do an exhaustive search because it's useful info for debugging.
|
||||||
for (y_minus1, row) in image[x + stride..].chunks(stride).enumerate() {
|
for (y_minus1, row) in image[x + stride..].chunks(stride).enumerate() {
|
||||||
let pix = Color::from(row[0], thresh);
|
let pix = Color::from(row[0], thresh);
|
||||||
if pix == last_color {
|
if pix == last_color {
|
||||||
|
|
@ -227,7 +823,7 @@ pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8,
|
||||||
seq_buffer.push(FinderSeq { run: run_length, pos: y_minus1 + 1, color: last_color });
|
seq_buffer.push(FinderSeq { run: run_length, pos: y_minus1 + 1, color: last_color });
|
||||||
last_color = pix;
|
last_color = pix;
|
||||||
run_length = 1;
|
run_length = 1;
|
||||||
if let Some(pos) = seq_buffer.search() {
|
if let Some((pos, width)) = seq_buffer.search() {
|
||||||
let search_point = Point::new(x as _, pos as _);
|
let search_point = Point::new(x as _, pos as _);
|
||||||
// crate::println!("col candidate {}, {}", x, pos);
|
// crate::println!("col candidate {}, {}", x, pos);
|
||||||
|
|
||||||
|
|
@ -239,6 +835,7 @@ pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8,
|
||||||
if candidate_index < candidates.len() {
|
if candidate_index < candidates.len() {
|
||||||
candidates[candidate_index] = rc;
|
candidates[candidate_index] = rc;
|
||||||
candidate_index += 1;
|
candidate_index += 1;
|
||||||
|
candidate_width += width;
|
||||||
} else {
|
} else {
|
||||||
// just abort the search if we run out of space to store results
|
// just abort the search if we run out of space to store results
|
||||||
break;
|
break;
|
||||||
|
|
@ -253,4 +850,5 @@ pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
if candidate_index != 0 { candidate_width / candidate_index } else { 0 }
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue