Gazebo Math

API Reference

7.4.0
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
28namespace gz
29{
30namespace math
31{
32
38template<typename T, typename V, typename S, typename P>
40{
42 public: S CreateSession() const;
43
47 public: S CreateSession(const T &_time) const;
48
50 public: bool IsValid(const S &_session) const;
51
57 public: std::optional<S> StepTo(const S &_session, const T &_time) const;
58
62 public: std::optional<V> LookUp(const S &_session, const Vector3<P> &_pos)
63 const;
64
67 public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session) const;
68};
69
73template<typename T, typename V, typename P>
75{
78 {
79 return indices.CreateSession();
80 }
81
83 public: InMemorySession<T, V> CreateSession(const T &_time) const
84 {
85 return indices.CreateSession(_time);
86 }
87
89 public: bool IsValid(const InMemorySession<T, P> &_session) const
90 {
91 return indices.IsValid(_session);
92 }
93
95 public: std::optional<InMemorySession<T, P>>
96 StepTo(const InMemorySession<T, double> &_session, const T &_time) const
97 {
98 return indices.StepTo(_session, _time);
99 }
100
104 public: std::optional<V>
106 const Vector3<P> &_pos,
107 const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6})
108 const
109 {
110 auto points = indices.LookUp(_session, _pos, _tol);
111 std::optional<V> result = indices.EstimateQuadrilinear(
112 _session,
113 points,
114 values,
115 values,
116 _pos);
117 return result;
118 }
119
123 const InMemorySession<T, P> &_session) const
124 {
125 return indices.Bounds(_session);
126 }
127
129 private: std::vector<V> values;
130
133 <T, V, InMemorySession<T, P>> indices;
134
135 template<typename U, typename S, typename X>
137};
138
141template<typename T, typename V = T, typename P = T>
144
146template<typename T, typename V, typename P = double>
148{
153 public: void AddPoint(
154 const T &_time, const Vector3<P> &_position, const V &_value)
155 {
156 _points[_time].emplace_back(_position, _value);
157 }
158
161 {
163 for (auto &[time, pts] : _points)
164 {
167 for (auto &[pt, val] : pts)
168 {
169 grid.values.push_back(val);
170 cloud.push_back(pt);
171 indices.push_back(grid.values.size() - 1);
172 }
173 VolumetricGridLookupField<V> field(cloud, indices);
174 grid.indices.AddVolumetricGridField(time, field);
175 }
176 return grid;
177 }
178
181};
182
183}
184}
185
186#endif