Kalman filter (#29)

Kalman filter implemented
insight-platform · Jul 25, 2022 · a84e22d · a84e22d
1 parent 6ca7690
commit a84e22d
Show file tree

Hide file tree

Showing 20 changed files with 507 additions and 221 deletions.
diff --git a/Cargo.toml b/Cargo.toml
@@ -7,7 +7,7 @@ repository = "https://github.com/insight-platform/Similari"
 readme = "README.md"
 keywords = ["machine-learning", "similarity", "tracking"]
 categories = ["algorithms", "data-structures", "database", "mathematics", "science"]
-version = "0.18.0"
+version = "0.18.1"
 edition = "2021"
 license="Apache-2.0"
 rust-version = "1.62"
@@ -22,6 +22,7 @@ ultraviolet = "0.9.0"
 crossbeam = "0.8.1"
 rand = "0.8.5"
 log = "0.4.17"
+nalgebra = "0.31.0"
 
 [dev-dependencies]
 wide = "0.7.4"

diff --git a/benches/feature_tracker.rs b/benches/feature_tracker.rs
@@ -3,8 +3,8 @@
 extern crate test;
 
 use similari::distance::euclidean;
+use similari::examples::FeatGen;
 use similari::store::TrackStore;
-use similari::test_stuff::FeatGen;
 use similari::track::{
     MetricOutput, NoopLookup, ObservationMetric, ObservationMetricOk, ObservationSpec,
     ObservationsDb, Track, TrackAttributes, TrackAttributesUpdate, TrackStatus,

diff --git a/benches/iou_tracker.rs b/benches/iou_tracker.rs
@@ -2,10 +2,11 @@
 
 extern crate test;
 
+use similari::examples::iou::{BBoxAttributes, BBoxAttributesUpdate, IOUMetric};
+use similari::examples::BoxGen2;
 use similari::store::TrackStore;
-use similari::test_stuff::iou::{BBoxAttributes, BBoxAttributesUpdate, IOUMetric, IOUTopNVoting};
-use similari::test_stuff::{BBox, BoxGen2};
 use similari::track::Track;
+use similari::utils::bbox::{BBox, IOUTopNVoting};
 use similari::voting::Voting;
 use std::time::Instant;
 use test::Bencher;

diff --git a/benches/simple_search.rs b/benches/simple_search.rs
@@ -3,8 +3,8 @@
 extern crate test;
 
 use rand::{distributions::Uniform, Rng};
+use similari::examples::{UnboundAttributeUpdate, UnboundAttrs, UnboundMetric};
 use similari::store;
-use similari::test_stuff::{UnboundAttributeUpdate, UnboundAttrs, UnboundMetric};
 use similari::track::notify::NoopNotifier;
 use similari::track::utils::FromVec;
 use similari::track::{Observation, Track};

diff --git a/benches/track_search.rs b/benches/track_search.rs
@@ -3,8 +3,8 @@
 extern crate test;
 
 use rand::{distributions::Uniform, Rng};
+use similari::examples::{UnboundAttributeUpdate, UnboundAttrs, UnboundMetric};
 use similari::store;
-use similari::test_stuff::{UnboundAttributeUpdate, UnboundAttrs, UnboundMetric};
 use similari::track::{Observation, Track};
 
 use similari::track::notify::NoopNotifier;

diff --git a/examples/incremental_track_build.rs b/examples/incremental_track_build.rs
@@ -1,10 +1,11 @@
 use similari::distance::euclidean;
+use similari::examples::{BoxGen2, FeatGen2};
 use similari::prelude::*;
-use similari::test_stuff::{BBox, BoxGen2, FeatGen2};
 use similari::track::{
     MetricOutput, NoopLookup, ObservationAttributes, ObservationMetric, ObservationMetricOk,
     ObservationSpec, ObservationsDb, TrackAttributes, TrackAttributesUpdate, TrackStatus,
 };
+use similari::utils::bbox::BBox;
 use similari::voting::topn::TopNVoting;
 use similari::voting::Voting;
 use std::thread;

diff --git a/examples/iou_tracker.rs b/examples/iou_tracker.rs
@@ -1,7 +1,8 @@
+use similari::examples::iou::{BBoxAttributes, BBoxAttributesUpdate, IOUMetric};
+use similari::examples::{current_time_ms, BoxGen2};
 use similari::store::TrackStore;
-use similari::test_stuff::iou::{BBoxAttributes, BBoxAttributesUpdate, IOUMetric, IOUTopNVoting};
-use similari::test_stuff::{current_time_ms, BBox, BoxGen2};
 use similari::track::Track;
+use similari::utils::bbox::{BBox, IOUTopNVoting};
 use similari::voting::Voting;
 use std::thread;
 use std::time::Duration;

diff --git a/examples/simple.rs b/examples/simple.rs
@@ -1,5 +1,5 @@
+use similari::examples::{vec2, SimpleAttributeUpdate, SimpleAttrs, SimpleMetric};
 use similari::store;
-use similari::test_stuff::{vec2, SimpleAttributeUpdate, SimpleAttrs, SimpleMetric};
 use similari::track::notify::NoopNotifier;
 use similari::track::Track;
 use similari::voting::topn::TopNVoting;

diff --git a/examples/track_merging.rs b/examples/track_merging.rs
@@ -3,9 +3,9 @@ use anyhow::Result;
 use itertools::Itertools;
 use once_cell::sync::OnceCell;
 use similari::distance::euclidean;
+use similari::examples::current_time_ms;
+use similari::examples::FeatGen2;
 use similari::store::TrackStore;
-use similari::test_stuff::current_time_ms;
-use similari::test_stuff::FeatGen2;
 use similari::track::notify::NoopNotifier;
 use similari::track::{
     MetricOutput, NoopLookup, ObservationAttributes, ObservationMetric, ObservationSpec,
@@ -204,7 +204,7 @@ fn cam_tracking_attributes_update_test() {
 
 #[test]
 fn feat_gen() {
-    use similari::test_stuff::FeatGen2;
+    use similari::examples::FeatGen2;
     use std::ops::Sub;
     use ultraviolet::f32x8;
 

diff --git a/src/test_stuff.rs → src/examples.rs b/src/test_stuff.rs → src/examples.rs
@@ -6,12 +6,11 @@ use crate::track::{
     MetricOutput, NoopLookup, Observation, ObservationAttributes, ObservationMetric,
     ObservationSpec, ObservationsDb, TrackAttributes, TrackAttributesUpdate, TrackStatus,
 };
-use crate::EPS;
+use crate::utils::bbox::BBox;
 use anyhow::Result;
 use rand::distributions::Uniform;
 use rand::prelude::ThreadRng;
 use rand::Rng;
-use std::cmp::Ordering;
 use std::time::{Duration, SystemTime, UNIX_EPOCH};
 use thiserror::Error;
 
@@ -165,31 +164,6 @@ pub fn vec2(x: f32, y: f32) -> Observation {
     Observation::from_vec(vec![x, y])
 }
 
-impl ObservationAttributes for f32 {
-    type MetricObject = f32;
-
-    fn calculate_metric_object(
-        left: &Option<Self>,
-        right: &Option<Self>,
-    ) -> Option<Self::MetricObject> {
-        if let (Some(left), Some(right)) = (left, right) {
-            Some((left - right).abs())
-        } else {
-            None
-        }
-    }
-}
-impl ObservationAttributes for () {
-    type MetricObject = ();
-
-    fn calculate_metric_object(
-        _left: &Option<Self>,
-        _right: &Option<Self>,
-    ) -> Option<Self::MetricObject> {
-        None
-    }
-}
-
 pub struct FeatGen2 {
     x: f32,
     y: f32,
@@ -286,111 +260,6 @@ pub fn current_time_sec() -> u64 {
     current_time_span().as_secs()
 }
 
-#[derive(Clone, Default, Debug)]
-pub struct BBox {
-    x: f32,
-    y: f32,
-    width: f32,
-    height: f32,
-}
-
-impl ObservationAttributes for BBox {
-    type MetricObject = f32;
-
-    fn calculate_metric_object(
-        _left: &Option<Self>,
-        _right: &Option<Self>,
-    ) -> Option<Self::MetricObject> {
-        match (_left, _right) {
-            (Some(l), Some(r)) => {
-                assert!(l.width > 0.0);
-                assert!(l.height > 0.0);
-                assert!(r.width > 0.0);
-                assert!(r.height > 0.0);
-
-                let (ax0, ay0, ax1, ay1) = (l.x, l.y, l.x + l.width, l.y + l.height);
-                let (bx0, by0, bx1, by1) = (r.x, r.y, r.x + r.width, r.y + r.height);
-
-                let (x1, y1) = (ax0.max(bx0), ay0.max(by0));
-                let (x2, y2) = (ax1.min(bx1), ay1.min(by1));
-
-                let int_width = x2 - x1;
-                let int_height = y2 - y1;
-
-                let intersection = if int_width > 0.0 && int_height > 0.0 {
-                    int_width * int_height
-                } else {
-                    0.0
-                };
-
-                let union = (ax1 - ax0) * (ay1 - ay0) + (bx1 - bx0) * (by1 - by0) - intersection;
-                Some(intersection / union)
-            }
-            _ => None,
-        }
-    }
-}
-
-impl PartialOrd for BBox {
-    fn partial_cmp(&self, _other: &Self) -> Option<Ordering> {
-        unreachable!()
-    }
-}
-
-impl PartialEq<Self> for BBox {
-    fn eq(&self, other: &Self) -> bool {
-        (self.x - other.x).abs() < EPS
-            && (self.y - other.y).abs() < EPS
-            && (self.width - other.width).abs() < EPS
-            && (self.height - other.height).abs() < EPS
-    }
-}
-
-#[cfg(test)]
-mod tests {
-    use crate::test_stuff::BBox;
-    use crate::track::ObservationAttributes;
-
-    #[test]
-    fn test_iou() {
-        let bb1 = BBox {
-            x: -1.0,
-            y: -1.0,
-            width: 2.0,
-            height: 2.0,
-        };
-
-        let bb2 = BBox {
-            x: -0.9,
-            y: -0.9,
-            width: 2.0,
-            height: 2.0,
-        };
-        let bb3 = BBox {
-            x: 1.0,
-            y: 1.0,
-            width: 3.0,
-            height: 3.0,
-        };
-
-        assert!(
-            BBox::calculate_metric_object(&Some(bb1.clone()), &Some(bb1.clone())).unwrap() > 0.999
-        );
-        assert!(
-            BBox::calculate_metric_object(&Some(bb2.clone()), &Some(bb2.clone())).unwrap() > 0.999
-        );
-        assert!(
-            BBox::calculate_metric_object(&Some(bb1.clone()), &Some(bb2.clone())).unwrap() > 0.8
-        );
-        assert!(
-            BBox::calculate_metric_object(&Some(bb1.clone()), &Some(bb3.clone())).unwrap() < 0.001
-        );
-        assert!(
-            BBox::calculate_metric_object(&Some(bb2.clone()), &Some(bb3.clone())).unwrap() < 0.001
-        );
-    }
-}
-
 pub struct FeatGen {
     x: f32,
     len: usize,

diff --git a/src/test_stuff/iou.rs → src/examples/iou.rs b/src/test_stuff/iou.rs → src/examples/iou.rs
@@ -1,13 +1,9 @@
-use crate::test_stuff::BBox;
 use crate::track::{
-    MetricOutput, NoopLookup, ObservationAttributes, ObservationMetric, ObservationMetricOk,
-    ObservationSpec, ObservationsDb, TrackAttributes, TrackAttributesUpdate, TrackStatus,
+    MetricOutput, NoopLookup, ObservationAttributes, ObservationMetric, ObservationSpec,
+    ObservationsDb, TrackAttributes, TrackAttributesUpdate, TrackStatus,
 };
-use crate::voting::topn::TopNVotingElt;
-use crate::voting::Voting;
+use crate::utils::bbox::BBox;
 use anyhow::Result;
-use itertools::Itertools;
-use std::collections::HashMap;
 
 #[derive(Debug, Clone, Default)]
 pub struct BBoxAttributes {
@@ -99,67 +95,3 @@ impl ObservationMetric<BBoxAttributes, BBox> for IOUMetric {
         Ok(())
     }
 }
-
-pub struct IOUTopNVoting {
-    pub topn: usize,
-    pub min_distance: f32,
-    pub min_votes: usize,
-}
-
-impl Voting<BBox> for IOUTopNVoting {
-    type WinnerObject = TopNVotingElt;
-
-    fn winners<T>(&self, distances: T) -> HashMap<u64, Vec<TopNVotingElt>>
-    where
-        T: IntoIterator<Item = ObservationMetricOk<BBox>>,
-    {
-        let counts: Vec<_> = distances
-            .into_iter()
-            .filter(
-                |ObservationMetricOk {
-                     from: _,
-                     to: _track,
-                     attribute_metric: attr_dist,
-                     feature_distance: _,
-                 }| match attr_dist {
-                    Some(e) => *e >= self.min_distance,
-                    _ => false,
-                },
-            )
-            .map(
-                |ObservationMetricOk {
-                     from: src_track,
-                     to: dest_track,
-                     attribute_metric: _,
-                     feature_distance: _,
-                 }| (src_track, dest_track),
-            )
-            .counts()
-            .into_iter()
-            .filter(|(_, count)| *count >= self.min_votes)
-            .map(|((q, w), c)| TopNVotingElt {
-                query_track: q,
-                winner_track: w,
-                votes: c,
-            })
-            .collect::<Vec<_>>();
-
-        let mut results: HashMap<u64, Vec<TopNVotingElt>> = HashMap::new();
-
-        for c in counts {
-            let key = c.query_track;
-            if let Some(val) = results.get_mut(&key) {
-                val.push(c);
-            } else {
-                results.insert(key, vec![c]);
-            }
-        }
-
-        for counts in results.values_mut() {
-            counts.sort_by(|l, r| r.votes.partial_cmp(&l.votes).unwrap());
-            counts.truncate(self.topn);
-        }
-
-        results
-    }
-}
diff --git a/src/lib.rs b/src/lib.rs
@@ -12,14 +12,15 @@ pub mod distance;
 
 /// Various auxiliary testing and example components
 ///
-pub mod test_stuff;
+pub mod examples;
 
 pub mod prelude;
 /// Holds basic abstractions for tracking - [Track](track::Track), auxiliary structures, traits, and functions. It defines the track's
 /// look and feel, provides `Track` structure that holds track attributes and features, can accumulate track features and
 /// calculate feature distances between pair of tracks.
 ///
 pub mod track;
+pub mod utils;
 
 pub use track::store;
 pub use track::voting;

diff --git a/src/track.rs b/src/track.rs
@@ -604,8 +604,8 @@ where
 #[cfg(test)]
 mod tests {
     use crate::distance::euclidean;
+    use crate::examples::current_time_sec;
     use crate::prelude::TrackBuilder;
-    use crate::test_stuff::current_time_sec;
     use crate::track::utils::{feature_attributes_sort_dec, FromVec};
     use crate::track::{
         LookupRequest, MetricOutput, NoopLookup, Observation, ObservationAttributes,

diff --git a/src/track/builder.rs b/src/track/builder.rs
@@ -164,7 +164,7 @@ where
 
 #[cfg(test)]
 mod tests {
-    use crate::test_stuff::{UnboundAttributeUpdate, UnboundAttrs, UnboundMetric};
+    use crate::examples::{UnboundAttributeUpdate, UnboundAttrs, UnboundMetric};
     use crate::track::builder::{ObservationBuilder, TrackBuilder};
     use crate::track::notify::NoopNotifier;
     use crate::track::utils::FromVec;