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 } +}