tracking.hpp

Go to the documentation of this file.

#pragma once
#include <ossia/detail/config.hpp>
 
#include <cmath>
 
#include <algorithm>
#include <array>
#include <cstdint>
#include <vector>
 
namespace ossia
{
 
struct kalman_pv_filter
{
  struct process_noise
  {
    float q00, q01, q11;
  };
 
  [[nodiscard]] static OSSIA_INLINE process_noise cwna(float sigma_a, float dt) noexcept
  {
    const float q = sigma_a * sigma_a;
    const float dt2 = dt * dt;
    return {q * dt2 * dt / 3.f, q * dt2 / 2.f, q * dt};
  }
 
  [[nodiscard]] static OSSIA_INLINE process_noise
  diagonal(float std_p, float std_v, float time_ratio) noexcept
  {
    return {std_p * std_p * time_ratio, 0.f, std_v * std_v * time_ratio};
  }
 
  float p{}, v{};
 
  float P00{}, P01{}, P11{};
 
  OSSIA_INLINE void initiate(float p0, float var_p, float var_v) noexcept
  {
    p = p0;
    v = 0.f;
    P00 = var_p;
    P01 = 0.f;
    P11 = var_v;
  }
 
  OSSIA_INLINE void predict(float dt, const process_noise& q) noexcept
  {
    p += v * dt;
    P00 += dt * (2.f * P01 + dt * P11) + q.q00;
    P01 += dt * P11 + q.q01;
    P11 += q.q11;
  }
 
  OSSIA_INLINE void update(float z, float r) noexcept
  {
    const float S = P00 + r;
    if(!(S > 0.f)) [[unlikely]]
      return; // degenerate (or NaN) innovation covariance: skip the update
    const float K0 = P00 / S;
    const float K1 = P01 / S;
    const float y = z - p;
    p += K0 * y;
    v += K1 * y;
    P01 -= K0 * P01;
    P11 -= K1 * P01; // uses the already-updated P01 = (1-K0)P01: exact (I-KH)P
    P00 -= K0 * P00;
  }
 
  [[nodiscard]] OSSIA_INLINE float gating_distance2(float z, float r) const noexcept
  {
    const float S = P00 + r;
    const float y = z - p;
    return S > 0.f ? (y * y) / S : 0.f;
  }
};
 
template <std::size_t N>
struct kalman_point_filter
{
  std::array<kalman_pv_filter, N> axes{};
 
  OSSIA_INLINE void
  initiate(const std::array<float, N>& p0, float var_p, float var_v) noexcept
  {
    for(std::size_t i = 0; i < N; i++)
      axes[i].initiate(p0[i], var_p, var_v);
  }
 
  OSSIA_INLINE void predict(float dt, float sigma_a) noexcept
  {
    const auto q = kalman_pv_filter::cwna(sigma_a, dt);
    for(auto& a : axes)
      a.predict(dt, q);
  }
 
  OSSIA_INLINE void update(const std::array<float, N>& z, float r) noexcept
  {
    for(std::size_t i = 0; i < N; i++)
      axes[i].update(z[i], r);
  }
 
  [[nodiscard]] OSSIA_INLINE float
  gating_distance2(const std::array<float, N>& z, float r) const noexcept
  {
    float d = 0.f;
    for(std::size_t i = 0; i < N; i++)
      d += axes[i].gating_distance2(z[i], r);
    return d;
  }
 
  [[nodiscard]] OSSIA_INLINE std::array<float, N> position() const noexcept
  {
    std::array<float, N> r;
    for(std::size_t i = 0; i < N; i++)
      r[i] = axes[i].p;
    return r;
  }
 
  [[nodiscard]] OSSIA_INLINE std::array<float, N> velocity() const noexcept
  {
    std::array<float, N> r;
    for(std::size_t i = 0; i < N; i++)
      r[i] = axes[i].v;
    return r;
  }
 
  OSSIA_INLINE void set_velocity(const std::array<float, N>& v) noexcept
  {
    for(std::size_t i = 0; i < N; i++)
      axes[i].v = v[i];
  }
};
 
struct match_candidate
{
  float cost;
  std::int32_t track, det;
};
 
inline void greedy_assignment(
    std::vector<match_candidate>& candidates, std::int32_t* track_match,
    std::size_t n_tracks, char* det_used, std::size_t n_dets) noexcept
{
  std::stable_sort(
      candidates.begin(), candidates.end(),
      [](const match_candidate& a, const match_candidate& b) {
    return a.cost < b.cost;
  });
  for(const auto& c : candidates)
  {
    if(c.track < 0 || std::size_t(c.track) >= n_tracks)
      continue;
    if(c.det < 0 || std::size_t(c.det) >= n_dets)
      continue;
    if(track_match[c.track] >= 0 || det_used[c.det])
      continue;
    track_match[c.track] = c.det;
    det_used[c.det] = 1;
  }
}
 
}