forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 34
/
Copy pathinterpolation_utils.hpp
114 lines (96 loc) · 3.51 KB
/
interpolation_utils.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
// Copyright 2021 Tier IV, 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 AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
#include <algorithm>
#include <array>
#include <stdexcept>
#include <vector>
namespace autoware::interpolation
{
inline bool isIncreasing(const std::vector<double> & x)
{
if (x.empty()) {
throw std::invalid_argument("Points is empty.");
}
for (size_t i = 0; i < x.size() - 1; ++i) {
if (x.at(i) >= x.at(i + 1)) {
return false;
}
}
return true;
}
inline bool isNotDecreasing(const std::vector<double> & x)
{
if (x.empty()) {
throw std::invalid_argument("Points is empty.");
}
for (size_t i = 0; i < x.size() - 1; ++i) {
if (x.at(i) > x.at(i + 1)) {
return false;
}
}
return true;
}
inline std::vector<double> validateKeys(
const std::vector<double> & base_keys, const std::vector<double> & query_keys)
{
// when vectors are empty
if (base_keys.empty() || query_keys.empty()) {
throw std::invalid_argument("Points is empty.");
}
// when size of vectors are less than 2
if (base_keys.size() < 2) {
throw std::invalid_argument(
"The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()));
}
// when indices are not sorted
if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) {
throw std::invalid_argument("Either base_keys or query_keys is not sorted.");
}
// when query_keys is out of base_keys (This function does not allow exterior division.)
constexpr double epsilon = 1e-3;
if (
query_keys.front() < base_keys.front() - epsilon ||
base_keys.back() + epsilon < query_keys.back()) {
throw std::invalid_argument("query_keys is out of base_keys");
}
// NOTE: Due to calculation error of double, a query key may be slightly out of base keys.
// Therefore, query keys are cropped here.
auto validated_query_keys = query_keys;
validated_query_keys.front() = std::max(validated_query_keys.front(), base_keys.front());
validated_query_keys.back() = std::min(validated_query_keys.back(), base_keys.back());
return validated_query_keys;
}
template <class T>
void validateKeysAndValues(
const std::vector<double> & base_keys, const std::vector<T> & base_values)
{
// when vectors are empty
if (base_keys.empty() || base_values.empty()) {
throw std::invalid_argument("Points is empty.");
}
// when size of vectors are less than 2
if (base_keys.size() < 2 || base_values.size() < 2) {
throw std::invalid_argument(
"The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
", base_values.size() = " + std::to_string(base_values.size()));
}
// when sizes of indices and values are not same
if (base_keys.size() != base_values.size()) {
throw std::invalid_argument("The size of base_keys and base_values are not the same.");
}
}
} // namespace autoware::interpolation
#endif // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_