cecile_supercool_tracker/utils/
kalman.rs

1use crate::prelude::{BoundingBox, Universal2DBox};
2use crate::Errors;
3use nalgebra::{SMatrix, SVector};
4
5/// Kalman filter for the prediction of axis-aligned and oriented bounding boxes
6///
7pub mod kalman_2d_box;
8/// Kalman filter for 2d point
9///
10pub mod kalman_2d_point;
11/// Kalman filter for Vector of 2d points
12///
13pub mod kalman_2d_point_vec;
14
15pub const CHI2_UPPER_BOUND: f32 = 100.0;
16
17pub const CHI2INV95: [f32; 9] = [
18    3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919,
19];
20
21macro_rules! pretty_print {
22    ($arr:expr) => {{
23        let indent = 4;
24        let prefix = String::from_utf8(vec![b' '; indent]).unwrap();
25        let mut result_els = vec!["".to_string()];
26        for i in 0..$arr.nrows() {
27            let mut row_els = vec![];
28            for j in 0..$arr.ncols() {
29                row_els.push(format!("{:12.3}", $arr[(i, j)]));
30            }
31            let row_str = row_els.into_iter().collect::<Vec<_>>().join(" ");
32            let row_str = format!("{}{}", prefix, row_str);
33            result_els.push(row_str);
34        }
35        result_els.into_iter().collect::<Vec<_>>().join("\n")
36    }};
37}
38
39/// Kalman filter current state
40///
41#[derive(Copy, Clone, Debug)]
42pub struct KalmanState<const X: usize> {
43    mean: SVector<f32, X>,
44    covariance: SMatrix<f32, X, X>,
45}
46
47impl<const X: usize> KalmanState<X> {
48    /// dump the state
49    ///
50    pub fn dump(&self) {
51        eprintln!("Mean={}", pretty_print!(self.mean.transpose()));
52        eprintln!("Covariance={}", pretty_print!(self.covariance));
53    }
54}
55
56impl<const X: usize> TryFrom<KalmanState<X>> for Universal2DBox {
57    type Error = Errors;
58
59    fn try_from(value: KalmanState<X>) -> Result<Self, Self::Error> {
60        if value.mean.len() < 5 {
61            Err(Self::Error::OutOfRange)
62        } else {
63            Ok(Universal2DBox::new(
64                value.mean[0],
65                value.mean[1],
66                if value.mean[2] == 0.0 {
67                    None
68                } else {
69                    Some(value.mean[2])
70                },
71                value.mean[3],
72                value.mean[4],
73            ))
74        }
75    }
76}
77
78impl<const X: usize> TryFrom<KalmanState<X>> for BoundingBox {
79    type Error = Errors;
80
81    fn try_from(value: KalmanState<X>) -> Result<Self, Self::Error> {
82        let bb = Universal2DBox::try_from(value)?;
83        BoundingBox::try_from(&bb)
84    }
85}
86
87pub const DT: u64 = 1;
cecile_supercool_tracker/utils/kalman.rs

cecile_supercool_tracker/utils/
kalman.rs
