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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
 
#ifndef STATE_ESTIMATION__MEASUREMENT__LINEAR_MEASUREMENT_HPP_
#define STATE_ESTIMATION__MEASUREMENT__LINEAR_MEASUREMENT_HPP_
 
#include <common/type_traits.hpp>
#include <state_estimation/measurement/measurement_interface.hpp>
 
#include <Eigen/Core>
 
#include <chrono>
#include <tuple>
 
namespace autoware
{
namespace common
{
namespace state_estimation
{
 
template<typename StateT>
class STATE_ESTIMATION_PUBLIC LinearMeasurement
  : public MeasurementInterface<LinearMeasurement<StateT>>
{
  template<typename OtherStateT>
  using MappingMatrixFrom =
    Eigen::Matrix<typename StateT::Scalar, StateT::size(), OtherStateT::size()>;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using State = StateT;
  LinearMeasurement() = default;
  explicit LinearMeasurement(
    const typename StateT::Vector & measurement,
    const typename StateT::Matrix & covariance)
  : m_measurement{measurement},
    m_covariance{covariance} {}
  static LinearMeasurement create_with_stddev(
    const typename StateT::Vector & measurement,
    const typename StateT::Vector & standard_deviation)
  {
    return LinearMeasurement{measurement,
      standard_deviation.array().square().matrix().asDiagonal()};
  }
 
  template<typename NewScalarT>
  auto cast() const noexcept
  {
    using NewState = decltype(m_measurement.template cast<NewScalarT>());
    return LinearMeasurement<NewState>{
      m_measurement.vector().template cast<NewScalarT>(),
      m_covariance.template cast<NewScalarT>()
    };
  }
 
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::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) {
        (void)this;  // avoid warning -Wunused-lambda-capture
        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;
      };
    using CommonVariablesTuple = typename autoware::common::type_traits::intersect<
      typename StateT::Variables, typename OtherStateT::Variables>::type;
    common::type_traits::visit(CommonVariablesTuple{}, fill_mapping_matrix);
    return m;
  }
 
  friend bool operator==(const LinearMeasurement & lhs, const LinearMeasurement & rhs)
  {
    return (lhs.state() == rhs.state()) &&
           lhs.covariance().isApprox(rhs.covariance());
  }
 
private:
  StateT m_measurement{};
  typename StateT::Matrix m_covariance{StateT::Matrix::Zero()};
};
 
}  // namespace state_estimation
}  // namespace common
}  // namespace autoware
 
#endif  // STATE_ESTIMATION__MEASUREMENT__LINEAR_MEASUREMENT_HPP_