Skip to content

Commit 9622e9e

Browse files
arjo129arjo129
authored
Add linear interpolation methods. (#412)
This PR adds linear interpolation methods to the ignition math library these interpolation methods are needed for the VolumetricScalarGrid field,. Signed-off-by: Arjo Chakravarty arjo@openrobotics.org
1 parent a8a5b94 commit 9622e9e

5 files changed

Lines changed: 882 additions & 0 deletions

File tree

‎include/ignition/math/VolumetricGridLookupField.hh‎

Lines changed: 223 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,223 @@
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 IGNITION_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
19+
#define IGNITION_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
20+
21+
#include <vector>
22+
#include <optional>
23+
24+
#include <ignition/math/Vector3.hh>
25+
#include <ignition/math/detail/InterpolationPoint.hh>
26+
27+
#include <ignition/math/detail/AxisIndex.hh>
28+
29+
namespace ignition
30+
{
31+
namespace math
32+
{
33+
// Inline bracket to help doxygen filtering.
34+
inline namespace IGNITION_MATH_VERSION_NAMESPACE {
35+
template<typename T>
36+
/// \brief Lookup table for a volumetric dataset. This class is used to
37+
/// lookup indices for a large dataset that's organized in a grid. This
38+
/// class is not meant to be used with non-grid like data sets. The grid
39+
/// may be sparse or non uniform and missing data points.
40+
class VolumetricGridLookupField
41+
{
42+
/// Get the index along the given axis
43+
private: AxisIndex<T> z_indices_by_depth;
44+
45+
private: AxisIndex<T> x_indices_by_lat;
46+
47+
private: AxisIndex<T> y_indices_by_lon;
48+
49+
private: std::vector<
50+
std::vector<std::vector<std::optional<std::size_t>>>> index_table;
51+
52+
/// \brief Constructor
53+
/// \param[in] _cloud The cloud of points to use to construct the grid.
54+
public: VolumetricGridLookupField(
55+
const std::vector<Vector3<T>> &_cloud)
56+
{
57+
// NOTE: This part of the code assumes an exact grid of points.
58+
// The grid may be distorted or the stride between different points
59+
// may not be the same, but fundamentally the data is structured still
60+
// forms a grid-like structure. It keeps track of the axis indices for
61+
// each point in the grid.
62+
for(auto pt : _cloud)
63+
{
64+
x_indices_by_lat.AddIndexIfNotFound(pt.X());
65+
y_indices_by_lon.AddIndexIfNotFound(pt.Y());
66+
z_indices_by_depth.AddIndexIfNotFound(pt.Z());
67+
}
68+
69+
int num_x = x_indices_by_lat.GetNumUniqueIndices();
70+
int num_y = y_indices_by_lon.GetNumUniqueIndices();
71+
int num_z = z_indices_by_depth.GetNumUniqueIndices();
72+
73+
index_table.resize(num_z);
74+
for(int i = 0; i < num_z; ++i)
75+
{
76+
index_table[i].resize(num_y);
77+
for(int j = 0; j < num_y; ++j)
78+
{
79+
index_table[i][j].resize(num_x);
80+
}
81+
}
82+
83+
for(std::size_t i = 0; i < _cloud.size(); ++i)
84+
{
85+
const auto &pt = _cloud[i];
86+
const std::size_t x_index =
87+
x_indices_by_lat.GetIndex(pt.X()).value();
88+
const std::size_t y_index =
89+
y_indices_by_lon.GetIndex(pt.Y()).value();
90+
const std::size_t z_index =
91+
z_indices_by_depth.GetIndex(pt.Z()).value();
92+
index_table[z_index][y_index][x_index] = i;
93+
}
94+
}
95+
96+
/// \brief Retrieves the indices of the points that are to be used for
97+
/// interpolation. Separate tolerance values are used for each axis as
98+
/// data may have different magnitudes on each axis.
99+
/// \param[in] _pt The point to get the interpolators for.
100+
/// \param[in] _xTol The tolerance for the x axis.
101+
/// \param[in] _yTol The tolerance for the y axis.
102+
/// \param[in] _zTol The tolerance for the z axis.
103+
/// \returns A list of points which are to be used for interpolation. If
104+
/// the point is in the middle of a grid cell then it returns the four
105+
/// corners of the grid cell. If its along a plane then it returns the
106+
/// four corners, if along an edge two points and if coincident with a
107+
/// corner one point. If the data is sparse and missing indices then a
108+
/// nullopt will be returned.
109+
public: std::vector<InterpolationPoint3D<T>>
110+
GetInterpolators(
111+
const Vector3<T> &_pt,
112+
const double _xTol = 1e-6,
113+
const double _yTol = 1e-6,
114+
const double _zTol = 1e-6) const
115+
{
116+
std::vector<InterpolationPoint3D<T>> interpolators;
117+
118+
auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol);
119+
auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol);
120+
auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol);
121+
122+
for(const auto &x_index : x_indices)
123+
{
124+
for(const auto &y_index : y_indices)
125+
{
126+
for(const auto &z_index : z_indices)
127+
{
128+
auto index =
129+
index_table[z_index.index][y_index.index][x_index.index];
130+
interpolators.push_back(
131+
InterpolationPoint3D<T>{
132+
Vector3<T>(
133+
x_index.position,
134+
y_index.position,
135+
z_index.position
136+
),
137+
std::optional{index}
138+
});
139+
}
140+
}
141+
}
142+
143+
return interpolators;
144+
}
145+
146+
/// \brief Estimates the values for a grid given a list of values to
147+
/// interpolate. This method uses Trilinear interpolation.
148+
/// \param[in] _pt The point to estimate for.
149+
/// \param[in] _values The values to interpolate.
150+
/// \param[in] _default If a value is not found at a specific point then
151+
/// this value will be used.
152+
/// \returns The estimated value for the point.
153+
public: template<typename V>
154+
std::optional<V> EstimateValueUsingTrilinear(
155+
const Vector3<T> &_pt,
156+
const std::vector<V> &_values,
157+
const V &_default = V(0)) const
158+
{
159+
auto interpolators = GetInterpolators(_pt);
160+
return EstimateValueUsingTrilinear(
161+
interpolators,
162+
_pt,
163+
_values,
164+
_default);
165+
}
166+
167+
/// \brief Estimates the values for a grid given a list of values to
168+
/// interpolate. This method uses Trilinear interpolation.
169+
/// \param[in] _interpolators The list of interpolators to use.
170+
/// Retrieved by calling GetInterpolators().
171+
/// \param[in] _pt The point to estimate for.
172+
/// \param[in] _values The values to interpolate.
173+
/// \param[in] _default If a value is not found at a specific point then
174+
/// this value will be used.
175+
/// \returns The estimated value for the point. Nullopt if we are
176+
/// outside the field. Default value if in the field but no value is
177+
/// in the index.
178+
public: template<typename V>
179+
std::optional<V> EstimateValueUsingTrilinear(
180+
const std::vector<InterpolationPoint3D<T>> _interpolators,
181+
const Vector3<T> &_pt,
182+
const std::vector<V> &_values,
183+
const V &_default = V(0)) const
184+
{
185+
if (_interpolators.size() == 0)
186+
{
187+
return std::nullopt;
188+
}
189+
else if (_interpolators.size() == 1)
190+
{
191+
if (!_interpolators[0].index.has_value())
192+
{
193+
return _default;
194+
}
195+
return _values[_interpolators[0].index.value()];
196+
}
197+
else if (_interpolators.size() == 2)
198+
{
199+
return LinearInterpolate(_interpolators[0], _interpolators[1],
200+
_values, _pt, _default);
201+
}
202+
else if (_interpolators.size() == 4)
203+
{
204+
return BiLinearInterpolate(
205+
_interpolators, 0, _values, _pt, _default);
206+
}
207+
else if (_interpolators.size() == 8)
208+
{
209+
return TrilinearInterpolate(_interpolators, _values, _pt, _default);
210+
}
211+
else
212+
{
213+
// should never get here
214+
return std::nullopt;
215+
}
216+
}
217+
218+
};
219+
}
220+
}
221+
}
222+
223+
#endif

‎include/ignition/math/detail/AxisIndex.hh‎

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
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 IGNITION_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_
19+
#define IGNITION_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_
20+
21+
#include <algorithm>
22+
#include <map>
23+
#include <optional>
24+
#include <vector>
25+
26+
#include <cassert>
27+
28+
#include <ignition/math/detail/InterpolationPoint.hh>
29+
30+
namespace ignition
31+
{
32+
namespace math
33+
{
34+
// Inline bracket to help doxygen filtering.
35+
inline namespace IGNITION_MATH_VERSION_NAMESPACE {
36+
37+
/// \brief Represents a sparse number line which can be searched via
38+
/// search for indices.
39+
template <typename T>
40+
class AxisIndex
41+
{
42+
/// \brief Binary tree with indices
43+
private: std::map<T, std::size_t> axisIndex;
44+
45+
/// \brief Number of indices
46+
private: int numIndices{0};
47+
48+
/// \brief Register the existance of a measurement at _value.
49+
/// \param[in] _value The position of the measurement.
50+
public: void AddIndexIfNotFound(T _value)
51+
{
52+
if (axisIndex.find(_value) == axisIndex.end())
53+
{
54+
axisIndex[_value] = numIndices;
55+
numIndices++;
56+
}
57+
}
58+
59+
/// \brief Get the number of unique indices.
60+
/// \return The number of unique indices.
61+
public: std::size_t GetNumUniqueIndices() const
62+
{
63+
return axisIndex.size();
64+
}
65+
66+
/// \brief Get the index of a measurement
67+
/// \param[in] _value The position of the measurement.
68+
/// \return The index of the measurement if found else return nullopt.
69+
public: std::optional<std::size_t> GetIndex(T _value) const
70+
{
71+
auto res = axisIndex.find(_value);
72+
if (res == axisIndex.end())
73+
{
74+
return std::nullopt;
75+
}
76+
else
77+
{
78+
return res->second;
79+
}
80+
}
81+
82+
/// \brief Get interpolators for a measurement.
83+
/// \param[in] _value The position of the measurement.
84+
/// \param[in] _tol The tolerance for the search. Cannot be zero.
85+
/// \return The indices of the measurements that should be used for
86+
/// interpolation. If the value is out of range, an empty vector is
87+
/// returned. If the value is exact, a vector with a single index is
88+
/// returned otherwise return the two indices which should be used for
89+
/// interpolation.
90+
public: std::vector<InterpolationPoint1D<T>> GetInterpolators(
91+
const T &_value,
92+
double _tol = 1e-6) const
93+
{
94+
assert(_tol > 0);
95+
// Performs a BST to find the first element that is greater than or
96+
// equal to the value.
97+
auto it = axisIndex.lower_bound(_value);
98+
if (it == axisIndex.end())
99+
{
100+
// Out of range
101+
return {};
102+
}
103+
else if (fabs(it->first - _value) < _tol)
104+
{
105+
// Exact match
106+
return {InterpolationPoint1D<T>{it->first, it->second}};
107+
}
108+
else if (it == axisIndex.begin())
109+
{
110+
// Below range
111+
return {};
112+
}
113+
else
114+
{
115+
// Interpolate
116+
auto itPrev = it;
117+
itPrev--;
118+
return {InterpolationPoint1D<T>{itPrev->first, itPrev->second},
119+
InterpolationPoint1D<T>{it->first, it->second}};
120+
}
121+
}
122+
};
123+
}
124+
}
125+
}
126+
#endif

0 commit comments

Comments
 (0)