lj_qualibration/src/qualibration/sequence/init_border.rs

use crate::draw::draw_line;
use crate::point::{Color, Point};
use crate::qualibration::{
    param::{HoughLine, Param},
    Sequence,
};

use crate::qualibration::annalyse::image_diff;
use crate::qualibration::borders::{
    bord_mult, get_extermities, get_intersection, probabilistic_hough, mix_borders,
};

use opencv::{
    calib3d,
    core::{Mat, Point as OcvPoint, Size, VecN, Vector, Scalar, BORDER_CONSTANT},
    imgproc::{self, canny, cvt_color, COLOR_BGR2GRAY, line},
    Result,
    highgui,
};

opencv::opencv_branch_4! {
    #[allow(unused)]
    use opencv::imgproc::LINE_AA;
}
opencv::not_opencv_branch_4! {
    use opencv::core::LINE_AA;
}

#[derive(Debug, Clone, Copy)]
pub struct InitBorder {
    finished: bool,
    cnt: usize,
    borders: [Point; 4],
    nb_millis: u64,
}

impl InitBorder {
    pub fn new(beg: Point, end: Point, nb_millis: u64) -> Self {
        InitBorder {
            borders: [
                Point {
                    x: beg.x,
                    y: beg.y,
                    color: end.color,
                },
                Point {
                    x: end.x,
                    y: beg.y,
                    color: end.color,
                },
                Point {
                    x: end.x,
                    y: end.y,
                    color: end.color,
                },
                Point {
                    x: beg.x,
                    y: end.y,
                    color: end.color,
                },
            ],
            nb_millis,
            cnt: 0,
            finished: false,
        }
    }
}

impl Sequence for InitBorder {
    //type Obj = Self;

    fn draw(&self, mem: &Param) -> Option<Vec<Point>> {
        if self.cnt > self.borders.len() || self.finished {
            return None;
        }
        if self.cnt == self.borders.len() {
            return Some(vec![]);
        }

        let color = Color {
            r: mem.r as u8,
            g: mem.g as u8,
            b: mem.b as u8,
        };
        let id1 = (self.cnt + 1) % self.borders.len();
        let p0 = Point {
            color,
            ..self.borders[self.cnt]
        };
        let p1 = Point {
            color,
            ..self.borders[id1]
        };
        Some(draw_line(
            &p0,
            &p1,
            mem.nb_all as usize,
            mem.nb_visible as usize,
        ))
    }

    fn compute_sequence(&mut self, mem: &mut Param) -> Result<(), Box<dyn std::error::Error>> {
        if self.cnt < self.borders.len() {
            self.cnt += 1;
            return Ok(());
        }

        let len = self.borders.len();
        let imgs = mem.imgs[mem.seq_id].clone();
        let borders: Vec<Mat> = imgs[..len].into();
        let background = imgs[len].clone();

        // on recupere chaqu'un des 4 bord
        let mut bords_pts = vec![];
        for (id, bord) in borders.iter().enumerate() {
            let lines = get_lines(
                &background,
                &bord,
                id,
                mem.canny_v1,
                mem.canny_v2,
                &mem.hough_param,
            )?;
            let bord_pt = get_extermities(&lines, id);
            bords_pts.push(bord_pt);
        }

        // on calcul le cadre
        let border_pt = get_intersection(&bords_pts);
        mem.border_pt = bord_mult(border_pt, 1.1);

        // on dessine le cadre
        let color: VecN<f64, 4> = VecN::new(255., 128., 0., 255.);
        let mut mixed = mix_borders(&background, borders)?;
        let b = &mem.border_pt;
        for i in 0..b.len() {
            let j = (i + 1) % mem.border_pt.len();
            let pa = VecN::from_array([b[i].0 as i32, b[i].1 as i32]);
            let pb = VecN::from_array([b[j].0 as i32, b[j].1 as i32]);
            let a = OcvPoint::from_vec2(pa);
            let b = OcvPoint::from_vec2(pb);
            line(&mut mixed, a, b, color, 1, LINE_AA, 0)?;
        }
        highgui::imshow("mixed bored", &mixed)?;

        // on calcule l'homography
        let size = mem.dst_size;
        // ici on va requadrer la partie de la projection laser de l'image
        let warped_image_size = Size::new(size, size);
        let roi_corners: Vec<OcvPoint> = mem
            .border_pt
            .iter()
            .map(|(x, y)| OcvPoint::new(*x as i32, *y as i32))
            .collect();
        //let dst = [(0, 0), (0, size), (size, size), (size, 0)]; // in: laser repere
        let dst = [(size, size), (size, 0), (0, 0), (0, size)];
        let dst_corners: Vec<OcvPoint> = dst.iter().map(|(x, y)| OcvPoint::new(*x, *y)).collect();
        let roi_corners_mat = Mat::from_slice(&roi_corners[..])?;
        let dst_corners_mat = Mat::from_slice(&dst_corners)?;
        let h = calib3d::find_homography(
            &roi_corners_mat,
            &dst_corners_mat,
            &mut Mat::default(),
            0,
            3.,
        )?; //get homography
        mem.homography = h.clone();
        mem.h_size = warped_image_size.clone();
        let mut warped_image = Mat::default();
        imgproc::warp_perspective(
            &mixed,
            &mut warped_image,
            &h,
            warped_image_size,
            imgproc::INTER_CUBIC, // I dont see difference with INTER_CUBIC
            BORDER_CONSTANT,
            Scalar::default(),
        )?; // do perspective transformation
        highgui::imshow("Warped Image", &warped_image)?;
        self.finished = true;
        Ok(())
    }

    fn is_capture(&self) -> bool {
        true
    }

    fn sequence_name(&self) -> String {
        "Init_Border".to_owned()
    }

    fn wait_milis(&self) -> u64 {
        self.nb_millis
    }
}

pub fn get_lines(
    background: &Mat,
    bord: &Mat,
    id: usize,
    canny_v1: i32,
    canny_v2: i32,
    hough_param: &HoughLine,
) -> Result<Vector<VecN<i32, 4>>> {
    let diff: Mat = image_diff(bord, background)?;

    // Pass the image to gray
    let mut diff_gray = Mat::default();
    cvt_color(&diff, &mut diff_gray, COLOR_BGR2GRAY, 0)?;
    // Apply Canny edge detector
    let mut edges = Mat::default();
    canny(
        &diff_gray,
        &mut edges,
        canny_v1 as f64,
        canny_v2 as f64,
        3,
        false,
    )?;
    let lines = probabilistic_hough(&edges, hough_param, id)?;
    //let ((x1, y1), (x2, y2)) = get_extermities(&lines, id);
    Ok(lines)
}