fix homography calculation + test
This commit is contained in:
parent
ee9e3f1da0
commit
3d53982e72
@ -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,
|
||||||
|
@ -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() }]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
@ -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 })
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user