mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-17 08:40:52 +00:00
integrate new qr finding code
This commit is contained in:
parent
16c9b2c7e2
commit
08f7252791
1 changed files with 163 additions and 114 deletions
|
|
@ -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<Point>; 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<Point> = None;
|
||||
let mut tr: Option<Point> = 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<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() {
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue