Feature/combined left and right rgbd by hello-ck · Pull Request #4 · hello-robot/stretch4_rgbd

hello-ck · 2026-07-07T21:36:19Z

This PR would merge updates that support simultaneous RGB-D imagery from the left and right sides of Stretch 4's head.

WARNING: These changes breaks downstream code! There is an associated update for stretch4_human_perception that should be considered at the same time.

This commit provides initial support for simultaneous left and right RGB-D image capture and use. Development used the following prompts with Antigravity IDE Version: 2.1.1 with Gemini 3.5 Flash (Medium and High). ____________ Prompt #1 With the stretch4_rgbd repository, spatially calibrated and temporally synchronized RGB-D images can be efficiently captured and used from either the left or the right side of the robot’s head. Now, the goal is to enable the efficient capture and usage of RGB-D images simultaneously captured from the left and the right side of the robot’s head. The left and right RGB-D images should be temporally synchronized with one another and acquired with low latency at a high frame rate. When capturing both simultaneously, the RGB-D images should be restricted to use the same image resolution for both sides and a single frame rate. The left and right sides should continue to be spatially calibrated independently, and simultaneous capture should make use of the calibrations. With respect to the README.md file, this update will impact the capabilities and code described in the following sections of README.md: + Visualize Live RGB-D Imagery with the New Calibration (On Robot) + Use PyZMQ to Send RGB-D Imagery + API Usage and Reference + Temporal Quality Overview The changes will be heavily reflected in the fast_emulated_rgbd.py file, which currently only supports independent use of the left or the right RGB-D camera and LiDAR pair: if is_left_only or is_right_only: camera_name = "left" if is_left_only else "right" lidar_name = "left" if is_left_only else "right" from stretch4_emulated_rgbd.fast_emulated_rgbd import FastEmulatedRGBDStreamer streamer = FastEmulatedRGBDStreamer( camera=camera_name, lidar=lidar_name, emulated_rgbd_fps=emulated_rgbd_fps, camera_fps=camera_fps, resolution_height=resolution_height, compress=compress, oak_buffer_size=oak_buffer_size, calibration=calibration, ignore_prior_optimizations=ignore_prior_optimizations ) return streamer, streamer.stream_rgbd() In addition, the code in the stretch4_rgbd examples directory should also be updated. Specifically, api_example.py should include example code for using the updated API to work with simultaneous left and right RGB-D images, and the examples that send RGB-D images across processes and machines via PyZMQ should be updated to support the simultaneous capture, transmission and visualization of left and right RGB-D images. These updates should also make it possible to eventually update the code under the stretch4_human_perception examples directory to use simultaneously streamed left and right RGB-D images, but nothing should be changed in the stretch4_human_perception repository at this time. The current goal is strictly to update the stretch4_rgbd repository, so that the new capabilities can be thoroughly tested prior to attempting to use them in other repositories like stretch4_human_perception. After making these changes, update README.md to document them and provide explicit instructions for testing these capabilities after calibrating the left and right RGB-D imagery. Also, make sure that the inline documentation is clear and thorough. ____________ Comment #1 Confirm that all of the code within the stretch4_rgbd will be updated to account for this change. Revise the implementation plan as needed to ensure that all of the code in the stretch4_rgbd will work after all the proposed changes. Provide the new implementation plan for review and feedback. ____________ Comment #2 Revise the implementation plan based on the comment and share the results. Do not proceed with the revised implementation plan until receiving feedback and approval. ____________ Prompt #2 The following commands worked, but the frame rate was low even when using the lowest resolution option of 400. The frame rate was 3Hz to 5Hz, but the LiDAR sensors operate at 10HZ. I would expect that a frame rate close to 10Hz would be achievable. python3 scripts/visualize_emulated_rgbd.py --camera left_right --lidar both python3 examples/send_rgbd_images_and_joint_states.py --camera left_right --show_fps python3 examples/recv_rgbd_images_and_joint_states.py ____________ Prompt #3 The frame rate is still significantly lower than 10Hz. With a resolution of 400 it has hit around 6.5Hz and with the default resolution of 800 it was around 5Hz. Carefully assess factors that could be resulting in frame rates significantly below 10Hz. In addition, by default, the left RGB camera should only be associated with depths from the left LiDAR and the right RGB camera should only be associated with depths from the right LiDAR. A new command line argument should be created that results in 3D points from both LiDAR sensors being used with single RGB cameras. ____________

This is a minor edit to the README.md documentation that corrects a few directories provided with commands from the scripts directory to the examples directory.

hello-lamsey · 2026-07-08T16:48:33Z

stretch4_rgb feedback so far

Working

All code still seems to work fine so far when running on just one side (i.e., left camera and left lidar). After walking through the README, the resulting merged RGBD pointcloud streamed using python3 scripts/estimate_validity_masks.py --camera left_right --lidar both looks good to me.

Not Working

Using data collected for L/R simultaneously with with scripts/capture_emulated_rgbd.py --camera left_right --lidar both is not working for several steps in the calibration pipeline. Bugs documented per file below.

scripts/create_validity_masks_for_extrinsic_optimization.py

Validity Masks only computing for left camera

When using data collected with scripts/capture_emulated_rgbd.py --camera left_right --lidar both, scripts/create_validity_masks_for_extrinsic_optimization.py only saves validity masks for the left camera. Also, the depth validity mask corresponds to the union of the left and right lidars, which may affect downstream calibration.

Output

(venv) lamsey@stretch-se4-4034:~/repos/stretch4_rgbd$ python3 scripts/create_validity_masks_for_extrinsic_optimization.py ./data/captured_emulated_rgbd_20260708_092733/
Accumulating RGB images and LiDAR projections...
Generating RGB vignetting mask...
Saved RGB vignetting mask to: data/captured_emulated_rgbd_20260708_092733/validity_masks_for_extrinsic_optimization/rgb_vignette_mask_left_camera.png
Generating Depth valid LiDAR mask...
Saved Depth valid mask to: data/captured_emulated_rgbd_20260708_092733/validity_masks_for_extrinsic_optimization/depth_valid_mask_left_camera_both_lidar.png
(venv) lamsey@stretch-se4-4034:~/repos/stretch4_rgbd$ python3 scripts/estimate_lidar_gap.py --data_path ./data/captured_emulated_rgbd_20260708_092733/

Analyzing LiDAR projection gaps in: captured_emulated_rgbd_20260708_092733...

--- LiDAR Gap Analysis Results ---
Total frames analyzed: 6
Average internal gap (pixels): 30.63
Median internal gap  (pixels): 27.77
Maximum internal gap (pixels): 45.56

Recommendation:
Set MAX_LIDAR_INTERPOLATION_DIST_PX in emulated_rgbd_config.py to a value slightly
larger than the maximum internal gap (45.56) to ensure dense depth fills completely.

scripts/optimize_extrinsics.py

Optimization using data captured using both L/R sensors

When using data collected with scripts/capture_emulated_rgbd.py --camera left_right --lidar both, the extrinsics optimizer does not work properly.

Traceback

(venv) lamsey@stretch-se4-4034:~/repos/stretch4_rgbd$ python3 scripts/optimize_extrinsics.py --camera left --lidar left --data_path ./data/captured_emulated_rgbd_20260708_092733/
/home/lamsey/.local/lib/python3.12/site-packages/matplotlib/projections/__init__.py:63: UserWarning: Unable to import Axes3D. This may be due to multiple versions of Matplotlib being installed (e.g. as a system package and as a pip package). As a result, the 3D projection is not available.
  warnings.warn("Unable to import Axes3D. This may be due to multiple versions of "
Loading data from data/captured_emulated_rgbd_20260708_092733 for camera 'left' and lidar 'left_lidar'...
Loaded 0 valid RGB-D sequence frames.
Traceback (most recent call last):
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 747, in <module>
    main()
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 622, in main
    optimizer = ExtrinsicOptimizer(
                ^^^^^^^^^^^^^^^^^^^
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 91, in __init__
    self._load_data()
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 226, in _load_data
    raise ValueError("No matching sequences found!")
ValueError: No matching sequences found!
(venv) lamsey@stretch-se4-4034:~/repos/stretch4_rgbd$ python3 scripts/optimize_extrinsics.py --camera right --lidar right --data_path ./data/captured_emulated_rgbd_20260708_092733/
/home/lamsey/.local/lib/python3.12/site-packages/matplotlib/projections/__init__.py:63: UserWarning: Unable to import Axes3D. This may be due to multiple versions of Matplotlib being installed (e.g. as a system package and as a pip package). As a result, the 3D projection is not available.
  warnings.warn("Unable to import Axes3D. This may be due to multiple versions of "
Loading data from data/captured_emulated_rgbd_20260708_092733 for camera 'right' and lidar 'right_lidar'...
Loaded 0 valid RGB-D sequence frames.
Traceback (most recent call last):
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 747, in <module>
    main()
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 622, in main
    optimizer = ExtrinsicOptimizer(
                ^^^^^^^^^^^^^^^^^^^
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 91, in __init__
    self._load_data()
  File "/home/lamsey/repos/stretch4_rgbd/scripts/optimize_extrinsics.py", line 226, in _load_data
    raise ValueError("No matching sequences found!")
ValueError: No matching sequences found!

Data Visualization

scripts/visualize_emulated_rgbd.py on the combined L/R data collection only shows left camera data in rerun.

examples/recv_rgbd_images.py

Image Only Streaming Errors

The script python3 examples/recv_rgbd_images.py needs to properly display various combinations of camera and LiDAR sensors.

The following displays data only from the right lidar and the right camera:

Terminal 1:

python3 examples/send_rgbd_images.py --camera left_right --lidar both

Terminal 2:

python3 examples/recv_rgbd_images.py

The following displays data only from the right camera and the left lidar:

Terminal 1:

python3 examples/send_rgbd_images.py --camera left_right --lidar left

Terminal 2:

python3 examples/recv_rgbd_images.py

The following displays data from the left camera and left lidar:

Terminal 1:

python3 examples/send_rgbd_images.py --camera left --lidar both

Terminal 2:

python3 examples/recv_rgbd_images.py

hello-lamsey · 2026-07-08T18:54:03Z

removed the readme item for simultaneous LR data collection, which was the cause of downstream errors in the optimization pipeline. code logic is still there - looks like the CLI args for left_right simultaneous data collection were introduced before public release.

hello-lamsey · 2026-07-13T15:45:04Z

api_example.py has some strange artifacts in rerun during rendering. Center pane looks like incorrect camera intrinsics, and right pane is misaligned. Would this be worth addressing within the API implementation or within the API example script?

Suggestions from Gemini 3.5 flash high:

Here are the technical findings explaining the visual issues you are seeing:

1. Why `camera/{c_name}/rgb` and `dense_depth` are misaligned in the overlay (Lines 161–162)

No rr.Pinhole logged: In api_example.py, the images are logged directly to camera/{c_name}/rgb and camera/{c_name}/dense_depth without a corresponding rr.Pinhole camera entity logged to their parent path (camera/{c_name}). Without this, Rerun's 2D overlay and 3D visualizers have no intrinsic parameters to relate or overlay the depth values to the color channels.
Fisheye Distortion: Even if we log a rr.Pinhole entity using the camera matrix, Rerun's internal visualizer only supports rectilinear (pinhole) camera models for depth image unprojection. Because the streamer outputs the RGB and Depth images in their raw, distorted fisheye space (to keep projection computationally cheap at 10Hz), Rerun's pinhole-assumed projection results in warped, misaligned overlays.
Why the Sparse Cloud looks correct: The sparse point cloud is logged via Points3D(frame.point_cloud_base, ...) which uses raw 3D LiDAR points. These points are measured in physical space by the LiDAR and transformed directly to the base frame without relying on camera projection models, keeping them geometrically perfect.

2. Why `dense_view/{c_name}/point_cloud` has incorrect camera intrinsics (Lines 169–172)

Rotated Coordinates vs. Unrotated Distortion Coefficients: When config.ROTATE_IMAGES_TO_VERTICAL is True (default for Stretch 4 head cameras), the streamer rotates the RGB and Depth images 90 degrees to make them upright.
When this happens:
1. cam_matrix is updated to a rotated intrinsic matrix (swapping $f_x \leftrightarrow f_y$ and translating $c_x, c_y$).
2. The distortion coefficients (frame.distortion_coefficients) are left in their original, unrotated state.
3. In create_point_cloud_from_depth(), the pixel coordinates from the rotated depth map are unprojected using cv2.fisheye.undistortPoints with the rotated camera matrix and unrotated distortion coefficients.
This combination causes OpenCV's undistortion solver to map coordinates incorrectly relative to the lens distortion center, resulting in distorted ray directions and warped 3D geometry in the reconstructed dense point cloud.

hello-ck added 2 commits July 2, 2026 15:18

minor documentation update

6f8d3da

This is a minor edit to the README.md documentation that corrects a few directories provided with commands from the scripts directory to the examples directory.

hello-ck requested a review from hello-lamsey July 7, 2026 21:36

hello-lamsey added 2 commits July 8, 2026 11:52

remove LR simultaneous data collection from readme

cf866ea

tune up LR stream parsing

19815d3

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Feature/combined left and right rgbd#4

Feature/combined left and right rgbd#4
hello-ck wants to merge 4 commits into
mainfrom
feature/combined_left_and_right_rgbd

hello-ck commented Jul 7, 2026

Uh oh!

hello-lamsey commented Jul 8, 2026

Uh oh!

hello-lamsey commented Jul 8, 2026

Uh oh!

hello-lamsey commented Jul 13, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Uh oh!

Conversation

hello-ck commented Jul 7, 2026

Uh oh!

hello-lamsey commented Jul 8, 2026

stretch4_rgb feedback so far

Working

Not Working

scripts/create_validity_masks_for_extrinsic_optimization.py

Validity Masks only computing for left camera

Output

scripts/optimize_extrinsics.py

Optimization using data captured using both L/R sensors

Traceback

Data Visualization

examples/recv_rgbd_images.py

Image Only Streaming Errors

Uh oh!

hello-lamsey commented Jul 8, 2026

Uh oh!

hello-lamsey commented Jul 13, 2026

Suggestions from Gemini 3.5 flash high:

1. Why camera/{c_name}/rgb and dense_depth are misaligned in the overlay (Lines 161–162)

2. Why dense_view/{c_name}/point_cloud has incorrect camera intrinsics (Lines 169–172)

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

1. Why `camera/{c_name}/rgb` and `dense_depth` are misaligned in the overlay (Lines 161–162)

2. Why `dense_view/{c_name}/point_cloud` has incorrect camera intrinsics (Lines 169–172)