From 08f725279108a36b90b9c72488b9025ac637fccc Mon Sep 17 00:00:00 2001 From: bunnie Date: Tue, 5 Nov 2024 01:44:53 +0800 Subject: [PATCH] integrate new qr finding code --- loader/src/platform/cramium/cramium.rs | 277 +++++++++++++++---------- 1 file changed, 163 insertions(+), 114 deletions(-) diff --git a/loader/src/platform/cramium/cramium.rs b/loader/src/platform/cramium/cramium.rs index 0410dadf3..96bbf0f6f 100644 --- a/loader/src/platform/cramium/cramium.rs +++ b/loader/src/platform/cramium/cramium.rs @@ -5,7 +5,7 @@ use cramium_hal::udma::UdmaGlobalConfig; use utralib::generated::*; #[cfg(feature = "qr")] -use crate::platform::cramium::qr; +use crate::platform::cramium::{homography, qr}; // Notes about the reset vector location // This can be set using fuses in the IFR (also called 'info') region @@ -76,7 +76,7 @@ pub const KERNEL_OFFSET: usize = 0x5_0000; #[cfg(feature = "cramium-soc")] pub fn early_init() -> u32 { - use cramium_hal::{axp2101::WhichLdo, iox::IoxValue, sh1107::Mono, udma::Udma}; + use cramium_hal::{axp2101::WhichLdo, iox::IoxValue, minigfx::ColorNative, sh1107::Mono, udma::Udma}; // Set up the initial clocks. This is done as a "poke array" into a table of addresses. // Why? because this is actually how it's done for the chip verification code. We can // make this nicer and more abstract with register meanings down the road, if necessary, @@ -433,142 +433,191 @@ pub fn early_init() -> u32 { let mut candidates: [Option; 64] = [None; 64]; crate::println!("\n\r------------- SEARCH -----------"); - qr::find_finders(&mut candidates, &frame, BW_THRESH, QR_WIDTH); + let finder_width = qr::find_finders(&mut candidates, &frame, BW_THRESH, QR_WIDTH) as isize; const CROSSHAIR_LEN: isize = 3; let mut candidates_found = 0; + let mut candidate3 = [Point::new(0, 0); 3]; for candidate in candidates.iter() { if let Some(c) = candidate { + if candidates_found < candidate3.len() { + candidate3[candidates_found] = *c; + } candidates_found += 1; crate::println!("****** 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); - // vertical cross hair - gfx::line( - &mut sh1107, - Line::new( - c_screen + Point::new(0, CROSSHAIR_LEN), - c_screen - Point::new(0, CROSSHAIR_LEN), - ), - None, - true, - ); - // horizontal cross hair - gfx::line( - &mut sh1107, - Line::new( - c_screen + Point::new(CROSSHAIR_LEN, 0), - c_screen - Point::new(CROSSHAIR_LEN, 0), - ), - None, - true, - ); + draw_crosshair(&mut sh1107, c_screen); } } + use crate::platform::cramium::qr::*; if candidates_found == 3 { - let mut tl: Option = None; - let mut tr: Option = None; - for candidate in candidates.iter() { - if let Some(c) = candidate { - if c.x < QR_WIDTH as isize / 2 && c.y < QR_HEIGHT as isize / 2 { - tl = Some(Point::new(c.x, c.y)); - } - if c.x > QR_WIDTH as isize / 2 && c.y < QR_HEIGHT as isize / 2 { - tr = Some(Point::new(c.x, c.y)); - } + let maybe_qr_corners = QrCorners::from_finders( + &candidate3, + Point::new(QR_WIDTH as isize, QR_HEIGHT as isize), + // add a search margin on the finder width + (finder_width + + (crate::platform::cramium::qr::FINDER_SEARCH_MARGIN * finder_width) + / (1 + 1 + 3 + 1 + 1)) as usize, + ); + // just doing this to avoid nesting another if level out in the test code; make this better! + if maybe_qr_corners.is_none() { + continue; + } + let mut qr_corners = maybe_qr_corners.unwrap(); + + let dims = Point::new(QR_WIDTH as isize, QR_HEIGHT as isize); + let mut il = ImageRoi::new(&mut frame, dims, BW_THRESH); + let (src, dst) = qr_corners.mapping(&mut il, crate::platform::cramium::qr::HOMOGRAPHY_MARGIN); + for s in src.iter() { + if let Some(p) = s { + crate::println!("src {:?}", p); + draw_crosshair(&mut sh1107, *p / 2); + } + } + for d in dst.iter() { + if let Some(p) = d { + crate::println!("dst {:?}", p); + draw_crosshair(&mut sh1107, *p / 2); } } - if tl.is_some() && tr.is_some() { - let at = - qr::AffineTransform::from_coordinates(tl.unwrap(), tr.unwrap(), QR_WIDTH, QR_HEIGHT); - let mut rotated_uninit: [core::mem::MaybeUninit; QR_WIDTH * QR_HEIGHT] = - unsafe { core::mem::MaybeUninit::uninit().assume_init() }; - at.transform(&frame, &mut rotated_uninit); - let rotated: [u8; QR_WIDTH * QR_HEIGHT] = unsafe { core::mem::transmute(rotated_uninit) }; - for (y, row) in rotated.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 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 { + use crate::platform::cramium::homography::*; + if let Some(h) = find_homography(src_f, dst_f) { + if let Some(h_inv) = h.try_inverse() { + crate::println!("{:?}", h_inv); + let h_inv_fp = matrix3_to_fixp(h_inv); + crate::println!("{:?}", 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) = + 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) { - 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 { - sh1107.put_pixel( - Point::new( - x as isize / 2, - (sh1107.dimensions().y - 1) - (y as isize / 2), - ), - Mono::Black.into(), - ); - } + // 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 { - break; + aligned[QR_WIDTH * y as usize + x as usize] = 255; } } } - } - } - gfx::msg( - &mut sh1107, - "Aligning...", - Point::new(0, 0), - Mono::White.into(), - Mono::Black.into(), - ); - - if udma_uart.read_async(&mut c) != 0 { - if c == ' ' as u32 as u8 { - crate::println!("Dumping..."); - - crate::println!("frame {}", frames); - udma_uart.write("------------------ divider ------------------\n\r".as_bytes()); - let hex_to_ascii = [ - '0' as u32 as u8, - '1' as u32 as u8, - '2' as u32 as u8, - '3' as u32 as u8, - '4' as u32 as u8, - '5' as u32 as u8, - '6' as u32 as u8, - '7' as u32 as u8, - '8' as u32 as u8, - '9' as u32 as u8, - 'A' as u32 as u8, - 'B' as u32 as u8, - 'C' as u32 as u8, - 'D' as u32 as u8, - 'E' as u32 as u8, - 'F' as u32 as u8, - ]; - for line in frame.chunks(32) { - let mut output = [0u8; 3 * 32 + 3]; - for (i, &b) in line.iter().enumerate() { - output[i * 3 + 0] = hex_to_ascii[(b >> 4) as usize]; - output[i * 3 + 1] = hex_to_ascii[(b & 0xF) as usize]; - output[i * 3 + 2] = ',' as u32 as u8; + // blit aligned to sh1107 + for (y, row) in aligned.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 { + sh1107.put_pixel( + Point::new( + x as isize / 2, + (sh1107.dimensions().y - 1) - (y as isize / 2), + ), + Mono::Black.into(), + ); + } + } else { + break; + } + } + } + } + } + + gfx::msg( + &mut sh1107, + "Aligning...", + Point::new(0, 0), + Mono::White.into(), + Mono::Black.into(), + ); + + if udma_uart.read_async(&mut c) != 0 { + if c == ' ' as u32 as u8 { + crate::println!("Dumping..."); + + crate::println!("frame {}", frames); + udma_uart.write( + "------------------ divider ------------------\n\r".as_bytes(), + ); + let hex_to_ascii = [ + '0' as u32 as u8, + '1' as u32 as u8, + '2' as u32 as u8, + '3' as u32 as u8, + '4' as u32 as u8, + '5' as u32 as u8, + '6' as u32 as u8, + '7' as u32 as u8, + '8' as u32 as u8, + '9' as u32 as u8, + 'A' as u32 as u8, + 'B' as u32 as u8, + 'C' as u32 as u8, + 'D' as u32 as u8, + 'E' as u32 as u8, + 'F' as u32 as u8, + ]; + for line in frame.chunks(32) { + let mut output = [0u8; 3 * 32 + 3]; + for (i, &b) in line.iter().enumerate() { + output[i * 3 + 0] = hex_to_ascii[(b >> 4) as usize]; + output[i * 3 + 1] = hex_to_ascii[(b & 0xF) as usize]; + output[i * 3 + 2] = ',' as u32 as u8; + } + output[3 * 32] = '\\' as u32 as u8; + output[3 * 32 + 1] = '\n' as u32 as u8; + output[3 * 32 + 2] = '\r' as u32 as u8; + udma_uart.write(&output); + } + // continue with boot + break; } - output[3 * 32] = '\\' as u32 as u8; - output[3 * 32 + 1] = '\n' as u32 as u8; - output[3 * 32 + 2] = '\r' as u32 as u8; - udma_uart.write(&output); } - // continue with boot - break; } } }