integrate new qr finding code

This commit is contained in:
bunnie 2024-11-05 01:44:53 +08:00
parent 16c9b2c7e2
commit 08f7252791

View file

@ -5,7 +5,7 @@ use cramium_hal::udma::UdmaGlobalConfig;
use utralib::generated::*; use utralib::generated::*;
#[cfg(feature = "qr")] #[cfg(feature = "qr")]
use crate::platform::cramium::qr; use crate::platform::cramium::{homography, qr};
// Notes about the reset vector location // Notes about the reset vector location
// This can be set using fuses in the IFR (also called 'info') region // 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")] #[cfg(feature = "cramium-soc")]
pub fn early_init() -> u32 { 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. // 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 // 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, // make this nicer and more abstract with register meanings down the road, if necessary,
@ -433,61 +433,105 @@ pub fn early_init() -> u32 {
let mut candidates: [Option<Point>; 64] = [None; 64]; let mut candidates: [Option<Point>; 64] = [None; 64];
crate::println!("\n\r------------- SEARCH -----------"); 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; const CROSSHAIR_LEN: isize = 3;
let mut candidates_found = 0; let mut candidates_found = 0;
let mut candidate3 = [Point::new(0, 0); 3];
for candidate in candidates.iter() { for candidate in candidates.iter() {
if let Some(c) = candidate { if let Some(c) = candidate {
if candidates_found < candidate3.len() {
candidate3[candidates_found] = *c;
}
candidates_found += 1; candidates_found += 1;
crate::println!("****** candidate: {}, {} ******", c.x, c.y); crate::println!("****** candidate: {}, {} ******", c.x, c.y);
// remap image to screen coordinates (it's 2:1) // remap image to screen coordinates (it's 2:1)
let mut c_screen = *c / 2; let mut c_screen = *c / 2;
// flip coordinates to match the camera data // flip coordinates to match the camera data
c_screen = Point::new(c_screen.x, sh1107.dimensions().y - 1 - c_screen.y); c_screen = Point::new(c_screen.x, sh1107.dimensions().y - 1 - c_screen.y);
// vertical cross hair draw_crosshair(&mut sh1107, c_screen);
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,
);
} }
} }
use crate::platform::cramium::qr::*;
if candidates_found == 3 { if candidates_found == 3 {
let mut tl: Option<Point> = None; let maybe_qr_corners = QrCorners::from_finders(
let mut tr: Option<Point> = None; &candidate3,
for candidate in candidates.iter() { Point::new(QR_WIDTH as isize, QR_HEIGHT as isize),
if let Some(c) = candidate { // add a search margin on the finder width
if c.x < QR_WIDTH as isize / 2 && c.y < QR_HEIGHT as isize / 2 { (finder_width
tl = Some(Point::new(c.x, c.y)); + (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;
} }
if c.x > QR_WIDTH as isize / 2 && c.y < QR_HEIGHT as isize / 2 { let mut qr_corners = maybe_qr_corners.unwrap();
tr = Some(Point::new(c.x, c.y));
}
}
}
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<u8>; 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() { 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);
}
}
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)
{
// 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 aligned to sh1107
for (y, row) in aligned.chunks(QR_WIDTH).enumerate() {
if y & 1 == 0 { if y & 1 == 0 {
for (x, &pixval) in row.iter().enumerate() { for (x, &pixval) in row.iter().enumerate() {
if x & 1 == 0 { if x & 1 == 0 {
@ -497,8 +541,9 @@ pub fn early_init() -> u32 {
{ {
let luminance = pixval & 0xff; let luminance = pixval & 0xff;
if luminance > BW_THRESH { if luminance > BW_THRESH {
// flip on y to adjust for sensor orientation. Lower left is (0, // flip on y to adjust for sensor orientation. Lower left
// 0) on the display. // is (0, 0)
// on the display.
sh1107.put_pixel( sh1107.put_pixel(
Point::new( Point::new(
x as isize / 2, x as isize / 2,
@ -536,7 +581,9 @@ pub fn early_init() -> u32 {
crate::println!("Dumping..."); crate::println!("Dumping...");
crate::println!("frame {}", frames); crate::println!("frame {}", frames);
udma_uart.write("------------------ divider ------------------\n\r".as_bytes()); udma_uart.write(
"------------------ divider ------------------\n\r".as_bytes(),
);
let hex_to_ascii = [ let hex_to_ascii = [
'0' as u32 as u8, '0' as u32 as u8,
'1' as u32 as u8, '1' as u32 as u8,
@ -572,6 +619,8 @@ pub fn early_init() -> u32 {
} }
} }
} }
}
}
} else { } else {
gfx::msg( gfx::msg(
&mut sh1107, &mut sh1107,