Add pybindings for VolumetricGridLookupField

src/python_pybind11/CMakeLists.txt

@@ -39,6 +39,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE
   src/Frustum.cc
   src/GaussMarkovProcess.cc
   src/Helpers.cc
+  src/InterpolationPoint.cc
   src/Interval.cc
   src/Kmeans.cc
   src/Line2.cc
@@ -70,6 +71,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE
   src/Vector3.cc
   src/Vector4.cc
   src/Vector3Stats.cc
+  src/VolumetricGridLookupField.cc
 )
 
 target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE

src/python_pybind11/src/InterpolationPoint.cc

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ */
+
+#include "InterpolationPoint.hh"
+#include <string>
+
+namespace gz {
+namespace math {
+namespace python {
+
+void defineMathInterpolationPoint3D(py::module &m, const std::string &typestr) {
+  helpDefineMathInterpolationPoint3D<double>(m, typestr + "d");
+  helpDefineMathInterpolationPoint3D<float>(m, typestr + "f");
+}
+
+}  // namespace python
+}  // namespace math
+}  // namespace gz

src/python_pybind11/src/InterpolationPoint.hh

@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2025 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 GZ_MATH_PYTHON__INTERPOLATION_POINT_HH_
+#define GZ_MATH_PYTHON__INTERPOLATION_POINT_HH_
+
+#include <sstream>
+#include <string>
+
+#include <pybind11/operators.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <gz/math/Vector3.hh>
+#include <gz/math/detail/InterpolationPoint.hh>
+#include <optional>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace gz {
+namespace math {
+namespace python {
+/// define a pybind11 wrapper for gz::math::InterpolationPoint3D
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+template <typename T>
+void helpDefineMathInterpolationPoint3D(py::module &m,
+                                        const std::string &typestr) {
+  using Class = gz::math::InterpolationPoint3D<T>;
+  using Vector3Type = gz::math::Vector3<T>;
+
+  auto toString = [](const Class &si) {
+    std::stringstream stream;
+    stream << "InterpolationPoint3D(position=" << si.position;
+    if (si.index.has_value()) {
+      stream << ", index=" << si.index.value();
+    } else {
+      stream << ", index=None";
+    }
+    stream << ")";
+    return stream.str();
+  };
+
+  std::string pyclass_name = typestr;
+  py::class_<Class>(m, pyclass_name.c_str(), py::dynamic_attr())
+      .def(py::init<>())
+      .def(py::init<Vector3Type, std::optional<std::size_t>>(),
+           py::arg("position"), py::arg("index") = std::nullopt)
+      .def_readwrite("position", &Class::position)
+      .def_readwrite("index", &Class::index)
+      .def("__str__", toString)
+      .def("__repr__", toString);
+}
+
+/// define a pybind11 wrapper for gz::math::InterpolationPoint3D
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void defineMathInterpolationPoint3D(py::module &m, const std::string &typestr);
+
+}  // namespace python
+}  // namespace math
+}  // namespace gz
+
+#endif  // GZ_MATH_PYTHON__INTERPOLATION_POINT_HH_

src/python_pybind11/src/VolumetricGridLookupField.cc

@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ */
+
+#include "VolumetricGridLookupField.hh"
+#include <string>
+
+namespace gz {
+namespace math {
+namespace python {
+
+void defineMathVolumetricGridLookupField(py::module &m,
+                                         const std::string &typestr) {
+  // Define for double type
+  helpDefineMathVolumetricGridLookupField<double>(m, typestr + "d");
+  // Define for float type
+  helpDefineMathVolumetricGridLookupField<float>(m, typestr + "f");
+}
+
+}  // namespace python
+}  // namespace math
+}  // namespace gz

src/python_pybind11/src/VolumetricGridLookupField.hh

@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2025 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 GZ_MATH_PYTHON__VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
+#define GZ_MATH_PYTHON__VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include <pybind11/operators.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <gz/math/Vector3.hh>
+#include <gz/math/VolumetricGridLookupField.hh>
+#include <gz/math/detail/InterpolationPoint.hh>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+namespace gz {
+namespace math {
+namespace python {
+/// define a pybind11 wrapper for a gz::math::VolumetricGridLookupField
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+template <typename T, typename I = std::size_t>
+void helpDefineMathVolumetricGridLookupField(py::module &m,
+                                             const std::string &typestr) {
+  using Class = gz::math::VolumetricGridLookupField<T, I>;
+  using Vector3Type = gz::math::Vector3<T>;
+
+  auto toString = [](const Class &si) {
+    std::stringstream stream;
+    stream << si;
+    return stream.str();
+  };
+
+  std::string pyclass_name = typestr;
+  py::class_<Class>(m, pyclass_name.c_str(), py::buffer_protocol(),
+                    py::dynamic_attr())
+      .def(py::init<const std::vector<Vector3Type> &>())
+      .def(py::init<const std::vector<Vector3Type> &, const std::vector<I> &>())
+      .def(
+          "get_interpolators",
+          [](const Class &self, const Vector3Type &pt, double xTol, double yTol,
+             double zTol) {
+            return self.GetInterpolators(pt, xTol, yTol, zTol);
+          },
+          "Get interpolators for a given point", py::arg("point"),
+          py::arg("x_tol") = 1e-6, py::arg("y_tol") = 1e-6,
+          py::arg("z_tol") = 1e-6)
+      .def(
+          "estimate_value_using_trilinear",
+          [](const Class &self, const Vector3Type &pt,
+             const std::vector<double> &values, double defaultVal = 0.0) {
+            return self.EstimateValueUsingTrilinear(pt, values, defaultVal);
+          },
+          "Estimate value using trilinear interpolation", py::arg("point"),
+          py::arg("values"), py::arg("default") = 0.0)
+      .def("bounds", &Class::Bounds, "Get the bounds of the grid field")
+      .def("__str__", toString)
+      .def("__repr__", toString);
+}
+
+/// Define a pybind11 wrapper for a gz::math::VolumetricGridLookupField
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void defineMathVolumetricGridLookupField(py::module &m,
+                                         const std::string &typestr);
+
+}  // namespace python
+}  // namespace math
+}  // namespace gz
+
+#endif  // GZ_MATH_PYTHON__VOLUMETRIC_GRID_LOOKUP_FIELD_HH_

src/python_pybind11/src/_gz_math_pybind11.cc

@@ -28,6 +28,7 @@
 #include "Frustum.hh"
 #include "GaussMarkovProcess.hh"
 #include "Helpers.hh"
+#include "InterpolationPoint.hh"
 #include "Inertial.hh"
 #include "Interval.hh"
 #include "Kmeans.hh"
@@ -63,6 +64,7 @@
 #include "Vector3.hh"
 #include "Vector3Stats.hh"
 #include "Vector4.hh"
+#include "VolumetricGridLookupField.hh"
 
 namespace py = pybind11;
 
@@ -193,4 +195,9 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m)
     m, "OnePoleVector3");
 
   gz::math::python::defineMathCoordinateVector3(m, "CoordinateVector3");
+
+  gz::math::python::defineMathVolumetricGridLookupField(
+      m, "VolumetricGridLookupField");
+
+  gz::math::python::defineMathInterpolationPoint3D(m, "InterpolationPoint3D");
 }

src/python_pybind11/test/VolumetricGridLookupField_TEST.py

@@ -0,0 +1,98 @@
+# Copyright (C) 2025 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.
+
+import unittest
+
+from gz.math8 import Vector3d, VolumetricGridLookupFieldd
+
+
+class TestVolumetricGridLookupField(unittest.TestCase):
+    def test_interpolation_box_eight_points(self):
+        # Create cloud points as in the C++ test
+        cloud = [
+            Vector3d(0, 0, 0),
+            Vector3d(0, 0, 1),
+            Vector3d(0, 1, 0),
+            Vector3d(0, 1, 1),
+            Vector3d(1, 0, 0),
+            Vector3d(1, 0, 1),
+            Vector3d(1, 1, 0),
+            Vector3d(1, 1, 1),
+        ]
+
+        # Create lookup field
+        field = VolumetricGridLookupFieldd(cloud)
+
+        # Test inside point (should return 8 interpolators??)
+        pos = Vector3d(0.5, 0.5, 0.5)
+        interpolators = field.get_interpolators(pos)
+        self.assertEqual(len(interpolators), 8)
+
+        # Test outside point (should return 0 interpolators)
+        pos = Vector3d(-0.5, -0.5, -0.5)
+        interpolators = field.get_interpolators(pos)
+        self.assertEqual(len(interpolators), 0)
+
+        # Test point on plane (should return 4 interpolators)
+        pos = Vector3d(0.5, 0.5, 0)
+        interpolators = field.get_interpolators(pos)
+        self.assertEqual(len(interpolators), 4)
+
+        # Test point on edge (should return 2 interpolators)
+        pos = Vector3d(0.5, 0, 0)
+        interpolators = field.get_interpolators(pos)
+        self.assertEqual(len(interpolators), 2)
+
+    def test_trilinear_interpolation(self):
+        # Create cloud points
+        cloud = [
+            Vector3d(0, 0, 0),
+            Vector3d(0, 0, 1),
+            Vector3d(0, 1, 0),
+            Vector3d(0, 1, 1),
+            Vector3d(1, 0, 0),
+            Vector3d(1, 0, 1),
+            Vector3d(1, 1, 0),
+            Vector3d(1, 1, 1),
+        ]
+
+        values = [0, 0, 0, 0, 1, 1, 1, 1]
+        field = VolumetricGridLookupFieldd(cloud)
+
+        # Test inside point
+        pos = Vector3d(0.5, 0.5, 0.5)
+        value = field.estimate_value_using_trilinear(pos, values)
+        self.assertIsNotNone(value)
+        self.assertAlmostEqual(value, 0.5, places=3)
+
+        # Test outside point
+        pos = Vector3d(-0.5, -0.5, -0.5)
+        value = field.estimate_value_using_trilinear(pos, values)
+        self.assertIsNone(value)
+
+        # Test point on plane
+        pos = Vector3d(0, 0.5, 0.5)
+        value = field.estimate_value_using_trilinear(pos, values)
+        self.assertIsNotNone(value)
+        self.assertAlmostEqual(value, 0.0, places=3)
+
+        # Test point on vertex
+        pos = Vector3d(0, 0, 0)
+        value = field.estimate_value_using_trilinear(pos, values)
+        self.assertIsNotNone(value)
+        self.assertAlmostEqual(value, 0.0, places=3)
+
+
+if __name__ == "__main__":
+    unittest.main()