linear_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 MEASUREMENT__LINEAR_MEASUREMENT_HPP_
#define MEASUREMENT__LINEAR_MEASUREMENT_HPP_

#include <common/type_traits.hpp>
#include <measurement/measurement_interface.hpp>

#include <Eigen/Core>

#include <chrono>
#include <tuple>

namespace autoware
{
namespace prediction
{

template<typename StateT>
class KALMAN_FILTER_PUBLIC LinearMeasurement
  : public MeasurementInterface<LinearMeasurement<StateT>>
{
  template<typename OtherStateT>
  using MappingMatrixFrom =
    Eigen::Matrix<typename StateT::Scalar, StateT::size(), OtherStateT::size()>;

public:
  using State = StateT;
  LinearMeasurement() = default;
  explicit LinearMeasurement(
    const typename StateT::Vector & measurement,
    const typename StateT::Vector & variances)
  : m_measurement{measurement},
    m_variances{variances},
    m_covariance{variances.array().square().matrix().asDiagonal()} {}

protected:
  // Allow the CRTP interface to call private functions from this class.
  friend MeasurementInterface<LinearMeasurement<StateT>>;

  inline StateT & crtp_state() noexcept {return m_measurement;}
  inline const StateT & crtp_state() const noexcept {return m_measurement;}
  inline typename StateT::Vector & crtp_variances() noexcept {return m_variances;}
  inline const typename StateT::Vector & crtp_variances() const noexcept {return m_variances;}
  inline typename StateT::Matrix & crtp_covariance() noexcept {return m_covariance;}
  inline const typename StateT::Matrix & crtp_covariance() const noexcept {return m_covariance;}
  template<typename OtherStateT>
  StateT crtp_create_new_instance_from(const OtherStateT & other_state) const
  {
    return other_state.template copy_into<StateT>();
  }
  template<typename OtherStateT>
  OtherStateT crtp_map_into(const OtherStateT & other_state) const noexcept
  {
    return m_measurement.copy_into(other_state);
  }
  template<typename OtherStateT>
  MappingMatrixFrom<OtherStateT> crtp_mapping_matrix_from(const OtherStateT &) const
  {
    MappingMatrixFrom<OtherStateT> m{MappingMatrixFrom<OtherStateT>::Zero()};
    auto fill_mapping_matrix = [&m, this](auto variable) {
        using VariableT = std::decay_t<decltype(variable)>;
        constexpr auto index_in_this_state = StateT::template index_of<VariableT>();
        constexpr auto index_in_other_state = OtherStateT::template index_of<VariableT>();
        m(index_in_this_state, index_in_other_state) = 1;
      };
    common::type_traits::visit(StateT::variables(), fill_mapping_matrix);
    return m;
  }

  friend bool operator==(const LinearMeasurement & lhs, const LinearMeasurement & rhs)
  {
    return (lhs.state() == rhs.state()) &&
           lhs.variances().isApprox(rhs.variances());
  }

private:
  StateT m_measurement{};
  typename StateT::Vector m_variances{StateT::Vector::Zero()};
  typename StateT::Matrix m_covariance{StateT::Matrix::Zero()};
};

}  // namespace prediction
}  // namespace autoware

#endif  // MEASUREMENT__LINEAR_MEASUREMENT_HPP_