mirror of
https://github.com/huggingface/lerobot.git
synced 2026-07-24 02:06:15 +00:00
navigation: port voxel map + occupancy/A* mapping backbone
Copied from the dyna360 research stack into lerobot.navigation: - voxel_map.py: sparse-hash VoxelMap (5 cm default), count-weighted running-mean geometry + optional per-voxel semantic feature; carve() for DynaMem-style free-space removal (dynamic updates), query() for top-k cosine text matches, remove_voxels_in_box() for scene mutation. Added an inverse.reshape(-1) guard for numpy 2.x's np.unique. - occupancy.py: derived 3-class top-down grid (UNOBSERVED/NAVIGABLE/ OBSTACLE) projected from the voxel map, A* with no corner-cutting, obstacle inflation, and frontier extraction for exploration. Added OccupancyGrid.is_obstacle() used by SafeBaseController's occupancy gate. Pure numpy, no torch/hardware/SDK. 42 new tests (71 total in tests/navigation/). Next: LingBot-Map geometry runner + integration. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
Pepijn
@@ -0,0 +1,266 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
"""Unit tests for the B1 occupancy projection + A*."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from lerobot.navigation.occupancy import (
|
||||
NAVIGABLE,
|
||||
OBSTACLE,
|
||||
UNOBSERVED,
|
||||
OccupancyGrid,
|
||||
astar,
|
||||
estimate_ground_y,
|
||||
find_frontier_cells,
|
||||
occupancy_to_rgb,
|
||||
project_voxel_map_to_grid,
|
||||
)
|
||||
from lerobot.navigation.voxel_map import VoxelMap
|
||||
|
||||
|
||||
def _vm_from_points(pts: list[tuple[float, float, float]], voxel_size: float = 0.1) -> VoxelMap:
|
||||
"""Build a VoxelMap from a list of world-XYZ points (all at conf=1.0)."""
|
||||
vm = VoxelMap(voxel_size=voxel_size)
|
||||
if not pts:
|
||||
return vm
|
||||
arr = np.asarray(pts, dtype=np.float32).reshape(-1, 1, 3)
|
||||
rgb = np.full((len(pts), 1, 3), 200, dtype=np.uint8)
|
||||
conf = np.ones((len(pts), 1), dtype=np.float32)
|
||||
vm.add(arr, rgb, conf, frame=0, t=0.0)
|
||||
return vm
|
||||
|
||||
|
||||
def test_estimate_ground_y_picks_high_percentile():
|
||||
# +y is down in OpenCV; ground = largest y. Numpy's default percentile
|
||||
# interpolation lands at 0.9 here (95% of way from 0.5 to 1.0 of the
|
||||
# last interval = 0.5 + 0.4·1.0 = 0.9), so we just check the estimate
|
||||
# is at least that high and clearly above the median.
|
||||
xyz = np.array(
|
||||
[[0, -1, 0], [0, -0.5, 0], [0, 0.0, 0], [0, 0.5, 0], [0, 1.0, 0]],
|
||||
dtype=np.float32,
|
||||
)
|
||||
g = estimate_ground_y(xyz)
|
||||
assert g >= 0.8
|
||||
median_y = float(np.median(xyz[:, 1]))
|
||||
assert g > median_y
|
||||
|
||||
|
||||
def test_projection_makes_ground_navigable_and_obstacles_red():
|
||||
# Ground row at y=1.0 across z=0..1; an obstacle at y=0.2 (above ground).
|
||||
pts: list[tuple[float, float, float]] = []
|
||||
for x in np.linspace(-0.5, 0.5, 11):
|
||||
for z in np.linspace(0.0, 1.0, 11):
|
||||
pts.append((float(x), 1.0, float(z))) # floor
|
||||
# Standing obstacle column at (x=0, z=0.5)
|
||||
for y in np.linspace(0.2, 0.9, 8):
|
||||
pts.append((0.0, float(y), 0.5))
|
||||
vm = _vm_from_points(pts, voxel_size=0.05)
|
||||
grid = project_voxel_map_to_grid(vm, cell_size=0.1, obstacle_y_range=(-2.0, -0.1))
|
||||
# The obstacle column should yield at least one OBSTACLE cell.
|
||||
assert (grid.classes == OBSTACLE).sum() >= 1
|
||||
# The floor away from the obstacle should be NAVIGABLE.
|
||||
iz, ix = grid.world_to_cell(-0.4, 0.0)
|
||||
assert grid.classes[iz, ix] == NAVIGABLE
|
||||
# Everywhere outside the observed area should still be UNOBSERVED.
|
||||
assert (grid.classes == UNOBSERVED).any()
|
||||
|
||||
|
||||
def test_empty_voxelmap_projects_to_single_unobserved_cell():
|
||||
vm = VoxelMap()
|
||||
grid = project_voxel_map_to_grid(vm)
|
||||
assert grid.shape == (1, 1)
|
||||
assert grid.classes[0, 0] == UNOBSERVED
|
||||
|
||||
|
||||
def test_world_to_cell_and_back_roundtrip():
|
||||
classes = np.full((4, 6), NAVIGABLE, dtype=np.int8)
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.5, origin_x=-1.0, origin_z=2.0, ground_y=0.0)
|
||||
iz, ix = grid.world_to_cell(0.25, 3.4)
|
||||
x, z = grid.cell_to_world(iz, ix)
|
||||
# The recovered (x, z) should land inside the same cell.
|
||||
iz2, ix2 = grid.world_to_cell(x, z)
|
||||
assert (iz, ix) == (iz2, ix2)
|
||||
|
||||
|
||||
def test_astar_finds_straight_path_when_unblocked():
|
||||
classes = np.full((10, 10), NAVIGABLE, dtype=np.int8)
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
path = astar(grid, (0.05, 0.05), (0.85, 0.85))
|
||||
assert path is not None
|
||||
assert len(path) >= 2
|
||||
# Should land near the goal.
|
||||
assert abs(path[-1][0] - 0.85) < 0.1 and abs(path[-1][1] - 0.85) < 0.1
|
||||
|
||||
|
||||
def test_astar_routes_around_obstacle_wall():
|
||||
# Vertical wall at column 5, rows 1..8.
|
||||
classes = np.full((10, 10), NAVIGABLE, dtype=np.int8)
|
||||
classes[1:9, 5] = OBSTACLE
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
path = astar(grid, (0.05, 0.45), (0.95, 0.45))
|
||||
assert path is not None
|
||||
# The path must not pass through any obstacle cell.
|
||||
for x, z in path:
|
||||
iz, ix = grid.world_to_cell(x, z)
|
||||
assert classes[iz, ix] != OBSTACLE
|
||||
|
||||
|
||||
def test_astar_returns_none_when_no_path():
|
||||
# Wall that completely separates start from goal.
|
||||
classes = np.full((10, 10), NAVIGABLE, dtype=np.int8)
|
||||
classes[:, 5] = OBSTACLE
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
path = astar(grid, (0.05, 0.5), (0.95, 0.5))
|
||||
assert path is None
|
||||
|
||||
|
||||
def test_astar_no_corner_cutting_through_obstacles():
|
||||
# Two diagonal obstacle cells that would let a naive A* squeeze through.
|
||||
classes = np.full((4, 4), NAVIGABLE, dtype=np.int8)
|
||||
classes[1, 2] = OBSTACLE
|
||||
classes[2, 1] = OBSTACLE
|
||||
grid = OccupancyGrid(classes=classes, cell_size=1.0, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
path = astar(grid, (0.5, 0.5), (2.5, 2.5))
|
||||
assert path is not None
|
||||
# The path should NOT step (1, 1) → (2, 2) since that diagonal cuts the
|
||||
# obstacle corner. Verify by checking no two consecutive cells are a
|
||||
# diagonal move with both perpendicular cells blocked.
|
||||
cells = [grid.world_to_cell(x, z) for x, z in path]
|
||||
for prev, nxt in zip(cells, cells[1:], strict=False):
|
||||
diz = nxt[0] - prev[0]
|
||||
dix = nxt[1] - prev[1]
|
||||
if abs(diz) == 1 and abs(dix) == 1:
|
||||
assert grid.is_navigable(prev[0] + diz, prev[1]) and grid.is_navigable(prev[0], prev[1] + dix), (
|
||||
f"Corner cut detected at step {prev}->{nxt}"
|
||||
)
|
||||
|
||||
|
||||
def test_obstacle_inflation_grows_obstacle_class():
|
||||
classes = np.full((5, 5), NAVIGABLE, dtype=np.int8)
|
||||
classes[2, 2] = OBSTACLE
|
||||
# Test the private helper directly — we don't need a voxel map for this.
|
||||
from lerobot.navigation.occupancy import _inflate_obstacles
|
||||
|
||||
inflated = _inflate_obstacles(classes, radius=1)
|
||||
# 3x3 block now obstacle (around the single original cell).
|
||||
assert (inflated[1:4, 1:4] == OBSTACLE).all()
|
||||
# Far corner stays navigable.
|
||||
assert inflated[0, 0] == NAVIGABLE
|
||||
|
||||
|
||||
def test_find_frontier_cells_are_navigable_next_to_unobserved():
|
||||
classes = np.full((5, 5), UNOBSERVED, dtype=np.int8)
|
||||
classes[1:4, 1:4] = NAVIGABLE
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
cells = find_frontier_cells(grid)
|
||||
# Every frontier cell is NAVIGABLE itself.
|
||||
for iz, ix in cells:
|
||||
assert classes[iz, ix] == NAVIGABLE
|
||||
# And every frontier cell has at least one UNOBSERVED 4-neighbour.
|
||||
for iz, ix in cells:
|
||||
adj_unobs = (
|
||||
(iz > 0 and classes[iz - 1, ix] == UNOBSERVED)
|
||||
or (iz < 4 and classes[iz + 1, ix] == UNOBSERVED)
|
||||
or (ix > 0 and classes[iz, ix - 1] == UNOBSERVED)
|
||||
or (ix < 4 and classes[iz, ix + 1] == UNOBSERVED)
|
||||
)
|
||||
assert adj_unobs
|
||||
|
||||
|
||||
def test_find_frontier_empty_when_no_unobserved():
|
||||
classes = np.full((5, 5), NAVIGABLE, dtype=np.int8)
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
cells = find_frontier_cells(grid)
|
||||
assert cells.shape == (0, 2)
|
||||
|
||||
|
||||
def test_occupancy_to_rgb_returns_uint8_image():
|
||||
classes = np.array([[UNOBSERVED, NAVIGABLE, OBSTACLE]], dtype=np.int8)
|
||||
grid = OccupancyGrid(classes=classes, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
img = occupancy_to_rgb(grid)
|
||||
assert img.shape == (1, 3, 3)
|
||||
assert img.dtype == np.uint8
|
||||
# Obstacle cell is reddish.
|
||||
assert img[0, 2, 0] > img[0, 2, 1] and img[0, 2, 0] > img[0, 2, 2]
|
||||
|
||||
|
||||
def test_carving_makes_obstacle_vanish_on_next_projection():
|
||||
"""End-to-end: an object voxel becomes an obstacle; after we delete it
|
||||
from the VoxelMap (simulating carving), the next projection no longer
|
||||
flags that cell as OBSTACLE — that's what makes the obstacle map a
|
||||
cheap derived view."""
|
||||
pts = []
|
||||
for x in np.linspace(-0.5, 0.5, 11):
|
||||
for z in np.linspace(0.0, 1.0, 11):
|
||||
pts.append((float(x), 1.0, float(z))) # floor
|
||||
for y in np.linspace(0.2, 0.9, 8):
|
||||
pts.append((0.0, float(y), 0.5)) # obstacle column
|
||||
vm = _vm_from_points(pts, voxel_size=0.05)
|
||||
grid1 = project_voxel_map_to_grid(vm, cell_size=0.1)
|
||||
assert (grid1.classes == OBSTACLE).sum() >= 1
|
||||
|
||||
# Surgically drop every voxel in the obstacle band.
|
||||
cnt = vm._count.astype(np.float64).reshape(-1, 1) # noqa: SLF001
|
||||
means = vm._xyz_sum / cnt # noqa: SLF001
|
||||
keep = (means[:, 1] >= 0.95) | (means[:, 1] <= 0.05)
|
||||
vm._idx = vm._idx[keep] # noqa: SLF001
|
||||
vm._count = vm._count[keep] # noqa: SLF001
|
||||
vm._xyz_sum = vm._xyz_sum[keep] # noqa: SLF001
|
||||
vm._rgb_sum = vm._rgb_sum[keep] # noqa: SLF001
|
||||
vm._last_frame = vm._last_frame[keep] # noqa: SLF001
|
||||
vm._last_time = vm._last_time[keep] # noqa: SLF001
|
||||
vm._lookup = { # noqa: SLF001
|
||||
(int(vm._idx[i, 0]), int(vm._idx[i, 1]), int(vm._idx[i, 2])): i # noqa: SLF001
|
||||
for i in range(len(vm._idx)) # noqa: SLF001
|
||||
}
|
||||
|
||||
grid2 = project_voxel_map_to_grid(vm, cell_size=0.1)
|
||||
assert (grid2.classes == OBSTACLE).sum() == 0
|
||||
|
||||
|
||||
def test_astar_works_after_inflation():
|
||||
# Without inflation, robot can hug a 1-cell-wide wall; with inflation,
|
||||
# it has to take a wider detour.
|
||||
classes = np.full((11, 11), NAVIGABLE, dtype=np.int8)
|
||||
classes[5, 1:9] = OBSTACLE # horizontal wall
|
||||
grid_raw = OccupancyGrid(classes=classes.copy(), cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
path_raw = astar(grid_raw, (0.45, 0.0), (0.45, 1.0))
|
||||
assert path_raw is not None # the wall has an opening at the edges
|
||||
from lerobot.navigation.occupancy import _inflate_obstacles
|
||||
|
||||
inflated = _inflate_obstacles(classes, radius=1)
|
||||
grid_inf = OccupancyGrid(classes=inflated, cell_size=0.1, origin_x=0.0, origin_z=0.0, ground_y=0.0)
|
||||
# After inflation the path is at least as long (often longer).
|
||||
path_inf = astar(grid_inf, (0.45, 0.0), (0.45, 1.0))
|
||||
if path_inf is not None:
|
||||
assert len(path_inf) >= len(path_raw)
|
||||
|
||||
|
||||
def test_obstacle_range_outside_voxel_y_produces_only_navigable():
|
||||
"""A range well above the actual voxel y values should classify the floor
|
||||
as NAVIGABLE only — no obstacles. (Replaces the prior sub-float32-epsilon
|
||||
test which exercised numpy's float-downcast quirk rather than the
|
||||
occupancy semantics.)"""
|
||||
pts = [(float(x), 1.0, float(z)) for x in [0, 0.1] for z in [0, 0.1]]
|
||||
vm = _vm_from_points(pts, voxel_size=0.05)
|
||||
# ground_y will be 1.0; this range looks for obstacles 5 m–10 m above
|
||||
# the floor, where there are none.
|
||||
grid = project_voxel_map_to_grid(vm, obstacle_y_range=(-10.0, -5.0))
|
||||
assert (grid.classes == OBSTACLE).sum() == 0
|
||||
assert (grid.classes == NAVIGABLE).sum() >= 1
|
||||
@@ -0,0 +1,149 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
"""Unit tests for ``VoxelMap`` — geometry-only behavior (M3)."""
|
||||
|
||||
# ruff: noqa: N803, N806 — H, W, D: conventional array-dimension names
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from lerobot.navigation.voxel_map import VoxelMap
|
||||
|
||||
|
||||
def _scatter(points: np.ndarray, color: tuple[int, int, int] = (200, 100, 50)) -> tuple[np.ndarray, ...]:
|
||||
"""Helper: build (points, rgb, conf) arrays from a list of xyz coords."""
|
||||
rgb = np.tile(np.array(color, dtype=np.uint8), (len(points), 1))
|
||||
conf = np.ones(len(points), dtype=np.float32)
|
||||
return points.astype(np.float32), rgb, conf
|
||||
|
||||
|
||||
def test_initially_empty():
|
||||
vm = VoxelMap(voxel_size=0.05)
|
||||
assert len(vm) == 0
|
||||
snap = vm.snapshot()
|
||||
assert snap.xyz.shape == (0, 3)
|
||||
assert snap.rgb.shape == (0, 3)
|
||||
|
||||
|
||||
def test_voxel_size_must_be_positive():
|
||||
with pytest.raises(ValueError):
|
||||
VoxelMap(voxel_size=0.0)
|
||||
with pytest.raises(ValueError):
|
||||
VoxelMap(voxel_size=-0.1)
|
||||
|
||||
|
||||
def test_single_point_creates_one_voxel():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts, rgb, conf = _scatter(np.array([[0.123, 0.456, 0.789]]))
|
||||
stats = vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
assert stats == type(stats)(n_voxels=1, n_added=1, n_updated=0)
|
||||
assert len(vm) == 1
|
||||
snap = vm.snapshot()
|
||||
np.testing.assert_allclose(snap.xyz[0], [0.123, 0.456, 0.789], atol=1e-5)
|
||||
|
||||
|
||||
def test_points_in_same_voxel_collapse_and_average():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
# Two points inside the voxel [0.0, 0.1) on each axis.
|
||||
pts, rgb, conf = _scatter(np.array([[0.01, 0.01, 0.01], [0.09, 0.09, 0.09]]))
|
||||
stats = vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
assert stats.n_voxels == 1
|
||||
assert stats.n_added == 1
|
||||
snap = vm.snapshot()
|
||||
np.testing.assert_allclose(snap.xyz[0], [0.05, 0.05, 0.05], atol=1e-5)
|
||||
assert int(snap.count[0]) == 2
|
||||
|
||||
|
||||
def test_second_keyframe_updates_running_mean():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts1, rgb1, conf1 = _scatter(np.array([[0.02, 0.02, 0.02]]), color=(100, 100, 100))
|
||||
vm.add(pts1, rgb1, conf1, frame=0, t=0.0)
|
||||
pts2, rgb2, conf2 = _scatter(np.array([[0.08, 0.08, 0.08]]), color=(200, 200, 200))
|
||||
stats = vm.add(pts2, rgb2, conf2, frame=1, t=0.5)
|
||||
assert stats.n_added == 0
|
||||
assert stats.n_updated == 1
|
||||
snap = vm.snapshot()
|
||||
# Mean position = (0.02 + 0.08) / 2 = 0.05; mean color = 150.
|
||||
np.testing.assert_allclose(snap.xyz[0], [0.05, 0.05, 0.05], atol=1e-5)
|
||||
np.testing.assert_allclose(snap.rgb[0], [150, 150, 150], atol=1)
|
||||
assert int(snap.last_frame[0]) == 1
|
||||
assert float(snap.last_time[0]) == pytest.approx(0.5)
|
||||
|
||||
|
||||
def test_conf_gate_drops_low_confidence_pixels():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts = np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]], dtype=np.float32)
|
||||
rgb = np.array([[10, 20, 30], [40, 50, 60]], dtype=np.uint8)
|
||||
conf = np.array([0.9, 0.1], dtype=np.float32)
|
||||
stats = vm.add(pts, rgb, conf, frame=0, t=0.0, conf_thresh=0.5)
|
||||
assert stats.n_voxels == 1
|
||||
snap = vm.snapshot()
|
||||
np.testing.assert_allclose(snap.xyz[0], [0.0, 0.0, 0.0], atol=1e-5)
|
||||
np.testing.assert_allclose(snap.rgb[0], [10, 20, 30], atol=1)
|
||||
|
||||
|
||||
def test_quantization_negative_coordinates():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts, rgb, conf = _scatter(np.array([[-0.05, -0.15, -0.25]]))
|
||||
stats = vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
assert stats.n_voxels == 1
|
||||
# floor(-0.05/0.1) = floor(-0.5) = -1 (voxel covers [-0.1, 0.0)).
|
||||
# Just check the mean equals the input single point.
|
||||
snap = vm.snapshot()
|
||||
np.testing.assert_allclose(snap.xyz[0], [-0.05, -0.15, -0.25], atol=1e-5)
|
||||
|
||||
|
||||
def test_image_shaped_input_is_flattened():
|
||||
"""``add`` accepts (H, W, 3) point arrays — typical Pi3X output shape."""
|
||||
vm = VoxelMap(voxel_size=0.5)
|
||||
H, W = 4, 4
|
||||
pts = np.zeros((H, W, 3), dtype=np.float32)
|
||||
pts[..., 0] = np.linspace(0, 5, W)[None, :] # 16 unique x values? No, 4.
|
||||
rgb = np.full((H, W, 3), 128, dtype=np.uint8)
|
||||
conf = np.ones((H, W), dtype=np.float32)
|
||||
stats = vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
# All x values land in voxel slots at 0, 1, 2, 3, 4 (different voxels at
|
||||
# 0.5m size); each row of the image contributes the same 4 unique voxels,
|
||||
# collapsed within the keyframe — but actually voxel indices depend on x.
|
||||
# The point of this test is just that flattening works without raising.
|
||||
assert stats.n_voxels >= 1
|
||||
assert stats.n_voxels <= H * W
|
||||
|
||||
|
||||
def test_nonfinite_points_are_dropped():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts = np.array(
|
||||
[[0.0, 0.0, 0.0], [np.nan, 0.0, 0.0], [0.0, np.inf, 0.0]],
|
||||
dtype=np.float32,
|
||||
)
|
||||
rgb = np.full((3, 3), 100, dtype=np.uint8)
|
||||
conf = np.ones(3, dtype=np.float32)
|
||||
stats = vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
assert stats.n_voxels == 1
|
||||
|
||||
|
||||
def test_snapshot_rgb_clipped_to_uint8():
|
||||
"""If RGB sums accumulate to > 255 per channel, the snapshot mean is still
|
||||
a clean uint8."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts, rgb, conf = _scatter(np.array([[0.0, 0.0, 0.0]]), color=(255, 255, 255))
|
||||
vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
vm.add(pts, rgb, conf, frame=1, t=0.5)
|
||||
snap = vm.snapshot()
|
||||
assert snap.rgb.dtype == np.uint8
|
||||
np.testing.assert_array_equal(snap.rgb[0], [255, 255, 255])
|
||||
@@ -0,0 +1,217 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
"""Unit tests for ``VoxelMap.carve`` — DynaMem-style free-space removal (M4)."""
|
||||
|
||||
# ruff: noqa: N803, N806 — H, W, D: conventional array-dimension names
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from lerobot.navigation.voxel_map import CarveResult, VoxelMap
|
||||
|
||||
|
||||
def _identity_pose() -> np.ndarray:
|
||||
"""Camera-to-world identity (camera == world)."""
|
||||
return np.eye(4, dtype=np.float64)
|
||||
|
||||
|
||||
def _shifted_pose(tx: float = 0.0, ty: float = 0.0, tz: float = 0.0) -> np.ndarray:
|
||||
p = np.eye(4, dtype=np.float64)
|
||||
p[:3, 3] = (tx, ty, tz)
|
||||
return p
|
||||
|
||||
|
||||
def _solid_depth_view(H: int, W: int, depth: float, conf: float = 1.0) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""A synthetic Pi3X view: every pixel sees a surface at ``depth`` meters."""
|
||||
fov_deg = 90.0
|
||||
focal = W / (2.0 * math.tan(math.radians(fov_deg) / 2.0))
|
||||
cx, cy = (W - 1) / 2.0, (H - 1) / 2.0
|
||||
us, vs = np.meshgrid(np.arange(W), np.arange(H))
|
||||
# local_points[v,u] = depth * (K^-1 @ [u,v,1]); z = depth.
|
||||
x = (us - cx) * depth / focal
|
||||
y = (vs - cy) * depth / focal
|
||||
z = np.full_like(x, depth, dtype=np.float64)
|
||||
local = np.stack([x, y, z], axis=-1).astype(np.float32)
|
||||
return local, np.full((H, W), conf, dtype=np.float32), focal
|
||||
|
||||
|
||||
def _seed_voxel(vm: VoxelMap, xyz: tuple[float, float, float], frame: int = 0) -> None:
|
||||
pts = np.asarray([xyz], dtype=np.float32)
|
||||
rgb = np.full((1, 3), 200, dtype=np.uint8)
|
||||
conf = np.ones(1, dtype=np.float32)
|
||||
vm.add(pts, rgb, conf, frame=frame, t=float(frame) * 0.5)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_empty_map_returns_zero():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=0, t=0.0)
|
||||
assert isinstance(result, CarveResult)
|
||||
assert result.n_removed == 0
|
||||
assert result.removed_xyz.shape == (0, 3)
|
||||
|
||||
|
||||
def test_voxel_in_front_of_surface_is_removed():
|
||||
"""Voxel at z=2 m, observed surface at z=5 m, margin 0.05: free space."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 2.0))
|
||||
assert len(vm) == 1
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 1
|
||||
assert len(vm) == 0
|
||||
assert result.removed_xyz.shape == (1, 3)
|
||||
|
||||
|
||||
def test_voxel_at_surface_is_kept():
|
||||
"""Voxel at z = 5.0 m, surface at 5.0 m — d is NOT < D - margin."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 5.0))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5, margin=0.05)
|
||||
assert result.n_removed == 0
|
||||
assert len(vm) == 1
|
||||
|
||||
|
||||
def test_voxel_behind_surface_is_kept():
|
||||
"""Voxel at z=10 m, surface at z=5 m: voxel is occluded, NOT free space."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 10.0))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 0
|
||||
|
||||
|
||||
def test_voxel_behind_camera_is_kept():
|
||||
"""A voxel with z<=0 in the camera frame can't be seen — must not be carved."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, -1.0))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 0
|
||||
|
||||
|
||||
def test_voxel_out_of_view_is_kept():
|
||||
"""A voxel inside no pixel's frustum should be left alone."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
# Way off to the side — projects outside the 32x32 image at 90° HFOV.
|
||||
_seed_voxel(vm, (50.0, 0.0, 2.0))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 0
|
||||
|
||||
|
||||
def test_low_confidence_blocks_carve():
|
||||
"""If conf at the projected pixel is below threshold, don't remove."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 2.0))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0, conf=0.2)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5, conf_thresh=0.5)
|
||||
assert result.n_removed == 0
|
||||
|
||||
|
||||
def test_invalid_depth_blocks_carve():
|
||||
"""NaN / non-positive depth at the projected pixel must not trigger carve."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 2.0))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
local[:, :, 2] = np.nan
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 0
|
||||
|
||||
|
||||
def test_margin_protects_near_surface():
|
||||
"""A voxel 3 cm in front of a 5 m surface, margin 5 cm: NOT carved."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 5.0 - 0.03))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5, margin=0.05)
|
||||
assert result.n_removed == 0
|
||||
|
||||
|
||||
def test_margin_zero_carves_near_surface():
|
||||
"""Same setup, margin 0: now the 3 cm gap counts as free space."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 5.0 - 0.03))
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5, margin=0.0)
|
||||
assert result.n_removed == 1
|
||||
|
||||
|
||||
def test_carve_compacts_arrays_and_lookup():
|
||||
"""After carving some but not all voxels, internal storage stays consistent."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
# Three voxels: in front (will be carved), at surface (kept), to the side (kept).
|
||||
_seed_voxel(vm, (0.0, 0.0, 2.0))
|
||||
_seed_voxel(vm, (0.0, 0.0, 5.0))
|
||||
_seed_voxel(vm, (50.0, 0.0, 5.0))
|
||||
assert len(vm) == 3
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, _identity_pose(), focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 1
|
||||
assert len(vm) == 2
|
||||
# All internal arrays must agree on the new length.
|
||||
assert vm._idx.shape == (2, 3) # noqa: SLF001
|
||||
assert vm._count.shape == (2,) # noqa: SLF001
|
||||
assert len(vm._lookup) == 2 # noqa: SLF001
|
||||
# Lookup rows must point at valid indices in the new arrays.
|
||||
for row in vm._lookup.values(): # noqa: SLF001
|
||||
assert 0 <= row < 2
|
||||
# A subsequent `add` to a removed voxel must work cleanly (revives it).
|
||||
pts = np.asarray([[0.0, 0.0, 2.0]], dtype=np.float32)
|
||||
rgb = np.full((1, 3), 100, dtype=np.uint8)
|
||||
conf1 = np.ones(1, dtype=np.float32)
|
||||
add_stats = vm.add(pts, rgb, conf1, frame=2, t=1.0)
|
||||
assert add_stats.n_added == 1
|
||||
assert len(vm) == 3
|
||||
|
||||
|
||||
def test_carve_shape_validation():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
_seed_voxel(vm, (0.0, 0.0, 2.0))
|
||||
bad_local = np.zeros((32, 32, 2), dtype=np.float32)
|
||||
conf = np.ones((32, 32), dtype=np.float32)
|
||||
with pytest.raises(ValueError, match=r"H, W, 3"):
|
||||
vm.carve(bad_local, conf, _identity_pose(), focal_px=16.0, frame=0, t=0.0)
|
||||
|
||||
local = np.zeros((32, 32, 3), dtype=np.float32)
|
||||
bad_conf = np.ones((16, 32), dtype=np.float32)
|
||||
with pytest.raises(ValueError, match=r"conf shape"):
|
||||
vm.carve(local, bad_conf, _identity_pose(), focal_px=16.0, frame=0, t=0.0)
|
||||
|
||||
bad_pose = np.eye(3)
|
||||
with pytest.raises(ValueError, match=r"pose must be"):
|
||||
vm.carve(local, conf, bad_pose, focal_px=16.0, frame=0, t=0.0)
|
||||
|
||||
|
||||
def test_carve_with_translated_camera():
|
||||
"""If the camera has moved, world-space voxels must be transformed correctly
|
||||
before the free-space test."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
# Voxel at world position (10, 0, 2). With camera at world (10, 0, 0),
|
||||
# the voxel is 2 m in front of the camera's local +Z; surface at 5 m → carve.
|
||||
_seed_voxel(vm, (10.0, 0.0, 2.0))
|
||||
pose = _shifted_pose(tx=10.0, ty=0.0, tz=0.0)
|
||||
local, conf, focal = _solid_depth_view(32, 32, depth=5.0)
|
||||
result = vm.carve(local, conf, pose, focal_px=focal, frame=1, t=0.5)
|
||||
assert result.n_removed == 1
|
||||
@@ -0,0 +1,99 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2026 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# 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.
|
||||
|
||||
"""Unit tests for the C2 scene-mutation helper on VoxelMap."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from lerobot.navigation.voxel_map import VoxelMap
|
||||
|
||||
|
||||
def test_remove_voxels_in_box_zero_when_empty():
|
||||
vm = VoxelMap()
|
||||
assert vm.remove_voxels_in_box((-1, -1, -1), (1, 1, 1)) == 0
|
||||
|
||||
|
||||
def test_remove_voxels_in_box_only_deletes_inside():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts = np.array(
|
||||
[
|
||||
[[0.0, 0.0, 0.0]], # inside
|
||||
[[0.1, 0.0, 0.0]], # inside
|
||||
[[2.0, 0.0, 0.0]], # outside
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
rgb = np.full((3, 1, 3), 200, dtype=np.uint8)
|
||||
conf = np.ones((3, 1), dtype=np.float32)
|
||||
vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
assert len(vm) == 3
|
||||
|
||||
n = vm.remove_voxels_in_box((-0.05, -0.05, -0.05), (0.15, 0.05, 0.05))
|
||||
assert n == 2
|
||||
assert len(vm) == 1
|
||||
snap = vm.snapshot()
|
||||
np.testing.assert_allclose(snap.xyz[0], [2.0, 0.0, 0.0], atol=1e-3)
|
||||
|
||||
|
||||
def test_remove_voxels_keeps_feature_arrays_aligned():
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts = np.array([[[0.0, 0.0, 0.0]], [[2.0, 0.0, 0.0]]], dtype=np.float64)
|
||||
rgb = np.full((2, 1, 3), 200, dtype=np.uint8)
|
||||
conf = np.ones((2, 1), dtype=np.float32)
|
||||
feat = np.array([[[1.0, 0.0, 0.0, 0.0]], [[0.0, 1.0, 0.0, 0.0]]], dtype=np.float16)
|
||||
vm.add(pts, rgb, conf, frame=0, t=0.0, feat_map=feat)
|
||||
assert len(vm) == 2
|
||||
assert vm._feat_sum.shape == (2, 4) # noqa: SLF001
|
||||
|
||||
vm.remove_voxels_in_box((-0.5, -0.5, -0.5), (0.5, 0.5, 0.5))
|
||||
assert len(vm) == 1
|
||||
# Feature arrays now have length 1 — matches _count.
|
||||
assert vm._feat_sum.shape == (1, 4) # noqa: SLF001
|
||||
snap = vm.snapshot(include_features=True)
|
||||
# Surviving voxel had vector [0, 1, 0, 0]; normalized stays the same.
|
||||
assert snap.feat is not None
|
||||
np.testing.assert_allclose(
|
||||
snap.feat[0].astype(np.float32),
|
||||
[0.0, 1.0, 0.0, 0.0],
|
||||
atol=1e-3,
|
||||
)
|
||||
|
||||
|
||||
def test_remove_voxels_compacts_lookup():
|
||||
"""After deletion the dict→row map must still point to the right rows."""
|
||||
vm = VoxelMap(voxel_size=0.1)
|
||||
pts = np.array(
|
||||
[[[0.0, 0.0, 0.0]], [[2.0, 0.0, 0.0]], [[4.0, 0.0, 0.0]]],
|
||||
dtype=np.float64,
|
||||
)
|
||||
rgb = np.full((3, 1, 3), 200, dtype=np.uint8)
|
||||
conf = np.ones((3, 1), dtype=np.float32)
|
||||
vm.add(pts, rgb, conf, frame=0, t=0.0)
|
||||
|
||||
vm.remove_voxels_in_box((1.5, -0.5, -0.5), (2.5, 0.5, 0.5)) # delete the middle
|
||||
assert len(vm) == 2
|
||||
|
||||
# Adding a new voxel at one of the surviving positions should UPDATE
|
||||
# (not append), which only works if the lookup row indices are correct.
|
||||
new_pt = np.array([[[0.0, 0.0, 0.0]]], dtype=np.float64)
|
||||
stats = vm.add(
|
||||
new_pt, np.full((1, 1, 3), 50, dtype=np.uint8), np.ones((1, 1), dtype=np.float32), frame=1, t=1.0
|
||||
)
|
||||
assert stats.n_added == 0
|
||||
assert stats.n_updated == 1
|
||||
assert len(vm) == 2
|
||||
Reference in New Issue
Block a user