Gazebo Math

API Reference

7.4.0
gz/math/VolumetricGridLookupField.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 */
17
18#ifndef GZ_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
19#define GZ_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
20
21#include <optional>
22#include <utility>
23#include <vector>
24
25#include <gz/math/Vector3.hh>
26#include <gz/math/detail/InterpolationPoint.hh>
27
28#include <gz/math/detail/AxisIndex.hh>
29
30namespace gz
31{
32 namespace math
33 {
34 // Inline bracket to help doxygen filtering.
35 inline namespace GZ_MATH_VERSION_NAMESPACE {
36 template<typename T, typename I = std::size_t>
42 {
44 private: AxisIndex<T> z_indices_by_depth;
45
46 private: AxisIndex<T> x_indices_by_lat;
47
48 private: AxisIndex<T> y_indices_by_lon;
49
50 private: std::vector<
52
56 const std::vector<Vector3<T>> &_cloud)
57 {
58 // NOTE: This part of the code assumes an exact grid of points.
59 // The grid may be distorted or the stride between different points
60 // may not be the same, but fundamentally the data is structured still
61 // forms a grid-like structure. It keeps track of the axis indices for
62 // each point in the grid.
63 for(auto pt : _cloud)
64 {
65 x_indices_by_lat.AddIndexIfNotFound(pt.X());
66 y_indices_by_lon.AddIndexIfNotFound(pt.Y());
67 z_indices_by_depth.AddIndexIfNotFound(pt.Z());
68 }
69
70 int num_x = x_indices_by_lat.GetNumUniqueIndices();
71 int num_y = y_indices_by_lon.GetNumUniqueIndices();
72 int num_z = z_indices_by_depth.GetNumUniqueIndices();
73
74 index_table.resize(num_z);
75 for(int i = 0; i < num_z; ++i)
76 {
77 index_table[i].resize(num_y);
78 for(int j = 0; j < num_y; ++j)
79 {
80 index_table[i][j].resize(num_x);
81 }
82 }
83
84 for(std::size_t i = 0; i < _cloud.size(); ++i)
85 {
86 const auto &pt = _cloud[i];
87 const std::size_t x_index =
88 x_indices_by_lat.GetIndex(pt.X()).value();
89 const std::size_t y_index =
90 y_indices_by_lon.GetIndex(pt.Y()).value();
91 const std::size_t z_index =
92 z_indices_by_depth.GetIndex(pt.Z()).value();
93 index_table[z_index][y_index][x_index] = i;
94 }
95 }
96
112 const Vector3<T> &_pt,
113 const double _xTol = 1e-6,
114 const double _yTol = 1e-6,
115 const double _zTol = 1e-6) const
116 {
118
119 auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol);
120 auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol);
121 auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol);
122
123 for(const auto &x_index : x_indices)
124 {
125 for(const auto &y_index : y_indices)
126 {
127 for(const auto &z_index : z_indices)
128 {
129 auto index =
130 index_table[z_index.index][y_index.index][x_index.index];
131 interpolators.push_back(
132 InterpolationPoint3D<T>{
134 x_index.position,
135 y_index.position,
136 z_index.position
137 ),
138 std::optional{index}
139 });
140 }
141 }
142 }
143
144 return interpolators;
145 }
146
151 const std::vector<Vector3<T>> &_cloud,
152 const std::vector<I> &_indices)
153 {
154 assert(_indices.size() == _cloud.size());
155 // NOTE: This part of the code assumes an exact grid of points.
156 // The grid may be distorted or the stride between different points
157 // may not be the same, but fundamentally the data is structured in
158 // a grid-like structure. It keeps track of the axis indices for
159 // each point in the grid.
160 for(auto pt : _cloud)
161 {
162 x_indices_by_lat.AddIndexIfNotFound(pt.X());
163 y_indices_by_lon.AddIndexIfNotFound(pt.Y());
164 z_indices_by_depth.AddIndexIfNotFound(pt.Z());
165 }
166
167 int num_x = x_indices_by_lat.GetNumUniqueIndices();
168 int num_y = y_indices_by_lon.GetNumUniqueIndices();
169 int num_z = z_indices_by_depth.GetNumUniqueIndices();
170
171 index_table.resize(num_z);
172 for(int i = 0; i < num_z; ++i)
173 {
174 index_table[i].resize(num_y);
175 for(int j = 0; j < num_y; ++j)
176 {
177 index_table[i][j].resize(num_x);
178 }
179 }
180
181 for(std::size_t i = 0; i < _cloud.size(); ++i)
182 {
183 const auto &pt = _cloud[i];
184 const std::size_t x_index =
185 x_indices_by_lat.GetIndex(pt.X()).value();
186 const std::size_t y_index =
187 y_indices_by_lon.GetIndex(pt.Y()).value();
188 const std::size_t z_index =
189 z_indices_by_depth.GetIndex(pt.Z()).value();
190 index_table[z_index][y_index][x_index] = _indices[i];
191 }
192 }
193
201 public: template<typename V>
203 const Vector3<T> &_pt,
204 const std::vector<V> &_values,
205 const V &_default = V(0)) const
206 {
207 auto interpolators = GetInterpolators(_pt);
208 return EstimateValueUsingTrilinear(
209 interpolators,
210 _pt,
211 _values,
212 _default);
213 }
214
226 public: template<typename V>
228 const std::vector<InterpolationPoint3D<T>> _interpolators,
229 const Vector3<T> &_pt,
230 const std::vector<V> &_values,
231 const V &_default = V(0)) const
232 {
233 if (_interpolators.size() == 0)
234 {
235 return std::nullopt;
236 }
237 else if (_interpolators.size() == 1)
238 {
239 if (!_interpolators[0].index.has_value())
240 {
241 return _default;
242 }
243 return _values[_interpolators[0].index.value()];
244 }
245 else if (_interpolators.size() == 2)
246 {
247 return LinearInterpolate(_interpolators[0], _interpolators[1],
248 _values, _pt, _default);
249 }
250 else if (_interpolators.size() == 4)
251 {
252 return BiLinearInterpolate(
253 _interpolators, 0, _values, _pt, _default);
254 }
255 else if (_interpolators.size() == 8)
256 {
257 return TrilinearInterpolate(_interpolators, _values, _pt, _default);
258 }
259 else
260 {
261 // should never get here
262 return std::nullopt;
263 }
264 }
265
269 {
272 x_indices_by_lat.MinKey(),
273 y_indices_by_lon.MinKey(),
274 z_indices_by_depth.MinKey()
275 },
277 x_indices_by_lat.MaxKey(),
278 y_indices_by_lon.MaxKey(),
279 z_indices_by_depth.MaxKey()
280 });
281 }
282
283 };
284 }
285 }
286}
287
288#endif