From 569de8146474b2155389f651f6f66fdd201f418b Mon Sep 17 00:00:00 2001
From: Addam Dominec <a.dominec@gmail.com>
Date: Sun, 14 Nov 2021 19:34:00 +0100
Subject: [PATCH] expose momentum_filter to Python (#2457)

---
 tools/python/src/other.cpp | 78 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 78 insertions(+)

diff --git a/tools/python/src/other.cpp b/tools/python/src/other.cpp
index 3e0149022..c6ede6f46 100644
--- a/tools/python/src/other.cpp
+++ b/tools/python/src/other.cpp
@@ -8,6 +8,7 @@
 #include <dlib/sparse_vector.h>
 #include <dlib/optimization.h>
 #include <dlib/statistics/running_gradient.h>
+#include <dlib/filtering.h>
 
 using namespace dlib;
 using namespace std;
@@ -119,6 +120,19 @@ void hit_enter_to_continue()
 
 // ----------------------------------------------------------------------------------------
 
+string print_momentum_filter(const momentum_filter& r)
+{
+    std::ostringstream sout;
+    sout << "momentum_filter(";
+    sout << "measurement_noise="<<r.get_measurement_noise();
+    sout << ", typical_acceleration="<<r.get_typical_acceleration();
+    sout << ", max_measurement_deviation="<<r.get_max_measurement_deviation();
+    sout << ")";
+    return sout.str();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void bind_other(py::module &m)
 {
     m.def("max_cost_assignment", _max_cost_assignment, py::arg("cost"),
@@ -264,5 +278,69 @@ ensures \n\
 
     m.def("probability_that_sequence_is_increasing",probability_that_sequence_is_increasing, py::arg("time_series"),
         "returns the probability that the given sequence of real numbers is increasing in value over time.");
+
+    {
+        typedef momentum_filter type;
+        py::class_<type>(m, "momentum_filter",
+            R"asdf( 
+                This object is a simple tool for filtering a single scalar value that
+                measures the location of a moving object that has some non-trivial
+                momentum.  Importantly, the measurements are noisy and the object can
+                experience sudden unpredictable accelerations.  To accomplish this
+                filtering we use a simple Kalman filter with a state transition model of:
+
+                    position_{i+1} = position_{i} + velocity_{i} 
+                    velocity_{i+1} = velocity_{i} + some_unpredictable_acceleration
+
+                and a measurement model of:
+                    
+                    measured_position_{i} = position_{i} + measurement_noise
+
+                Where some_unpredictable_acceleration and measurement_noise are 0 mean Gaussian 
+                noise sources with standard deviations of get_typical_acceleration() and
+                get_measurement_noise() respectively.
+
+                To allow for really sudden and large but infrequent accelerations, at each
+                step we check if the current measured position deviates from the predicted
+                filtered position by more than get_max_measurement_deviation()*get_measurement_noise() 
+                and if so we adjust the filter's state to keep it within these bounds.
+                This allows the moving object to undergo large unmodeled accelerations, far
+                in excess of what would be suggested by get_typical_acceleration(), without
+                then experiencing a long lag time where the Kalman filter has to "catch
+                up" to the new position.  )asdf"
+        )
+        .def(py::init<double,double,double>(), py::arg("measurement_noise"), py::arg("typical_acceleration"), py::arg("max_measurement_deviation"))
+        .def("measurement_noise",   [](const momentum_filter& a){return a.get_measurement_noise();})
+        .def("typical_acceleration",   [](const momentum_filter& a){return a.get_typical_acceleration();})
+        .def("max_measurement_deviation",   [](const momentum_filter& a){return a.get_max_measurement_deviation();})
+        .def("__call__", [](momentum_filter& f, const double r){return f(r); })
+        .def("__repr__", print_momentum_filter)
+        .def(py::pickle(&getstate<type>, &setstate<type>));
+    }
+
+    m.def("find_optimal_momentum_filter",
+        [](const py::object sequence, const double smoothness ) {
+            return find_optimal_momentum_filter(python_list_to_vector<double>(sequence), smoothness);
+        },
+        py::arg("sequence"),
+        py::arg("smoothness")=1,
+        R"asdf(requires
+            - sequences.size() != 0
+            - for all valid i: sequences[i].size() > 4
+            - smoothness >= 0
+        ensures
+            - This function finds the "optimal" settings of a momentum_filter based on
+              recorded measurement data stored in sequences.  Here we assume that each
+              vector in sequences is a complete track history of some object's measured
+              positions.  What we do is find the momentum_filter that minimizes the
+              following objective function:
+                 sum of abs(predicted_location[i] - measured_location[i]) + smoothness*abs(filtered_location[i]-filtered_location[i-1])
+                 Where i is a time index.
+              The sum runs over all the data in sequences.  So what we do is find the
+              filter settings that produce smooth filtered trajectories but also produce
+              filtered outputs that are as close to the measured positions as possible.
+              The larger the value of smoothness the less jittery the filter outputs will
+              be, but they might become biased or laggy if smoothness is set really high.)asdf"
+    );
 }