mirror of
https://github.com/saymrwulf/xous-core.git
synced 2026-11-15 08:30:02 +00:00
add homography searching routines
This commit is contained in:
parent
77a8aa88b5
commit
f6d6db566a
1 changed files with 85 additions and 0 deletions
85
loader/src/platform/cramium/homography.rs
Normal file
85
loader/src/platform/cramium/homography.rs
Normal file
|
|
@ -0,0 +1,85 @@
|
|||
use nalgebra::{Matrix3, SVD, Vector3};
|
||||
|
||||
pub type Matrix8x9<T> = nalgebra::Matrix<T, nalgebra::U8, nalgebra::U9, nalgebra::ArrayStorage<T, 8, 9>>;
|
||||
|
||||
// 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<Matrix3<f32>> {
|
||||
// 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<f32>) -> Matrix3<i32> {
|
||||
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<i32>, 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)
|
||||
}
|
||||
Loading…
Reference in a new issue