gz-math/src/python_pybind11/test/VolumetricGridLookupField_TEST.py at 4f993ce11e3d6d28fdc6662ee5225848e439cce2 · gazebosim/gz-math · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
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()