rspp 0.1.7

use num_traits::Pow;
extern crate nalgebra as na;
use crate::tools::tool::{self, gaussian_likehood};
use na::*;
use nalgebra::base::Vector3;
use nalgebra::{
    base::allocator::Allocator, DefaultAllocator, Dim, DimName, DimNameAdd, MatrixMN, RealField,
    Scalar,
};
extern crate docify;
use anyhow::{bail, Result};
use nalgebra;
use nalgebra::DimMinimum;
use rand::prelude::*;
use statrs::distribution::{Continuous, Normal};
use statrs::statistics::Distribution;
use std::collections::HashMap;
use std::f64::consts::PI;
use std::ops::Add;
/// 求法线  1 球内找点,2 点到平面距离最小  3 平面垂线
/// ransac(选点建模,然后看普适性) 匹配. 两个电源各取三个点,计算转移矩阵,用这个矩阵去测量别的点,误差内点和外点,找到误差最小的匹配矩阵
/// https://misoraclette.github.io/2018/08/04/data_manipulation.html
pub struct RansacNormal {
    data: Vec<f64>,
    round: usize,
    in_ratio: f64,
    in_thresh: f64,
}
pub fn calc_norm_vector(p1: Vector3<f64>, p2: Vector3<f64>, p3: Vector3<f64>) -> Vector3<f64> {
    let v1 = p2 - p1;
    let v2 = p3 - p1;
    let normal = v1.cross(&v2);
    normal / normal.norm()
}
pub fn dis_to_plane(p: Vector3<f64>, normal: Vector3<f64>, origin: Vector3<f64>) -> f64 {
    /// p在法线投影-圆心在法线投影
    let dot = normal.dot(&p) - normal.dot(&origin);
    if dot.abs() < 1e-3 {
        return 0.;
    }
    let dis = dot.abs() / normal.norm();
    dis
}
impl RansacNormal {
    pub fn rows(&self) -> usize {
        self.data.len() / 3
    }
    pub fn center(&self) -> Vec<f64> {
        let pcl = DMatrix::from_row_slice(self.rows(), 3, &self.data);
        let x = pcl.column(0);
        let y = pcl.column(1);
        let z = pcl.column(2);
        vec![x.mean(), y.mean(), z.mean()]
    }
    pub fn ransac_normal(&self, round: usize) -> Result<Vector3<f64>> {
        let pcl = DMatrix::from_row_slice(self.rows(), 3, &self.data);

        let center = self.center();
        let origin = Vector3::new(center[0], center[1], center[2]);
        for step in 0..round {
            let ids = tool::rand_n_index(self.rows(), 3);
            let p1 = Vector3::new(pcl[(ids[0], 0)], pcl[(ids[0], 1)], pcl[(ids[0], 2)]);
            let p2 = Vector3::new(pcl[(ids[1], 0)], pcl[(ids[1], 1)], pcl[(ids[1], 2)]);
            let p3 = Vector3::new(pcl[(ids[2], 0)], pcl[(ids[2], 1)], pcl[(ids[2], 2)]);
            let norm = calc_norm_vector(p1, p2, p3);
            let mut n_in = 0;
            for index in 0..self.rows() {
                let p = Vector3::new(pcl[(index, 0)], pcl[(index, 1)], pcl[(index, 2)]);
                let dis = dis_to_plane(p, norm, origin);
                if dis < self.in_thresh {
                    n_in += 1
                }
            }
            let this_ratio = n_in as f64 / self.rows() as f64;
            if this_ratio > self.in_ratio {
                return Ok(norm);
            }
        }
        bail!("无法找到法线,调整参数")
    }
    pub fn voxel_grid(&self, voxel_size: f64) -> HashMap<(i32, i32, i32), Vec<Vec<f64>>> {
        let mut res: HashMap<(i32, i32, i32), Vec<Vec<f64>>> = HashMap::new();
        for i in 0..self.rows() {
            let xyz = self.data[i * 3..i * 3 + 3].to_vec();
            let ix = (xyz[0] / voxel_size) as i32;
            let iy = (xyz[1] / voxel_size) as i32;
            let iz = (xyz[2] / voxel_size) as i32;
            let index = (ix, iy, iz);
            if res.contains_key(&index) {
                let mut old: Vec<Vec<f64>> = res[&index].clone();
                old.push(xyz);
                res.insert(index, old);
            } else {
                let mut l = Vec::new();
                l.push(xyz);
                res.insert(index, l);
            }
        }
        res
    }
}

#[test]
fn test_normal() {
    let pcl = vec![
        0.0365432,
        0.08029224,
        0.12673997,
        1.31284406,
        0.36646423,
        -0.17070852,
        1.07315926,
        0.27918677,
        -0.19051423,
        -0.76582586,
        0.3525164,
        -0.20910616,
        -0.01234001,
        -0.04495755,
        -0.03833314,
        -0.15851892,
        -0.20942741,
        0.1233298,
        0.42856567,
        0.32450773,
        -0.20543443,
        0.14900326,
        0.23802697,
        -0.07059914,
        1.95312744,
        -0.08339773,
        0.24432046,
        1.39618658,
        -0.60088018,
        0.46474034,
    ];
    let pcl = RansacNormal {
        data: pcl,
        round: 200,
        in_ratio: 0.7,
        in_thresh: 0.1,
    };
    let center = pcl.center();
    let p1 = Vector3::new(0.0365432, 0.08029224, 0.12673997);
    let p2 = Vector3::new(1.31284406, 0.36646423, -0.17070852);
    let p3 = Vector3::new(1.07315926, 0.27918677, -0.19051423);
    let res = pcl.ransac_normal(200);
    dbg!(res);
    let res2 = pcl.voxel_grid(1.);
    dbg!(res2);
}