first push

pour l'instant le programe detecte chacun des bord puis recadre le
trapeze en carrer avec une marge en cas de depassement.

src/qualibration/annalyse.rs

@@ -0,0 +1,465 @@
+use super::Qualibration;
+use super::DEBUG;
+//use opencv::prelude::MatTraitConst;
+use opencv::prelude::*; //MatTraitConst;
+
+use opencv::core::{add, subtract, Mat, Point as OcvPoint, Point3_, VecN, CV_8UC3};
+use opencv::highgui::{self, create_trackbar, named_window, WINDOW_AUTOSIZE};
+use opencv::imgproc::{cvt_color, line, COLOR_BGR2GRAY};
+use opencv::Result;
+
+opencv::opencv_branch_4! {
+    use opencv::imgproc::LINE_AA;
+}
+opencv::not_opencv_branch_4! {
+    use opencv::core::LINE_AA;
+}
+
+use super::Treshold;
+const MAX_TRACKBAR: i32 = 255;
+
+fn draw_histograme_dbg(
+    window_name: &str,
+    histo: &Vec<f64>,
+    (from, to): (usize, usize),
+) -> Result<()> {
+    let v: VecN<f64, 4> = VecN::new(0., 0., 0., 255.);
+    let c1: VecN<f64, 4> = VecN::new(128., 128., 128., 255.);
+    let c2: VecN<f64, 4> = VecN::new(255., 255., 255., 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 max = 0.;
+    for i in 0..256 {
+        if histo[i] > max {
+            max = histo[i];
+        }
+    }
+
+    let v_log = 10.;
+
+    for i in 0..255 {
+        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 color = if i >= from && i <= to { c2 } else { c1 };
+        let pt1 = OcvPoint::new(x1, y1);
+        let pt2 = OcvPoint::new(x2, y2);
+        line(&mut img, pt1, pt2, color, 1, LINE_AA, 0)?;
+    }
+
+    highgui::imshow(window_name, &img)?;
+
+    Ok(())
+}
+
+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 max = 0.;
+    for i in 0..256 {
+        if histo[i] > max {
+            max = histo[i];
+        }
+    }
+
+    let v_log = 10.;
+
+    for i in 0..255 {
+        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 pt1 = OcvPoint::new(x1, y1);
+        let pt2 = OcvPoint::new(x2, y2);
+        line(&mut img, pt1, pt2, color, 1, LINE_AA, 0)?;
+    }
+
+    highgui::imshow(window_name, &img)?;
+
+    Ok(())
+}
+
+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.);
+    let r: VecN<f64, 4> = VecN::new(0., 0., 255., 255.);
+    let color = vec![b, g, r];
+    let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?;
+
+    let mut range = vec![vec![f64::MAX, f64::MIN]; 3];
+    for j in 0..3 {
+        for i in 0..256 {
+            if histo[j][i] > range[j][1] {
+                range[j][1] = histo[j][i];
+            }
+            if histo[j][i] < range[j][0] {
+                range[j][0] = histo[j][i];
+            }
+        }
+    }
+
+    //let v_log = 10.;
+
+    for j in 0..3 {
+        for i in 0..255 {
+            let x1 = ((i + 0) * 2) as i32;
+            let x2 = ((i + 1) * 2) as i32;
+            let y1 = ((histo[j][i + 0] + 1.).log10() / range[j][1].log10() * 2. * 256.) as i32;
+            let y2 = ((histo[j][i + 1] + 1.).log10() / range[j][1].log10() * 2. * 256.) as i32;
+            let pt1 = OcvPoint::new(x1, y1);
+            let pt2 = OcvPoint::new(x2, y2);
+            line(&mut img, pt1, pt2, color[j], 1, LINE_AA, 0)?;
+        }
+    }
+
+    highgui::imshow(window_name, &img)?;
+
+    Ok(())
+}
+
+fn draw_histograme_bgr_tresh(
+    window_name: &str,
+    histo: &Vec<Vec<f64>>,
+    tresh: &Treshold,
+) -> 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.);
+    let r: VecN<f64, 4> = VecN::new(0., 0., 255., 255.);
+    let color1 = vec![b, g, r];
+    let color2 = vec![b / 2., g / 2., r / 2.];
+    let mut img = Mat::new_rows_cols_with_default(256 * 2, 256 * 2, CV_8UC3, v)?;
+
+    let mut vmax = vec![f64::MIN; 3];
+    for j in 0..histo.len() {
+        for i in 0..histo[j].len() {
+            if histo[j][i] > vmax[j] {
+                vmax[j] = histo[j][i];
+            }
+        }
+    }
+
+    //let v_log = 10.;
+    let max: Vec<f64> = [tresh.max_0 as f64, tresh.max_1 as f64, tresh.max_2 as f64].into();
+    let min: Vec<f64> = [tresh.min_0 as f64, tresh.min_1 as f64, tresh.min_2 as f64].into();
+
+    //println!("min: {min:?}\tmax: {max:?}");
+
+    for j in 0..3 {
+        for i in 0..255 {
+            let x1 = ((i + 0) * 2) as i32;
+            let x2 = ((i + 1) * 2) as i32;
+            let y1 = ((histo[j][i + 0] + 1.).log10() / vmax[j].log10() * 2. * 256.) as i32;
+            let y2 = ((histo[j][i + 1] + 1.).log10() / vmax[j].log10() * 2. * 256.) as i32;
+            let pt1 = OcvPoint::new(x1, y1);
+            let pt2 = OcvPoint::new(x2, y2);
+
+            //let val = (histo[j][i] + 1.).log10() / max[j].log10();
+            let (color, thickness) = if i as f64 >= min[j] && i as f64 <= max[j] {
+                (color1[j], 2)
+            } else {
+                (color2[j], 1)
+            };
+            line(&mut img, pt1, pt2, color, thickness, LINE_AA, 0)?;
+        }
+    }
+
+    highgui::imshow(window_name, &img)?;
+
+    Ok(())
+}
+
+// limit = 0.35 c'est bien
+pub fn is_same_frame(frame: &Mat, frame_prev: &Mat) -> Result<bool> {
+    let nb_liss: i32 = 50; // plus on lisse la courbe plus on attein la limite facilement
+    let limit = 0.45; // plus c'est haut, plus on tolere de changement entre 2 image
+
+    let d_bgr = image_diff(frame, frame_prev)?;
+    let histo = histogram_1d(&d_bgr, nb_liss)?;
+    let ((_id1, v1), (_id2, v2)) = first_invert(&histo);
+
+    if DEBUG {
+        // on affiche l'image de la cam
+        highgui::imshow("cam image", frame)?;
+        // on affiche l'image de la cam
+        highgui::imshow("prev image", frame_prev)?;
+        // on affiche la difference
+        highgui::imshow("diff image", &d_bgr)?;
+        // on affiche l'histograme
+        let ids = ((128 - _id2), (128 + _id1));
+        draw_histograme_dbg("histograme", &histo, ids)?;
+        // -- pour chaque image enregistrer on l'affiche ma ca se fait autre part
+    }
+
+    if DEBUG {
+        println!("v1[{_id1}]:{v1}\tv2[{_id1}:{v2}");
+    }
+
+    if v1 >= limit || v2 >= limit {
+        println!("\t XXX DIFFERENT XXX");
+        Ok(false)
+    } else {
+        println!("\t  :)   Same    (: ");
+        Ok(true)
+    }
+}
+
+pub fn image_diff(frame: &Mat, frame_prev: &Mat) -> Result<Mat> {
+    let mut diff_bgr = Mat::default();
+    let mut diff_bgr_2 = Mat::default();
+    let mut d_bgr = Mat::default();
+    let (row, col) = (frame.rows(), frame.cols());
+    let mask = Mat::default();
+    let v: VecN<f64, 4> = VecN::new(128., 128., 128., 128.);
+    let mid: Mat = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
+
+    // ca parait etonant d'enlever la difference dans l'autre sens mais paradoxalement, ca permet
+    // d'avoir toutes les valeur, pck a chaque fois les valeur negative sont mise a 0 dans
+    // l'operation de soustraction
+    subtract(frame, frame_prev, &mut diff_bgr, &mask, -1)?;
+    add(&diff_bgr, &mid, &mut diff_bgr_2, &mask, -1)?;
+    subtract(frame_prev, frame, &mut diff_bgr, &mask, -1)?;
+    subtract(&diff_bgr_2, &diff_bgr, &mut d_bgr, &mask, -1)?;
+
+    Ok(d_bgr)
+}
+
+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];
+
+    // on calcule l'histograme
+    for j in 0..rows {
+        for i in 0..cols {
+            let v: &Point3_<u8> = m.at_2d(j, i)?;
+            let (b, g, r) = (v.x as usize, v.y as usize, v.z as usize);
+            histo[2][r] += 1.;
+            histo[1][g] += 1.;
+            histo[0][b] += 1.;
+        }
+    }
+
+    // on lisse l'histograme
+    for j in 0..3 {
+        let mut tmp = histo[j].clone();
+        for _ in 0..nb_liss {
+            for i in 1..(tmp.len() - 1) {
+                histo[j][i] = (tmp[i - 1] + 1. * tmp[i] + tmp[i + 1]) / 3.;
+            }
+            tmp = histo[j].clone();
+        }
+    }
+
+    Ok(histo)
+}
+
+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();
+
+    // on convertie en gris
+    cvt_color(m, &mut m_gray, COLOR_BGR2GRAY, 0)?;
+    // on calcule l'histograme
+    for j in 0..rows {
+        for i in 0..cols {
+            let v: &u8 = m_gray.at_2d(j, i)?;
+            let id = *v as usize;
+            histo[id] += 1;
+        }
+    }
+
+    // on lisse l'histograme
+    let mut histo: Vec<f64> = histo.iter().map(|x| *x as f64).collect();
+    let mut tmp = histo.clone();
+    for _ in 0..nb_liss {
+        for i in 1..(histo.len() - 1) {
+            histo[i] = (tmp[i - 1] + 2. * tmp[i] + tmp[i + 1]) / 4.;
+        }
+        tmp = histo.clone();
+    }
+
+    Ok(histo)
+}
+
+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];
+    let mut p2 = vec![0.; histo.len() / 2];
+    let mut dp1 = vec![0.; histo.len() / 2];
+    let mut dp2 = vec![0.; histo.len() / 2];
+    let mid = (histo.len() + 1) / 2;
+    let max = (histo[mid] as f64).log10(); // on par du principe que le max est au centre
+
+    for i in 0..histo.len() {
+        normalised[i] = (histo[i] as f64 + 1.).log10() / max;
+    }
+    for i in (mid)..(histo.len() - 1) {
+        p1[i - mid] = mid as f64 * ((normalised[mid] - normalised[i + 1]) / (i - mid + 2) as f64);
+    }
+    for i in (1..mid).rev() {
+        p2[mid - i - 1] = mid as f64 * ((normalised[mid] - normalised[i]) / (mid - i) as f64);
+    }
+    for i in 0..(mid - 1) {
+        dp1[i] = p1[i + 1] - p1[i];
+        dp2[i] = p2[i + 1] - p2[i];
+    }
+
+    let mut dist_1 = 0;
+    for (i, v) in dp1.iter().enumerate() {
+        if v < &0. {
+            dist_1 = i;
+            break;
+        }
+    }
+    let mut dist_2 = 0;
+    for (i, v) in dp2.iter().enumerate() {
+        if v < &0. {
+            dist_2 = i;
+            break;
+        }
+    }
+
+    (
+        (dist_1, normalised[mid + dist_1]),
+        (dist_2, normalised[mid - dist_2]),
+    )
+}
+
+pub fn adding_trackbar(mem: &mut Qualibration, winname: &str) -> Result<()> {
+    //println!("winname: {winname}");
+    named_window(winname, WINDOW_AUTOSIZE)?;
+    associate_trackbar(winname, &mut mem.tresh)?;
+    create_trackbar(
+        "nb_liss",
+        winname,
+        Some(&mut mem.nb_liss),
+        MAX_TRACKBAR,
+        None,
+    )?;
+
+    //highgui
+    let winname = format!("{}: {}", winname, 0); //"bord selected: 0";
+    named_window(winname.as_str(), WINDOW_AUTOSIZE)?;
+    highgui::move_window(winname.as_str(), 20, 20)?;
+    //
+    create_trackbar(
+        "canny min",
+        winname.as_str(),
+        Some(&mut mem.canny_v1),
+        MAX_TRACKBAR,
+        None,
+    )?;
+    create_trackbar(
+        "canny max",
+        winname.as_str(),
+        Some(&mut mem.canny_v2),
+        MAX_TRACKBAR,
+        None,
+    )?;
+
+    create_trackbar(
+        "rho     : ",
+        winname.as_str(),
+        Some(&mut mem.hough_param.rho),
+        1000,
+        None,
+    )?;
+    create_trackbar(
+        "theta   : ",
+        winname.as_str(),
+        Some(&mut mem.hough_param.theta),
+        1000,
+        None,
+    )?;
+    create_trackbar(
+        "treshold: ",
+        winname.as_str(),
+        Some(&mut mem.hough_param.treshold),
+        255,
+        None,
+    )?;
+    create_trackbar(
+        "min_leng: ",
+        winname.as_str(),
+        Some(&mut mem.hough_param.min_length),
+        1000,
+        None,
+    )?;
+    create_trackbar(
+        "max_gap : ",
+        winname.as_str(),
+        Some(&mut mem.hough_param.max_line_gap),
+        500000,
+        None,
+    )?;
+
+    //let winname = "bord selected: 0";
+    //create_trackbar("scale   : ", winname, Some(&mut mem.lsd_param.scale      ), 1000, None)?;
+    //create_trackbar("sigma_scal", winname, Some(&mut mem.lsd_param.sigma_scale), 1000, None)?;
+    //create_trackbar("quant   : ", winname, Some(&mut mem.lsd_param.quant      ), 1000, None)?;
+    //create_trackbar("ang_th  : ", winname, Some(&mut mem.lsd_param.ang_th     ), 1000, None)?;
+    //create_trackbar("log_eps : ", winname, Some(&mut mem.lsd_param.log_eps    ), 1000, None)?;
+    //create_trackbar("density_th", winname, Some(&mut mem.lsd_param.density_th ), 1000, None)?;
+    //create_trackbar("n_bins  : ", winname, Some(&mut mem.lsd_param.n_bins     ), 1000, None)?;
+    Ok(())
+}
+
+fn associate_trackbar(winname: &str, tresh: &mut Treshold) -> Result<()> {
+    create_trackbar(
+        "blue min: ",
+        winname,
+        Some(&mut tresh.min_0),
+        MAX_TRACKBAR,
+        None,
+    )?;
+    create_trackbar(
+        "blue max: ",
+        winname,
+        Some(&mut tresh.max_0),
+        MAX_TRACKBAR,
+        None,
+    )?;
+
+    create_trackbar(
+        "green min: ",
+        winname,
+        Some(&mut tresh.min_1),
+        MAX_TRACKBAR,
+        None,
+    )?;
+    create_trackbar(
+        "green max: ",
+        winname,
+        Some(&mut tresh.max_1),
+        MAX_TRACKBAR,
+        None,
+    )?;
+
+    create_trackbar(
+        "red min: ",
+        winname,
+        Some(&mut tresh.min_2),
+        MAX_TRACKBAR,
+        None,
+    )?;
+    create_trackbar(
+        "red max: ",
+        winname,
+        Some(&mut tresh.max_2),
+        MAX_TRACKBAR,
+        None,
+    )?;
+
+    Ok(())
+}

src/qualibration/borders.rs

@@ -0,0 +1,182 @@
+use super::HoughLine;
+use crate::utils::{CartesianEquation, EqAffine, Pt};
+use opencv::core::{add, subtract, Mat, VecN, Vector, CV_8UC3};
+//use opencv::prelude::MatTraitConst;
+use opencv::imgproc::{cvt_color, hough_lines_p, COLOR_GRAY2BGR};
+use opencv::prelude::*; //MatTraitConst;
+use opencv::types::VectorOfVec4i;
+use opencv::Result;
+
+pub fn mix_borders(background: &Mat, borders: Vec<Mat>) -> Result<Mat> {
+    let (row, col) = (background.rows(), background.cols());
+    //let mask = Mat::default();
+    let v: VecN<f64, 4> = VecN::new(0., 0., 0., 0.);
+    let mut sum_diff_0 = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
+    let mut sum_diff_1 = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
+    let mask = Mat::default();
+    let mut tmp = Mat::default();
+
+    // on va faire la somme des difference
+    // on va les ajouter a l'image du fond
+    for bord in borders {
+        //let diff = image_diff(&bord, background)?;
+        //add(&diff, &mid, &mut diff_bgr_2, &mask, -1)?;
+        let mut diff = Mat::default();
+        subtract(&bord, background, &mut diff, &mask, -1)?;
+        add(&diff, &sum_diff_0, &mut tmp, &mask, -1)?;
+        sum_diff_0 = tmp.clone();
+
+        subtract(background, &bord, &mut diff, &mask, -1)?;
+        add(&diff, &sum_diff_1, &mut tmp, &mask, -1)?;
+        sum_diff_1 = tmp.clone();
+    }
+    //let v: VecN<f64, 4> = VecN::new(128., 128., 128., 128.);
+    //let mid = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
+    let mut tmp = Mat::default();
+    //let mut tmp2 = Mat::default();
+    let mut mix = Mat::default();
+    add(&sum_diff_0, &background, &mut tmp, &mask, -1)?;
+    subtract(&tmp, &sum_diff_1, &mut mix, &mask, -1)?;
+
+    Ok(mix)
+}
+
+//impl Add for (f64, f64) {
+//}
+pub fn bord_mult(pt: Vec<(f64, f64)>, factor: f64) -> Vec<(f64, f64)> {
+    let pt: Vec<Pt> = pt.iter().map(|p| Pt::from(p)).collect();
+
+    let mut pa = vec![];
+    let mut pb = vec![];
+    for i in 0..pt.len() {
+        let k = (i + pt.len() - 1) % pt.len();
+        let j = (i + 1) % pt.len();
+        pa.push((pt[i] - pt[j]) * factor + pt[j]);
+        pb.push((pt[i] - pt[k]) * factor + pt[k]);
+    }
+
+    let mut eq = vec![];
+    for i in 0..pt.len() {
+        let j = (i + 1) % pt.len();
+        eq.push(CartesianEquation::new_from_pt(&pb[i], &pa[j]));
+    }
+
+    let mut p_out = vec![];
+    for i in 0..pt.len() {
+        let k = (i + pt.len() - 1) % pt.len();
+        p_out.push(eq[i].intersection(&eq[k]).unwrap()); // TODO: faire un truc pour le unwrap...
+                                                         // normalement c'est un gars sur ta
+                                                         // compris ;)... mais bon... un alignement
+                                                         // malencontreux ca arrive vite
+    }
+
+    p_out.iter().map(|p| (p.x, p.y)).collect()
+}
+
+// en fait ca marche pas dutout...next time
+pub fn bord_mult_v2(pt: Vec<(f64, f64)>, factor: f64) -> Vec<(f64, f64)> {
+    let mut pt: Vec<Pt> = pt.iter().map(|p| Pt::from(p)).collect();
+
+    let mut pn = vec![];
+    for i in 0..pt.len() {
+        let j = (i + 2) % pt.len();
+        pn.push((pt[i] - pt[j]) * factor + pt[j]);
+    }
+
+    pn.iter().map(|p| (p.x, p.y)).collect()
+}
+
+pub fn get_intersection(pts: &[((f64, f64), (f64, f64))]) -> Vec<(f64, f64)> {
+    let mut eq_cart = vec![];
+    for i in 0..pts.len() {
+        eq_cart.push(CartesianEquation::new_from_tuple(pts[i]));
+    }
+
+    let mut points = vec![];
+    for i in 0..eq_cart.len() {
+        let id_next = (i + 1) % eq_cart.len();
+        let pt = eq_cart[i].intersection(&eq_cart[id_next]).unwrap(); // TODO verifier quand meme la sortie au lieu de unwrap salement... xD
+        points.push((pt.x, pt.y));
+    }
+
+    points
+}
+
+pub fn get_extermities(lines: &Vector<VecN<i32, 4>>, id: usize) -> ((f64, f64), (f64, f64)) {
+    let mut p0: (f64, f64) = (0., 0.);
+    let mut p1: (f64, f64) = (0., 0.);
+    let (mut min, mut max): (f64, f64) = (f64::MAX, f64::MIN);
+    //let mut eq: (f64, f64, f64) = (0., 0., 0.);
+    let mut dst_sum = 0.;
+    let mut v_eq = vec![];
+
+    // on cherche les extremite
+    for l in lines {
+        // rename value and switch x and y if necessery
+        let (mut a0, mut b0, mut a1, mut b1) = if id % 2 == 0 {
+            (l[0] as f64, l[1] as f64, l[2] as f64, l[3] as f64)
+        } else {
+            (l[1] as f64, l[0] as f64, l[3] as f64, l[2] as f64) // switch x <-> y
+        };
+
+        // reorder if not
+        if a0 > a1 {
+            (a0, b0, a1, b1) = (a1, b1, a0, b0);
+        }
+
+        // update min/max
+        min = min.min(a0).min(a1);
+        max = max.max(a0).max(a1);
+
+        // cancel computation if devide by zero
+        if a1 - a0 == 0. {
+            continue;
+        }
+
+        let eq = EqAffine::new(a0, b0, a1, b1);
+        dst_sum += eq.dst;
+        v_eq.push(eq);
+    }
+
+    p0.0 = min;
+    p1.0 = max;
+    for eq in v_eq {
+        p0.1 += eq.get_val_dst(min);
+        p1.1 += eq.get_val_dst(max);
+    }
+    p0.1 /= dst_sum;
+    p1.1 /= dst_sum;
+
+    // revert x-y if already reverted previously
+    if id % 2 != 0 {
+        p0 = (p0.1, p0.0);
+        p1 = (p1.1, p1.0);
+    }
+
+    (p0, p1)
+}
+
+pub fn probabilistic_hough(
+    edges: &Mat,
+    hough_param: &HoughLine,
+    id: usize,
+) -> Result<Vector<VecN<i32, 4>>> {
+    let mut p_lines = VectorOfVec4i::new();
+    let mut probabalistic_hough = Mat::default();
+
+    cvt_color(edges, &mut probabalistic_hough, COLOR_GRAY2BGR, 0)?;
+
+    // 2. Use Probabilistic Hough Transform
+    let p = hough_param.get_param();
+    hough_lines_p(
+        edges,
+        &mut p_lines,
+        p.rho,
+        p.theta,
+        p.treshold,
+        p.min_length,
+        p.max_line_gap,
+    )?;
+
+    Ok(p_lines)
+}