diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 4e7d5a40..efd7d9d4 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/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 diff --git a/src/python_pybind11/src/InterpolationPoint.cc b/src/python_pybind11/src/InterpolationPoint.cc new file mode 100644 index 00000000..3edd966f --- /dev/null +++ b/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 + +namespace gz { +namespace math { +namespace python { + +void defineMathInterpolationPoint3D(py::module &m, const std::string &typestr) { + helpDefineMathInterpolationPoint3D(m, typestr + "d"); + helpDefineMathInterpolationPoint3D(m, typestr + "f"); +} + +} // namespace python +} // namespace math +} // namespace gz diff --git a/src/python_pybind11/src/InterpolationPoint.hh b/src/python_pybind11/src/InterpolationPoint.hh new file mode 100644 index 00000000..97e0b413 --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#include +#include +#include + +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 +void helpDefineMathInterpolationPoint3D(py::module &m, + const std::string &typestr) { + using Class = gz::math::InterpolationPoint3D; + using Vector3Type = gz::math::Vector3; + + 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_(m, pyclass_name.c_str(), py::dynamic_attr()) + .def(py::init<>()) + .def(py::init>(), + 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_ diff --git a/src/python_pybind11/src/VolumetricGridLookupField.cc b/src/python_pybind11/src/VolumetricGridLookupField.cc new file mode 100644 index 00000000..687382e1 --- /dev/null +++ b/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 + +namespace gz { +namespace math { +namespace python { + +void defineMathVolumetricGridLookupField(py::module &m, + const std::string &typestr) { + // Define for double type + helpDefineMathVolumetricGridLookupField(m, typestr + "d"); + // Define for float type + helpDefineMathVolumetricGridLookupField(m, typestr + "f"); +} + +} // namespace python +} // namespace math +} // namespace gz diff --git a/src/python_pybind11/src/VolumetricGridLookupField.hh b/src/python_pybind11/src/VolumetricGridLookupField.hh new file mode 100644 index 00000000..b3dc377f --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +#include +#include +#include + +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 +void helpDefineMathVolumetricGridLookupField(py::module &m, + const std::string &typestr) { + using Class = gz::math::VolumetricGridLookupField; + using Vector3Type = gz::math::Vector3; + + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + + std::string pyclass_name = typestr; + py::class_(m, pyclass_name.c_str(), py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init &>()) + .def(py::init &, const std::vector &>()) + .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 &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_ diff --git a/src/python_pybind11/src/_gz_math_pybind11.cc b/src/python_pybind11/src/_gz_math_pybind11.cc index 51f4f627..79de6308 100644 --- a/src/python_pybind11/src/_gz_math_pybind11.cc +++ b/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"); } diff --git a/src/python_pybind11/test/VolumetricGridLookupField_TEST.py b/src/python_pybind11/test/VolumetricGridLookupField_TEST.py new file mode 100644 index 00000000..33b5f113 --- /dev/null +++ b/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()