diff --git a/include/ignition/math/InterpolationPoint.hh b/include/ignition/math/InterpolationPoint.hh new file mode 100644 index 000000000..fdc3ec702 --- /dev/null +++ b/include/ignition/math/InterpolationPoint.hh @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_MATH_INTERPOLATION_POINT_HH_ +#define IGNITION_MATH_INTERPOLATION_POINT_HH_ + + +namespace ignition +{ + namespace math + { + /// \brief Describes an interpolation point. + template + struct InterpolationPoint3D + { + /// \brief The position of the point + Vector3 position; + + /// \brief The index from which this point was retrieved. + /// Can be used by the application calling it to index. The reason + /// this is optional is that data may be missing from a sparse grid. + std::optional index; + }; + + /// \brief Describes an interpolation point in1d. + template + struct InterpolationPoint1D + { + /// \brief The position of the point + T position; + + /// \brief The index from which this point was retrieved. + /// Can be used by the application calling it to index. + std::size_t index; + }; + } +} +#endif \ No newline at end of file diff --git a/include/ignition/math/VolumetricGridLookupField.hh b/include/ignition/math/VolumetricGridLookupField.hh index c7b8ff0b6..9aaa3bb8d 100644 --- a/include/ignition/math/VolumetricGridLookupField.hh +++ b/include/ignition/math/VolumetricGridLookupField.hh @@ -22,6 +22,7 @@ #include #include +#include #include @@ -48,18 +49,6 @@ namespace ignition private: std::vector< std::vector>>> index_table; - /// \brief Describes an interpolation point. - public: struct InterpolationPoint3D - { - /// \brief The position of the point - Vector3 position; - - /// \brief The index from which this point was retrieved. - /// Can be used by the application calling it to index. The reason - /// this is optional is that data may be missing from a sparse grid. - std::optional index; - }; - /// \brief Constructor /// \param[in] _cloud The cloud of points to use to construct the grid. public: VolumetricGridLookupField( @@ -113,23 +102,31 @@ namespace ignition /// four corners, if along an edge two points and if coincident with a /// corner one point. If the data is sparse and missing indices then a /// nullopt will be returned. - public: std::vector> + public: std::vector> GetInterpolators(const Vector3 &_pt) const { - std::vector> interpolators; + std::vector> interpolators; auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X()); auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y()); auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z()); - for(auto x_index : x_indices) + for(const auto &x_index : x_indices) { - for(auto y_index : y_indices) + for(const auto &y_index : y_indices) { - for(auto z_index : z_indices) + for(const auto &z_index : z_indices) { - auto index = index_table[z_index][y_index][x_index]; - interpolators.push_back(index); + auto index = index_table[z_index.index][y_index.index][x_index.index]; + interpolators.push_back( + InterpolationPoint3D{ + Vector3( + x_index.position, + y_index.position, + z_index.position + ), + std::optional{index} + }); } } } diff --git a/include/ignition/math/detail/AxisIndex.hh b/include/ignition/math/detail/AxisIndex.hh index b75f68fed..dada9bf44 100644 --- a/include/ignition/math/detail/AxisIndex.hh +++ b/include/ignition/math/detail/AxisIndex.hh @@ -25,6 +25,8 @@ #include +#include + namespace ignition { namespace math @@ -43,13 +45,6 @@ namespace ignition /// \brief Number of indices private: int numIndices{0}; - public: struct InterpolationPoint1D - { - T position; - - std::size_t index; - }; - /// \brief Register the existance of a measurement at _value. /// \param[in] _value The position of the measurement. public: void AddIndexIfNotFound(T _value) @@ -92,7 +87,8 @@ namespace ignition /// returned. If the value is exact, a vector with a single index is /// returned otherwise return the two indices which should be used for /// interpolation. - public: std::vector GetInterpolators(const T &_value, + public: std::vector> GetInterpolators( + const T &_value, double _tol = 1e-6) const { // Performs a BST to find the first element that is greater than or @@ -106,7 +102,7 @@ namespace ignition else if (fabs(it->first - _value) < _tol) { // Exact match - return {it->second}; + return {InterpolationPoint1D{it->first, it->second}}; } else if (it == axisIndex.begin()) { @@ -118,7 +114,8 @@ namespace ignition // Interpolate auto itPrev = it; itPrev--; - return {itPrev->second, it->second}; + return {InterpolationPoint1D{itPrev->first, itPrev->second}, + InterpolationPoint1D{it->first, it->second}}; } } }; diff --git a/src/VolumetricGridLookupField_TEST.cc b/src/VolumetricGridLookupField_TEST.cc index 71defc813..ebc621b01 100644 --- a/src/VolumetricGridLookupField_TEST.cc +++ b/src/VolumetricGridLookupField_TEST.cc @@ -44,7 +44,7 @@ TEST(VolumetricGridLookupField, CheckInterpolationExact) { auto val = scalarIndex.GetInterpolators(cloud[i]); ASSERT_EQ(val.size(), 1UL); - ASSERT_EQ(val[0], i); + ASSERT_EQ(val[0].index, i); } }