Merge branch 'enh-add-kalman-filter' of github.com:oddkiva/sara into …

…enh-add-kalman-filter

cpp/src/DO/Sara/Core/PhysicalQuantities.hpp

@@ -299,10 +299,12 @@ namespace DO::Sara {
   constexpr auto centimeter = meter / 100;
   constexpr auto millimeter = meter / 1000;
 
-  constexpr auto foot = Length{0.3048};
-  constexpr auto inch = Length{0.0254};
+  constexpr auto foot = 0.3048L * meter;
+  constexpr auto inch = 0.0254L * meter;
+  constexpr auto mile = 1609.34L * meter;
 
   constexpr auto second = Time{1};
+  constexpr auto hour = Time{3600};
 
   constexpr auto hertz = Frequency{1};
 
@@ -343,6 +345,11 @@ namespace DO::Sara {
     return v * second;
   }
 
+  constexpr auto operator""_hour(long double v)
+  {
+    return v * hour;
+  }
+
   constexpr auto operator""_Hz(long double v)
   {
     return v * hertz;

cpp/src/DO/Sara/KalmanFilter/KalmanFilter.hpp

@@ -3,51 +3,24 @@
 
 namespace DO::Sara::KalmanFilter {
 
-  //! @brief The step-by-step evolution of the 3D bounding box of the
-  //! pedestrian.
-  template <typename StateVector,            //
-            typename StateTransitionMatrix,  //
-            typename ProcessNoiseVector>
-  struct StateTransitionEquation
+  template <typename StateTransitionEquation, typename ObservationEquation>
+  struct KalmanFilter
   {
-    StateTransitionMatrix F;
-    ProcessNoiseVector w;
+    using State = typename StateTransitionEquation::State;
+    using Observation = typename StateTransitionEquation::Observation;
 
-    inline auto operator()(const StateVector& x) const -> StateVector
+    auto predict(const State& x) -> State
     {
-      return F * x + w;
+      return _state_transition_equation.predict(x);
     }
-  };
 
-  //! @brief The projection of the 3D bounding box into the image plane.
-  //! Sort of...
-  template <typename StateVector,        //
-            typename ObservationVector,  //
-            typename ObservationMatrix,  //
-            typename MeasurementNoiseVector>
-  struct ObservationEquation
-  {
-    ObservationMatrix H;
-    MeasurementNoiseVector v;
-
-    inline auto operator()(const StateVector& x) const -> ObservationVector
+    auto update(const State& x_predicted, const Observation& z) -> State
     {
-      // z = observation vector.
-      const auto z = H * x + v;
-      return z;
+      return _observation_equation.update(x_predicted, z);
     }
-  };
-
-
-  template <typename StateTransitionEquation, typename ObservationEquation>
-  struct KalmanFilter
-  {
-    template <typename StateVector>
-    auto predict(const StateVector& x)
-        -> std::pair<APrioriStateMeanVector, APrioriStateCovarianceMatrix>;
 
-    auto update() -> std::pair<APosterioriStateMeanVector,
-                               APosterioriStateCovarianceMatrix>;
+    StateTransitionEquation _state_transition_equation;
+    ObservationEquation _observation_equation;
   };
 
 }  // namespace DO::Sara::KalmanFilter

cpp/src/DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <DO/Sara/Core/EigenExtension.hpp>
+
+
+namespace DO::Sara::MultipleObjectTracking {
+
+  //! A pedestrian has a state which we model as a cylindric box and encode as a
+  //! 4D vector
+  //!   (x, y, a, h)
+  //! where:
+  //!   - (x, y) is the position of the pedestrian in meters w.r.t. the vehicle
+  //!     frame
+  //!   - a is the aspect ratio
+  //!   - h is the height of the pedestrian
+  //!
+  //! In this modelling, we assume that the road is flat and on the plane
+  //!   z = 0.
+  //!
+  //! That means that the 3D coordinates of the pedestrian feet is
+  //!   (x, y, 0)
+  //! in the vehicle frame.
+  template <typename T>
+  using CylindricBoxObservationVector = Eigen::Vector4<T>;
+
+  //! In practice the pedestrian state is expanded into a larger 12D vector that
+  //! concatenates the 3 4D vectors:
+  //!   (x, y, a, h)         the base state vector
+  //!   (dx, dy, da, dh)     1st-order differential of the base state vector
+  //!   (d2x, d2y, d2a, d2h) 2nd-order differential of the base state vector
+  //!
+  //! The differentials are used indeed in the transition matrix in the Kalman
+  //! filter.
+  template <typename T>
+  using CylindricBoxStateVector = Eigen::Vector<T, 12>;
+
+
+  template <typename T>
+  struct ObservationDistribution
+  {
+    using Mean = CylindricBoxObservationVector<T>;
+    using CovarianceMatrix = Eigen::Matrix4<T>;
+
+    Mean μ;
+    CovarianceMatrix Σ;
+  };
+
+  template <typename T>
+  struct StateDistribution
+  {
+    using Mean = CylindricBoxStateVector<T>;
+    using CovarianceMatrix = Eigen::Matrix<T, 12, 12>;
+
+    Mean μ;
+    CovarianceMatrix Σ;
+  };
+
+
+}  // namespace DO::Sara::MultipleObjectTracking

cpp/src/DO/Sara/MultipleObjectTracking/CosineDistance.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <DO/Sara/Core/EigenExtension.hpp>
+
+
+namespace DO::Sara {
+
+  template <typename Derived>
+  inline auto cosine_similarity(const Eigen::MatrixBase<Derived>& a,
+                                const Eigen::MatrixBase<Derived>& b,
+                                Eigen::MatrixBase<Derived>& c) -> void
+  {
+    c = a * b.transpose();
+  }
+
+  template <typename Derived>
+  inline auto cosine_distance(const Eigen::MatrixBase<Derived>& a,
+                              const Eigen::MatrixBase<Derived>& b,
+                              Eigen::MatrixBase<Derived>& c) -> void
+  {
+    using T = typename Eigen::MatrixBase<Derived>::Scalar;
+    static constexpr auto one = static_cast<T>(1);
+    c = cosine_similarity(a, b);
+    c.array() = one - c.array();
+  }
+
+}  // namespace DO::Sara

cpp/src/DO/Sara/MultipleObjectTracking/ObservationModel.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp>
+#include <DO/Sara/MultipleObjectTracking/ObservationNoiseDistribution.hpp>
+
+
+namespace DO::Sara::MultipleObjectTracking {
+
+  template <typename T>
+  struct ObservationEquation
+  {
+    using State = StateDistribution<T>;
+
+    using Observation = ObservationDistribution<T>;
+    using ObservationNoise = ObservationNoiseDistribution<T>;
+    using ObservationModelMatrix = Eigen::Matrix<T, 4, 12>;
+
+    using Innovation = Observation;
+    using KalmanGain = typename ObservationDistribution<T>::CovarianceMatrix;
+
+    inline static auto observation_model_matrix() -> ObservationModelMatrix
+    {
+      auto H = ObservationModelMatrix{};
+
+      static const auto I = Eigen::Matrix4<T>::Identity();
+      static const auto O = Eigen::Matrix<T, 4, 8>::Zero();
+      H << I, O;
+
+      return H;
+    }
+
+    inline auto innovation(const State& x_a_priori,
+                           const Observation& z) const  //
+        -> Innovation
+    {
+      return {
+          .μ = z - H * x_a_priori,                     //
+          .Σ = H * x_a_priori.Σ * H.transpose() + v.Σ  //
+      };
+    }
+
+    inline auto kalman_gain_matrix(const Observation& x_predicted,
+                                   const Innovation& S) const  //
+        -> KalmanGain
+    {
+      return x_predicted.Σ * H.transpose() * S.Σ.inverse();
+    }
+
+    inline auto update(const State& x_predicted, const Observation& z) -> State
+    {
+      const auto y = innovation(x_predicted, z);
+      const auto K = kalman_gain_matrix(x_predicted, y);
+
+      static const auto I = State::CovarianceMatrix::Identity();
+      return {
+          .μ = x_predicted.μ + K * y.μ,     //
+          .Σ = (I - K * H) * x_predicted.Σ  //
+      };
+    }
+
+    inline auto residual(const Observation& z,
+                         const State& x) const  //
+        -> typename Observation::Mean
+    {
+      return z.μ - H * x.μ;
+    }
+
+    const ObservationModelMatrix H = observation_model_matrix();
+    ObservationNoise v;
+  };
+
+}  // namespace DO::Sara::MultipleObjectTracking

cpp/src/DO/Sara/MultipleObjectTracking/ObservationNoiseDistribution.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp>
+
+#include <DO/Sara/Core/PhysicalQuantities.hpp>
+
+
+namespace DO::Sara::MultipleObjectTracking {
+
+  //! The observation noise is the discrepancy between the state predicted by
+  //! state-transition equation and the observation i.e. the cylindric bounding
+  //! box.
+  //!
+  //! The observation is in our case obtained from metrologic formulas and a
+  //! specified camera calibration parameter.
+  //!
+  //! The observation noise should also factor in measurement error when we
+  //! obtain the observation vector.
+  //!
+  //! In the Kalman filter, the observation noise follows a zero-mean Gaussian
+  //! distribution.
+  template <typename T>
+  struct ObservationNoiseDistribution
+  {
+    using Mean = typename ObservationDistribution<T>::Mean;
+    using CovarianceMatrix =
+        typename ObservationDistribution<T>::CovarianceMatrix;
+
+    static constexpr auto distance_error = 0.5_m;
+    static constexpr auto typical_pedestrian_height = 1.75_m;
+
+    static inline auto default_standard_deviation(  //
+        const Length σ_x = distance_error,          //
+        const Length σ_y = distance_error,          //
+        const T σ_a = static_cast<T>(0.1),
+        const Length σ_h = 5._percent * typical_pedestrian_height)
+        -> CovarianceMatrix
+    {
+      return {σ_x.as<T>(), σ_y.as<T>(), σ_a, σ_h.as<T>()};
+    }
+
+    inline static auto default_covariance_matrix() -> CovarianceMatrix
+    {
+      return default_standard_deviation().array().square().asDiagonal();
+    }
+
+    CovarianceMatrix Σ;
+  };
+
+}  // namespace DO::Sara::MultipleObjectTracking

cpp/src/DO/Sara/MultipleObjectTracking/ProcessNoiseDistribution.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp>
+#include <DO/Sara/MultipleObjectTracking/ObservationNoiseDistribution.hpp>
+
+
+namespace DO::Sara::MultipleObjectTracking {
+
+  //! The process noise is the error that the state transition equation makes.
+  //!
+  //! In the Kalman filter, the process noise follows a zero-mean Gaussian
+  //! distribution.
+  template <typename T>
+  struct ProcessNoiseDistribution
+  {
+    using Mean = typename StateDistribution<T>::Mean;
+    using CovarianceMatrix = typename StateDistribution<T>::CovarianceMatrix;
+
+    static constexpr auto distance_error =
+        ObservationNoiseDistribution<T>::distance_error;
+    static constexpr auto typical_pedestrian_height =
+        ObservationNoiseDistribution<T>::typical_pedestrian_height;
+
+    static constexpr auto speed_error = 5._km / hour;
+    static constexpr auto acceleration_error = (3._km / hour) / 0.5_s;
+
+    static constexpr auto car_speed_acceleration = (60.L * mile / hour) / 5._s;
+    static constexpr auto car_speed_acceleration_error =
+        0.2L * car_speed_acceleration;
+
+    inline static auto default_standard_deviation() -> Mean
+    {
+      auto σ = Mean{};
+
+      // clang-format off
+      σ <<
+        ObservationNoiseDistribution<T>::standard_deviation(),
+        speed_error,
+        speed_error,
+        0,
+        0,
+        acceleration_error,
+        acceleration_error,
+        0,
+        0;
+      // clang-format on
+
+      return σ;
+    }
+
+    inline static auto default_covariance_matrix() -> CovarianceMatrix
+    {
+      return default_standard_deviation().array().square().asDiagonal();
+    }
+
+    CovarianceMatrix Σ;
+  };
+
+}  // namespace DO::Sara::MultipleObjectTracking