Autoware.Auto
wiener_noise.hpp
Go to the documentation of this file.
1 // Copyright 2021 Apex.AI, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 
17 #ifndef STATE_ESTIMATION__NOISE_MODEL__WIENER_NOISE_HPP_
18 #define STATE_ESTIMATION__NOISE_MODEL__WIENER_NOISE_HPP_
19 
23 
24 #include <algorithm>
25 #include <array>
26 #include <vector>
27 
28 namespace autoware
29 {
30 namespace common
31 {
32 namespace state_estimation
33 {
34 
40 template<typename StateT>
41 struct number_of_acceleration_components : public std::integral_constant<std::size_t, 0UL>
42 {
43  static_assert(sizeof(StateT) == 0, "This class must be specialized to a specific state type.");
44 };
45 
53 template<typename StateT>
54 class STATE_ESTIMATION_PUBLIC WienerNoise : public NoiseInterface<WienerNoise<StateT>>
55 {
56  using AccelerationArray = std::array<
57  typename StateT::Scalar, number_of_acceleration_components<StateT>::value>;
58 
59 public:
60  using State = StateT;
61 
73  explicit WienerNoise(const AccelerationArray & acceleration_variances)
74  : m_acceleration_variances{acceleration_variances} {}
75 
76 protected:
77  // Required to allow the crtp interface call the following functions.
79 
85  typename State::Matrix crtp_covariance(const std::chrono::nanoseconds &) const;
86 
87 private:
88  AccelerationArray m_acceleration_variances{};
89 };
90 
91 template<typename StateT, typename OtherScalarT>
92 auto make_wiener_noise(const std::vector<OtherScalarT> & acceleration_variances)
93 {
94  std::array<typename StateT::Scalar, number_of_acceleration_components<StateT>::value> variances;
95  if (acceleration_variances.size() != variances.size()) {
96  throw std::runtime_error(
97  "There must be " + std::to_string(variances.size()) + " acceleration variances");
98  }
99  std::copy(acceleration_variances.begin(), acceleration_variances.end(), variances.begin());
100  return WienerNoise<StateT>{variances};
101 }
102 
107 template<typename ScalarT>
109  : public std::integral_constant<std::size_t, 2UL> {};
110 
115 template<typename ScalarT>
117  : public std::integral_constant<std::size_t, 3UL> {};
118 
123 template<typename ScalarT>
125  : public std::integral_constant<std::size_t, 3UL> {};
126 
131 template<typename ScalarT>
133  : public std::integral_constant<std::size_t, 4UL> {};
134 
139 template<typename ScalarT>
141  : public std::integral_constant<std::size_t, 6UL> {};
142 
143 
144 } // namespace state_estimation
145 } // namespace common
146 } // namespace autoware
147 
148 #endif // STATE_ESTIMATION__NOISE_MODEL__WIENER_NOISE_HPP_
autoware::common::state_estimation::WienerNoise::State
StateT State
Definition: wiener_noise.hpp:60
autoware::common::state_estimation::NoiseInterface
A CRTP interface for implementing noise models used for providing motion model noise covariance.
Definition: noise_interface.hpp:38
autoware::common::state_estimation::WienerNoise::WienerNoise
WienerNoise(const AccelerationArray &acceleration_variances)
Constructor from acceleration variances.
Definition: wiener_noise.hpp:73
autoware::common::state_vector::GenericState
A representation of a generic state vectors with specified variables.
Definition: generic_state.hpp:60
autoware::common::state_estimation::make_wiener_noise
auto make_wiener_noise(const std::vector< OtherScalarT > &acceleration_variances)
Definition: wiener_noise.hpp:92
autoware::common::state_estimation::number_of_acceleration_components
A trait that defines the number of acceleration components.
Definition: wiener_noise.hpp:41
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
noise_interface.hpp
autoware_auto_create_pkg.main.copy
def copy(src, dest)
Definition: main.py:27
autoware::common::state_estimation::WienerNoise
A class that describes the Wiener process noise.
Definition: wiener_noise.hpp:54
visibility_control.hpp
common_states.hpp
This file holds a collection of states that are commonly used in this package.