mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-21 09:14:34 +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::*;
|
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,142 +433,191 @@ 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,
|
||||||
if c.x > QR_WIDTH as isize / 2 && c.y < QR_HEIGHT as isize / 2 {
|
);
|
||||||
tr = Some(Point::new(c.x, c.y));
|
// 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() {
|
let mut src_f: [(f32, f32); 4] = [(0.0, 0.0); 4];
|
||||||
if y & 1 == 0 {
|
let mut dst_f: [(f32, f32); 4] = [(0.0, 0.0); 4];
|
||||||
for (x, &pixval) in row.iter().enumerate() {
|
let mut all_found = true;
|
||||||
if x & 1 == 0 {
|
for (s, s_f32) in src.iter().zip(src_f.iter_mut()) {
|
||||||
if x < sh1107.dimensions().x as usize * 2
|
if let Some(p) = s {
|
||||||
&& y < sh1107.dimensions().y as usize * 2
|
*s_f32 = p.to_f32();
|
||||||
- (gfx::CHAR_HEIGHT as usize + 1) * 2
|
} 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;
|
// println!("{},{} -> {},{}", x_src as i32, y_src as i32, x, y);
|
||||||
if luminance > BW_THRESH {
|
aligned[QR_WIDTH * y as usize + x as usize] =
|
||||||
// flip on y to adjust for sensor orientation. Lower left is (0,
|
frame[QR_WIDTH * y_src as usize + x_src as usize];
|
||||||
// 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 {
|
} else {
|
||||||
break;
|
aligned[QR_WIDTH * y as usize + x as usize] = 255;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
gfx::msg(
|
// blit aligned to sh1107
|
||||||
&mut sh1107,
|
for (y, row) in aligned.chunks(QR_WIDTH).enumerate() {
|
||||||
"Aligning...",
|
if y & 1 == 0 {
|
||||||
Point::new(0, 0),
|
for (x, &pixval) in row.iter().enumerate() {
|
||||||
Mono::White.into(),
|
if x & 1 == 0 {
|
||||||
Mono::Black.into(),
|
if x < sh1107.dimensions().x as usize * 2
|
||||||
);
|
&& y < sh1107.dimensions().y as usize * 2
|
||||||
|
- (gfx::CHAR_HEIGHT as usize + 1) * 2
|
||||||
if udma_uart.read_async(&mut c) != 0 {
|
{
|
||||||
if c == ' ' as u32 as u8 {
|
let luminance = pixval & 0xff;
|
||||||
crate::println!("Dumping...");
|
if luminance > BW_THRESH {
|
||||||
|
// flip on y to adjust for sensor orientation. Lower left
|
||||||
crate::println!("frame {}", frames);
|
// is (0, 0)
|
||||||
udma_uart.write("------------------ divider ------------------\n\r".as_bytes());
|
// on the display.
|
||||||
let hex_to_ascii = [
|
sh1107.put_pixel(
|
||||||
'0' as u32 as u8,
|
Point::new(
|
||||||
'1' as u32 as u8,
|
x as isize / 2,
|
||||||
'2' as u32 as u8,
|
(sh1107.dimensions().y - 1) - (y as isize / 2),
|
||||||
'3' as u32 as u8,
|
),
|
||||||
'4' as u32 as u8,
|
Mono::White.into(),
|
||||||
'5' as u32 as u8,
|
);
|
||||||
'6' as u32 as u8,
|
} else {
|
||||||
'7' as u32 as u8,
|
sh1107.put_pixel(
|
||||||
'8' as u32 as u8,
|
Point::new(
|
||||||
'9' as u32 as u8,
|
x as isize / 2,
|
||||||
'A' as u32 as u8,
|
(sh1107.dimensions().y - 1) - (y as isize / 2),
|
||||||
'B' as u32 as u8,
|
),
|
||||||
'C' as u32 as u8,
|
Mono::Black.into(),
|
||||||
'D' as u32 as u8,
|
);
|
||||||
'E' as u32 as u8,
|
}
|
||||||
'F' as u32 as u8,
|
} else {
|
||||||
];
|
break;
|
||||||
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;
|
|
||||||
|
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