Gazebo Math
API Reference
7.4.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
include
gz
math
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
21
#include <
gz/math/TimeVaryingVolumetricGridLookupField.hh
>
22
#include <
gz/math/Vector3.hh
>
23
24
#include <map>
25
#include <utility>
26
#include <vector>
27
28
namespace
gz
29
{
30
namespace
math
31
{
32
38
template
<
typename
T,
typename
V,
typename
S,
typename
P>
39
class
TimeVaryingVolumetricGrid
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
73
template
<
typename
T,
typename
V,
typename
P>
74
class
TimeVaryingVolumetricGrid
<T, V,
InMemorySession
<T, P>, P>
75
{
77
public
:
InMemorySession<T, V>
CreateSession
()
const
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>
105
LookUp
(
const
InMemorySession<T, P>
&_session,
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
122
public
:
std::pair<Vector3<V>
,
Vector3<V>
>
Bounds
(
123
const
InMemorySession<T, P>
&_session)
const
124
{
125
return
indices.Bounds(_session);
126
}
127
129
private
:
std::vector<V>
values;
130
132
private
:
TimeVaryingVolumetricGridLookupField
133
<T, V,
InMemorySession<T, P>
> indices;
134
135
template
<
typename
U,
typename
S,
typename
X>
136
friend
class
InMemoryTimeVaryingVolumetricGridFactory
;
137
};
138
141
template
<
typename
T,
typename
V = T,
typename
P = T>
142
using
InMemoryTimeVaryingVolumetricGrid
=
143
TimeVaryingVolumetricGrid<T, V, InMemorySession<T, P>
, P>;
144
146
template
<
typename
T,
typename
V,
typename
P =
double
>
147
class
InMemoryTimeVaryingVolumetricGridFactory
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
160
public
:
InMemoryTimeVaryingVolumetricGrid<T, V, P>
Build
()
const
161
{
162
InMemoryTimeVaryingVolumetricGrid<T, V, P>
grid;
163
for
(
auto
&[time, pts] : _points)
164
{
165
std::vector<Vector3d>
cloud;
166
std::vector<std::size_t>
indices;
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
180
private
:
std::map<T, std::vector<std::pair<Vector3d, V>
>> _points;
181
};
182
183
}
184
}
185
186
#endif