r1pro: Rust stereo depth for the head at camera rate, with calibration - #4230
Draft
jeff-hykin wants to merge 13 commits into
Draft
jeff-hykin wants to merge 13 commits into
jeff-hykin wants to merge 13 commits into
Conversation
Two bugs, both silent, both in the ROS <-> LCM field copy. Together they meant
every CameraInfo crossing the bridge arrived useless -- any ROS camera, not just
the R1's.
ROS 1 spelled CameraInfo's matrices D K R P. ROS 2 lowercased them to d k r p
and the LCM definitions kept the capitals, so the copy loops asked
hasattr(lcm_msg, "k"), got False, and skipped the field. Measured off an R1 Pro:
K came back [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] and D empty, from a topic publishing
real intrinsics. _lcm_field_for falls back to the upper-case spelling, and only
when the exact name is absent, so it cannot pull a differently-named field into
place.
With the names resolving, K then arrived as [65.0, 141.0, 141.0, 86.0, 186.0,
164.0] with D 64 entries long -- the raw IEEE bytes of the matrix. ROS 2 hands
`k` over as a numpy float64 ndarray, which has tobytes() exactly like the uint8
image buffers, so it fell into the branch meant for image data. itemsize is the
discriminator: image data is array.array("B") or a uint8 ndarray, itemsize 1, and
still wants tobytes().
Nothing raised in either case. A camera whose intrinsics are all zero, or are
byte values, is not obviously broken until something unprojects through it --
which is how this surfaced, as StereoCloud refusing every frame pair with "Head
CameraInfo has no usable intrinsics".
After both, on the robot: fx=253.298, horizontal_fov_deg=86.91, against an
expected 1012.6/4 = 253.15 and ~87 degrees.
Claude-Session: https://claude.ai/code/session_01ExBQYh7BJX5bnGHTKpNVDy
(cherry picked from commit e343fd0c9fe554fb9b04ecd6b939d8115d1211c5)
…e chain and a height cutoff The two RGB eyes matched into a depth image and a cloud by semi-global matching, in a native module, with what a week of measuring against the chassis lidar settled: the rectification takes how the right eye is aimed, the depth is denoised by a chainable filter set (median, plane fit in inverse depth, hole fill, speckle, steep, bilateral, coarse, mean; the shipped chain is median:8+plane:16:1+fill:8), and the cloud can be gated by height above the robot's base through a configured camera pose, so a map that has a lidar for the rest can take only the band the lidar cannot see. Built for camera rate on an Orin: the eyes are decoded at 1/8 scale inside libjpeg's IDCT and in parallel, and the window median is a sliding histogram rather than a per-pixel sort, which took the chain from 16 ms to 8 on the Orin. stereo_bench runs the module's exact per-frame path over a directory of recorded pairs; on dimensional-orin-095 (8 cores, idle) the R1 config runs 27.6-29.7 ms a frame median over eight runs of 150 pairs, 34-36 fps, worst frame 40 ms. Every stage is timed and logged every 5 s. Built through the cargo workspace like the other Rust native modules.
…te the cameras run The connection puts the head's per-eye intrinsics on the wire (the robot self-reports them on /calib), publishes the head and wrist camera links by forward kinematics off the live joint angles so a cloud in the head's frame can be placed, publishes the colour streams at the ~28 Hz they actually arrive at instead of a 5 Hz cap, and lets a run drop the wrist cameras. Wire topics are picked in one table (topics.py) rather than a hand-kept dict. raw_replay reads a recording back stream by stream, which the calibration fitter needs.
…a rate check calibration.json (R1StereoCalibration: baseline, the right eye's roll/pitch/ yaw, per-eye intrinsics, provenance) is loaded by config.stereo_calibration() from ~/.dimos/r1pro/calibration.json or DIMOS_R1_STEREO_CALIBRATION, with the committed rig numbers as the fallback. r1pro-calibration-recorder records both eyes and infos at 30 fps with Point-LIO's cloud, odometry and tf; calibrate_stereo fits the eye rotation against the lidar's floor from such a recording and writes the file, refusing with a specific message and its own exit code when the recording is too short, barely moved, has no floor, is missing a stream, or the matcher is not built. recording_rates reports Hz per stream of a recording and can be told what to require.
One place for what belongs to this head -- the frame, the range, the resolution the Orin can afford, the camera's pose in base_link for the height cutoff (from the URDF, checked against FK in the test) -- and a blueprint that runs it beside Point-LIO so the depth can be looked at in the viewer before anything plans on it.
…tification on every CameraInfo The module held one frame per eye, and the two eyes are two free-running streams, so a left frame's partner was often still in flight when the next left landed and evicted it. Replaying a 30 Hz recording paired 14.7 fps and dropped four frames in five, at 10 ms of work a frame -- the pipeline was idle, waiting on a pairing rule. Each eye now keeps its last eight frames and the closest-stamped pair within max_pair_skew_s is matched, everything older than the pair dropped. The same replay pairs 30 fps with nothing dropped. A CameraInfo that repeats the last one no longer discards the rectification: the robot republishes its calibration at a fixed rate, and rebuilding the maps each time was the module's most expensive step, done for nothing.
# Conflicts: # dimos/robot/galaxea/r1pro/README.md
…recorder's nix fileset
Codecov Report❌ Patch coverage is @@ Coverage Diff @@
## jeff/feat/r1_pointlio #4230 +/- ##
=========================================================
+ Coverage 79.73% 79.98% +0.24%
=========================================================
Files 1511 1532 +21
Lines 142713 144811 +2098
Branches 12210 12359 +149
=========================================================
+ Hits 113791 115821 +2030
- Misses 25483 25523 +40
- Partials 3439 3467 +28
Flags with carried forward coverage won't be shown. Click here to find out more.
... and 4 files with indirect coverage changes 🚀 New features to boost your workflow:
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
A Rust stereo matcher for the R1's head (denoise chain, depth and height cutoffs, 34-36 fps on an Orin), the calibration file it reads, the recorder and fitter that produce it, and r1pro-stereo to look at the result.