From ead67fc8137891a858932879f2fa8682305127d3 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 21 Jun 2026 00:40:06 +0900 Subject: [PATCH 1/2] refactor(examples): merge demos into a camera-synced 2x2 viewer Combine insertPointCloud.py and insertPointCloudColor.py into a single pointcloud_to_octree.py that builds an OcTree and a ColorOcTree from one scan and shows pointcloud / occupied / occupied (color) / empty in a 2x2 window. The four panes share one trackball, so dragging any pane rotates all four together. Move the rendering machinery (the synced widget and the grid window) into examples/viewer.py so the example reads as a concise walk through the octomap API. --- examples/insertPointCloud.py | 160 --------------------------- examples/insertPointCloudColor.py | 157 -------------------------- examples/pointcloud_to_octree.py | 106 ++++++++++++++++++ examples/viewer.py | 178 ++++++++++++++++++++++++++++++ 4 files changed, 284 insertions(+), 317 deletions(-) delete mode 100755 examples/insertPointCloud.py delete mode 100644 examples/insertPointCloudColor.py create mode 100644 examples/pointcloud_to_octree.py create mode 100644 examples/viewer.py diff --git a/examples/insertPointCloud.py b/examples/insertPointCloud.py deleted file mode 100755 index 23d455c..0000000 --- a/examples/insertPointCloud.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python - -import glooey -import imgviz -import numpy as np -import pyglet -import trimesh -import trimesh.transformations as tf -import trimesh.viewer - -import octomap - - -def pointcloud_from_depth(depth, fx, fy, cx, cy): - assert depth.dtype.kind == "f", "depth must be float and have meter values" - - rows, cols = depth.shape - c, r = np.meshgrid(np.arange(cols), np.arange(rows), sparse=True) - valid = ~np.isnan(depth) - z = np.where(valid, depth, np.nan) - x = np.where(valid, z * (c - cx) / fx, np.nan) - y = np.where(valid, z * (r - cy) / fy, np.nan) - pc = np.dstack((x, y, z)) - - return pc - - -def labeled_scene_widget(scene, label): - vbox = glooey.VBox() - vbox.add(glooey.Label(text=label, color=(255, 255, 255)), size=0) - vbox.add(trimesh.viewer.SceneWidget(scene)) - return vbox - - -def visualize( - occupied, empty, K, width, height, rgb, pcd, mask, resolution, aabb -): - window = pyglet.window.Window( - width=int(640 * 0.9 * 3), height=int(480 * 0.9) - ) - - @window.event - def on_key_press(symbol, modifiers): - if modifiers == 0: - if symbol == pyglet.window.key.Q: - window.on_close() - - gui = glooey.Gui(window) - hbox = glooey.HBox() - hbox.set_padding(5) - - camera = trimesh.scene.Camera( - resolution=(width, height), focal=(K[0, 0], K[1, 1]) - ) - camera_marker = trimesh.creation.camera_marker(camera, marker_height=0.1) - # trimesh's camera_marker opens toward -Z (OpenGL convention) but the - # depth point cloud uses OpenCV (+Z forward). Rotate the marker 180 deg - # about X so the frustum opens toward the data instead of away from it. - opencv_from_opengl = tf.rotation_matrix(np.pi, [1, 0, 0]) - for geometry in camera_marker: - geometry.apply_transform(opencv_from_opengl) - - # initial camera pose - camera_transform = np.array( - [ - [0.73256052, -0.28776419, 0.6168848, 0.66972396], - [-0.26470017, -0.95534823, -0.13131483, -0.12390466], - [0.62712751, -0.06709345, -0.77602162, -0.28781298], - [0.0, 0.0, 0.0, 1.0], - ], - ) - - aabb_min, aabb_max = aabb - bbox = trimesh.path.creation.box_outline( - aabb_max - aabb_min, - tf.translation_matrix((aabb_min + aabb_max) / 2), - ) - - geom = trimesh.PointCloud(vertices=pcd[mask], colors=rgb[mask]) - scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) - scene.camera_transform = camera_transform - hbox.add(labeled_scene_widget(scene, label="pointcloud")) - - geom = trimesh.voxel.ops.multibox( - occupied, pitch=resolution, colors=[1.0, 0, 0, 0.5] - ) - scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) - scene.camera_transform = camera_transform - hbox.add(labeled_scene_widget(scene, label="occupied")) - - geom = trimesh.voxel.ops.multibox( - empty, pitch=resolution, colors=[0.5, 0.5, 0.5, 0.5] - ) - scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) - scene.camera_transform = camera_transform - hbox.add(labeled_scene_widget(scene, label="empty")) - - gui.add(hbox) - pyglet.app.run() - - -def main(): - data = imgviz.data.arc2017() - camera_info = data["camera_info"] - K = np.array(camera_info["K"]).reshape(3, 3) - rgb = data["rgb"] - pcd = pointcloud_from_depth( - data["depth"], fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2] - ) - - nonnan = ~np.isnan(pcd).any(axis=2) - mask = np.less(pcd[:, :, 2], 2) - - resolution = 0.01 - octree = octomap.OcTree(resolution) - octree.insertPointCloud( - pointcloud=pcd[nonnan], - origin=np.array([0, 0, 0], dtype=float), - maxrange=2, - ) - occupied, empty = octree.extractPointCloud() - - aabb_min = octree.getMetricMin() - aabb_max = octree.getMetricMax() - - # Alternatively, classify a dense query grid with getLabels() instead of - # reading the tree's leaves with extractPointCloud(). getLabels() returns - # -1 (unknown / never observed), 0 (free), or 1 (occupied) for arbitrary - # points -- the typical collision-query use. Swap the block above for: - # - # center = (octree.getMetricMin() + octree.getMetricMax()) / 2 - # dimension = np.array([64, 64, 64]) - # origin = center - dimension / 2 * resolution - # aabb_min = origin - resolution / 2 - # aabb_max = origin + dimension * resolution + resolution / 2 - # grid = np.full(dimension, -1, np.int32) - # transform = tf.scale_and_translate(scale=resolution, translate=origin) - # points = trimesh.voxel.VoxelGrid( - # encoding=grid, transform=transform - # ).points - # labels = octree.getLabels(points) - # occupied = points[labels == 1] - # empty = points[labels == 0] - - visualize( - occupied=occupied, - empty=empty, - K=K, - width=camera_info["width"], - height=camera_info["height"], - rgb=rgb, - pcd=pcd, - mask=mask, - resolution=resolution, - aabb=(aabb_min, aabb_max), - ) - - -if __name__ == "__main__": - main() diff --git a/examples/insertPointCloudColor.py b/examples/insertPointCloudColor.py deleted file mode 100644 index 3bae79e..0000000 --- a/examples/insertPointCloudColor.py +++ /dev/null @@ -1,157 +0,0 @@ -#!/usr/bin/env python - -import glooey -import imgviz -import numpy as np -import pyglet -import trimesh -import trimesh.transformations as tf -import trimesh.viewer -from insertPointCloud import labeled_scene_widget -from insertPointCloud import pointcloud_from_depth - -import octomap - - -def build_color_octree(pcd, rgb, mask, resolution, maxrange): - octree = octomap.ColorOcTree(resolution) - octree.insertPointCloud( - pointcloud=pcd[mask], - origin=np.array([0, 0, 0], dtype=float), - maxrange=maxrange, - ) - for point, color in zip(pcd[mask], rgb[mask]): - octree.averageNodeColor( - point, int(color[0]), int(color[1]), int(color[2]) - ) - octree.updateInnerOccupancy() - return octree - - -def extract_colored_voxels(octree, resolution): - points = [] - colors = [] - for it in octree.begin_leafs(): - if not octree.isNodeOccupied(it): - continue - center = it.getCoordinate() - # a pruned leaf can be larger than one voxel; expand it into - # resolution-sized voxels that all share the leaf's color - dimension = max(1, round(it.getSize() / resolution)) - origin = center - (dimension / 2 - 0.5) * resolution - indices = np.column_stack( - np.nonzero(np.ones((dimension, dimension, dimension))) - ) - points.append(origin + indices * resolution) - colors.append(np.tile(it.getColor(), (len(indices), 1))) - - if not points: - return np.zeros((0, 3)), np.zeros((0, 3), dtype=np.uint8) - return ( - np.concatenate(points, axis=0), - np.concatenate(colors, axis=0).astype(np.uint8), - ) - - -def visualize( - occupied, - occupied_colors, - K, - width, - height, - rgb, - pcd, - mask, - resolution, - aabb, -): - window = pyglet.window.Window( - width=int(640 * 0.9 * 2), height=int(480 * 0.9) - ) - - @window.event - def on_key_press(symbol, modifiers): - if modifiers == 0: - if symbol == pyglet.window.key.Q: - window.on_close() - - gui = glooey.Gui(window) - hbox = glooey.HBox() - hbox.set_padding(5) - - camera = trimesh.scene.Camera( - resolution=(width, height), focal=(K[0, 0], K[1, 1]) - ) - camera_marker = trimesh.creation.camera_marker(camera, marker_height=0.1) - opencv_from_opengl = tf.rotation_matrix(np.pi, [1, 0, 0]) - for geometry in camera_marker: - geometry.apply_transform(opencv_from_opengl) - - camera_transform = np.array( - [ - [0.73256052, -0.28776419, 0.6168848, 0.66972396], - [-0.26470017, -0.95534823, -0.13131483, -0.12390466], - [0.62712751, -0.06709345, -0.77602162, -0.28781298], - [0.0, 0.0, 0.0, 1.0], - ], - ) - - aabb_min, aabb_max = aabb - bbox = trimesh.path.creation.box_outline( - aabb_max - aabb_min, - tf.translation_matrix((aabb_min + aabb_max) / 2), - ) - - geom = trimesh.PointCloud(vertices=pcd[mask], colors=rgb[mask]) - scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) - scene.camera_transform = camera_transform - hbox.add(labeled_scene_widget(scene, label="pointcloud")) - - colors = np.column_stack( - (occupied_colors, np.full(len(occupied_colors), 255, dtype=np.uint8)) - ) - geom = trimesh.voxel.ops.multibox( - occupied, pitch=resolution, colors=colors - ) - scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) - scene.camera_transform = camera_transform - hbox.add(labeled_scene_widget(scene, label="occupied (color)")) - - gui.add(hbox) - pyglet.app.run() - - -def main(): - data = imgviz.data.arc2017() - camera_info = data["camera_info"] - K = np.array(camera_info["K"]).reshape(3, 3) - rgb = data["rgb"] - pcd = pointcloud_from_depth( - data["depth"], fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2] - ) - - nonnan = ~np.isnan(pcd).any(axis=2) - mask = nonnan & np.less(pcd[:, :, 2], 2) - - resolution = 0.01 - octree = build_color_octree( - pcd=pcd, rgb=rgb, mask=mask, resolution=resolution, maxrange=2 - ) - occupied, occupied_colors = extract_colored_voxels(octree, resolution) - - visualize( - occupied=occupied, - occupied_colors=occupied_colors, - K=K, - width=camera_info["width"], - height=camera_info["height"], - rgb=rgb, - pcd=pcd, - mask=mask, - resolution=resolution, - aabb=(octree.getMetricMin(), octree.getMetricMax()), - ) - - -if __name__ == "__main__": - main() diff --git a/examples/pointcloud_to_octree.py b/examples/pointcloud_to_octree.py new file mode 100644 index 0000000..b6ab80a --- /dev/null +++ b/examples/pointcloud_to_octree.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python + +import imgviz +import numpy as np +from viewer import pointcloud_from_depth +from viewer import visualize + +import octomap + + +def _build_color_octree(points, colors, resolution, maxrange): + octree = octomap.ColorOcTree(resolution) + octree.insertPointCloud( + pointcloud=points, + origin=np.array([0, 0, 0], dtype=float), + maxrange=maxrange, + ) + for point, color in zip(points, colors): + octree.averageNodeColor( + point, int(color[0]), int(color[1]), int(color[2]) + ) + octree.updateInnerOccupancy() + return octree + + +def _extract_colored_voxels(octree, resolution): + points = [] + colors = [] + for it in octree.begin_leafs(): + if not octree.isNodeOccupied(it): + continue + center = it.getCoordinate() + # a pruned leaf can be larger than one voxel; expand it into + # resolution-sized voxels that all share the leaf's color + dimension = max(1, round(it.getSize() / resolution)) + origin = center - (dimension / 2 - 0.5) * resolution + indices = np.indices((dimension, dimension, dimension)) + indices = indices.reshape(3, -1).T + points.append(origin + indices * resolution) + colors.append(np.tile(it.getColor(), (len(indices), 1))) + + if not points: + return np.zeros((0, 3)), np.zeros((0, 3), dtype=np.uint8) + return ( + np.concatenate(points, axis=0), + np.concatenate(colors, axis=0).astype(np.uint8), + ) + + +def main(): + data = imgviz.data.arc2017() + camera_info = data["camera_info"] + K = np.array(camera_info["K"]).reshape(3, 3) + rgb = data["rgb"] + pcd = pointcloud_from_depth( + data["depth"], fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2] + ) + + nonnan = ~np.isnan(pcd).any(axis=2) + mask = nonnan & np.less(pcd[:, :, 2], 2) + masked_pcd = pcd[mask] + masked_rgb = rgb[mask] + + resolution = 0.01 + + octree = octomap.OcTree(resolution) + # The OcTree takes every valid point (maxrange clips the raycast at 2 m); + # the ColorOcTree below uses the tighter z < 2 m mask so its per-point + # color loop only visits points it actually inserted. + octree.insertPointCloud( + pointcloud=pcd[nonnan], + origin=np.array([0, 0, 0], dtype=float), + maxrange=2, + ) + occupied, empty = octree.extractPointCloud() + + # Alternatively, classify a dense query grid with getLabels() instead of + # reading the tree's leaves with extractPointCloud(). getLabels() returns + # -1 (unknown / never observed), 0 (free), or 1 (occupied) for arbitrary + # points -- the typical collision-query use: + # + # points = ... # (N, 3) query points + # labels = octree.getLabels(points) + # occupied = points[labels == 1] + # empty = points[labels == 0] + + color_octree = _build_color_octree( + points=masked_pcd, colors=masked_rgb, resolution=resolution, maxrange=2 + ) + occupied_color = _extract_colored_voxels(color_octree, resolution) + + visualize( + pointcloud=(masked_pcd, masked_rgb), + occupied=occupied, + occupied_color=occupied_color, + empty=empty, + K=K, + width=camera_info["width"], + height=camera_info["height"], + resolution=resolution, + aabb=(octree.getMetricMin(), octree.getMetricMax()), + ) + + +if __name__ == "__main__": + main() diff --git a/examples/viewer.py b/examples/viewer.py new file mode 100644 index 0000000..197b2ed --- /dev/null +++ b/examples/viewer.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python + +"""A 2x2 trimesh viewer with camera-synced panes, used by the examples.""" + +import glooey +import numpy as np +import pyglet +import trimesh +import trimesh.transformations as tf +import trimesh.viewer + + +def pointcloud_from_depth(depth, fx, fy, cx, cy): + assert depth.dtype.kind == "f", "depth must be float and have meter values" + + rows, cols = depth.shape + c, r = np.meshgrid(np.arange(cols), np.arange(rows), sparse=True) + valid = ~np.isnan(depth) + z = np.where(valid, depth, np.nan) + x = np.where(valid, z * (c - cx) / fx, np.nan) + y = np.where(valid, z * (r - cy) / fy, np.nan) + return np.dstack((x, y, z)) + + +class SyncedSceneWidget(trimesh.viewer.SceneWidget): + """A SceneWidget whose camera is kept in sync with its peers. + + All synced widgets share a single trackball, so a press/drag/scroll on + any one of them moves every camera together. After each interaction the + peers' scenes are pointed at the shared pose and redrawn. + """ + + def __init__(self, scene, **kwargs): + super().__init__(scene, **kwargs) + self.peers = [] + + def sync(self, peers): + ball = peers[0].view["ball"] + self.view["ball"] = ball + self.scene.camera_transform = ball.pose + self.peers = [peer for peer in peers if peer is not self] + + def _sync_peers(self): + pose = self.view["ball"].pose + for peer in self.peers: + peer.scene.camera_transform = pose + peer._draw() + + def on_mouse_press(self, x, y, buttons, modifiers): + super().on_mouse_press(x, y, buttons, modifiers) + self._sync_peers() + + def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers): + super().on_mouse_drag(x, y, dx, dy, buttons, modifiers) + self._sync_peers() + + def on_mouse_scroll(self, x, y, dx, dy): + super().on_mouse_scroll(x, y, dx, dy) + self._sync_peers() + + +def _labeled_scene_widget(scene, label): + widget = SyncedSceneWidget(scene, background=(1.0, 1.0, 1.0, 1.0)) + + vbox = glooey.VBox() + vbox.add(glooey.Label(text=label, color=(0, 0, 0)), size=0) + vbox.add(widget) + return vbox, widget + + +def visualize( + *, + pointcloud, + occupied, + occupied_color, + empty, + K, + width, + height, + resolution, + aabb, +): + window = pyglet.window.Window( + width=int(640 * 0.9 * 2), height=int(480 * 0.9 * 2) + ) + + @window.event + def on_key_press(symbol, modifiers): + if modifiers == 0: + if symbol == pyglet.window.key.Q: + window.on_close() + + gui = glooey.Gui(window) + grid = glooey.Grid(num_rows=2, num_cols=2) + grid.set_padding(5) + + camera = trimesh.scene.Camera( + resolution=(width, height), focal=(K[0, 0], K[1, 1]) + ) + camera_marker = trimesh.creation.camera_marker(camera, marker_height=0.1) + # trimesh's camera_marker opens toward -Z (OpenGL convention) but the + # depth point cloud uses OpenCV (+Z forward). Rotate the marker 180 deg + # about X so the frustum opens toward the data instead of away from it. + opencv_from_opengl = tf.rotation_matrix(np.pi, [1, 0, 0]) + for geometry in camera_marker: + geometry.apply_transform(opencv_from_opengl) + + # initial camera pose + camera_transform = np.array( + [ + [0.73256052, -0.28776419, 0.6168848, 0.66972396], + [-0.26470017, -0.95534823, -0.13131483, -0.12390466], + [0.62712751, -0.06709345, -0.77602162, -0.28781298], + [0.0, 0.0, 0.0, 1.0], + ], + ) + + aabb_min, aabb_max = aabb + bbox = trimesh.path.creation.box_outline( + aabb_max - aabb_min, + tf.translation_matrix((aabb_min + aabb_max) / 2), + ) + + widgets = [] + + def add_panel(row, col, label, geom): + scene = trimesh.Scene( + camera=camera, geometry=[bbox, geom, camera_marker] + ) + scene.camera_transform = camera_transform + vbox, widget = _labeled_scene_widget(scene, label) + widgets.append(widget) + grid.add(row, col, vbox) + + pcd_points, pcd_colors = pointcloud + add_panel( + 0, + 0, + "pointcloud", + trimesh.PointCloud(vertices=pcd_points, colors=pcd_colors), + ) + + add_panel( + 0, + 1, + "occupied", + trimesh.voxel.ops.multibox( + occupied, pitch=resolution, colors=[1.0, 0, 0, 0.5] + ), + ) + + voxel_points, voxel_colors = occupied_color + voxel_colors = np.column_stack( + (voxel_colors, np.full(len(voxel_colors), 255, dtype=np.uint8)) + ) + add_panel( + 1, + 0, + "occupied (color)", + trimesh.voxel.ops.multibox( + voxel_points, pitch=resolution, colors=voxel_colors + ), + ) + + add_panel( + 1, + 1, + "empty", + trimesh.voxel.ops.multibox( + empty, pitch=resolution, colors=[0.5, 0.5, 0.5, 0.5] + ), + ) + + for widget in widgets: + widget.sync(widgets) + + gui.add(grid) + pyglet.app.run() From 62bf74833ebee939a575b8939aff86ff4d14264e Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sun, 21 Jun 2026 00:40:06 +0900 Subject: [PATCH 2/2] docs: point the examples section at the new synced viewer --- README.md | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index cee4dce..f0d7278 100644 --- a/README.md +++ b/README.md @@ -82,9 +82,10 @@ restored = octomap.ColorOcTree.read("tree.ot") ## Examples -Runnable demos live in [`examples/`](examples); the teaser above combines -`insertPointCloud.py` (pointcloud / occupied / empty) and -`insertPointCloudColor.py` (occupied in color): +Runnable demos live in [`examples/`](examples). `pointcloud_to_octree.py` +shows the four views from the teaser above (pointcloud / occupied / occupied +(color) / empty) in a single window, with the cameras synchronized so dragging +one view rotates all four together: ```bash git clone --recursive https://github.com/wkentaro/octomap-python.git @@ -92,11 +93,13 @@ cd octomap-python uv sync --group examples cd examples -uv run python insertPointCloud.py +uv run python pointcloud_to_octree.py ``` -`insertPointCloudColor.py` is the same demo on a `ColorOcTree`, rendering each -occupied voxel in its measured RGB color. +It builds both an `OcTree` and a `ColorOcTree` so the same scan renders as a +red occupancy grid and in its measured RGB color side by side. The viewer +itself (the synced 2x2 window) lives in `examples/viewer.py`, so the example +reads as a concise walk through the `octomap` API. ## Release