generic_state.hpp

Go to the documentation of this file.

// Copyright 2021 the Autoware Foundation
//
// 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.
//
// Developed by Apex.AI, Inc.


#ifndef KALMAN_FILTER__GENERIC_STATE_HPP_
#define KALMAN_FILTER__GENERIC_STATE_HPP_

#include <common/type_traits.hpp>
#include <common/types.hpp>
#include <helper_functions/type_name.hpp>
#include <kalman_filter/common_variables.hpp>
#include <kalman_filter/visibility_control.hpp>

#include <Eigen/Core>

#include <tuple>

namespace autoware
{
namespace prediction
{

template<typename StateT>
struct is_state;

template<typename ScalarT, typename ... VariableTs>
class KALMAN_FILTER_PUBLIC GenericState
{
  static_assert(
    common::type_traits::conjunction<is_variable<VariableTs>...>::value,
    "\n\nState can only be generated from variables, i.e. types that inherit from Variable.\n\n");
  static_assert(
    std::is_arithmetic<ScalarT>::value, "\n\nThe provided scalar type is not arithmetic.\n\n");
  static_assert(
    sizeof...(VariableTs) > 0, "\n\nCannot create state without variables.\n\n");
  // Hide this under private to make sure it is never ODR-used.
  constexpr static std::int32_t kSize = sizeof...(VariableTs);

public:
  using Variables = std::tuple<VariableTs...>;
  using Vector = Eigen::Matrix<ScalarT, kSize, 1>;
  using Matrix = Eigen::Matrix<ScalarT, kSize, kSize>;
  using Scalar = ScalarT;

  GenericState() = default;

  explicit GenericState(const Vector & vector)
  : m_state{vector} {}

  inline Vector & vector() noexcept {return m_state;}
  inline const Vector & vector() const noexcept {return m_state;}

  inline ScalarT & operator[](const Eigen::Index idx) {return m_state[idx];}
  inline const ScalarT & operator[](const Eigen::Index idx) const {return m_state[idx];}

  template<typename VariableT>
  inline ScalarT & at() noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  template<typename VariableT>
  inline ScalarT & at(const VariableT) noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  template<typename VariableT>
  inline const ScalarT & at() const noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  template<typename VariableT>
  inline const ScalarT & at(const VariableT) const noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }


  template<typename OtherStateT>
  inline OtherStateT copy_into(OtherStateT other_state = OtherStateT{}) const noexcept
  {
    static_assert(
      is_state<OtherStateT>::value,
      "\n\nOtherStateT is not a GenericState.\n\n");
    auto copy_variables = [&other_state, this](auto variable) {
        other_state.at(variable) = this->at(variable);
      };
    const auto & variables_tuple =
      std::conditional_t<(OtherStateT::size() > kSize), GenericState, OtherStateT>::variables();
    common::type_traits::visit(variables_tuple, copy_variables);
    return other_state;
  }

  template<typename VariableT>
  inline constexpr static Eigen::Index index_of() noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return common::type_traits::index<VariableT, Variables>::value;
  }

  inline constexpr static Eigen::Index size() noexcept {return kSize;}

  inline constexpr static Variables variables() noexcept{return Variables{};}

  friend constexpr bool operator==(const GenericState & lhs, const GenericState & rhs)
  {
    return lhs.m_state.isApprox(rhs.m_state);
  }

  friend std::ostream & operator<<(std::ostream & out, const GenericState & state)
  {
    out << "State:";
    auto print = [&out, &state](auto element) {
        out << "\n  " << helper_functions::get_type_name(element) << ": " << state.at(element);
      };
    common::type_traits::visit(GenericState::variables(), print);
    return out;
  }

private:
  Vector m_state{Vector::Zero()};
};

template<typename StateT>
struct KALMAN_FILTER_PUBLIC is_state : public std::false_type {};

template<typename ScalarT, typename ... VariableTs>
struct KALMAN_FILTER_PUBLIC is_state<GenericState<ScalarT, VariableTs...>>
  : public std::true_type {};

template<typename ... Ts>
using FloatState = GenericState<common::types::float32_t, Ts...>;

template<typename ... Ts>
using DoubleState = GenericState<common::types::float64_t, Ts...>;

}  // namespace prediction
}  // namespace autoware

#endif  // KALMAN_FILTER__GENERIC_STATE_HPP_