history.hpp

Go to the documentation of this file.

// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.


#ifndef STATE_ESTIMATION_NODES__HISTORY_HPP_
#define STATE_ESTIMATION_NODES__HISTORY_HPP_

#include <common/types.hpp>
#include <helper_functions/mahalanobis_distance.hpp>
#include <mpark_variant_vendor/variant.hpp>
#include <state_estimation_nodes/steady_time_grid.hpp>

#include <chrono>
#include <map>
#include <memory>
#include <utility>

namespace autoware
{
namespace prediction
{

namespace detail
{

template<std::int32_t kNumOfStates>
bool passes_mahalanobis_gate(
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, 1> & sample,
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, 1> & mean,
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, kNumOfStates> & covariance_factor,
  const common::types::float32_t mahalanobis_threshold)
{
  return common::helper_functions::calculate_squared_mahalanobis_distance(
    sample, mean, covariance_factor) < mahalanobis_threshold * mahalanobis_threshold;
}

}  // namespace detail

struct PredictionEvent {};

template<typename FilterT>
struct ResetEvent
{
  typename FilterT::state_vec_t state;
  typename FilterT::square_mat_t covariance_factor;
};

template<
  typename FilterT,
  std::int32_t kNumOfStates,
  typename ... EventT>
class History
{
  class HistoryEntry;
  class EkfStateUpdater;

  using Timestamp = std::chrono::system_clock::time_point;
  using HistoryMap = std::multimap<Timestamp, HistoryEntry>;

public:
  explicit History(
    FilterT & filter,
    const std::size_t max_history_size,
    const common::types::float32_t mahalanobis_threshold)
  : m_filter{filter},
    m_max_history_size{max_history_size},
    m_mahalanobis_threshold{mahalanobis_threshold} {}

  void emplace_event(const Timestamp & timestamp, const HistoryEntry & entry);
  inline bool empty() const noexcept {return m_history.empty();}
  inline std::size_t size() const noexcept {return m_history.size();}
  inline const Timestamp & get_last_timestamp() const noexcept {return m_history.rbegin()->first;}
  inline const HistoryEntry & get_last_event() const noexcept {return m_history.rbegin()->second;}
  const FilterT & get_filter() const noexcept {return m_filter;}
  FilterT & get_filter() noexcept {return m_filter;}

private:
  inline void drop_oldest_event_if_needed()
  {
    if ((m_history.size() >= m_max_history_size) && (m_max_history_size > 0U)) {
      (void) m_history.erase(m_history.begin());
    }
  }

  void update_impacted_events(const typename HistoryMap::iterator & start_iter);

  HistoryMap m_history{};  
  FilterT & m_filter{};  
  std::size_t m_max_history_size{};  
  common::types::float32_t m_mahalanobis_threshold{};  
};

template<typename FilterT, std::int32_t kNumOfStates, typename ... EventT>
class History<FilterT, kNumOfStates, EventT...>::HistoryEntry
{
public:
  template<typename SingleEventT>
  // cppcheck-suppress noExplicitConstructor; Conversion to the variant type takes place.
  HistoryEntry(const SingleEventT & event) : m_event {event} {}

  void update_stored_state(const typename FilterT::state_vec_t & state) noexcept
  {
    m_stored_state = state;
  }
  const typename FilterT::state_vec_t & stored_state() const noexcept {return m_stored_state;}
  void update_stored_covariance_factor(const typename FilterT::square_mat_t & factor) noexcept
  {
    m_stored_covariance_factor = factor;
  }
  const typename FilterT::square_mat_t & stored_covariance_factor() const noexcept
  {
    return m_stored_covariance_factor;
  }
  const mpark::variant<EventT...> & event() const {return m_event;}

private:
  typename FilterT::state_vec_t m_stored_state{FilterT::state_vec_t::Zero()};
  typename FilterT::square_mat_t m_stored_covariance_factor{FilterT::square_mat_t::Zero()};
  mpark::variant<EventT...> m_event;
};

template<typename FilterT, std::int32_t kNumOfStates, typename ... EventT>
class History<FilterT, kNumOfStates, EventT...>::EkfStateUpdater
{
public:
  explicit EkfStateUpdater(
    FilterT & filter,
    const common::types::float32_t mahalanobis_threshold,
    const std::chrono::system_clock::duration & dt = std::chrono::milliseconds{0})
    : m_filter {filter}, m_mahalanobis_threshold{mahalanobis_threshold}, m_dt{dt}
  {}

  template<typename MeasurementT>
  void operator()(const MeasurementT & event)
  {
    m_filter.temporal_update(m_dt);
    // TODO(#887): I see a couple of ways to check mahalanobis distance in case the measurement does
    // not cover the full state. Here I upscale it to the full state, copying the values of the
    // current state for ones missing in the observation. We can alternatively apply the H matrix
    // and only compute the distance in the measurement world. Don't really know which one is best
    // here.
    if (!detail::passes_mahalanobis_gate(
        event.get_values_in_full_state(m_filter.get_state()),
        m_filter.get_state(),
        m_filter.get_covariance(),
        m_mahalanobis_threshold)) {return;}
    (void) m_filter.observation_update(
      event.get_values(),
      MeasurementT::template get_observation_to_state_mapping<kNumOfStates>(),
      event.get_variances());
  }

  void operator()(const ResetEvent<FilterT> & event)
  {
    m_filter.reset(event.state, event.covariance_factor);
  }

  void operator()(const PredictionEvent &)
  {
    m_filter.temporal_update(m_dt);
  }

private:
  FilterT & m_filter{};  
  common::types::float32_t m_mahalanobis_threshold{};  
  std::chrono::system_clock::duration m_dt{};  
};

template<typename FilterT, std::int32_t kNumOfStates, typename ... EventT>
void History<FilterT, kNumOfStates, EventT...>::emplace_event(
  const Timestamp & timestamp, const HistoryEntry & entry)
{
  drop_oldest_event_if_needed();
  const auto iterator_to_inserted_position = m_history.emplace(timestamp, entry);
  update_impacted_events(iterator_to_inserted_position);
}

template<typename FilterT, std::int32_t kNumOfStates, typename ... EventT>
void History<FilterT, kNumOfStates, EventT...>::update_impacted_events(
  const typename HistoryMap::iterator & start_iter)
{
  Timestamp previous_timestamp{};
  if (start_iter == m_history.begin()) {
    if (!mpark::holds_alternative<ResetEvent<FilterT>>(start_iter->second.event())) {
      (void) m_history.erase(start_iter);
      throw std::runtime_error(
              "Non-reset event inserted to the beginning of history. This might "
              "happen if a very old event is inserted into the queue. Consider "
              "increasing the queue size or debug program latencies.");
    }
  } else {
    const auto prev_iter = std::prev(start_iter);
    previous_timestamp = prev_iter->first;
    const auto & prev_entry = prev_iter->second;
    m_filter.reset(prev_entry.stored_state(), prev_entry.stored_covariance_factor());
  }
  for (auto iter = start_iter; iter != m_history.end(); ++iter) {
    const auto current_timestamp = iter->first;
    auto & entry = iter->second;
    mpark::visit(
      EkfStateUpdater{m_filter, m_mahalanobis_threshold, current_timestamp - previous_timestamp},
      entry.event());
    entry.update_stored_state(m_filter.get_state());
    entry.update_stored_covariance_factor(m_filter.get_covariance());
    previous_timestamp = iter->first;
  }
}


}  // namespace prediction
}  // namespace autoware


#endif  // STATE_ESTIMATION_NODES__HISTORY_HPP_