From df238bdb911b51835147aa742b914b99660dda56 Mon Sep 17 00:00:00 2001 From: stash Date: Thu, 10 Sep 2026 23:16:53 -0700 Subject: [PATCH 1/2] feat(evals): pointcloud comprehension suite over go2_office_pc recording five hand-labelled questions (room counts, doorways, occupancy, size compare) against the final global_map frame + odom path. brings over PointCloud2.agent_encode so the map is legible to the model on main. dataset (3mb slice) in lfs as go2_office_pc.db. --- data/.lfs/go2_office_pc.db.tar.gz | 3 + dimos/evals/suites/pointcloud_office.py | 93 ++++++++++ dimos/msgs/sensor_msgs/PointCloud2.py | 174 +++++++++++++++++++ dimos/msgs/sensor_msgs/test_PointCloud2.py | 187 +++++++++++++++++++++ 4 files changed, 457 insertions(+) create mode 100644 data/.lfs/go2_office_pc.db.tar.gz create mode 100644 dimos/evals/suites/pointcloud_office.py diff --git a/data/.lfs/go2_office_pc.db.tar.gz b/data/.lfs/go2_office_pc.db.tar.gz new file mode 100644 index 0000000000..6973203001 --- /dev/null +++ b/data/.lfs/go2_office_pc.db.tar.gz @@ -0,0 +1,3 @@ +version https://git.lfs.github.com/spec/v1 +oid sha256:5a9444a2d2174f7dec5699f64cf95c0f9f551adb3e609383fc6fc1c1ef65814c +size 1054229 diff --git a/dimos/evals/suites/pointcloud_office.py b/dimos/evals/suites/pointcloud_office.py new file mode 100644 index 0000000000..a96faddbd2 --- /dev/null +++ b/dimos/evals/suites/pointcloud_office.py @@ -0,0 +1,93 @@ +# Copyright 2026 Dimensional Inc. +# +# 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. + +"""Point-cloud comprehension over the go2_office_pc recording. + +The agent sees the final ``global_map`` frame (the whole accumulated floor +plan, via ``PointCloud2.agent_encode``) and the ``odom`` path. Ground truth +was hand-labelled from the recording. Counts grade with off-by-one partial +credit; the two categorical cases are exact. + + dimos evals run dimos.evals.suites.pointcloud_office --agent dimos.evals.agents.question_answer +""" + +from __future__ import annotations + +from dimos.evals.environments.dataset import Dataset +from dimos.evals.scorers import choice, exact, first_number, within, yes_no +from dimos.evals.types import EvalCase, Suite + +DATASET = "go2_office_pc" + + +def _env() -> Dataset: + """Final accumulated map frame plus the odom path.""" + return Dataset( + DATASET, + select=( + lambda s: s.streams.global_map, + lambda s: s.streams.odom, + ), + ) + + +SUITE: Suite = [ + EvalCase( + id="pc_rooms_entered", + inputs="How many distinct rooms did you walk into? Answer with just the number.", + environment=_env(), + grade=lambda o: within(1.0)(2.0, first_number(o.trajectory.final_answer)), + tags=frozenset({"pointcloud", "count"}), + ), + EvalCase( + id="pc_rooms_passed", + inputs=( + "How many distinct rooms did you walk past or into in total? " + "Answer with just the number." + ), + environment=_env(), + grade=lambda o: within(1.0)(3.0, first_number(o.trajectory.final_answer)), + tags=frozenset({"pointcloud", "count"}), + ), + EvalCase( + id="pc_open_doorways", + inputs=( + "How many open doorways did you see? The building has only standard " + "residential-width doors. Answer with just the number." + ), + environment=_env(), + grade=lambda o: within(1.0)(4.0, first_number(o.trajectory.final_answer)), + tags=frozenset({"pointcloud", "count"}), + ), + EvalCase( + id="pc_biggest_room_occupied", + inputs=( + "In the largest room, do objects occupy more than 50% of its 2D floor " + "area? Answer yes or no." + ), + environment=_env(), + grade=lambda o: exact("yes", yes_no(o.trajectory.final_answer)), + tags=frozenset({"pointcloud", "yesno"}), + ), + EvalCase( + id="pc_first_vs_second_room_size", + inputs=( + "Was the first room you walked through bigger or smaller than the " + "second room you walked through? Answer with one word: bigger or smaller." + ), + environment=_env(), + grade=lambda o: exact("bigger", choice(["bigger", "smaller"])(o.trajectory.final_answer)), + tags=frozenset({"pointcloud", "compare"}), + ), +] diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py index 9ec2bf6ea8..86fdd46b79 100644 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ b/dimos/msgs/sensor_msgs/PointCloud2.py @@ -15,6 +15,7 @@ from __future__ import annotations import functools +import json import struct from typing import TYPE_CHECKING, Any @@ -339,6 +340,179 @@ def from_rgbd( def __str__(self) -> str: return f"PointCloud2(frame_id='{self.frame_id}', num_points={len(self)})" + ENCODE_SOFT_CAP = 6000 + """Ceiling on one frame's encoding, JSON bytes: a full frame fits in one + readout of a tool that caps its output here.""" + + AGENT_ENCODE_LEGEND = ( + "World-frame meters throughout: +x is east and +y is north. For numeric " + "full-cloud geometry, use window_m rather than raster or body-height boxes: " + "horizontal extent is max(xmax-xmin, ymax-ymin), and vertical span is " + "zmax-zmin. centroid_xy_m is the full-cloud horizontal center. Across a " + "sequence, read overall motion or gained-coverage direction from dx,dy = " + "last centroid_xy_m minus first centroid_xy_m; range edges are too noisy for " + "direction. For the eight compass directions, if |dx| > 2.41*|dy| use east " + "when dx>0 or west when dx<0; if |dy| > 2.41*|dx| use north when dy>0 or " + "south when dy<0; otherwise use the diagonal determined by the signs of dx " + "and dy (northeast, northwest, southeast, or southwest). " + "floor_footprint_m2 is this frame's own measured footprint: the count of " + "0.2 m cells with any stored return times 0.04 m2. Compare each frame's own " + "value for an area trend; it can decrease as well as increase, so do not " + "accumulate it across frames or substitute bounding-box area. " + "boxes are exact x-y extents of stored returns within z_m, in world meters, " + "as xmin:xmax@ymin:ymax (a lone value is zero width). Horizontal clearance " + "from a point qx,qy is the minimum over boxes of hypot(dx,dy), where " + "dx=max(0,xmin-qx,qx-xmax) and dy=max(0,ymin-qy,qy-ymax). Each term is zero " + "only when the point lies inside that coordinate extent. " + "raster.rows: one row per cell_m of y, north to south, prefixed with its y; " + "two characters per cell, west to east from origin_xy_m. First character is " + "the lowest return in the cell, second the highest, as " + "round((z - z_min_m) / z_step_m) in the alphabet 0-9A-U, clamped. " + ".. is a cell with no stored return; point absence carries no visibility provenance. " + "Lidar z is 0.05 m voxels, so the first character wavers by one level " + "across flat ground. window_m is the min/max coordinate bound of stored " + "returns in frame_id." + ) + """The whole vocabulary of agent_encode(). The prose gate audits it, and + every key it names is present on every frame.""" + + _RASTER_ALPHABET = "0123456789ABCDEFGHIJKLMNOPQRSTU" + _RASTER_Z_MIN = -0.5 + _RASTER_Z_STEP = 0.1 + _RASTER_MAX_CELLS = 48 + _BOX_Z = (0.15, 1.0) + + def agent_encode(self) -> dict[str, object]: + """What the lidar measured, laid out for a language model. + + World-frame meters throughout. Scalars, stored-point coordinate bounds, + a min/max height raster and exact x-y extents of returns in one z band. The + format is described once, in AGENT_ENCODE_LEGEND; every key is present + on every frame, empty when there is nothing to fill it. + """ + pts = self.points_f32() + n = int(pts.shape[0]) + out: dict[str, object] = { + "frame_id": self.frame_id, + "ts": None if self.ts is None else round(float(self.ts), 2), + "num_points": n, + "window_m": {"x": [], "y": [], "z": []}, + "centroid_xy_m": [], + "floor_footprint_m2": 0.0, + "raster": { + "cell_m": 0.0, + "origin_xy_m": [], + "z_step_m": self._RASTER_Z_STEP, + "z_min_m": self._RASTER_Z_MIN, + "rows": [], + }, + "boxes": {"z_m": list(self._BOX_Z), "xmin:xmax@ymin:ymax": ""}, + } + if n == 0: + return out + xy = pts[:, :2] + z = pts[:, 2] + mins = pts.min(axis=0) + maxs = pts.max(axis=0) + out["window_m"] = { + "x": [round(float(mins[0]), 2), round(float(maxs[0]), 2)], + "y": [round(float(mins[1]), 2), round(float(maxs[1]), 2)], + "z": [round(float(mins[2]), 2), round(float(maxs[2]), 2)], + } + cx, cy = xy.mean(axis=0) + out["centroid_xy_m"] = [round(float(cx), 2), round(float(cy), 2)] + floor_cells = np.unique(np.floor(xy / 0.2).astype(np.int64), axis=0) + out["floor_footprint_m2"] = round(float(floor_cells.shape[0]) * 0.04, 1) + out["raster"] = self._height_raster(pts) + band = xy[(z >= self._BOX_Z[0]) & (z <= self._BOX_Z[1])] + boxes = self._body_height_boxes(band) + # The raster is the picture and is never cut; the box list is the one + # channel that shortens without changing what the rest means. + room = self.ENCODE_SOFT_CAP - len(json.dumps(out)) - 2 + if len(boxes) > room: + boxes = boxes[: max(0, room)].rpartition(",")[0] + out["boxes"] = {"z_m": list(self._BOX_Z), "xmin:xmax@ymin:ymax": boxes} + return out + + @classmethod + def _height_raster(cls, pts: np.ndarray) -> dict[str, object]: + """Lowest and highest return per x-y cell, quantized to one character each. + + The cell is the smallest of 0.25 m, doubling, that keeps both axes within + _RASTER_MAX_CELLS, so a single sweep renders at 0.25 m and a fused map of + a building at 0.5 or 1.0 m. + """ + xy = pts[:, :2] + lo = xy.min(axis=0) + hi = xy.max(axis=0) + cell = 0.25 + while True: + origin = np.floor(lo / cell) * cell + shape = np.floor((hi - origin) / cell).astype(np.int64) + 1 + if int(shape.max()) <= cls._RASTER_MAX_CELLS: + break + cell *= 2.0 + nx, ny = int(shape[0]), int(shape[1]) + ij = np.floor((xy - origin) / cell).astype(np.int64) + lin = ij[:, 1] * nx + ij[:, 0] + levels = len(cls._RASTER_ALPHABET) + q = np.clip(np.rint((pts[:, 2] - cls._RASTER_Z_MIN) / cls._RASTER_Z_STEP), 0, levels - 1) + q = q.astype(np.int64) + qmin = np.full(nx * ny, levels, dtype=np.int64) + qmax = np.full(nx * ny, -1, dtype=np.int64) + np.minimum.at(qmin, lin, q) + np.maximum.at(qmax, lin, q) + glyph = np.array([*cls._RASTER_ALPHABET, "."]) + qmin[qmax < 0] = levels # empty cells index the trailing "." + qmax[qmax < 0] = levels + pairs = np.char.add(glyph[qmin], glyph[qmax]).reshape(ny, nx) + labels = [f"{origin[1] + j * cell:.2f}" for j in range(ny)] + width = max(len(s) for s in labels) + rows = [ + f"{labels[j]:>{width}} " + "".join(pairs[j].tolist()) for j in range(ny - 1, -1, -1) + ] + return { + "cell_m": cell, + "origin_xy_m": [round(float(origin[0]), 2), round(float(origin[1]), 2)], + "z_step_m": cls._RASTER_Z_STEP, + "z_min_m": cls._RASTER_Z_MIN, + "rows": rows, + } + + @staticmethod + def _body_height_boxes(xy: np.ndarray, max_cells: int = 28) -> str: + """Exact x-y extents of clusters of the given points, world meters. + + y is binned into bands to segment clusters; the emitted extents are + exact point min/max. Listed north to south, comma separated, as + xmin:xmax@ymin:ymax with a lone value where an extent is zero. + """ + if xy.shape[0] == 0: + return "" + lo = xy.min(axis=0) + hi = xy.max(axis=0) + span = float(max(hi[0] - lo[0], hi[1] - lo[1])) + cell = next((c for c in (0.25, 0.4, 0.8, 1.6, 3.2) if span / c < max_cells), 6.4) + iy = np.floor((xy[:, 1] - lo[1]) / cell).astype(int) + parts = [] + for r in range(int(iy.max()), -1, -1): + sel = xy[iy == r] + if sel.shape[0] == 0: + continue + sel = sel[np.argsort(sel[:, 0])] + rx = sel[:, 0] + breaks = np.flatnonzero(np.diff(rx) > cell) + starts = np.concatenate(([0], breaks + 1)) + ends = np.concatenate((breaks, [rx.size - 1])) + for s, e in zip(starts, ends, strict=False): + a, b = f"{rx[s]:.2f}", f"{rx[e]:.2f}" + run = a if a == b else f"{a}:{b}" + ry = sel[s : e + 1, 1] + ya, yb = f"{ry.min():.2f}", f"{ry.max():.2f}" + run += f"@{ya}" if ya == yb else f"@{ya}:{yb}" + parts.append(run) + return ",".join(parts) + @functools.cached_property def center(self) -> Vector3: """Calculate the center of the pointcloud in world frame.""" diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index f02c07957a..0fde771aea 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -14,6 +14,8 @@ # limitations under the License. +import json + import numpy as np import pytest @@ -255,3 +257,188 @@ def test_to_rerun_keeps_the_clouds_own_rgb() -> None: ramp = cloud.to_rerun(mode="points", rgb=False) assert ramp.colors is None assert ramp.class_ids is not None + + +def _grid(half: float, pitch: float = 0.05) -> np.ndarray: + g = np.stack(np.meshgrid(np.arange(-half, half, pitch), np.arange(-half, half, pitch)), -1) + return g.reshape(-1, 2) + + +def _keys(encoded: dict[str, object]) -> set[str]: + out: set[str] = set() + for k, v in encoded.items(): + out.add(k) + if isinstance(v, dict): + out |= {f"{k}.{kk}" for kk in v} + return out + + +LEGEND_KEYS = { + "frame_id", + "ts", + "num_points", + "window_m", + "window_m.x", + "window_m.y", + "window_m.z", + "centroid_xy_m", + "floor_footprint_m2", + "raster", + "raster.cell_m", + "raster.origin_xy_m", + "raster.z_step_m", + "raster.z_min_m", + "raster.rows", + "boxes", + "boxes.z_m", + "boxes.xmin:xmax@ymin:ymax", +} + + +def test_agent_encode_scalars_are_exact() -> None: + """The scalars the eval suite quizzes must match numpy, not approximate it — + a slab wall at x=2, plus floor points that must not enter the body band.""" + wall = np.stack( + [ + np.full(200, 2.0), + np.linspace(-1.0, 1.0, 200), + np.linspace(0.2, 0.9, 200), + ], + axis=1, + ) + floor = np.stack([np.linspace(-3, 3, 100), np.linspace(-3, 3, 100), np.zeros(100)], axis=1) + encoded = PointCloud2.from_numpy(np.vstack([wall, floor]), timestamp=12.345).agent_encode() + + assert encoded["num_points"] == 300 + assert encoded["ts"] == 12.35 + assert encoded["window_m"] == {"x": [-3.0, 3.0], "y": [-3.0, 3.0], "z": [0.0, 0.9]} + boxes = encoded["boxes"] + assert isinstance(boxes, dict) + assert boxes["z_m"] == [0.15, 1.0] + # body-height boxes see the wall only: x pinned at 2.0, floor (z=0) excluded + assert all(b.startswith("2.00@") for b in str(boxes["xmin:xmax@ymin:ymax"]).split(",")) + assert encoded["centroid_xy_m"] == [1.33, 0.0] # 200 wall pts at x=2, 100 floor at mean 0 + + +def test_agent_encode_every_key_on_every_frame() -> None: + """A reader indexes the encoding by key; a frame that drops one is a KeyError + in the agent's code. The empty cloud carries the same keys, empty.""" + empty = PointCloud2.from_numpy(np.zeros((0, 3))).agent_encode() + assert empty["num_points"] == 0 + assert _keys(empty) == LEGEND_KEYS + one = PointCloud2.from_numpy(np.array([[1.0, 2.0, 0.3]])).agent_encode() + assert _keys(one) == LEGEND_KEYS + floor = PointCloud2.from_numpy(np.column_stack([_grid(3.0), np.zeros(14400)])).agent_encode() + assert _keys(floor) == LEGEND_KEYS + raster = floor["raster"] + assert isinstance(raster, dict) + assert raster["rows"] and all(len(r) == len(raster["rows"][0]) for r in raster["rows"]) + + +def test_agent_encode_empty_cloud() -> None: + encoded = PointCloud2.from_numpy(np.zeros((0, 3))).agent_encode() + assert encoded["num_points"] == 0 + assert encoded["centroid_xy_m"] == [] + assert encoded["window_m"] == {"x": [], "y": [], "z": []} + raster = encoded["raster"] + assert isinstance(raster, dict) + assert raster["rows"] == [] + + +def test_agent_encode_raster_quantization_round_trips() -> None: + """One point per level, one level per cell: the character decodes back to + the z it came from at the step, and the clamp holds at both ends.""" + alphabet = PointCloud2._RASTER_ALPHABET + z_min, step = PointCloud2._RASTER_Z_MIN, PointCloud2._RASTER_Z_STEP + levels = np.arange(len(alphabet)) + xs = levels * 0.25 + 0.125 # one cell each along x + pts = np.column_stack([xs, np.zeros_like(xs), z_min + levels * step]) + pts = np.vstack([pts, [[-0.875, 0.0, -9.0], [-0.625, 0.0, 9.0]]]) # beyond the clamp + raster = PointCloud2.from_numpy(pts).agent_encode()["raster"] + assert isinstance(raster, dict) + assert raster["cell_m"] == 0.25 + (row,) = raster["rows"] + cells = row.split(" ", 1)[1] + pairs = [cells[i : i + 2] for i in range(0, len(cells), 2)] + assert pairs[0] == "00" # clamped low + assert pairs[1] == alphabet[-1] * 2 # clamped high + assert pairs[2] == ".." # the cell at x in [-0.5, -0.25) has no return + assert pairs[3] == ".." + for level, pair in zip(levels, pairs[4:], strict=True): + assert pair == alphabet[level] * 2 + assert z_min + alphabet.index(pair[0]) * step == pytest.approx(z_min + level * step) + + +def test_agent_encode_raster_cell_rule() -> None: + """0.25 m for a single sweep, 0.5 m once a fused map would pass 48 cells.""" + small = PointCloud2.from_numpy(np.column_stack([_grid(3.0, 0.1), np.zeros(3600)])) + large = PointCloud2.from_numpy(np.column_stack([_grid(7.5, 0.1), np.zeros(22500)])) + small_raster = small.agent_encode()["raster"] + large_raster = large.agent_encode()["raster"] + assert isinstance(small_raster, dict) and isinstance(large_raster, dict) + assert small_raster["cell_m"] == 0.25 + assert small_raster["origin_xy_m"] == [-3.0, -3.0] + assert len(small_raster["rows"]) == 24 + assert large_raster["cell_m"] == 0.5 + assert len(large_raster["rows"]) == 30 + + +def test_agent_encode_raster_single_return_is_min_equals_max() -> None: + raster = PointCloud2.from_numpy(np.array([[0.1, 0.1, 0.62]])).agent_encode()["raster"] + assert isinstance(raster, dict) + (row,) = raster["rows"] + pair = row.split(" ", 1)[1] + assert len(pair) == 2 + assert pair[0] == pair[1] + assert pair[0] == PointCloud2._RASTER_ALPHABET[round((0.62 + 0.5) / 0.1)] + + +def test_agent_encode_stays_within_prompt_budget() -> None: + """The encoding is prompt text, so its size is a hard product constraint. + + A busy room -- floor, four walls, scattered clutter -- is the verbose case. + The encoder trims its own tail rather than letting a dense frame run over. + """ + rng = np.random.default_rng(7) + grid = _grid(6.0, 0.04) + parts = [np.column_stack([grid, np.zeros(len(grid))])] + for edge in (-6.0, 6.0): + span = np.arange(-6, 6, 0.02) + height = rng.uniform(0.15, 1.0, span.size) + parts.append(np.column_stack([np.full(span.size, edge), span, height])) + parts.append(np.column_stack([span, np.full(span.size, edge), height])) + parts.append( + np.column_stack( + [rng.uniform(-6, 6, 4000), rng.uniform(-6, 6, 4000), rng.uniform(0.15, 1.0, 4000)] + ) + ) + + encoded = PointCloud2.from_numpy(np.vstack(parts)).agent_encode() + + assert len(json.dumps(encoded)) <= PointCloud2.ENCODE_SOFT_CAP + assert _keys(encoded) == LEGEND_KEYS + + +@pytest.mark.self_hosted +def test_agent_encode_fused_map_fits_the_cap() -> None: + """A whole recording fused into one map is the largest cloud the agent + reads; it must still come back in one readout, at a coarser cell.""" + from dimos.evals.suites.lib.generate import _dataset + from dimos.mapping.voxels.module import VoxelMapTransformer + from dimos.memory.transform import downsample + + with _dataset("go2_china_office") as store: + fused = ( + store.streams.lidar.range_time(0, 138) + .transform(downsample(6)) + .transform(VoxelMapTransformer(voxel_size=0.05, device="CPU:0", emit_every=0)) + .last() + .data + ) + encoded = fused.agent_encode() + raster = encoded["raster"] + assert isinstance(raster, dict) + assert len(json.dumps(encoded)) <= PointCloud2.ENCODE_SOFT_CAP + assert raster["cell_m"] == 0.5 + assert len(raster["rows"]) <= 48 + assert _keys(encoded) == LEGEND_KEYS From 6eddfadda90a7a25f69529b4b44872a7cc438c5c Mon Sep 17 00:00:00 2001 From: stash Date: Thu, 10 Sep 2026 23:22:18 -0700 Subject: [PATCH 2/2] chore(evals): use the full go2_office_pc recording (all streams, untruncated) --- data/.lfs/go2_office_pc.db.tar.gz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/data/.lfs/go2_office_pc.db.tar.gz b/data/.lfs/go2_office_pc.db.tar.gz index 6973203001..9343471e41 100644 --- a/data/.lfs/go2_office_pc.db.tar.gz +++ b/data/.lfs/go2_office_pc.db.tar.gz @@ -1,3 +1,3 @@ version https://git.lfs.github.com/spec/v1 -oid sha256:5a9444a2d2174f7dec5699f64cf95c0f9f551adb3e609383fc6fc1c1ef65814c -size 1054229 +oid sha256:53f9a3efe56a839dbe65f557fbd052d77e5335ce80c4e8b1925fee1bb7caad2f +size 444215469