Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions dimos/perception/detection/module3D.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,12 @@
from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D
from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC
from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC
from dimos.perception.detection.visualization import letterbox_image
from dimos.types.timestamped import align_timestamped
from dimos.utils.reactive import backpressure

_VISUALIZATION_CROP_SIZE = (320, 320)


class Detection3DModule(Detection2DModule):
color_image: In[Image]
Expand All @@ -45,6 +48,12 @@ class Detection3DModule(Detection2DModule):
detected_pointcloud_1: Out[PointCloud2]
detected_pointcloud_2: Out[PointCloud2]

# Crops paired with the successful 3D results above. These are distinct from
# detected_image_*, which are ranked before failed 3D projections are removed.
detected_3d_image_0: Out[Image]
detected_3d_image_1: Out[Image]
detected_3d_image_2: Out[Image]

# just for visualization, emits latest top 3 detections in a frame
detected_image_0: Out[Image]
detected_image_1: Out[Image]
Expand Down Expand Up @@ -183,6 +192,14 @@ def _publish_detections(self, detections: ImageDetections3DPC) -> None:
if not detections:
return

width, height = _VISUALIZATION_CROP_SIZE
for index, detection in enumerate(detections[:3]):
# Publish the crop first so selecting the following pointcloud event
# sees both components on the shared Rerun entity.
image_topic = getattr(self, "detected_3d_image_" + str(index))
crop = letterbox_image(detection.cropped_image(), width, height)
crop.frame_id = "" # UI-only: do not attach the crop to the 3D TF tree.
image_topic.publish(crop)

pointcloud_topic = getattr(self, "detected_pointcloud_" + str(index))
pointcloud_topic.publish(detection.pointcloud)
46 changes: 46 additions & 0 deletions dimos/perception/detection/visualization.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
# Copyright 2025-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.

import numpy as np

from dimos.msgs.sensor_msgs.Image import Image


def letterbox_image(image: Image, width: int, height: int) -> Image:
"""Create a fixed-size preview while preserving the image's aspect ratio.

DimOS sends color images to Rerun as JPEG ``EncodedImage`` values. Detection
crops naturally change dimensions between frames, but reusing one Rerun
entity for those varying dimensions triggers ``Detected change of video
encoding properties over time`` and can panic the Viewer. Letterboxing keeps
the encoded dimensions stable without stretching the crop.
"""
if width <= 0 or height <= 0:
raise ValueError("letterbox dimensions must be positive")

scale = min(width / image.width, height / image.height)
resized_width = max(1, min(width, round(image.width * scale)))
resized_height = max(1, min(height, round(image.height * scale)))
resized = image.resize(resized_width, resized_height)

output_shape = (height, width, *resized.data.shape[2:])
output = np.zeros(output_shape, dtype=resized.data.dtype)
x_offset = (width - resized_width) // 2
y_offset = (height - resized_height) // 2
output[
y_offset : y_offset + resized_height,
x_offset : x_offset + resized_width,
...,
] = resized.data
return Image(data=output, format=image.format, frame_id=image.frame_id, ts=image.ts)
57 changes: 47 additions & 10 deletions dimos/robot/unitree/go2/blueprints/smart/unitree_go2_detection.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,19 +14,56 @@
# limitations under the License.

from dimos.core.coordination.blueprints import autoconnect
from dimos.core.global_config import global_config
from dimos.core.transport import LCMTransport
from dimos.msgs.sensor_msgs.Image import Image
from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray
from dimos.perception.detection.module3D import Detection3DModule
from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config
from dimos.robot.unitree.go2.blueprints.smart.unitree_go2 import unitree_go2
from dimos.robot.unitree.go2.connection import GO2Connection
from dimos.visualization.vis_module import vis_module


def _topic_path(topic: object) -> str:
topic_str = getattr(topic, "name", None) or str(topic)
raw = getattr(topic, "topic", topic_str)
if isinstance(raw, str):
topic_str = raw
topic_str = topic_str.split("#")[0]
if topic_str.startswith("dimos/"):
topic_str = "/" + topic_str.removeprefix("dimos/")
elif not topic_str.startswith("/"):
topic_str = "/" + topic_str
return topic_str


def _detection_topic_to_entity(topic: object) -> str:
path = _topic_path(topic)
parts = path.strip("/").split("/")
if len(parts) == 4 and parts[:2] == ["detector3d", "3d"]:
return f"world/detections/3d/{parts[2]}"
return f"world{path}"


detection_rerun_config = {
**rerun_config,
"topic_to_entity": _detection_topic_to_entity,
}

unitree_go2_detection = (
autoconnect(
unitree_go2,
# Replace the inherited viewer bundle so its Rerun bridge uses the
# detection-specific topic mapping while preserving viewer="none".
vis_module(
viewer_backend=global_config.viewer,
rerun_config=detection_rerun_config,
),
Detection3DModule.blueprint(
camera_info=GO2Connection.camera_info_static,
publish_detection_images=False,
),
)
.remappings(
Expand All @@ -37,21 +74,21 @@
.transports(
{
# Detection 3D module outputs
("detections", Detection3DModule): LCMTransport(
("detections", Detection2DArray): LCMTransport(
"/detector3d/detections", Detection2DArray
),
("detected_pointcloud_0", Detection3DModule): LCMTransport(
"/detector3d/pointcloud/0", PointCloud2
("detected_pointcloud_0", PointCloud2): LCMTransport(
"/detector3d/3d/slot_0/pointcloud", PointCloud2
),
("detected_pointcloud_1", Detection3DModule): LCMTransport(
"/detector3d/pointcloud/1", PointCloud2
("detected_pointcloud_1", PointCloud2): LCMTransport(
"/detector3d/3d/slot_1/pointcloud", PointCloud2
),
("detected_pointcloud_2", Detection3DModule): LCMTransport(
"/detector3d/pointcloud/2", PointCloud2
("detected_pointcloud_2", PointCloud2): LCMTransport(
"/detector3d/3d/slot_2/pointcloud", PointCloud2
),
("detected_image_0", Detection3DModule): LCMTransport("/detector3d/image/0", Image),
("detected_image_1", Detection3DModule): LCMTransport("/detector3d/image/1", Image),
("detected_image_2", Detection3DModule): LCMTransport("/detector3d/image/2", Image),
("detected_3d_image_0", Image): LCMTransport("/detector3d/3d/slot_0/image", Image),
("detected_3d_image_1", Image): LCMTransport("/detector3d/3d/slot_1/image", Image),
("detected_3d_image_2", Image): LCMTransport("/detector3d/3d/slot_2/image", Image),
}
)
)
Loading