mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-15 08:30:02 +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
|
||||
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;
|
||||
|
||||
|
|
@ -13,6 +19,434 @@ const UPPER_1: usize = 2 << SEQ_FP_SHIFT;
|
|||
const LOWER_3: usize = 2 << 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)]
|
||||
struct FinderSeq {
|
||||
/// run length of the pixels leading up to the current position
|
||||
|
|
@ -24,7 +458,7 @@ struct FinderSeq {
|
|||
}
|
||||
|
||||
#[derive(Copy, Clone, PartialEq, Eq, Debug)]
|
||||
enum Color {
|
||||
pub enum Color {
|
||||
Black,
|
||||
White,
|
||||
}
|
||||
|
|
@ -33,53 +467,210 @@ impl Color {
|
|||
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 {
|
||||
fn default() -> Self { Color::Black }
|
||||
}
|
||||
|
||||
const AFF_FP_SHIFT: isize = 16;
|
||||
const AFF_FP_ONE: f32 = (1 << AFF_FP_SHIFT) as f32;
|
||||
pub struct AffineTransform {
|
||||
pub a: isize,
|
||||
pub b: isize,
|
||||
pub c: isize,
|
||||
pub d: isize,
|
||||
pub tx: isize,
|
||||
pub ty: isize,
|
||||
pub rows: isize,
|
||||
pub cols: isize,
|
||||
impl Not for Color {
|
||||
type Output = Color;
|
||||
|
||||
fn not(self) -> Self::Output {
|
||||
match self {
|
||||
Color::Black => Color::White,
|
||||
Color::White => Color::Black,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl AffineTransform {
|
||||
pub fn from_coordinates(tl: Point, tr: Point, cols: usize, rows: usize) -> Self {
|
||||
// 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);
|
||||
// crate::println!("angle: {}", angle);
|
||||
impl BitXor for Color {
|
||||
type Output = Color;
|
||||
|
||||
fn bitxor(self, rhs: Self) -> Self::Output { if self == rhs { Color::Black } else { Color::White } }
|
||||
}
|
||||
|
||||
#[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 {
|
||||
a: (libm::cosf(angle) * AFF_FP_ONE) as isize,
|
||||
b: (-libm::sinf(angle) * AFF_FP_ONE) as isize,
|
||||
c: (libm::sinf(angle) * AFF_FP_ONE) as isize,
|
||||
d: (libm::cosf(angle) * AFF_FP_ONE) as isize,
|
||||
tx: 0,
|
||||
ty: 0,
|
||||
cols: cols as isize,
|
||||
rows: rows as isize,
|
||||
data,
|
||||
width: dimensions.x as usize,
|
||||
height: dimensions.y as usize,
|
||||
thresh,
|
||||
x0: 0,
|
||||
x1: dimensions.x as usize,
|
||||
y0: 0,
|
||||
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>]) {
|
||||
for y in 0..self.rows as usize {
|
||||
for x in 0..self.cols as usize {
|
||||
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;
|
||||
dst[y * self.cols as usize + x] = core::mem::MaybeUninit::new(
|
||||
if x_src >= 0 && x_src < self.cols && y_src >= 0 && y_src < self.rows {
|
||||
src[x_src as usize + y_src as usize * self.cols as usize]
|
||||
} else {
|
||||
0
|
||||
},
|
||||
);
|
||||
}
|
||||
pub fn binarize(&self, luma: u8) -> Color { if luma > self.thresh { Color::White } else { Color::Black } }
|
||||
|
||||
pub fn get_pixel(&self, x: usize, y: usize) -> u8 { self.data[x + y * self.width] }
|
||||
|
||||
pub fn get_roi_binary_pixel(&self, roi_point: Point) -> Option<Color> {
|
||||
if let Some(abs_p) = self.roi_to_absolute(roi_point) {
|
||||
let p = self.get_pixel(abs_p.x as usize, abs_p.y as usize);
|
||||
Some(self.binarize(p))
|
||||
} else {
|
||||
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
|
||||
}
|
||||
|
||||
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 seq: [FinderSeq; SEQ_LEN] = [FinderSeq::default(); SEQ_LEN];
|
||||
if self.retrieve(&mut seq) == SEQ_LEN {
|
||||
|
|
@ -156,7 +748,7 @@ impl SeqBuffer {
|
|||
{
|
||||
// crate::println!(" seq {:?}", &seq);
|
||||
// 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 {
|
||||
None
|
||||
}
|
||||
|
|
@ -170,7 +762,8 @@ impl SeqBuffer {
|
|||
}
|
||||
|
||||
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];
|
||||
|
||||
// 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;
|
||||
|
||||
// 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);
|
||||
row_candidates[row_candidate_index] = Some(Point::new(pos as _, y as _));
|
||||
row_candidate_index += 1;
|
||||
|
|
@ -214,11 +807,14 @@ pub fn find_finders(candidates: &mut [Option<Point>], image: &[u8], thresh: u8,
|
|||
|
||||
// crate::println!("CCCCCCCCCCCCCCCC");
|
||||
let mut candidate_index = 0;
|
||||
let mut candidate_width = 0;
|
||||
for x in 0..stride {
|
||||
seq_buffer.clear();
|
||||
|
||||
let mut last_color = Color::from(image[x], thresh);
|
||||
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() {
|
||||
let pix = Color::from(row[0], thresh);
|
||||
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 });
|
||||
last_color = pix;
|
||||
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 _);
|
||||
// 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() {
|
||||
candidates[candidate_index] = rc;
|
||||
candidate_index += 1;
|
||||
candidate_width += width;
|
||||
} else {
|
||||
// just abort the search if we run out of space to store results
|
||||
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