Rework API to make it easier to use with interpolation.

Signed-off-by: Arjo Chakravarty <[email protected]>

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<typename T>
+    struct InterpolationPoint3D
+    {
+      /// \brief The position of the point
+      Vector3<T> 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<std::size_t> index;
+    };
+
+    /// \brief Describes an interpolation point in1d.
+    template<typename T>
+    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

include/ignition/math/VolumetricGridLookupField.hh

@@ -22,6 +22,7 @@
 #include <optional>
 
 #include <ignition/math/Vector3.hh>
+#include <ignition/math/InterpolationPoint.hh>
 
 #include <ignition/math/detail/AxisIndex.hh>
 
@@ -48,18 +49,6 @@ namespace ignition
         private: std::vector<
           std::vector<std::vector<std::optional<std::size_t>>>> index_table;
 
-        /// \brief Describes an interpolation point.
-        public: struct InterpolationPoint3D
-        {
-          /// \brief The position of the point
-          Vector3<T> 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<std::size_t> 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<std::optional<std::size_t>>
+        public: std::vector<InterpolationPoint3D<T>>
           GetInterpolators(const Vector3<T> &_pt) const
         {
-          std::vector<std::optional<std::size_t>> interpolators;
+          std::vector<InterpolationPoint3D<T>> 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<T>{
+                    Vector3<T>(
+                      x_index.position,
+                      y_index.position,
+                      z_index.position
+                    ),
+                    std::optional{index}
+                  });
               }
             }
           }

include/ignition/math/detail/AxisIndex.hh

@@ -25,6 +25,8 @@
 
 #include <iostream>
 
+#include <ignition/math/InterpolationPoint.hh>
+
 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<std::size_t> GetInterpolators(const T &_value,
+        public: std::vector<InterpolationPoint1D<T>> 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<T>{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<T>{itPrev->first, itPrev->second},
+              InterpolationPoint1D<T>{it->first, it->second}};
           }
         }
       };

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);
   }
 }