From 2692d374c64c3222684530c67aad7ea0a0671c73 Mon Sep 17 00:00:00 2001 From: bunnie Date: Thu, 7 Nov 2024 16:14:29 +0800 Subject: [PATCH] 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. --- services/bao-video/Cargo.toml | 27 + services/bao-video/src/gfx.rs | 141 +++++ services/bao-video/src/homography.rs | 85 +++ services/bao-video/src/main.rs | 233 ++++++++ services/bao-video/src/qr.rs | 824 +++++++++++++++++++++++++++ 5 files changed, 1310 insertions(+) create mode 100644 services/bao-video/Cargo.toml create mode 100644 services/bao-video/src/gfx.rs create mode 100644 services/bao-video/src/homography.rs create mode 100644 services/bao-video/src/main.rs create mode 100644 services/bao-video/src/qr.rs diff --git a/services/bao-video/Cargo.toml b/services/bao-video/Cargo.toml new file mode 100644 index 000000000..6add7c00f --- /dev/null +++ b/services/bao-video/Cargo.toml @@ -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 = [] diff --git a/services/bao-video/src/gfx.rs b/services/bao-video/src/gfx.rs new file mode 100644 index 000000000..2e3759efd --- /dev/null +++ b/services/bao-video/src/gfx.rs @@ -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, 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; + } + } +} diff --git a/services/bao-video/src/homography.rs b/services/bao-video/src/homography.rs new file mode 100644 index 000000000..a1c8cb7eb --- /dev/null +++ b/services/bao-video/src/homography.rs @@ -0,0 +1,85 @@ +use nalgebra::{Matrix3, SVD}; + +pub type Matrix8x9 = nalgebra::Matrix>; + +// 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> { + // 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) -> Matrix3 { + 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, 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) +} diff --git a/services/bao-video/src/main.rs b/services/bao-video/src/main.rs new file mode 100644 index 000000000..45bb6a50b --- /dev/null +++ b/services/bao-video/src/main.rs @@ -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::::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; + } +} diff --git a/services/bao-video/src/qr.rs b/services/bao-video/src/qr.rs new file mode 100644 index 000000000..5084a93b2 --- /dev/null +++ b/services/bao-video/src/qr.rs @@ -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.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), + Down(core::iter::Rev>), +} + +impl Iterator for DirectionalRange { + type Item = usize; + + fn next(&mut self) -> Option { + 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 { + 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, + 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 { + 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::() == 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 { 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; 4], [Option; 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 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 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 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 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 for Direction { + type Error = &'static str; + + fn try_from(value: usize) -> Result { + 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, + col_iter_start: usize, + col_iter_row_index: RefCell, + iter_col: RefCell, +} +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 { + 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 { + 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 { + 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 { + 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; 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, image: &[u8], thresh: u8, stride: usize) -> usize { + let mut row_candidates = Vec::::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 } +}