Gazebo Math

API Reference

7.5.1
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 
30 namespace gz::math
31 {
32  // Inline bracket to help doxygen filtering.
33  inline namespace GZ_MATH_VERSION_NAMESPACE {
34  template<typename T, typename I = std::size_t>
40  {
42  private: AxisIndex<T> z_indices_by_depth;
43 
44  private: AxisIndex<T> x_indices_by_lat;
45 
46  private: AxisIndex<T> y_indices_by_lon;
47 
48  private: std::vector<
50 
54  const std::vector<Vector3<T>> &_cloud)
55  {
56  // NOTE: This part of the code assumes an exact grid of points.
57  // The grid may be distorted or the stride between different points
58  // may not be the same, but fundamentally the data is structured still
59  // forms a grid-like structure. It keeps track of the axis indices for
60  // each point in the grid.
61  for(auto pt : _cloud)
62  {
63  x_indices_by_lat.AddIndexIfNotFound(pt.X());
64  y_indices_by_lon.AddIndexIfNotFound(pt.Y());
65  z_indices_by_depth.AddIndexIfNotFound(pt.Z());
66  }
67 
68  int num_x = x_indices_by_lat.GetNumUniqueIndices();
69  int num_y = y_indices_by_lon.GetNumUniqueIndices();
70  int num_z = z_indices_by_depth.GetNumUniqueIndices();
71 
72  index_table.resize(num_z);
73  for(int i = 0; i < num_z; ++i)
74  {
75  index_table[i].resize(num_y);
76  for(int j = 0; j < num_y; ++j)
77  {
78  index_table[i][j].resize(num_x);
79  }
80  }
81 
82  for(std::size_t i = 0; i < _cloud.size(); ++i)
83  {
84  const auto &pt = _cloud[i];
85  const std::size_t x_index =
86  x_indices_by_lat.GetIndex(pt.X()).value();
87  const std::size_t y_index =
88  y_indices_by_lon.GetIndex(pt.Y()).value();
89  const std::size_t z_index =
90  z_indices_by_depth.GetIndex(pt.Z()).value();
91  index_table[z_index][y_index][x_index] = i;
92  }
93  }
94 
110  const Vector3<T> &_pt,
111  const double _xTol = 1e-6,
112  const double _yTol = 1e-6,
113  const double _zTol = 1e-6) const
114  {
116 
117  auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol);
118  auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol);
119  auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol);
120 
121  for(const auto &x_index : x_indices)
122  {
123  for(const auto &y_index : y_indices)
124  {
125  for(const auto &z_index : z_indices)
126  {
127  auto index =
128  index_table[z_index.index][y_index.index][x_index.index];
129  interpolators.push_back(
130  InterpolationPoint3D<T>{
131  Vector3<T>(
132  x_index.position,
133  y_index.position,
134  z_index.position
135  ),
136  std::optional{index}
137  });
138  }
139  }
140  }
141 
142  return interpolators;
143  }
144 
149  const std::vector<Vector3<T>> &_cloud,
150  const std::vector<I> &_indices)
151  {
152  assert(_indices.size() == _cloud.size());
153  // NOTE: This part of the code assumes an exact grid of points.
154  // The grid may be distorted or the stride between different points
155  // may not be the same, but fundamentally the data is structured in
156  // a grid-like structure. It keeps track of the axis indices for
157  // each point in the grid.
158  for(auto pt : _cloud)
159  {
160  x_indices_by_lat.AddIndexIfNotFound(pt.X());
161  y_indices_by_lon.AddIndexIfNotFound(pt.Y());
162  z_indices_by_depth.AddIndexIfNotFound(pt.Z());
163  }
164 
165  int num_x = x_indices_by_lat.GetNumUniqueIndices();
166  int num_y = y_indices_by_lon.GetNumUniqueIndices();
167  int num_z = z_indices_by_depth.GetNumUniqueIndices();
168 
169  index_table.resize(num_z);
170  for(int i = 0; i < num_z; ++i)
171  {
172  index_table[i].resize(num_y);
173  for(int j = 0; j < num_y; ++j)
174  {
175  index_table[i][j].resize(num_x);
176  }
177  }
178 
179  for(std::size_t i = 0; i < _cloud.size(); ++i)
180  {
181  const auto &pt = _cloud[i];
182  const std::size_t x_index =
183  x_indices_by_lat.GetIndex(pt.X()).value();
184  const std::size_t y_index =
185  y_indices_by_lon.GetIndex(pt.Y()).value();
186  const std::size_t z_index =
187  z_indices_by_depth.GetIndex(pt.Z()).value();
188  index_table[z_index][y_index][x_index] = _indices[i];
189  }
190  }
191 
199  public: template<typename V>
200  std::optional<V> EstimateValueUsingTrilinear(
201  const Vector3<T> &_pt,
202  const std::vector<V> &_values,
203  const V &_default = V(0)) const
204  {
205  auto interpolators = GetInterpolators(_pt);
206  return EstimateValueUsingTrilinear(
207  interpolators,
208  _pt,
209  _values,
210  _default);
211  }
212 
224  public: template<typename V>
225  std::optional<V> EstimateValueUsingTrilinear(
226  const std::vector<InterpolationPoint3D<T>> _interpolators,
227  const Vector3<T> &_pt,
228  const std::vector<V> &_values,
229  const V &_default = V(0)) const
230  {
231  if (_interpolators.size() == 0)
232  {
233  return std::nullopt;
234  }
235  else if (_interpolators.size() == 1)
236  {
237  if (!_interpolators[0].index.has_value())
238  {
239  return _default;
240  }
241  return _values[_interpolators[0].index.value()];
242  }
243  else if (_interpolators.size() == 2)
244  {
245  return LinearInterpolate(_interpolators[0], _interpolators[1],
246  _values, _pt, _default);
247  }
248  else if (_interpolators.size() == 4)
249  {
250  return BiLinearInterpolate(
251  _interpolators, 0, _values, _pt, _default);
252  }
253  else if (_interpolators.size() == 8)
254  {
255  return TrilinearInterpolate(_interpolators, _values, _pt, _default);
256  }
257  else
258  {
259  // should never get here
260  return std::nullopt;
261  }
262  }
263 
267  {
269  Vector3<T>{
270  x_indices_by_lat.MinKey(),
271  y_indices_by_lon.MinKey(),
272  z_indices_by_depth.MinKey()
273  },
274  Vector3<T>{
275  x_indices_by_lat.MaxKey(),
276  y_indices_by_lon.MaxKey(),
277  z_indices_by_depth.MaxKey()
278  });
279  }
280 
281  };
282  } // namespace GZ_MATH_VERSION_NAMESPACE
283 } // namespace gz::math
284 #endif // GZ_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
GRID_LOOKUP_FIELD_HH_