Gazebo Math

API Reference

7.5.1
TimeVaryingVolumetricGridLookupField.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_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
19 #define GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
21 #include <gz/math/detail/InterpolationPoint.hh>
22 
23 #include <map>
24 #include <optional>
25 #include <utility>
26 #include <vector>
27 
28 namespace gz::math
29 {
35  template<typename T, typename V, typename S>
37  {
40 
42  public: void AddVolumetricGridField(
43  const T& time, const VolumetricGridLookupField<V> &_field);
44 
46  public: S CreateSession() const;
47 
50  public: S CreateSession(const T &_time) const;
51 
57  public: std::optional<S> StepTo(const S &_session, const T &_time);
58 
65  LookUp(const S &_session,
66  const Vector3<V> &_point,
67  const Vector3<V> &_tol) const;
68 
80  public: template<typename X>
81  std::optional<X> EstimateQuadrilinear(
82  const S &_session,
83  const std::vector<InterpolationPoint4D<T, V>> &_points,
84  const std::vector<X> &_values1,
85  const std::vector<X> &_values2,
86  const X _default = X(0)
87  ) const;
88 
91  public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session);
92  };
93 
96  template<typename T, typename V>
98  {
102  private:
103  typename std::map<T, VolumetricGridLookupField<V>>::const_iterator iter;
104 
106  public: T time;
107 
108  friend class
110  };
111 
114  template<typename T, typename V>
116  {
119  {}
120 
123  const T &_time, const VolumetricGridLookupField<V> &_field) {
124  this->gridFields.emplace(_time, _field);
125  }
126 
130  sess.iter = this->gridFields.begin();
131  sess.time = T(0);
132  return sess;
133  }
134 
136  public: InMemorySession<T, V> CreateSession(const T &_time) const {
138  sess.iter = this->gridFields.lower_bound(_time);
139  sess.time = _time;
140  return sess;
141  }
142 
144  public: bool IsValid(const InMemorySession<T, V> &_session) const {
145  return this->gridFields.end() != _session.iter;
146  }
147 
149  public: std::optional<InMemorySession<T, V>> StepTo(
150  const InMemorySession<T, V> &_session, const T &_time) const {
151  if (_session.iter == gridFields.end())
152  {
153  return std::nullopt;
154  }
155 
156  InMemorySession<T, V> newSess(_session);
157 
158  auto nextTime = std::next(_session.iter);
159  if (nextTime == this->gridFields.end() || _time < _session.iter->first)
160  {
161  return std::nullopt;
162  }
163 
164  while (nextTime != this->gridFields.end()
165  && nextTime->first <= _time)
166  {
167  newSess.iter = nextTime;
168  nextTime = std::next(nextTime);
169  }
170  newSess.time = _time;
171  return newSess;
172  }
173 
176  LookUp(const InMemorySession<T, V> &_session,
177  const Vector3<V> &_point,
178  const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6}) const {
179 
181 
182  if (_session.iter == this->gridFields.end())
183  {
184  // Out of bounds
185  return res;
186  }
187 
188  InterpolationPoint4D<T, V> slice1;
189  slice1.timeSlice = _session.iter->second.GetInterpolators(
190  _point, _tol.X(), _tol.Y(), _tol.Z());
191  slice1.time = _session.iter->first;
192  res.push_back(slice1);
193 
194  auto nextTime = std::next(_session.iter);
195  if (nextTime != this->gridFields.end())
196  {
197  // Only add next item if field exists
198  InterpolationPoint4D<T, V> slice2;
199  slice2.timeSlice = nextTime->second.GetInterpolators(
200  _point, _tol.X(), _tol.Y(), _tol.Z());
201  slice2.time = nextTime->first;
202  res.push_back(slice2);
203  }
204  return res;
205  }
206 
219  public: template<typename X>
220  std::optional<X> EstimateQuadrilinear(
221  const InMemorySession<T, V> &_session,
222  const std::vector<InterpolationPoint4D<T, V>> &_interpolators,
223  const std::vector<X> &_values1,
224  const std::vector<X> &_values2,
225  const Vector3<X> &_position,
226  const X _default = X(0)
227  ) const
228  {
229  if (_session.iter == this->gridFields.end())
230  {
231  // Out of bounds
232  return std::nullopt;
233  }
234 
235  auto time = _session.time;
236  if (_interpolators.size() == 0) return std::nullopt;
237  if (_interpolators.size() == 1)
238  {
239  // This happens we reach the end of time
240  return _session.iter->second.EstimateValueUsingTrilinear(
241  _position,
242  _values2,
243  _default);
244  }
245 
247  if (_interpolators[0].timeSlice.size() == 0
248  && _interpolators[1].timeSlice.size() == 0)
249  return std::nullopt;
250 
252  auto next = std::next(_session.iter);
253  if (_interpolators[1].timeSlice.size() == 0)
254  {
255  return _session.iter->second.EstimateValueUsingTrilinear(
256  _position,
257  _values1,
258  _default
259  );
260  }
261  if (_interpolators[0].timeSlice.size() == 0)
262  {
263  return next->second.EstimateValueUsingTrilinear(
264  _position,
265  _values2,
266  _default
267  );
268  }
269 
271  auto res1 = _session.iter->second.EstimateValueUsingTrilinear(
272  _position,
273  _values1,
274  _default
275  );
276 
277  auto res2 = next->second.EstimateValueUsingTrilinear(
278  _position,
279  _values2,
280  _default
281  );
282 
283  if (res1.has_value() || res2.has_value())
284  {
285  InterpolationPoint1D<T>
286  pt1{_session.iter->first, 0}, pt2{next->first, 1};
287  // If either has value interpolate using default value.
288  std::vector<X> times{
289  res1.value_or(_default), res2.value_or(_default)};
290  return LinearInterpolate(pt1, pt2, times, time);
291  }
292  // Return nullopt if we are out of range
293  return std::nullopt;
294  }
295 
299  const InMemorySession<T, V> &_session) const
300  {
301  if (_session.iter == this->gridFields.end())
302  {
303  // Out of bounds
305  Vector3<T>{0, 0, 0}, Vector3<T>{0, 0, 0}
306  );
307  }
308  return _session.iter->second.Bounds();
309  }
310 
311  private: std::map<T, VolumetricGridLookupField<V>> gridFields;
312  };
313 } // namespace gz::math
314 #endif // GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_