fix homography calculation + test

This commit is contained in:
Marc Planard 2023-07-01 17:14:01 +02:00
parent ee9e3f1da0
commit 3d53982e72
3 changed files with 62 additions and 15 deletions

View File

@ -1,11 +1,11 @@
#[derive(Debug,Clone,Copy,Default)] #[derive(Debug,Clone,Copy,Default,PartialEq)]
pub struct Point { pub struct Point {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
pub color: Color pub color: Color
} }
#[derive(Debug,Clone,Copy,Default)] #[derive(Debug,Clone,Copy,Default,PartialEq)]
pub struct Color { pub struct Color {
r: u8, r: u8,
g: u8, g: u8,

View File

@ -1,9 +1,10 @@
use crate::transformer::Transformers; use crate::transformer::Transformers;
use crate::point::Point; use crate::point::Point;
use crate::worldstate::WorldState; use crate::worldstate::{WorldState,EDH};
use serde::{Serialize,Deserialize}; use serde::{Serialize,Deserialize};
use log::info; use log::info;
use nalgebra::Matrix3; use nalgebra::Matrix3;
use nalgebra::Matrix1x3;
/// Homography /// Homography
@ -12,25 +13,63 @@ pub struct Homography {}
impl Transformers for Homography { impl Transformers for Homography {
fn apply(&self, point_list: &[Point], ws: &WorldState) -> Vec<Point> { fn apply(&self, point_list: &[Point], ws: &WorldState) -> Vec<Point> {
let m = ws.edh.matrix; let edh : &EDH = &ws.edh;
point_list.iter() point_list.iter()
.map(| point | { .map(| point | {
let p = Matrix1x3::new(point.x, point.y, 1.0);
// THIS IS CERTAINLY ALL WRONG! NEEDS DEBUGING!!! let p = p * edh.matrix;
let new_p = Point { x: p[0] / p[2], y: p[1] / p[2], ..*point };
let p = Matrix3::new(point.x, point.y, 1.0, dbg!("{:?} => {:?}", point, new_p);
1.0, 1.0, 1.0,
1.0, 1.0, 1.0);
let dot = p.tr_dot(&m);
let new_p = Point { x: point.x / dot, y: point.y / dot,
..*point
};
info!("{:?} => {:?}", point, new_p);
new_p new_p
}) })
.collect() .collect()
} }
} }
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_homography_identity() {
let p0 = Point { x: -1500.0, y: 1500.0, ..Point::default() };
let edh = EDH::new(vec![
vec![ 1.0, 0.0, 0.0 ],
vec![ 0.0, 1.0, 0.0 ],
vec![ 0.0, 0.0, 1.0 ]
]).unwrap();
let ws = WorldState { edh : edh, ..WorldState::default() };
let homography = Homography{};
let result = homography.apply(&[p0], &ws);
assert_eq!(result, vec![Point { x: -1500.0,
y: 1500.0,
..Point::default() }]);
}
#[test]
fn test_homography_rotation() {
let p0 = Point { x: -1500.0, y: 1500.0, ..Point::default() };
let edh = EDH::new(vec![
vec![ 1.24107321e-03, 1.00500127e-03, 7.15439347e-01],
vec![-9.93223912e-04, 1.22652939e-03,-6.98671238e-01],
vec![ 1.06017142e-17,-4.69459541e-17, 3.32700590e-05]
]).unwrap();
let ws = WorldState { edh : edh, ..WorldState::default() };
let homography = Homography{};
let result = homography.apply(&[p0], &ws);
assert_eq!(result, vec![Point { x: 10860.557,
y: 79078.87,
..Point::default() }]);
}
}

View File

@ -18,9 +18,17 @@ impl EDH {
// //
// [FIX] Not sure of the order, if is it's vec[x][y] or vec[y][x] ... // [FIX] Not sure of the order, if is it's vec[x][y] or vec[y][x] ...
// //
/*
let matrix = Matrix3::new(vec[0][0], vec[0][1], vec[0][2], let matrix = Matrix3::new(vec[0][0], vec[0][1], vec[0][2],
vec[1][0], vec[1][1], vec[1][2], vec[1][0], vec[1][1], vec[1][2],
vec[2][0], vec[2][1], vec[2][2]); vec[2][0], vec[2][1], vec[2][2]);
*/
// this is the matrix already transposed.
let matrix = Matrix3::new(vec[0][0], vec[1][0], vec[2][0],
vec[0][1], vec[1][1], vec[2][1],
vec[0][2], vec[1][2], vec[2][2]);
Ok(EDH { matrix }) Ok(EDH { matrix })
} }
} }