feat: adding segment detection for the laser
The feature need to be better but it's usable.
This commit is contained in:
parent
550021a014
commit
6145b585f4
4 changed files with 549 additions and 66 deletions
|
|
@ -1,5 +1,9 @@
|
|||
use std::collections::HashSet;
|
||||
use std::f64::consts::PI;
|
||||
|
||||
use super::Qualibration;
|
||||
use super::DEBUG;
|
||||
use crate::utils::Pt;
|
||||
//use opencv::prelude::MatTraitConst;
|
||||
use opencv::prelude::*; //MatTraitConst;
|
||||
|
||||
|
|
@ -8,6 +12,12 @@ use opencv::highgui::{self, create_trackbar, named_window, WINDOW_AUTOSIZE};
|
|||
use opencv::imgproc::{cvt_color, line, COLOR_BGR2GRAY};
|
||||
use opencv::Result;
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
enum Cnt {
|
||||
Beg(usize),
|
||||
End(usize),
|
||||
}
|
||||
|
||||
opencv::opencv_branch_4! {
|
||||
use opencv::imgproc::LINE_AA;
|
||||
}
|
||||
|
|
@ -18,7 +28,7 @@ opencv::not_opencv_branch_4! {
|
|||
use super::Treshold;
|
||||
const MAX_TRACKBAR: i32 = 255;
|
||||
|
||||
fn draw_histograme_dbg(
|
||||
pub fn draw_histograme_dbg(
|
||||
window_name: &str,
|
||||
histo: &Vec<f64>,
|
||||
(from, to): (usize, usize),
|
||||
|
|
@ -56,13 +66,18 @@ fn draw_histograme_dbg(
|
|||
Ok(())
|
||||
}
|
||||
|
||||
fn draw_histograme(window_name: &str, histo: &Vec<f64>) -> Result<()> {
|
||||
pub fn draw_histograme(window_name: &str, histo: &Vec<f64>) -> Result<()> {
|
||||
let v: VecN<f64, 4> = VecN::new(0., 0., 0., 255.);
|
||||
let color: VecN<f64, 4> = VecN::new(255., 255., 255., 255.);
|
||||
let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?;
|
||||
let mut img = Mat::new_rows_cols_with_default(
|
||||
histo.len() as i32 * 2,
|
||||
histo.len() as i32 * 2,
|
||||
CV_8UC3,
|
||||
v,
|
||||
)?;
|
||||
|
||||
let mut max = 0.;
|
||||
for i in 0..256 {
|
||||
for i in 0..(histo.len() - 1) {
|
||||
if histo[i] > max {
|
||||
max = histo[i];
|
||||
}
|
||||
|
|
@ -70,13 +85,15 @@ fn draw_histograme(window_name: &str, histo: &Vec<f64>) -> Result<()> {
|
|||
|
||||
let v_log = 10.;
|
||||
|
||||
for i in 0..255 {
|
||||
for i in 0..(histo.len() - 1) {
|
||||
let x1 = ((i + 0) * 2) as i32;
|
||||
let x2 = ((i + 1) * 2) as i32;
|
||||
let y1 =
|
||||
((histo[i + 0] as f64 + 1.).log(v_log) / (max as f64).log(v_log) * 2. * 256.) as i32;
|
||||
let y2 =
|
||||
((histo[i + 1] as f64 + 1.).log(v_log) / (max as f64).log(v_log) * 2. * 256.) as i32;
|
||||
let y1 = ((histo[i + 0] as f64 + 1.).log(v_log) / (max as f64).log(v_log)
|
||||
* 2.
|
||||
* histo.len() as f64) as i32;
|
||||
let y2 = ((histo[i + 1] as f64 + 1.).log(v_log) / (max as f64).log(v_log)
|
||||
* 2.
|
||||
* histo.len() as f64) as i32;
|
||||
let pt1 = OcvPoint::new(x1, y1);
|
||||
let pt2 = OcvPoint::new(x2, y2);
|
||||
line(&mut img, pt1, pt2, color, 1, LINE_AA, 0)?;
|
||||
|
|
@ -87,7 +104,7 @@ fn draw_histograme(window_name: &str, histo: &Vec<f64>) -> Result<()> {
|
|||
Ok(())
|
||||
}
|
||||
|
||||
fn draw_histograme_bgr(window_name: &str, histo: &Vec<Vec<f64>>) -> Result<()> {
|
||||
pub fn draw_histograme_bgr(window_name: &str, histo: &Vec<Vec<f64>>) -> Result<()> {
|
||||
let v: VecN<f64, 4> = VecN::new(0., 0., 0., 255.);
|
||||
let b: VecN<f64, 4> = VecN::new(255., 0., 0., 255.);
|
||||
let g: VecN<f64, 4> = VecN::new(0., 255., 0., 255.);
|
||||
|
|
@ -126,7 +143,7 @@ fn draw_histograme_bgr(window_name: &str, histo: &Vec<Vec<f64>>) -> Result<()> {
|
|||
Ok(())
|
||||
}
|
||||
|
||||
fn draw_histograme_bgr_tresh(
|
||||
pub fn draw_histograme_bgr_tresh(
|
||||
window_name: &str,
|
||||
histo: &Vec<Vec<f64>>,
|
||||
tresh: &Treshold,
|
||||
|
|
@ -213,6 +230,290 @@ pub fn is_same_frame(frame: &Mat, frame_prev: &Mat) -> Result<bool> {
|
|||
}
|
||||
}
|
||||
|
||||
// On cherche des segment regourper par ilot de point. chaque illot a une plage de valeur en y qui
|
||||
// lui est propre, aucun autre ilot aura des point dans une plage de valeurs d'un autre illot.
|
||||
pub fn get_vertical_segment(m: &Mat) -> Result<Vec<((f32, f32), (f32, f32))>> {
|
||||
// on va faire un histogram des point selon leur position en y
|
||||
// ca permetera des les differencier
|
||||
// on fait cette histo gramme pour connaitre ces plage de valeur en y
|
||||
let mut seg_pt = HashSet::from([]);
|
||||
let (cols, rows) = (m.cols(), m.rows());
|
||||
let mut histo_y = vec![0.; cols.max(rows) as usize];
|
||||
for j in 0..rows {
|
||||
for i in 0..cols {
|
||||
let v: &Point3_<u8> = m.at_2d(j, i)?;
|
||||
if v.x != 0 && v.y != 0 && v.z != 0 {
|
||||
seg_pt.insert((i, j));
|
||||
histo_y[j as usize] += 1.;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// on determine le debut et la fin de ces palge de l=valeur en y
|
||||
let mut histo_limit = vec![];
|
||||
for i in (0..(histo_y.len()-1)).rev() {
|
||||
if histo_y[i] != 0. && histo_y[i + 1] == 0. {
|
||||
histo_limit.push(Cnt::End(i));
|
||||
}
|
||||
if histo_y[i] == 0. && histo_y[i + 1] != 0. {
|
||||
histo_limit.push(Cnt::Beg(i + 1));
|
||||
}
|
||||
}
|
||||
let mut limits = vec![];
|
||||
for k in 0..(histo_limit.len() / 2) {
|
||||
if let (Cnt::Beg(a), Cnt::End(b)) = (histo_limit[2 * k + 1], histo_limit[2 * k]) {
|
||||
limits.push((a, b));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// on regroupe les point par illot.
|
||||
let mut segment_iland = vec![vec![]; limits.len()];
|
||||
for (x, y) in seg_pt {
|
||||
let id = get_id_groups(&limits, y as usize).unwrap();
|
||||
segment_iland[id].push((x, y));
|
||||
}
|
||||
|
||||
// on transforme chaque point en pt: (f32, f32) -> Pt
|
||||
// toujours avec la meme structure d'ilot.
|
||||
let segment_iland_pt: Vec<Vec<Pt>> = segment_iland
|
||||
.iter()
|
||||
.map(|iland| {
|
||||
iland
|
||||
.iter()
|
||||
.map(|(x, y)| Pt {
|
||||
x: *x as f64,
|
||||
y: *y as f64,
|
||||
})
|
||||
.collect()
|
||||
})
|
||||
.collect();
|
||||
|
||||
// Pour chaque ilot de pixel: on prend le centre, on cherche l'axe qui passe le plus au centre
|
||||
// de l'illot. Pour trouver cet axe, pour chaque pixel de l'ilot, on va calculer l'eccart au
|
||||
// carree avec cet axe. On selectionne l'axe qui a l'erreur la plus faible
|
||||
// TODO: peut etre un meileur algo de recheche de l'axe (dicotomie en partie)
|
||||
// En suite on tris ces pixel et on prend la moiter la plus haute et la moiter la plus basse
|
||||
// part raport a l'axe. On fait la mayenne des ces 2 groupe et on a les extremiter haute et
|
||||
// basse pour cet ilot de pixel. En suite on multiplie par 2 ce segement pour qui soit de la
|
||||
// taille de l'ilots.
|
||||
//
|
||||
// TODO: La selection de l'axe qui passe au centre de l'ilot pourrauiut aussi etre meilleur
|
||||
// au lieux d'utiliser l'arreur, on pourrait regarder la valeur absolue de la coordoner x la plus petit
|
||||
// DONE=> j'ai tester une autre methode mais il y a plus d'erreur... mais
|
||||
// l'orientation des segment est pas mal. En gros l'orientation de l'axe n'est pas
|
||||
// toujours la meme. C'est du a la fonction de tris. La fonction ne s'execute pas dans
|
||||
// le meme ordre sur les valeur, Et quand 2 valeurs sont identique, elle peuvent etre
|
||||
// inter changer.
|
||||
// TODO: La selection des pixel pour chaque illot pourrait etre ameliorer
|
||||
// En fait elle me va bien. C'est vrai que il ne sont pas ouf mais bon...
|
||||
let mut segments = vec![];
|
||||
for (i, iland) in segment_iland_pt.iter().enumerate() {
|
||||
let mut center = Pt{x: 0., y: 0.};
|
||||
for p in iland {
|
||||
center += *p;
|
||||
}
|
||||
center /= iland.len() as f64;
|
||||
|
||||
let max_deg = 360;
|
||||
let (mut err_min, mut rad_min, mut x_min) = (f64::MAX, 0., f64::MAX);
|
||||
let mut iland_min = vec![];
|
||||
for deg in 0..max_deg {
|
||||
let rad = (deg as f64) / (max_deg as f64) * PI * 2.;
|
||||
let y_axis = Pt{x: rad.sin(), y: rad.cos()};
|
||||
let x_axis = Pt{x: -y_axis.y, y: y_axis.x};
|
||||
let mut err = 0.;
|
||||
let mut tmp_iland = vec![];
|
||||
let mut x_abs_max = f64::MIN;
|
||||
for pt in iland {
|
||||
let mut p = *pt - center;
|
||||
p = Pt{x: p.cross(&x_axis), y: p.cross(&y_axis)};
|
||||
err += p.x * p.x;
|
||||
tmp_iland.push(p);
|
||||
if x_abs_max < p.x.abs(){
|
||||
x_abs_max = p.x.abs();
|
||||
}
|
||||
}
|
||||
if x_abs_max < x_min {
|
||||
x_min = x_abs_max;
|
||||
rad_min = rad;
|
||||
iland_min = tmp_iland;
|
||||
}
|
||||
//if err < err_min {
|
||||
// err_min = err;
|
||||
// rad_min = rad;
|
||||
// iland_min = tmp_iland;
|
||||
//}
|
||||
}
|
||||
iland_min.sort_by(|pta, ptb|{
|
||||
if pta.y < ptb.y {
|
||||
std::cmp::Ordering::Greater
|
||||
} else if pta.y == ptb.y {
|
||||
if pta.x.abs() < ptb.x.abs() {
|
||||
std::cmp::Ordering::Greater
|
||||
} else if pta.x.abs() == ptb.x.abs() {
|
||||
std::cmp::Ordering::Equal
|
||||
} else {
|
||||
std::cmp::Ordering::Less
|
||||
}
|
||||
} else {
|
||||
std::cmp::Ordering::Less
|
||||
}
|
||||
});
|
||||
let id1 = iland_min.len() / 2;
|
||||
let id2 = iland_min.len() - id1;
|
||||
let mean_up = Pt::mean(&iland_min[..id1]);
|
||||
let mean_down = Pt::mean(&iland_min[id2..]);
|
||||
//let mean_up = iland_min[0];
|
||||
//let mean_down = iland_min.last().unwrap();
|
||||
|
||||
let y_axis = Pt{x: rad_min.sin(), y: rad_min.cos()};
|
||||
let x_axis = Pt{x: -y_axis.y, y: y_axis.x};
|
||||
let pt_up = center + (y_axis * mean_up.y) + (x_axis * mean_up.x);
|
||||
let pt_down = center + (y_axis * mean_down.y) + (x_axis * mean_down.x);
|
||||
//segments.push(((pt_down.x as f32, pt_down.y as f32), (pt_up.x as f32, pt_up.y as f32)));
|
||||
let pt_up_2 = pt_down + (pt_up - pt_down)*1.5;
|
||||
let pt_down_2 = pt_up + (pt_down - pt_up)*1.5;
|
||||
segments.push(((pt_down_2.x as f32, pt_down_2.y as f32), (pt_up_2.x as f32, pt_up_2.y as f32)));
|
||||
}
|
||||
|
||||
Ok(segments)
|
||||
}
|
||||
|
||||
fn average_pt_i32(vals: &[(i32, i32)]) -> (f32, f32) {
|
||||
let (mut mean_x, mut mean_y) = (0., 0.);
|
||||
let len = vals.len() as f32;
|
||||
|
||||
for (x, y) in vals {
|
||||
mean_x += *x as f32;
|
||||
mean_y += *y as f32;
|
||||
}
|
||||
(mean_x / len, mean_y / len)
|
||||
}
|
||||
|
||||
fn get_id_groups(limits: &Vec<(usize, usize)>, id: usize) -> Option<usize> {
|
||||
for (id_seg, (min, max)) in limits.iter().enumerate() {
|
||||
if id >= *min && id <= *max {
|
||||
return Some(id_seg);
|
||||
}
|
||||
}
|
||||
None
|
||||
//return usize::MAX; // im lazy to have Option return...
|
||||
}
|
||||
|
||||
pub fn annalyse_segment(m: &Mat) -> Result<Vec<Vec<(i32, i32)>>> {
|
||||
// on recupere les coordoner des point selectioner
|
||||
let mut seg_pt = HashSet::from([]);
|
||||
let (cols, rows) = (m.cols(), m.rows());
|
||||
for j in 0..rows {
|
||||
for i in 0..cols {
|
||||
let v: &Point3_<u8> = m.at_2d(j, i)?;
|
||||
if v.x != 0 && v.y != 0 && v.z != 0 {
|
||||
seg_pt.insert((i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// on garde que ceux qui sont frontiere
|
||||
//let around_all = [(-1, -1), (-1, 0), (-1, 1), (0, 1), (1, 1), (1, 0), (1, -1), (0, -1)];
|
||||
let around_all = [(-1, 0), (0, 1), (1, 0), (0, -1)];
|
||||
let mut selected: HashSet<(i32, i32)> = seg_pt
|
||||
.iter()
|
||||
.filter_map(|(x, y)| {
|
||||
for (k, (i, j)) in around_all.iter().enumerate() {
|
||||
if seg_pt.get(&(*x + i, *y + j)).is_none() {
|
||||
return Some((*x, *y));
|
||||
}
|
||||
}
|
||||
None
|
||||
})
|
||||
.collect();
|
||||
|
||||
//let around = [(-1, 0), (0, -1), (1, 0), (0, 1), (-1, -1), (1, -1), (1, 1), (-1, 1)];
|
||||
let around = [
|
||||
(-1, 1),
|
||||
(0, 1),
|
||||
(1, 1),
|
||||
(1, 0),
|
||||
(1, -1),
|
||||
(0, -1),
|
||||
(-1, -1),
|
||||
(-1, 0),
|
||||
];
|
||||
let mut lines = vec![];
|
||||
while selected.len() > 0 {
|
||||
let mut outed: HashSet<(i32, i32)> = HashSet::from([]);
|
||||
let (x, y) = selected.iter().next().unwrap();
|
||||
let mut line = vec![(*x, *y)];
|
||||
|
||||
outed.insert((*x, *y));
|
||||
let mut last = 0;
|
||||
'line: loop {
|
||||
let (x, y) = line[line.len() - 1];
|
||||
for k in 0..around.len() {
|
||||
let (i, j) = around[(k + last) % around.len()];
|
||||
if seg_pt.get(&(x + i, y + j)).is_some() && outed.get(&(x + i, y + j)).is_none() {
|
||||
line.push((x + i, y + j));
|
||||
outed.insert((x + i, y + j));
|
||||
last = k + last + around.len() - 2;
|
||||
// ici on pourrait cleaner le rest
|
||||
//for l in (k+1)..around.len() {
|
||||
// let (i, j) = around[(l+last)%around.len()];
|
||||
// outed.insert((x+i, y+j));
|
||||
// //
|
||||
//}
|
||||
continue 'line;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
lines.push(line);
|
||||
for (x, y) in outed {
|
||||
selected.remove(&(x, y));
|
||||
}
|
||||
}
|
||||
println!("\nseg: {}", lines.len());
|
||||
Ok(lines)
|
||||
}
|
||||
|
||||
pub fn image_mean(frames: &[Mat]) -> Result<Mat> {
|
||||
/*
|
||||
* Il faudrait pouvoir changer les matrice de type pour avoir des valeur plus grande
|
||||
* */
|
||||
let mut frames_big: Vec<Mat> = vec![];
|
||||
let len = frames.len() as i16;
|
||||
|
||||
for frame in frames {
|
||||
let mut tmp = Mat::default();
|
||||
frame.convert_to(&mut tmp, 19, 1., 0.)?; // 19 is for: CV_16SC3
|
||||
frames_big.push(tmp);
|
||||
}
|
||||
|
||||
let mut img_sum: Mat = frames_big[0].clone();
|
||||
let mask = Mat::default();
|
||||
for frame in frames_big[1..].iter() {
|
||||
let mut tmp = Mat::default();
|
||||
add(&img_sum, &frame, &mut tmp, &mask, -1)?;
|
||||
img_sum = tmp;
|
||||
}
|
||||
|
||||
let (cols, rows) = (img_sum.cols(), img_sum.rows());
|
||||
for j in 0..rows {
|
||||
for i in 0..cols {
|
||||
let v: &mut Point3_<i16> = img_sum.at_2d_mut(j, i)?;
|
||||
v.x /= len;
|
||||
v.y /= len;
|
||||
v.z /= len;
|
||||
}
|
||||
}
|
||||
|
||||
let mut mean = Mat::default();
|
||||
img_sum.convert_to(&mut mean, 16, 1., 0.)?; // 16 is for: CV_8UC3
|
||||
|
||||
Ok(mean)
|
||||
}
|
||||
|
||||
pub fn image_diff(frame: &Mat, frame_prev: &Mat) -> Result<Mat> {
|
||||
let mut diff_bgr = Mat::default();
|
||||
let mut diff_bgr_2 = Mat::default();
|
||||
|
|
@ -233,7 +534,7 @@ pub fn image_diff(frame: &Mat, frame_prev: &Mat) -> Result<Mat> {
|
|||
Ok(d_bgr)
|
||||
}
|
||||
|
||||
fn histogram_3d(m: &Mat, nb_liss: i32) -> Result<Vec<Vec<f64>>> {
|
||||
pub fn histogram_3d(m: &Mat, nb_liss: i32) -> Result<Vec<Vec<f64>>> {
|
||||
let (cols, rows) = (m.cols(), m.rows());
|
||||
let mut histo = vec![vec![0.; 256]; 3];
|
||||
|
||||
|
|
@ -262,7 +563,7 @@ fn histogram_3d(m: &Mat, nb_liss: i32) -> Result<Vec<Vec<f64>>> {
|
|||
Ok(histo)
|
||||
}
|
||||
|
||||
fn histogram_1d(m: &Mat, nb_liss: i32) -> Result<Vec<f64>> {
|
||||
pub fn histogram_1d(m: &Mat, nb_liss: i32) -> Result<Vec<f64>> {
|
||||
let (cols, rows) = (m.cols(), m.rows());
|
||||
let mut histo = vec![0; 256];
|
||||
let mut m_gray = Mat::default();
|
||||
|
|
@ -291,7 +592,7 @@ fn histogram_1d(m: &Mat, nb_liss: i32) -> Result<Vec<f64>> {
|
|||
Ok(histo)
|
||||
}
|
||||
|
||||
fn first_invert(histo: &Vec<f64>) -> ((usize, f64), (usize, f64)) {
|
||||
pub fn first_invert(histo: &Vec<f64>) -> ((usize, f64), (usize, f64)) {
|
||||
// on applique un log puis on normalise mar le log du max
|
||||
let mut normalised = vec![0.; histo.len()];
|
||||
let mut p1 = vec![0.; histo.len() / 2];
|
||||
|
|
@ -356,7 +657,11 @@ pub fn trackbar_line_segment(mem: &mut Qualibration, winname: &str) -> Result<()
|
|||
//highgui
|
||||
let winname = format!("{}: {}", winname, 0); //"bord selected: 0";
|
||||
named_window(winname.as_str(), WINDOW_AUTOSIZE)?;
|
||||
highgui::move_window(winname.as_str(), 20, 20)?;
|
||||
highgui::move_window(winname.as_str(), 20, 520)?;
|
||||
//highgui::move_window(winname, 20, 20)?;
|
||||
let v: VecN<f64, 4> = VecN::new(0., 0., 0., 255.);
|
||||
let m = Mat::new_rows_cols_with_default(1, 1800, CV_8UC3, v)?;
|
||||
highgui::imshow(winname.as_str(), &m)?;
|
||||
//
|
||||
create_trackbar(
|
||||
"canny min",
|
||||
|
|
@ -405,7 +710,7 @@ pub fn trackbar_line_segment(mem: &mut Qualibration, winname: &str) -> Result<()
|
|||
"max_gap : ",
|
||||
winname.as_str(),
|
||||
Some(&mut mem.hough_param.max_line_gap),
|
||||
500000,
|
||||
50000,
|
||||
None,
|
||||
)?;
|
||||
Ok(())
|
||||
|
|
@ -433,23 +738,25 @@ pub fn line_pos(mem: &mut Qualibration, winname: &str) -> Result<()> {
|
|||
|
||||
pub fn adding_trackbar(mut mem: &mut Qualibration, winname: &str) -> Result<()> {
|
||||
//println!("winname: {winname}");
|
||||
line_pos(&mut mem , "Play Line")?;
|
||||
trackbar_init_param(mem, "init_param")?;
|
||||
//named_window(winname, WINDOW_AUTOSIZE)?;
|
||||
//associate_trackbar(winname, &mut mem.tresh)?;
|
||||
//create_trackbar(
|
||||
// "nb_liss",
|
||||
// winname,
|
||||
// Some(&mut mem.nb_liss),
|
||||
// MAX_TRACKBAR,
|
||||
// None,
|
||||
//)?;
|
||||
//trackbar_line_segment(mem, winname)?;
|
||||
//line_pos(&mut mem, "Play Line")?;
|
||||
//trackbar_init_param(mem, "init_param")?;
|
||||
|
||||
named_window("histo bgr", WINDOW_AUTOSIZE)?;
|
||||
associate_trackbar("histo bgr", &mut mem.tresh)?;
|
||||
create_trackbar(
|
||||
"nb_liss",
|
||||
"histo bgr",
|
||||
Some(&mut mem.nb_liss),
|
||||
MAX_TRACKBAR,
|
||||
None,
|
||||
)?;
|
||||
|
||||
//trackbar_line_segment(mem, "line detector")?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn associate_trackbar(winname: &str, tresh: &mut Treshold) -> Result<()> {
|
||||
pub fn associate_trackbar(winname: &str, tresh: &mut Treshold) -> Result<()> {
|
||||
create_trackbar(
|
||||
"blue min: ",
|
||||
winname,
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue