Gazebo Math

API Reference

7.5.1
TimeVaryingVolumetricGrid.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_HH_
19 #define GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_HH_
20 
22 #include <gz/math/Vector3.hh>
23 
24 #include <map>
25 #include <utility>
26 #include <vector>
27 
28 namespace gz::math
29 {
35 template<typename T, typename V, typename S, typename P>
37 {
39  public: S CreateSession() const;
40 
44  public: S CreateSession(const T &_time) const;
45 
47  public: bool IsValid(const S &_session) const;
48 
54  public: std::optional<S> StepTo(const S &_session, const T &_time) const;
55 
59  public: std::optional<V> LookUp(const S &_session, const Vector3<P> &_pos)
60  const;
61 
64  public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session) const;
65 };
66 
70 template<typename T, typename V, typename P>
72 {
75  {
76  return indices.CreateSession();
77  }
78 
80  public: InMemorySession<T, V> CreateSession(const T &_time) const
81  {
82  return indices.CreateSession(_time);
83  }
84 
86  public: bool IsValid(const InMemorySession<T, P> &_session) const
87  {
88  return indices.IsValid(_session);
89  }
90 
92  public: std::optional<InMemorySession<T, P>>
93  StepTo(const InMemorySession<T, double> &_session, const T &_time) const
94  {
95  return indices.StepTo(_session, _time);
96  }
97 
101  public: std::optional<V>
102  LookUp(const InMemorySession<T, P> &_session,
103  const Vector3<P> &_pos,
104  const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6})
105  const
106  {
107  auto points = indices.LookUp(_session, _pos, _tol);
108  std::optional<V> result = indices.EstimateQuadrilinear(
109  _session,
110  points,
111  values,
112  values,
113  _pos);
114  return result;
115  }
116 
120  const InMemorySession<T, P> &_session) const
121  {
122  return indices.Bounds(_session);
123  }
124 
126  private: std::vector<V> values;
127 
130  <T, V, InMemorySession<T, P>> indices;
131 
132  template<typename U, typename S, typename X>
134 };
135 
138 template<typename T, typename V = T, typename P = T>
141 
143 template<typename T, typename V, typename P = double>
145 {
150  public: void AddPoint(
151  const T &_time, const Vector3<P> &_position, const V &_value)
152  {
153  _points[_time].emplace_back(_position, _value);
154  }
155 
158  {
160  for (auto &[time, pts] : _points)
161  {
162  std::vector<Vector3d> cloud;
163  std::vector<std::size_t> indices;
164  for (auto &[pt, val] : pts)
165  {
166  grid.values.push_back(val);
167  cloud.push_back(pt);
168  indices.push_back(grid.values.size() - 1);
169  }
170  VolumetricGridLookupField<V> field(cloud, indices);
171  grid.indices.AddVolumetricGridField(time, field);
172  }
173  return grid;
174  }
175 
178 };
179 } // namespace gz::math
180 #endif // GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_HH_