add some test scaffolding for userspace video processing

The whole graphical interface - camera + display & QR code processing -
is incorporated into a single process space to minimize on the amount
of swapping/memory message passing necessary to implement a low-latency,
high-frame rate video interface.

The current service just starts a camera-to-display loopback
while searching for and aligning QR code markers, without consulting
any other service on whether it should do that or not. Suitable
for debug and performance characterization, but not usable for
product integration.

The good news is we're getting an acceptable frame rate on
the camera during search mode with the kernel running.
This commit is contained in:
bunnie 2024-11-07 16:14:29 +08:00
parent fd6c2ad4ff
commit 2692d374c6
5 changed files with 1310 additions and 0 deletions

View file

@ -0,0 +1,27 @@
[package]
name = "bao-video"
version = "0.1.0"
edition = "2021"
[dependencies]
utralib = { version = "0.1.25", optional = true, default-features = false }
xous-names = { package = "xous-api-names", version = "0.9.63" }
ticktimer = { package = "xous-api-ticktimer", version = "0.9.61" }
xous = "0.9.64"
log-server = { package = "xous-api-log", version = "0.1.61" }
log = "0.4.14"
num-derive = { version = "0.3.3", default-features = false }
num-traits = { version = "0.2.14", default-features = false }
cramium-hal = { path = "../../libs/cramium-hal", features = ["std"] }
cram-hal-service = { path = "../cram-hal-service" }
# QR decoding
libm = { version = "0.2.8" }
nalgebra = { version = "0.33", default-features = false, features = ["libm"] }
rqrr = "0.8.0"
locales = { path = "../../locales" }
[features]
cramium-soc = ["utralib/cramium-soc"]
default = []

View file

@ -0,0 +1,141 @@
use cramium_hal::{minigfx::*, sh1107::Mono};
// The discipline for all the APIs in this module are that they act on a FrameBuffer which is
// passed to the function. This allows us to bind the drawing computation on the caller-side of
// of a shared frame buffer.
// this font is from the embedded graphics crate https://docs.rs/embedded-graphics/0.7.1/embedded_graphics/
const FONT_IMAGE: &'static [u8] = include_bytes!("../../../loader/src/font6x12_1bpp.raw");
pub const CHAR_HEIGHT: isize = 12;
pub const CHAR_WIDTH: isize = 6;
const FONT_IMAGE_WIDTH: isize = 96;
const LEFT_MARGIN: isize = 10;
fn char_offset(c: char) -> isize {
let fallback = ' ' as isize - ' ' as isize;
if c < ' ' {
return fallback;
}
if c <= '~' {
return c as isize - ' ' as isize;
}
fallback
}
pub fn msg<'a>(fb: &mut dyn FrameBuffer, text: &'a str, ll_pos: Point, fg: ColorNative, bg: ColorNative) {
let mut ll_pos = ll_pos.clone();
// this routine is adapted from the embedded graphics crate https://docs.rs/embedded-graphics/0.7.1/embedded_graphics/
let char_per_row = FONT_IMAGE_WIDTH / CHAR_WIDTH;
let mut idx = 0;
let mut x_update = 0;
for current_char in text.chars() {
let mut char_walk_x = 0;
let mut char_walk_y = 0;
loop {
// Char _code_ offset from first char, most often a space
// E.g. first char = ' ' (32), target char = '!' (33), offset = 33 - 32 = 1
let char_offset = char_offset(current_char);
let row = char_offset / char_per_row;
// Top left corner of character, in pixels
let char_x = (char_offset - (row * char_per_row)) * CHAR_WIDTH;
let char_y = row * CHAR_HEIGHT;
// Bit index
// = X pixel offset for char
// + Character row offset (row 0 = 0, row 1 = (192 * 8) = 1536)
// + X offset for the pixel block that comprises this char
// + Y offset for pixel block
let bitmap_bit_index =
char_x + (FONT_IMAGE_WIDTH * char_y) + char_walk_x + (char_walk_y * FONT_IMAGE_WIDTH);
let bitmap_byte = bitmap_bit_index / 8;
let bitmap_bit = 7 - (bitmap_bit_index % 8);
let color = if FONT_IMAGE[bitmap_byte as usize] & (1 << bitmap_bit) != 0 { fg } else { bg };
let x = ll_pos.x + CHAR_WIDTH * idx + char_walk_x;
let y = ll_pos.y + (CHAR_HEIGHT - char_walk_y);
// draw color at x, y
if (current_char as u8 != 0xd) && (current_char as u8 != 0xa) {
// don't draw CRLF specials
fb.put_pixel(Point::new(x, y), color);
}
char_walk_x += 1;
if char_walk_x >= CHAR_WIDTH {
char_walk_x = 0;
char_walk_y += 1;
// Done with this char, move on to the next one
if char_walk_y >= CHAR_HEIGHT {
if current_char as u8 == 0xd {
// '\n'
ll_pos.y += CHAR_HEIGHT;
} else if current_char as u8 == 0xa {
// '\r'
ll_pos.x = LEFT_MARGIN;
x_update = 0;
} else {
idx += 1;
x_update += CHAR_WIDTH;
}
break;
}
}
}
}
ll_pos.x += x_update;
}
pub fn line(fb: &mut dyn FrameBuffer, l: Line, clip: Option<Rectangle>, xor: bool) {
let color: ColorNative;
if l.style.stroke_color.is_some() {
color = l.style.stroke_color.unwrap();
} else {
return;
}
let mut x0 = l.start.x;
let mut y0 = l.start.y;
let x1 = l.end.x;
let y1 = l.end.y;
let dx = (x1 - x0).abs();
let sx = if x0 < x1 { 1 } else { -1 };
let dy = -((y1 - y0).abs());
let sy = if y0 < y1 { 1 } else { -1 };
let mut err = dx + dy; // error value e_xy
loop {
if x0 >= 0 && y0 >= 0 && x0 < (fb.dimensions().x as _) && y0 < (fb.dimensions().y as _) {
if clip.is_none() || (clip.unwrap().intersects_point(Point::new(x0, y0))) {
if !xor {
fb.put_pixel(Point::new(x0 as _, y0 as _), color);
} else {
if let Some(existing) = fb.get_pixel(Point::new(x0 as _, y0 as _)) {
if existing == Mono::Black.into() {
fb.put_pixel(Point::new(x0 as _, y0 as _), Mono::White.into());
} else {
fb.put_pixel(Point::new(x0 as _, y0 as _), Mono::Black.into());
}
}
}
}
}
if x0 == x1 && y0 == y1 {
break;
}
let e2 = 2 * err;
if e2 >= dy {
err += dy;
x0 += sx;
}
if e2 <= dx {
err += dx;
y0 += sy;
}
}
}

View file

@ -0,0 +1,85 @@
use nalgebra::{Matrix3, SVD};
pub type Matrix8x9<T> = nalgebra::Matrix<T, nalgebra::U8, nalgebra::U9, nalgebra::ArrayStorage<T, 8, 9>>;
// This function calculates the homography matrix from 4 corresponding point pairs
pub fn find_homography(src_points: [(f32, f32); 4], dst_points: [(f32, f32); 4]) -> Option<Matrix3<f32>> {
// Prepare the A matrix (8x9) for 4 point pairs
let mut a = Matrix8x9::zeros();
let mut b = nalgebra::Vector::from_column_slice(&[0.0f32; 9]);
for (i, (src, dst)) in src_points.iter().zip(dst_points.iter()).enumerate() {
let (x, y) = *src;
let (xp, yp) = *dst;
let row1 = 2 * i;
let row2 = 2 * i + 1;
a[(row1, 0)] = -x;
a[(row1, 1)] = -y;
a[(row1, 2)] = -1.0;
a[(row1, 6)] = x * xp;
a[(row1, 7)] = y * xp;
b[row1] = -xp;
a[(row2, 3)] = -x;
a[(row2, 4)] = -y;
a[(row2, 5)] = -1.0;
a[(row2, 6)] = x * yp;
a[(row2, 7)] = y * yp;
b[row2] = -yp;
}
// Solve the least-squares solution using Singular Value Decomposition (SVD)
let svd = SVD::new_unordered(a.clone(), true, true);
let solution = svd.solve(&b, f32::EPSILON);
if let Ok(h) = solution {
// Construct the homography matrix from the result
let mut homography = Matrix3::identity();
homography[(0, 0)] = h[0];
homography[(0, 1)] = h[1];
homography[(0, 2)] = h[2];
homography[(1, 0)] = h[3];
homography[(1, 1)] = h[4];
homography[(1, 2)] = h[5];
homography[(2, 0)] = h[6];
homography[(2, 1)] = h[7];
homography[(2, 2)] = 1.0;
Some(homography)
} else {
None
}
}
const HOM_FP_SHIFT: i32 = 16;
const HOM_FP_SCALE: i32 = 1 << HOM_FP_SHIFT;
pub fn matrix3_to_fixp(h: Matrix3<f32>) -> Matrix3<i32> {
Matrix3::new(
(h[(0, 0)] * HOM_FP_SCALE as f32) as i32,
(h[(0, 1)] * HOM_FP_SCALE as f32) as i32,
(h[(0, 2)] * HOM_FP_SCALE as f32) as i32,
(h[(1, 0)] * HOM_FP_SCALE as f32) as i32,
(h[(1, 1)] * HOM_FP_SCALE as f32) as i32,
(h[(1, 2)] * HOM_FP_SCALE as f32) as i32,
(h[(2, 0)] * HOM_FP_SCALE as f32) as i32,
(h[(2, 1)] * HOM_FP_SCALE as f32) as i32,
(h[(2, 2)] * HOM_FP_SCALE as f32) as i32,
)
}
pub fn apply_fixp_homography(homography: &Matrix3<i32>, point: (i32, i32)) -> (i32, i32) {
let (x, y) = point;
// Transform the point using fixed-point arithmetic
let tx = homography[(0, 0)] * x + homography[(0, 1)] * y + homography[(0, 2)];
let ty = homography[(1, 0)] * x + homography[(1, 1)] * y + homography[(1, 2)];
let tw = homography[(2, 0)] * x + homography[(2, 1)] * y + homography[(2, 2)];
// Normalize by w to get the final coordinates
let x_transformed = tx / tw;
let y_transformed = ty / tw;
(x_transformed, y_transformed)
}

View file

@ -0,0 +1,233 @@
use cramium_hal::iox::IoGpio;
use cramium_hal::iox::{IoxPort, IoxValue};
use cramium_hal::sh1107::{Mono, Oled128x128};
use cramium_hal::udma::PeriphId;
use cramium_hal::{minigfx::*, sh1107};
use nalgebra::QR;
mod gfx;
mod homography;
mod qr;
const QR_WIDTH: usize = 256;
const QR_HEIGHT: usize = 240;
const BW_THRESH: u8 = 128;
pub fn blit_to_display(sh1107: &mut Oled128x128, frame: &[u8], display_cleared: bool) {
for (y, row) in frame.chunks(QR_WIDTH).enumerate() {
if y & 1 == 0 {
for (x, &pixval) in row.iter().enumerate() {
if x & 1 == 0 {
if x < sh1107.dimensions().x as usize * 2
&& y < sh1107.dimensions().y as usize * 2 - (gfx::CHAR_HEIGHT as usize + 1) * 2
{
let luminance = pixval & 0xff;
if luminance > BW_THRESH {
// flip on y to adjust for sensor orientation. Lower left is (0, 0)
// on the display.
sh1107.put_pixel(
Point::new(x as isize / 2, (sh1107.dimensions().y - 1) - (y as isize / 2)),
Mono::White.into(),
);
} else {
// optimization to avoid some computation if we're blitting to an already-black
// buffer
if !display_cleared {
sh1107.put_pixel(
Point::new(
x as isize / 2,
(sh1107.dimensions().y - 1) - (y as isize / 2),
),
Mono::Black.into(),
);
}
}
} else {
break;
}
}
}
}
}
}
fn main() -> ! {
let stack_size = 1 * 1024 * 1024;
std::thread::Builder::new().stack_size(stack_size).spawn(wrapped_main).unwrap().join().unwrap()
}
fn wrapped_main() -> ! {
log_server::init_wait().unwrap();
log::set_max_level(log::LevelFilter::Info);
let tt = ticktimer::Ticktimer::new().unwrap();
let xns = xous_names::XousNames::new().unwrap();
let iox = cram_hal_service::IoxHal::new();
let udma_global = cram_hal_service::UdmaGlobal::new();
let mut i2c = cram_hal_service::I2c::new();
let mut sh1107 = cramium_hal::sh1107::Oled128x128::new(cram_hal_service::PERCLK, &iox, &udma_global);
sh1107.init();
sh1107.buffer_swap();
sh1107.draw();
// setup camera pins
let (cam_pdwn_bnk, cam_pdwn_pin) = cramium_hal::board::setup_ov2640_pins(&iox);
// disable camera powerdown
iox.set_gpio_pin_value(cam_pdwn_bnk, cam_pdwn_pin, cramium_hal::iox::IoxValue::Low);
udma_global.udma_clock_config(PeriphId::Cam, true);
// this is safe because we turned on the clocks before calling it
let mut cam = unsafe { cramium_hal::ov2640::Ov2640::new().expect("couldn't allocate camera") };
tt.sleep_ms(100).ok();
let (pid, mid) = cam.read_id(&mut i2c);
log::info!("Camera pid {:x}, mid {:x}", pid, mid);
cam.init(&mut i2c, cramium_hal::ov2640::Resolution::Res320x240);
cam.poke(&mut i2c, 0xFF, 0x00);
cam.poke(&mut i2c, 0xDA, 0x01); // YUV LE
tt.sleep_ms(1).ok();
let (cols, _rows) = cam.resolution();
let border = (cols - QR_WIDTH) / 2;
cam.set_slicing((border, 0), (cols - border, QR_HEIGHT));
log::info!("320x240 resolution setup with 256x240 slicing");
let mut frames = 0;
let mut frame = [0u8; QR_WIDTH * QR_HEIGHT];
// while iox.get_gpio_pin_value(IoxPort::PB, 9) == IoxValue::High {}
loop {
cam.capture_async();
let mut candidates = Vec::<Point>::new();
log::info!("------------- SEARCH -----------");
let finder_width = qr::find_finders(&mut candidates, &frame, BW_THRESH, QR_WIDTH) as isize;
const CROSSHAIR_LEN: isize = 3;
if candidates.len() == 3 {
gfx::msg(&mut sh1107, "Aligning...", Point::new(0, 0), Mono::White.into(), Mono::Black.into());
for c in candidates.iter() {
log::info!("****** candidate: {}, {} ******", c.x, c.y);
// remap image to screen coordinates (it's 2:1)
let mut c_screen = *c / 2;
// flip coordinates to match the camera data
c_screen = Point::new(c_screen.x, sh1107.dimensions().y - 1 - c_screen.y);
qr::draw_crosshair(&mut sh1107, c_screen);
}
if let Some(mut qr_corners) = qr::QrCorners::from_finders(
&candidates.try_into().unwrap(),
Point::new(QR_WIDTH as isize, QR_HEIGHT as isize),
// add a search margin on the finder width
(finder_width + (qr::FINDER_SEARCH_MARGIN * finder_width) / (1 + 1 + 3 + 1 + 1)) as usize,
) {
let dims = Point::new(QR_WIDTH as isize, QR_HEIGHT as isize);
let mut il = qr::ImageRoi::new(&mut frame, dims, BW_THRESH);
let (src, dst) = qr_corners.mapping(&mut il, qr::HOMOGRAPHY_MARGIN);
for s in src.iter() {
if let Some(p) = s {
log::info!("src {:?}", p);
qr::draw_crosshair(&mut sh1107, *p / 2);
}
}
for d in dst.iter() {
if let Some(p) = d {
log::info!("dst {:?}", p);
qr::draw_crosshair(&mut sh1107, *p / 2);
}
}
let mut src_f: [(f32, f32); 4] = [(0.0, 0.0); 4];
let mut dst_f: [(f32, f32); 4] = [(0.0, 0.0); 4];
let mut all_found = true;
for (s, s_f32) in src.iter().zip(src_f.iter_mut()) {
if let Some(p) = s {
*s_f32 = p.to_f32();
} else {
all_found = false;
}
}
for (d, d_f32) in dst.iter().zip(dst_f.iter_mut()) {
if let Some(p) = d {
*d_f32 = p.to_f32();
} else {
all_found = false;
}
}
if all_found {
if let Some(h) = homography::find_homography(src_f, dst_f) {
if let Some(h_inv) = h.try_inverse() {
log::info!("{:?}", h_inv);
let h_inv_fp = homography::matrix3_to_fixp(h_inv);
log::info!("{:?}", h_inv_fp);
// apply homography to generate a new buffer for processing
let mut aligned = [0u8; QR_WIDTH * QR_HEIGHT];
// iterate through pixels and apply homography
for y in 0..dims.y {
for x in 0..dims.x {
let (x_src, y_src) =
homography::apply_fixp_homography(&h_inv_fp, (x as i32, y as i32));
if (x_src as i32 >= 0)
&& ((x_src as i32) < dims.x as i32)
&& (y_src as i32 >= 0)
&& ((y_src as i32) < dims.y as i32)
{
// println!("{},{} -> {},{}", x_src as i32, y_src as i32, x, y);
aligned[QR_WIDTH * y as usize + x as usize] =
frame[QR_WIDTH * y_src as usize + x_src as usize];
} else {
aligned[QR_WIDTH * y as usize + x as usize] = 255;
}
}
}
blit_to_display(&mut sh1107, &aligned, true);
let mut search_img =
rqrr::PreparedImage::prepare_from_greyscale(QR_WIDTH, QR_HEIGHT, |x, y| {
aligned[y * QR_WIDTH + x]
});
let grids = search_img.detect_grids();
log::info!("grids len {}", grids.len());
let rawdata = grids[0].get_raw_data();
match rawdata {
Ok((md, rd)) => {
log::info!("{:?}, {}:{:x?}", md, rd.len, &rd.data[..(rd.len / 8) + 1]);
}
Err(e) => {
log::info!("Error: {:?}", e);
}
}
log::info!("{:?}", grids[0].decode());
}
}
}
}
} else {
// blit fb to sh1107
blit_to_display(&mut sh1107, &frame, true);
gfx::msg(&mut sh1107, "Searching...", Point::new(0, 0), Mono::White.into(), Mono::Black.into());
}
// swap the double buffer and update to the display
sh1107.buffer_swap();
sh1107.draw();
// clear the front buffer
sh1107.clear();
// wait for the transfer to finish
cam.capture_await(true);
let fb: &[u32] = cam.rx_buf();
// fb is non-cacheable, slow memory. If we stride through it in u16 chunks, we end
// up fetching each location *twice*, because the native width of the bus is a u32
// Stride through the slice as a u32, allowing us to make the most out of each slow
// read from IFRAM, and unpack the values into fast SRAM.
for (&u32src, u8dest) in fb.iter().zip(frame.chunks_mut(2)) {
u8dest[0] = (u32src & 0xff) as u8;
u8dest[1] = ((u32src >> 16) & 0xff) as u8;
}
frames += 1;
}
}

View file

@ -0,0 +1,824 @@
use core::cell::RefCell;
use core::convert::TryFrom;
use core::ops::{BitXor, Not};
use cramium_hal::minigfx::{ColorNative, FrameBuffer, Point};
use crate::gfx;
const SEQ_LEN: usize = 5;
// use fixed point for the maths. This defines where we fix the point at.
const SEQ_FP_SHIFT: usize = 4;
// searching for a 1:1:3:1:1 black-white-black-white-black pattern
// upper/lower thresholds for recognizing a "1" in the ratio
const LOWER_1: usize = (1 << SEQ_FP_SHIFT) / 2; // "0.5"
const UPPER_1: usize = 2 << SEQ_FP_SHIFT;
// upper/lower thresholds for recognizing a "3" in the ratio
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 {
log::info!("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]);
}
}
log::info!("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
pub run: usize,
/// the position
pub pos: usize,
/// the luminance of the pixels in the run leading up to the current position
pub color: Color,
}
#[derive(Copy, Clone, PartialEq, Eq, Debug)]
pub enum Color {
Black,
White,
}
impl Color {
pub fn from(luminance: u8, thresh: u8) -> Self {
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 }
}
impl Not for Color {
type Output = Color;
fn not(self) -> Self::Output {
match self {
Color::Black => Color::White,
Color::White => Color::Black,
}
}
}
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 {
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 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
}
}
}
struct SeqBuffer {
buffer: [Option<FinderSeq>; 5],
start: usize,
end: usize,
count: usize,
}
impl SeqBuffer {
pub const fn new() -> Self { SeqBuffer { buffer: [None; SEQ_LEN], start: 0, end: 0, count: 0 } }
pub fn clear(&mut self) {
self.buffer = [None; SEQ_LEN];
self.start = 0;
self.end = 0;
self.count = 0;
}
pub fn push(&mut self, item: FinderSeq) {
self.buffer[self.end] = Some(item);
self.end = (self.end + 1) % SEQ_LEN;
if self.count < SEQ_LEN {
self.count += 1;
} else {
self.start = (self.start + 1) % SEQ_LEN; // Overwrite the oldest element
}
}
/// Don't use options because we iterate through the list once to extract the
/// correct ordering, and we know how many valid items are in there. This is less
/// idiomatic, but it saves us the computational overhead of constantly iterating
/// through to test for None when we know how many there are in the first place,
/// and it saves the lexical verbosity of `unwrap()` everywhere (and unwrap does
/// actually have a computational cost, it is not a zero-cost abstraction).
pub fn retrieve(&self, output: &mut [FinderSeq; SEQ_LEN]) -> usize {
let mut copied_count = 0;
for i in 0..self.count {
let index = (self.start + i) % SEQ_LEN;
output[i] = self.buffer[index].expect("circular buffer logic error").clone();
copied_count += 1;
}
for i in copied_count..5 {
output[i] = FinderSeq::default(); // Clear the remaining elements in the output if any
}
copied_count
}
// 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 {
// only look for sequences that start with black
if seq[0].color == Color::Black {
let denom = seq[0].run;
ratios[0] = 1 << SEQ_FP_SHIFT; // by definition
for (ratio, s) in ratios[1..].iter_mut().zip(seq[1..].iter()) {
*ratio = (s.run << SEQ_FP_SHIFT) / denom;
}
if ratios[1] >= LOWER_1
&& ratios[1] <= UPPER_1
&& ratios[2] >= LOWER_3
&& ratios[2] <= UPPER_3
&& ratios[3] >= LOWER_1
&& ratios[3] <= UPPER_1
&& ratios[4] >= LOWER_1
&& ratios[4] <= UPPER_1
{
// log::info!(" seq {:?}", &seq);
// log::info!(" ratios {:?}", &ratios);
Some((seq[2].pos - seq[2].run / 2 - 1, seq.iter().map(|s| s.run).sum()))
} else {
None
}
} else {
None
}
} else {
None
}
}
}
/// Returns the average width of the finder regions found.
pub fn find_finders(candidates: &mut Vec<Point>, image: &[u8], thresh: u8, stride: usize) -> usize {
let mut row_candidates = Vec::<Point>::new();
let mut seq_buffer = SeqBuffer::new();
// log::info!("ROWSROWSROWSROWS");
for (y, line) in image.chunks(stride).enumerate() {
seq_buffer.clear();
let mut last_color = Color::from(line[0], thresh);
let mut run_length = 1;
for (x_minus1, &pix) in line[1..].iter().enumerate() {
let pix = Color::from(pix, thresh);
if pix == last_color {
run_length += 1;
} else {
seq_buffer.push(FinderSeq { run: run_length, pos: x_minus1 + 1, color: last_color });
last_color = pix;
run_length = 1;
// manage the sequence index
if let Some((pos, _width)) = seq_buffer.search() {
// log::info!("row candidate {}, {}", pos, y);
row_candidates.push(Point::new(pos as _, y as _));
}
}
}
}
// log::info!("CCCCCCCCCCCCCCCC");
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 {
run_length += 1;
} else {
seq_buffer.push(FinderSeq { run: run_length, pos: y_minus1 + 1, color: last_color });
last_color = pix;
run_length = 1;
if let Some((pos, width)) = seq_buffer.search() {
let search_point = Point::new(x as _, pos as _);
// log::info!("col candidate {}, {}", x, pos);
// now cross the candidate against row candidates; only report those that match
for &rc in row_candidates.iter().filter(|&&p| p == search_point) {
candidates.push(rc);
candidate_width += width;
}
}
}
}
}
if candidates.len() != 0 { candidate_width / candidates.len() } else { 0 }
}