measurement.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__MEASUREMENT_HPP_
#define STATE_ESTIMATION_NODES__MEASUREMENT_HPP_

#include <common/types.hpp>
#include <state_estimation_nodes/visibility_control.hpp>

#include <Eigen/Core>

#include <chrono>
#include <cstdint>
#include <tuple>

namespace autoware
{
namespace prediction
{
template<typename T, std::uint32_t... kMeasurementModalities>
class STATE_ESTIMATION_NODES_PUBLIC Measurement
{
  static constexpr std::int32_t EntriesCount = sizeof...(kMeasurementModalities);

  using MeasurementModalityArray =
    std::array<std::uint32_t, static_cast<std::size_t>(EntriesCount)>;

  template<typename S, std::int32_t kSize>
  using Vector = Eigen::Matrix<S, kSize, 1U>;
  template<typename S, std::int32_t kNumOfColumns>
  using MapMatrix = Eigen::Matrix<S, EntriesCount, kNumOfColumns>;

  using MeasurementVector = Vector<T, EntriesCount>;
  using VarianceVector = Vector<T, EntriesCount>;

public:
  explicit Measurement(
    const std::chrono::system_clock::time_point & acquisition_time,
    const MeasurementVector & values, const VarianceVector & variances = {})
  : m_acquisition_time{acquisition_time}, m_measurement_values{values}, m_variances{variances} {}

  inline const MeasurementVector & get_values() const noexcept {return m_measurement_values;}

  inline const VarianceVector & get_variances() const noexcept {return m_variances;}

  inline const std::chrono::system_clock::time_point & get_acquisition_time() const noexcept
  {
    return m_acquisition_time;
  }

  template<std::int32_t kNumOfStates>
  static MapMatrix<T, kNumOfStates> get_observation_to_state_mapping()
  {
    using MatrixT = MapMatrix<T, kNumOfStates>;
    MatrixT observation_to_state_mapping{MatrixT::Zero()};
    for (auto i = 0U; i < kMeasurementModalitiesArray.size(); ++i) {
      observation_to_state_mapping(i, kMeasurementModalitiesArray[i]) = static_cast<T>(1.0);
    }
    return observation_to_state_mapping;
  }

  template<std::int32_t kFullStateSize>
  Vector<T, kFullStateSize> get_values_in_full_state(
    const Vector<T, kFullStateSize> & donor_state = Vector<T, kFullStateSize>::Zero()) const
  {
    static_assert(kFullStateSize >= EntriesCount, "State too small to contain this measurement.");
    Vector<T, kFullStateSize> state{donor_state};
    for (auto i = 0U; i < kMeasurementModalitiesArray.size(); ++i) {
      state[kMeasurementModalitiesArray[i]] = m_measurement_values[i];
    }
    return state;
  }

private:
  static constexpr MeasurementModalityArray kMeasurementModalitiesArray {
    kMeasurementModalities ...};
  std::chrono::system_clock::time_point m_acquisition_time;
  MeasurementVector m_measurement_values{};
  VarianceVector m_variances{};
};

template<typename T, std::uint32_t... kMeasurementModalities>
constexpr typename Measurement<T, kMeasurementModalities...>::MeasurementModalityArray
Measurement<T, kMeasurementModalities...>::kMeasurementModalitiesArray;

}  // namespace prediction
}  // namespace autoware


#endif  // STATE_ESTIMATION_NODES__MEASUREMENT_HPP_