Branch data Line data Source code
1 : : // Copyright 2017-2020 the Autoware Foundation
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 : : /// \file
18 : : /// \brief This file contains a 1D linear lookup table implementation
19 : :
20 : : #ifndef GEOMETRY__LOOKUP_TABLE_HPP_
21 : : #define GEOMETRY__LOOKUP_TABLE_HPP_
22 : :
23 : : #include <cmath>
24 : : #include <stdexcept>
25 : : #include <vector>
26 : :
27 : : #include "common/types.hpp"
28 : : #include "geometry/interval.hpp"
29 : :
30 : : namespace autoware
31 : : {
32 : : namespace common
33 : : {
34 : : namespace helper_functions
35 : : {
36 : :
37 : : namespace
38 : : {
39 : :
40 : : /**
41 : : * @brief Compute a scaling between 'a' and 'b'
42 : : * @note scaling is clamped to be in the interval [0, 1]
43 : : */
44 : : template<typename T, typename U>
45 : 18 : T interpolate(const T & a, const T & b, U scaling)
46 : : {
47 [ + + + - : 18 : static const geometry::Interval<U> UNIT_INTERVAL(static_cast<U>(0), static_cast<U>(1));
+ - ]
48 : 18 : scaling = geometry::Interval<U>::clamp_to(UNIT_INTERVAL, scaling);
49 : :
50 : 18 : const auto m = (b - a);
51 : 18 : const auto offset = static_cast<U>(m) * scaling;
52 : 18 : return a + static_cast<T>(offset);
53 : : }
54 : :
55 : : // TODO(c.ho) support more forms of interpolation as template functor
56 : : // Actual lookup logic, assuming all invariants hold:
57 : : // Throw if value is not finite
58 : : template<typename T>
59 [ + + ]: 16 : T lookup_impl_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
60 : : {
61 [ - + ]: 8 : if (!std::isfinite(value)) {
62 [ # # ]: 0 : throw std::domain_error{"Query value is not finite (NAN or INF)"};
63 : : }
64 [ + + ]: 16 : if (value <= domain.front()) {
65 : 4 : return range.front();
66 [ + + ]: 12 : } else if (value >= domain.back()) {
67 : 4 : return range.back();
68 : : } else {
69 : : // Fall through to normal case
70 : : }
71 : :
72 : : auto second_idx{0U};
73 [ + - ]: 12 : for (auto idx = 1U; idx < domain.size(); ++idx) {
74 [ + + ]: 12 : if (value < domain[idx]) {
75 : : second_idx = idx;
76 : : break;
77 : : }
78 : : }
79 : : // T must be a floating point between 0 and 1
80 : 8 : const auto num = static_cast<double>(value - domain[second_idx - 1U]);
81 : 8 : const auto den = static_cast<double>(domain[second_idx] - domain[second_idx - 1U]);
82 : 8 : const auto t = num / den;
83 : 8 : const auto val = interpolate(range[second_idx - 1U], range[second_idx], t);
84 : 8 : return static_cast<T>(val);
85 : : }
86 : :
87 : : // Check invariants for table lookup:
88 : : template<typename T>
89 : 28 : void check_table_lookup_invariants(const std::vector<T> & domain, const std::vector<T> & range)
90 : : {
91 [ + + ]: 28 : if (domain.size() != range.size()) {
92 [ + - ]: 4 : throw std::domain_error{"Domain's size does not match range's"};
93 : : }
94 [ + + ]: 24 : if (domain.empty()) {
95 [ + - ]: 4 : throw std::domain_error{"Empty domain or range"};
96 : : }
97 : : // Check if sorted: Can start at 1 because not empty
98 [ + + ]: 52 : for (auto idx = 1U; idx < domain.size(); ++idx) {
99 [ + + ]: 36 : if (domain[idx] <= domain[idx - 1U]) { // This is safe because indexing starts at 1
100 [ + - ]: 4 : throw std::domain_error{"Domain is not sorted"};
101 : : }
102 : : }
103 : 16 : }
104 : : } // namespace
105 : :
106 : : /// Do a 1D table lookup: Does some semi-expensive O(N) error checking first.
107 : : /// If query value fall out of the domain, then the value at the corresponding edge of the domain is
108 : : /// returned.
109 : : /// \param[in] domain The domain, or set of x values
110 : : /// \param[in] range The range, or set of y values
111 : : /// \param[in] value The point in the domain to query, x
112 : : /// \return A linearly interpolated value y, corresponding to the query, x
113 : : /// \throw std::domain_error If domain or range is empty
114 : : /// \throw std::domain_error If range is not the same size as domain
115 : : /// \throw std::domain_error If domain is not sorted
116 : : /// \throw std::domain_error If value is not finite (NAN or INF)
117 : : /// \tparam T The type of the function, must be interpolatable
118 : : template<typename T>
119 : : T lookup_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
120 : : {
121 [ - + - + : 12 : check_table_lookup_invariants(domain, range);
- + - + -
+ - + - +
- + - + -
+ - + -
+ ]
122 : :
123 [ # # # # : 0 : return lookup_impl_1d(domain, range, value);
# # # # #
# # # # #
# # # # #
# # # #
# ]
124 : : }
125 : :
126 : : /// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed
127 : : /// into the constructor and not done in the lookup function call
128 : : /// \tparam T The type of the function, must be interpolatable
129 : : template<typename T>
130 : : class LookupTable1D
131 : : {
132 : : public:
133 : : /// Constructor
134 : : /// \param[in] domain The domain, or set of x values
135 : : /// \param[in] range The range, or set of y values
136 : : /// \throw std::domain_error If domain or range is empty
137 : : /// \throw std::domain_error If range is not the same size as domain
138 : : /// \throw std::domain_error If domain is not sorted
139 : 16 : LookupTable1D(const std::vector<T> & domain, const std::vector<T> & range)
140 : : : m_domain{domain},
141 [ + - ]: 16 : m_range{range}
142 : : {
143 [ + - ]: 16 : check_table_lookup_invariants(m_domain, m_range);
144 : 16 : }
145 : :
146 : : /// Move constructor
147 : : /// \param[in] domain The domain, or set of x values
148 : : /// \param[in] range The range, or set of y values
149 : : /// \throw std::domain_error If domain or range is empty
150 : : /// \throw std::domain_error If range is not the same size as domain
151 : : /// \throw std::domain_error If domain is not sorted
152 : : LookupTable1D(std::vector<T> && domain, std::vector<T> && range)
153 : : : m_domain{domain},
154 : : m_range{range}
155 : : {
156 : : check_table_lookup_invariants(m_domain, m_range);
157 : : }
158 : :
159 : : /// Do a 1D table lookup
160 : : /// If query value fall out of the domain, then the value at the corresponding edge of the domain
161 : : /// is returned.
162 : : /// \param[in] value The point in the domain to query, x
163 : : /// \return A linearly interpolated value y, corresponding to the query, x
164 : : /// \throw std::domain_error If value is not finite
165 : : T lookup(const T value) const
166 : : {
167 : 16 : return lookup_impl_1d(m_domain, m_range, value);
168 : : }
169 : : /// Get the domain table
170 : : const std::vector<T> & domain() const noexcept {return m_domain;}
171 : : /// Get the range table
172 : : const std::vector<T> & range() const noexcept {return m_range;}
173 : :
174 : : private:
175 : : std::vector<T> m_domain;
176 : : std::vector<T> m_range;
177 : : }; // class LookupTable1D
178 : :
179 : : } // namespace helper_functions
180 : : } // namespace common
181 : : } // namespace autoware
182 : :
183 : :
184 : : #endif // GEOMETRY__LOOKUP_TABLE_HPP_
|
