uniform_noise.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__NOISE_MODEL__UNIFORM_NOISE_HPP_
#define STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_
 
#include <state_estimation/noise_model/noise_interface.hpp>
#include <state_estimation/visibility_control.hpp>
 
#include <Eigen/Core>
 
#include <algorithm>
#include <array>
#include <chrono>
#include <type_traits>
#include <vector>
 
namespace autoware
{
namespace common
{
namespace state_estimation
{
 
template<typename StateT>
class STATE_ESTIMATION_PUBLIC UniformNoise : public NoiseInterface<UniformNoise<StateT>>
{
  using Scalar = typename StateT::Scalar;
  using Matrix = typename StateT::Matrix;
  template<typename OtherScalarT>
  using VarianceVector = Eigen::Matrix<OtherScalarT, StateT::size(), 1>;
 
public:
  using State = StateT;
 
  explicit UniformNoise(const Matrix & covariance) noexcept
  : m_covariance{covariance} {}
 
  template<typename OtherScalarT>
  explicit UniformNoise(
    const std::array<OtherScalarT, State::size()> & variances) noexcept
  : m_covariance{
      Eigen::Map<const VarianceVector<OtherScalarT>>(variances.data())
      .template cast<Scalar>().array().square().matrix().asDiagonal()} {}
 
  template<typename OtherScalarT>
  explicit UniformNoise(const std::vector<OtherScalarT> & variances)
  {
    if (variances.size() != static_cast<std::size_t>(State::size())) {
      throw std::runtime_error(
              "There must be " + std::to_string(State::size()) +
              " variances for initializing the uniform noise model, but " +
              std::to_string(variances.size()) + " provided");
    }
    m_covariance =
      Eigen::Map<const VarianceVector<OtherScalarT>>(variances.data())
      .template cast<Scalar>().array().square().matrix().asDiagonal();
  }
 
 
  template<typename ... VarianceTs>
  explicit UniformNoise(const Scalar variance, const VarianceTs ... variances)
  : UniformNoise{std::array<Scalar, State::size()> {variance, variances ...}}
  {
    static_assert(
      sizeof...(VarianceTs) + 1 == State::size(),
      "Wrong number of variances passed into the UniformNoise constructor");
  }
 
protected:
  friend NoiseInterface<UniformNoise<StateT>>;
 
  Matrix crtp_covariance(const std::chrono::nanoseconds & dt) const noexcept
  {
    return m_covariance * std::chrono::duration_cast<std::chrono::duration<Scalar>>(dt).count();
  }
 
private:
  Matrix m_covariance{};
};
 
}  // namespace state_estimation
}  // namespace common
}  // namespace autoware
 
#endif  // STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_