diff --git a/docker/development/Dockerfile b/docker/development/Dockerfile
index 17838d69..ba585728 100644
--- a/docker/development/Dockerfile
+++ b/docker/development/Dockerfile
@@ -42,6 +42,7 @@ RUN apt-get update && apt-get install -y \
     gnupg \
     iputils-ping \
     jq \
+    libturbojpeg0-dev \
     nano \
     net-tools \
     ninja-build \
diff --git a/docs/configuration.md b/docs/configuration.md
index 3b8def50..4c2b2d64 100644
--- a/docs/configuration.md
+++ b/docs/configuration.md
@@ -22,6 +22,7 @@ a YAML configuration file for runtime parameters.
     - [Capping the outbound forward rate](#capping-the-outbound-forward-rate)
     - [Latched topics (`latched: true`)](#latched-topics-latched-true)
     - [Data track encoding (`encoding`)](#data-track-encoding-encoding)
+    - [Outbound video](#outbound-video)
   - [Video Options](#video-options)
 
 ## Prerequisites
@@ -423,6 +424,28 @@ Notes:
 - Like `max_rate_hz` and `latched`, `encoding` is matched by **literal topic
   name**, not regex.
 
+#### Outbound video
+
+ROS Portal sends an outbound topic of type `sensor_msgs/Image` or
+`sensor_msgs/CompressedImage` as a LiveKit video track. Other types use a data
+track.
+
+- `sensor_msgs/Image` supports `rgba8`, `rgb8`, `bgr8`, `bgra8`, and `mono8`.
+  The track has the same name as the topic.
+- `sensor_msgs/CompressedImage` supports JPEG only. The `format` field must
+  contain `jpeg` or `jpg` (case-insensitive).
+  ROS Portal decodes each frame before LiveKit encodes it as video.
+- Video tracks follow the same naming convention as data tracks.
+
+```yaml
+topics:
+  # Sends the LiveKit video track /insta/cam0/image_raw/compressed.
+  - topic: "/insta/cam0/image_raw/compressed"
+    direction: "out"
+```
+
+The track size comes from the first frame.
+
 ### Video Options
 
 | Field | Type | Required | Description |
diff --git a/docs/diagnostics.md b/docs/diagnostics.md
index d02c9a31..0dc65ba6 100644
--- a/docs/diagnostics.md
+++ b/docs/diagnostics.md
@@ -199,9 +199,9 @@ inbound reader thread has stopped.
 | `inbound.data_tracks` | Number of inbound LiveKit data tracks being republished. |
 | `inbound.failures` | Cumulative inbound failures, aggregating tracks rejected because no ROS type could be resolved, because they matched no topic pattern, or because a ROS topic name could not be produced, plus failures publishing inbound frames on ROS, invalid inbound JSON frames, empty inbound CDR payloads, streams that ended with a terminal error, and inbound schema validation rejections. |
 | `outbound.data_tracks` | Number of discovered outbound data topics. |
-| `outbound.failures` | Cumulative outbound failures, aggregating LiveKit data-frame push failures, ROS-to-JSON conversion failures, ROS subscription creation failures, and outbound schema define, render, and encoding-mismatch failures. |
+| `outbound.failures` | Cumulative outbound failures, aggregating LiveKit data-frame push failures, ROS-to-JSON conversion failures, ROS subscription creation failures, and outbound schema define, render, and encoding-mismatch failures. Also counts `CompressedImage` frames that are not JPEG or fail to decode, and outbound video topics skipped because another topic already uses their track name. |
 | `outbound.subscriptions` | Number of active outbound ROS subscriptions. |
-| `outbound.video_tracks` | Number of discovered outbound image topics. |
+| `outbound.video_tracks` | Number of discovered outbound image topics, `sensor_msgs/Image` and `sensor_msgs/CompressedImage`. |
 
 `outbound.failures` and `inbound.failures` are deliberately coarse. Every
 increment is logged individually with its specific cause, including the track,
diff --git a/docs/limitations.md b/docs/limitations.md
index 1df57e6b..bf7c8cba 100644
--- a/docs/limitations.md
+++ b/docs/limitations.md
@@ -6,6 +6,9 @@
   recreated.
 - Only `rgba8`, `rgb8`, `bgr8`, `bgra8`, and `mono8` encodings are handled.
   Other encodings are dropped with a throttled warning.
+- Outbound `sensor_msgs/CompressedImage` topics support JPEG only. Each frame
+  is decoded on the CPU before LiveKit encodes it as video. A 1472x1440 frame
+  takes about 6.5 ms to decode on an aarch64 devcontainer.
 - Packed `rgba8`, `bgra8`, and `rgb8` images go to LiveKit without a pixel
   copy. `bgr8`, `mono8`, and images with row padding need one CPU pass inside
   the subscription callback. The LiveKit SDK then converts each frame to I420.
diff --git a/src/livekit_scarf_slam/CMakeLists.txt b/src/livekit_scarf_slam/CMakeLists.txt
new file mode 100644
index 00000000..e9882095
--- /dev/null
+++ b/src/livekit_scarf_slam/CMakeLists.txt
@@ -0,0 +1,43 @@
+#
+# Copyright 2026 LiveKit
+#
+# 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.
+
+cmake_minimum_required(VERSION 3.8)
+project(livekit_scarf_slam)
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/convert_stereo_calibration.py
+  scripts/slam_bag_recorder_node
+  scripts/stereo_rectify_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(FILES README.md calibration.md
+  DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(livekit_scarf_slam_unit_tests test
+    WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    TIMEOUT 120)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE config launch)
diff --git a/src/livekit_scarf_slam/README.md b/src/livekit_scarf_slam/README.md
new file mode 100644
index 00000000..5d3fae40
--- /dev/null
+++ b/src/livekit_scarf_slam/README.md
@@ -0,0 +1,402 @@
+# livekit_scarf_slam
+
+This package runs a
+[ScaRF-SLAM](https://github.com/ori-drs/ScaRF-SLAM) demo through `ros_portal`.
+The robot (a Jetson) sends one stereo camera stream through LiveKit. A SLAM
+host (a MacBook Pro for now) does the localization and mapping.
+
+The robot has no IMU, so the demo does not use OpenVINS, the default
+ScaRF-SLAM pose source. RTAB-Map stereo SLAM (stereo odometry, loop closure,
+and pose graph optimization) gives the poses instead.
+
+The data flows in this order:
+
+1. The camera driver on the robot publishes one side-by-side (SBS) JPEG frame
+   on `/stereo/image_raw/compressed`. An SBS frame holds the left and right
+   images next to each other, so both eyes have one stamp.
+2. `ros_portal` sends the topic as the LiveKit video track `/stereo/image_raw`.
+3. `ros_portal` on the SLAM host publishes the track on
+   `/stereo/image_raw/compressed`.
+4. `stereo_rectify_node` splits each frame and rectifies both fisheye images
+   for RTAB-Map. Rectified images have matching points on the same row.
+5. RTAB-Map calculates the poses.
+6. `slam_bag_recorder_node` writes the ScaRF-SLAM input bag. For each RTAB-Map
+   keyframe, the bag has the left fisheye image, the odometry pose, and the
+   loop-corrected trajectory.
+7. After the run, ScaRF-SLAM builds the dense map from the bag.
+
+ScaRF-SLAM always reads a recorded bag, also in its upstream online mode.
+RTAB-Map runs live on the SLAM host. The dense map comes after the run.
+
+## Python Environment
+
+The upstream page uses a conda environment with Python 3.11. Do not use conda
+in the devcontainer. ROS 2 Jazzy, `rclpy`, and colcon use the system Python
+3.12 at `/usr/bin/python3`, and a different Python version breaks them.
+
+Instead, you create a venv (an isolated Python package folder) from the system
+Python with `--system-site-packages`. With this flag, the venv sees the ROS
+Python packages and colcon. The ScaRF-SLAM packages go into the venv only.
+
+Do not use `/livekit_ws/venv`. That folder is a macOS venv from the host
+mount, and it does not run in the container.
+
+The devcontainer ships `numpy` 1.26.4. Depth Anything 3 requires `numpy<2`, and
+the Jazzy apt packages, for example `cv_bridge`, use numpy 1.x. Do not upgrade
+numpy to 2.x in the venv.
+
+## Prerequisites
+
+You run all commands in this page inside the devcontainer, as `root`.
+
+1. Start the devcontainer from the repository root on the host:
+
+   ```bash
+   devcontainer up --workspace-folder .
+   ```
+
+2. Open a shell in the container:
+
+   ```bash
+   devcontainer exec --workspace-folder . bash
+   ```
+
+3. Install the apt packages:
+
+   ```bash
+   apt update
+   apt install -y python3-venv ros-$ROS_DISTRO-rtabmap-ros
+   ```
+
+   The development image installs `ros-$ROS_DISTRO-rtabmap-ros`. If you use an
+   older container, install it here.
+
+The venv in `/opt` and the clones in `~/git` are outside the host mount. If
+you recreate the container, do all the steps again.
+
+## Create The Venv
+
+1. Create the venv from the system Python:
+
+   ```bash
+   python3 -m venv --system-site-packages /opt/scarf_venv
+   ```
+
+2. Add a `COLCON_IGNORE` file, so that colcon does not scan the venv:
+
+   ```bash
+   touch /opt/scarf_venv/COLCON_IGNORE
+   ```
+
+3. Source ROS 2, then activate the venv:
+
+   ```bash
+   source /opt/ros/jazzy/setup.bash
+   source /opt/scarf_venv/bin/activate
+   ```
+
+4. Make sure that the venv uses Python 3.12 and sees `rclpy`:
+
+   ```bash
+   python -c "import sys, rclpy, numpy; print(sys.version, numpy.__version__)"
+   ```
+
+Source ROS 2 before you activate the venv in every new shell. If you do it in
+the opposite order, the ROS setup script can put the system Python first.
+
+## Install The Python Dependencies
+
+Do these steps in a shell with the venv active.
+
+1. Clone ScaRF-SLAM and Depth Anything 3:
+
+   ```bash
+   mkdir -p ~/git && cd ~/git
+   git clone https://github.com/ori-drs/ScaRF-SLAM.git
+   git clone https://github.com/ByteDance-Seed/Depth-Anything-3.git
+   ```
+
+2. Create a pip constraints file (a list of version limits for every
+   `pip install`):
+
+   ```bash
+   printf 'numpy<2\nsetuptools>=77.0.3,<80\n' > /opt/scarf_venv/constraints.txt
+   ```
+
+   Depth Anything 3 and the Jazzy apt packages need `numpy<2`. torch needs
+   `setuptools>=77.0.3`, and `colcon-core` needs `setuptools<80`. If
+   setuptools is 80 or newer, `colcon build --symlink-install` can fail for
+   Python packages. Pass `-c /opt/scarf_venv/constraints.txt` to every
+   `pip install` in this section.
+
+3. Install PyTorch and xformers:
+
+   ```bash
+   pip install -c /opt/scarf_venv/constraints.txt "torch>=2" torchvision xformers
+   ```
+
+4. On an `x86_64` host, install Depth Anything 3 in editable mode:
+
+   ```bash
+   cd ~/git/Depth-Anything-3
+   pip install -c /opt/scarf_venv/constraints.txt -e . addict
+   ```
+
+   Depth Anything 3 imports `addict` but does not list it as a dependency, so
+   you install it in the same command.
+
+5. On an `aarch64` host, do the steps in
+   [Depth Anything 3 On aarch64](#depth-anything-3-on-aarch64) instead of
+   step 4.
+
+6. Install the other ScaRF-SLAM dependencies:
+
+   ```bash
+   pip install -c /opt/scarf_venv/constraints.txt rosbags open3d gtsam vismatch
+   ```
+
+7. Make sure that the packages import, that numpy is still 1.x, and that
+   setuptools is still below 80:
+
+   ```bash
+   python -c "import torch, depth_anything_3.api, open3d, gtsam, rosbags, vismatch, numpy, setuptools; print(torch.__version__, numpy.__version__, setuptools.__version__)"
+   ```
+
+   The warning about `gsplat` is normal. ScaRF-SLAM does not need 3D Gaussian
+   splatting rendering.
+
+The devcontainer on an Apple silicon host is `aarch64` and has no NVIDIA GPU.
+On that host, PyTorch installs the CUDA build, but it runs on the CPU only.
+xformers has no `aarch64` wheel, so pip compiles it without the CUDA kernels.
+For the real-time demo, use an `x86_64` host with an NVIDIA GPU.
+
+Optional: to add sky masks to the Depth Anything 3 output, edit `da3.py`. In
+the `forward` function of `NestedDepthAnything3Net`, add this line just before
+the `return` statement:
+
+```python
+output.sky = metric_output.sky
+```
+
+The upstream page also points to
+[pull request #7](https://github.com/ori-drs/ScaRF-SLAM/pull/7). It is a
+faster implementation for large environments, but nobody reviewed it
+independently.
+
+### Depth Anything 3 On aarch64
+
+`pycolmap` has no Linux `aarch64` wheel on PyPI, so `pip install -e .` fails
+with this error:
+
+```text
+ERROR: No matching distribution found for pycolmap
+```
+
+Depth Anything 3 uses `pycolmap` only for COLMAP export, but
+`depth_anything_3.api` imports it at load time. You install the other
+dependencies, and then you make that import optional.
+
+1. Install the Depth Anything 3 dependencies without `pycolmap`:
+
+   ```bash
+   cd ~/git/Depth-Anything-3
+   pip install -c /opt/scarf_venv/constraints.txt pre-commit trimesh einops \
+     huggingface_hub imageio opencv-python open3d fastapi uvicorn requests \
+     typer pillow omegaconf evo e3nn "moviepy==1.0.3" plyfile pillow_heif \
+     safetensors addict
+   ```
+
+2. Install Depth Anything 3 without its dependency list:
+
+   ```bash
+   pip install -c /opt/scarf_venv/constraints.txt --no-deps -e .
+   ```
+
+3. In `src/depth_anything_3/utils/export/colmap.py`, replace
+   `import pycolmap` with this block:
+
+   ```python
+   try:
+       import pycolmap
+   except ImportError:  # no aarch64 wheel; only export_to_colmap needs it
+       pycolmap = None
+   ```
+
+4. In the same file, add this check in `export_to_colmap`, just before the
+   `# 2. Set Reconstruction` comment:
+
+   ```python
+   if pycolmap is None:
+       raise ImportError("pycolmap is required for COLMAP export")
+   ```
+
+After these steps, `pip check` reports
+`depth-anything-3 0.0.0 requires pycolmap, which is not installed`. This
+message is expected on `aarch64`. COLMAP export does not work on this host.
+
+## Build The Workspace
+
+ScaRF-SLAM and Depth Anything 3 are plain Python projects, so colcon does not
+build them. colcon builds this package and `ros_portal`.
+
+1. Build the demo packages:
+
+   ```bash
+   cd /livekit_ws
+   colcon build --symlink-install --packages-up-to livekit_scarf_slam
+   ```
+
+   `--packages-up-to` builds only this package and its dependencies. Do
+   not build the full workspace. The full build also builds the
+   `client-sdk-cpp` checkout as the `livekit` package, and that build needs
+   Rust. `ros_portal` downloads a prebuilt LiveKit SDK, so it does not need
+   that package. Without Rust, the full build fails with this error:
+
+   ```text
+   Could not find CARGO_EXECUTABLE using the following names: cargo
+   ```
+
+   If the build fails with
+   `c++: fatal error: Killed signal terminated program cc1plus`, the
+   container ran out of memory. Build one package at a time with two compile
+   jobs:
+
+   ```bash
+   MAKEFLAGS="-j2" colcon build --symlink-install \
+     --executor sequential \
+     --packages-up-to livekit_scarf_slam
+   ```
+
+2. Source the workspace:
+
+   ```bash
+   source install/setup.bash
+   ```
+
+3. Run the tests:
+
+   ```bash
+   colcon test --packages-select livekit_scarf_slam
+   colcon test-result --verbose
+   ```
+
+If you built the workspace before without `--symlink-install`, the build can
+fail with this error:
+
+```text
+failed to create symbolic link '/livekit_ws/build/ros_portal_msgs/ament_cmake_python/ros_portal_msgs/ros_portal_msgs' because existing path cannot be removed: Is a directory
+```
+
+The old build copied the Python package as a folder, and colcon cannot
+replace that folder with a symlink. Delete the build and install output of
+the package in the error, then build again:
+
+```bash
+rm -rf build/ros_portal_msgs install/ros_portal_msgs
+colcon build --symlink-install --packages-up-to livekit_scarf_slam
+```
+
+## Configuration
+
+Before the first run, calibrate the stereo camera. See
+[calibration.md](calibration.md). The launch file reads the calibration from
+`/livekit_ws/stereo-calibration/stereo_calibration.json` by default.
+
+| File | Purpose |
+| --- | --- |
+| `config/ros_portal/edge_robot.yaml` | ROS Portal on the robot. Sends `/stereo/image_raw/compressed` as a video track at 8000 kbps. |
+| `config/ros_portal/slam_host.yaml` | ROS Portal on the SLAM host. Receives the `/stereo/image_raw` track. |
+| `config/rtabmap/stereo.yaml` | RTAB-Map parameters. `frame_id` is `stereo_link`. RTAB-Map adds at most 3 keyframes per second. |
+| `config/scarf_slam/cobra_stereo.yaml` | ScaRF-SLAM configuration. The camera blocks come from the calibration. |
+
+The camera driver on the robot must use the calibrated mode, 3840x1080. A
+frame rate of 15 fps is enough for RTAB-Map. If LiveKit lowers the stream
+resolution, `stereo_rectify_node` scales the intrinsics, and the rectified
+images keep the same size. A frame with a different aspect ratio is dropped.
+
+`stereo_slam.launch.py` has these arguments:
+
+| Argument | Default | Description |
+| --- | --- | --- |
+| `calibration_path` | `/livekit_ws/stereo-calibration/stereo_calibration.json` | The stereo calibration. |
+| `sbs_topic` | `/stereo/image_raw/compressed` | The SBS JPEG topic. |
+| `output_dir` | `~/scarf_slam_runs/<date>_<time>` | The run folder. It gets `slam_bag` and `rtabmap.db`. |
+| `decode_reduction` | `2` | Decode the JPEG at 1/2 size. Use 1, 2, 4, or 8. |
+| `rect_scale` | `0.5` | Size of the rectified images. 0.5 gives 960x540. |
+| `use_ros_portal` | `true` | Set to `false` to play a recorded SBS bag instead. |
+| `rtabmap_viz` | `false` | Start the RTAB-Map GUI. The GUI needs a display. |
+
+The static transforms are `stereo_link` to `cam0_optical` (the left lens)
+to `cam0_rect_optical` (the rectified left image). `stereo_link` has x
+forward, y left, and z up. To connect the camera to the rover body, publish a
+transform from your robot base frame to `stereo_link`.
+
+## Run
+
+1. On the robot, start the camera driver, so that it publishes
+   `/stereo/image_raw/compressed`.
+
+2. On the robot, start ROS Portal:
+
+   ```bash
+   ros2 launch livekit_scarf_slam edge_robot.launch.py \
+     livekit_url:=<url> room_name:=<room>
+   ```
+
+3. On the SLAM host, in the devcontainer, start the SLAM host:
+
+   ```bash
+   ros2 launch livekit_scarf_slam stereo_slam.launch.py \
+     livekit_url:=<url> room_name:=<room>
+   ```
+
+4. Drive the rover. Keep the first two seconds slow, so that the odometry
+   can start.
+
+5. Stop the launch with `Ctrl+C`. The recorder writes the final trajectory
+   and closes the bag. The log line `Closed <path> with <n> keyframes`
+   shows the bag path.
+
+6. Build the dense map from the bag, in a shell with the venv active:
+
+   ```bash
+   cd ~/git/ScaRF-SLAM
+   python3 run_mapping.py --slam_folder <output_dir> \
+     --input_bag <output_dir>/slam_bag \
+     --config /livekit_ws/src/livekit_scarf_slam/config/scarf_slam/cobra_stereo.yaml
+   ```
+
+On the Mac, Depth Anything 3 runs on the CPU, so this step is slow. For
+the real-time demo, use an `x86_64` host with an NVIDIA GPU.
+
+To test the SLAM host without the robot, record the SBS topic once with
+`ros2 bag record /stereo/image_raw/compressed`. Then start the launch with
+`use_ros_portal:=false` and play the bag.
+
+If RTAB-Map logs `quality=0` for many frames, the odometry lost track. Drive
+slower, add light, or point the camera at surfaces with more texture.
+
+For more ScaRF-SLAM detail, see the upstream wiki:
+
+- [Online Reconstruction with SLAM](https://github.com/ori-drs/ScaRF-SLAM/wiki/4.-%F0%9F%9A%80-Online-Reconstruction-with-SLAM)
+- [Configuration](https://github.com/ori-drs/ScaRF-SLAM/wiki/6.-%E2%9A%99%EF%B8%8F-Configuration)
+
+In every new shell, source the environments in this order:
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source /opt/scarf_venv/bin/activate
+source /livekit_ws/install/setup.bash
+```
+
+## Add An IMU Later
+
+With an IMU on the robot, OpenVINS can give the poses instead of RTAB-Map.
+OpenVINS uses the IMU for scale and fast motion. Then you need these steps:
+
+1. Calibrate the camera to the IMU. See the IMU part of
+   [calibration.md](calibration.md).
+2. Clone `open_vins` and `ov_secondary_scarf` into `src/externals`. On
+   Jazzy, use the branch of
+   [open_vins pull request #543](https://github.com/rpng/open_vins/pull/543).
+3. Add `ov_msckf` and `ov_secondary_loop_fusion` to `package.xml`.
diff --git a/src/livekit_scarf_slam/calibration.md b/src/livekit_scarf_slam/calibration.md
new file mode 100644
index 00000000..1ee8802c
--- /dev/null
+++ b/src/livekit_scarf_slam/calibration.md
@@ -0,0 +1,367 @@
+# Camera Calibration
+
+This page tells you how to calibrate the stereo fisheye camera for the
+ScaRF-SLAM demo. The SLAM host reads the camera model from a calibration
+file. It does not find the model on its own.
+
+The stereo demo needs only the stereo calibration. The IMU calibration is
+for a later version of the robot with an IMU.
+
+## Stereo Calibration
+
+You calibrate the stereo camera with the ChArUco scripts in the
+`stereo_cal` repository. A ChArUco board is a checkerboard with a coded
+marker in each white square. The scripts show the board on a MacBook Pro
+screen, so you do not need a printer.
+
+The calibration gives these values:
+
+- The intrinsics of each lens: focal length, image center, and lens
+  distortion.
+- The rotation and translation of the right lens relative to the left lens.
+- The stereo rectification of both lenses.
+
+Do these steps on the Mac:
+
+1. Set the camera to the mode that the robot streams, 3840x1080. The
+   intrinsics are valid for one resolution only.
+
+2. Follow the `stereo_cal` README to show the board, capture the images, and
+   calculate the calibration.
+
+3. Make sure that the baseline in the summary matches the distance between
+   the lens centers, measured with a ruler.
+
+4. Copy `stereo_calibration.json` to `stereo-calibration/` in this
+   repository. The devcontainer sees it as
+   `/livekit_ws/stereo-calibration/stereo_calibration.json`.
+
+5. In the devcontainer, update the camera blocks of the ScaRF-SLAM
+   configuration:
+
+   ```bash
+   ros2 run livekit_scarf_slam convert_stereo_calibration.py scarf \
+     stereo-calibration/stereo_calibration.json \
+     src/livekit_scarf_slam/config/scarf_slam/cobra_stereo.yaml
+   ```
+
+At start, `stereo_rectify_node` and `slam_bag_recorder_node` read the JSON
+file. ScaRF-SLAM reads only `cobra_stereo.yaml`, so do step 5 after
+each new calibration.
+
+In `cobra_stereo.yaml`, `T_cam_imu` of `fisheye_cam0` is the identity, and
+`T_cam_imu` of `fisheye_cam1` is the transform from the left lens to the
+right lens. ScaRF-SLAM uses each pose in the bag as the left lens pose, and
+it reads `T_cam_imu` only to find the offset between the two lenses.
+
+If you mount the camera again or change its focus, calibrate again.
+
+## Add An IMU Later
+
+With an IMU, OpenVINS can give the poses instead of RTAB-Map. OpenVINS reads
+the camera and IMU model from its configuration files. The rest of this page
+tells you how to calibrate the camera to the IMU.
+
+You get these values from the IMU calibration:
+
+- The extrinsics of each camera: its position and rotation relative to the
+  IMU.
+- The time offset between the camera clock and the IMU clock.
+- The IMU noise values.
+
+OpenVINS refines the intrinsics, the extrinsics, and the time offset while it
+runs, because `calib_cam_intrinsics`, `calib_cam_extrinsics`, and
+`calib_cam_timeoffset` are `true` in `estimator_config.yaml`. It corrects
+only small errors. It does not work without a good start value.
+
+### Assumptions
+
+This page uses these assumptions. If your setup is different, change the
+topic names in the commands.
+
+- The robot streams the camera through LiveKit, and `ros_portal` publishes
+  each frame as `sensor_msgs/msg/CompressedImage`.
+- A split node publishes the left lens on `/insta/cam0/image_raw/compressed`
+  and the right lens on `/insta/cam1/image_raw/compressed`. Both images of a
+  pair have the same `header.stamp`.
+- The robot IMU arrives as `sensor_msgs/msg/Imu` on `/insta/imu/data_raw`.
+- The robot stamps the video frames and the IMU messages with one clock.
+
+The ScaRF-SLAM launch file and its OpenVINS configuration use the `/insta/...`
+topic names. If you use the same names, you change fewer files.
+
+### Prerequisites
+
+1. Mount the camera rigidly to the IMU. If the camera moves relative to the
+   IMU after calibration, do the calibration again.
+
+2. Set the camera to the resolution and frame rate that you will use in the
+   demo. The intrinsics are valid for one resolution only.
+
+3. Print an AprilGrid target (a board of square AprilTag markers) from the
+   [Kalibr calibration targets](https://github.com/ethz-asl/kalibr/wiki/calibration-targets)
+   page. Glue it to a flat, rigid board.
+
+4. Measure the printed tags with a ruler. Write a target file with your
+   values, for example `april_6x6.yaml`:
+
+   ```yaml
+   target_type: 'aprilgrid'
+   tagCols: 6
+   tagRows: 6
+   tagSize: 0.088   # edge of one tag, in meters
+   tagSpacing: 0.3  # gap between tags, as a fraction of tagSize
+   ```
+
+5. Build the Kalibr Docker image on the host. Kalibr uses ROS 1, so it does
+   not run in the ROS 2 devcontainer:
+
+   ```bash
+   git clone https://github.com/ethz-asl/kalibr.git
+   cd kalibr
+   docker build -t kalibr -f Dockerfile_ros1_20_04 .
+   ```
+
+6. Make a data folder on the host for the bags and the results, for example
+   `~/calib`. Put `april_6x6.yaml` in this folder.
+
+### Step 1: Record The Bags
+
+You record the bags in the devcontainer, through the same LiveKit path that
+the demo uses. Then the calibration includes the effects of the video codec
+and the stream resolution.
+
+Kalibr reads `sensor_msgs/Image` reliably. You decompress the images before
+you record them.
+
+1. Start `ros_portal` and the split node, as you do for the demo.
+
+2. Decompress both camera topics, each in its own shell:
+
+   ```bash
+   ros2 run image_transport republish compressed raw --ros-args \
+     -r in/compressed:=/insta/cam0/image_raw/compressed -r out:=/insta/cam0/image_raw
+   ros2 run image_transport republish compressed raw --ros-args \
+     -r in/compressed:=/insta/cam1/image_raw/compressed -r out:=/insta/cam1/image_raw
+   ```
+
+3. Record the IMU noise bag. Put the IMU on a still surface, and do not touch
+   it for at least 3 hours:
+
+   ```bash
+   ros2 bag record -o imu_static /insta/imu/data_raw
+   ```
+
+4. Record the camera bag. Move the target slowly in front of the camera for
+   about 2 minutes. Show the target in all areas of each image, and at
+   different distances and angles. Keep the target in the overlap area of the
+   two lenses for part of the time.
+
+   ```bash
+   ros2 bag record -o cam_stereo /insta/cam0/image_raw /insta/cam1/image_raw
+   ```
+
+5. Record the camera and IMU bag. Keep the target still, and move the camera
+   and IMU together for about 1 minute. Rotate around all three axes and move
+   along all three axes. Move fast enough to excite the IMU, but not so fast
+   that the images blur.
+
+   ```bash
+   ros2 bag record -o cam_imu /insta/cam0/image_raw /insta/cam1/image_raw /insta/imu/data_raw
+   ```
+
+6. Convert the ROS 2 bags to ROS 1 bags with `rosbags-convert`. The
+   `rosbags` package is in `/opt/scarf_venv`:
+
+   ```bash
+   rosbags-convert --src imu_static --dst ~/calib/imu_static.bag
+   rosbags-convert --src cam_stereo --dst ~/calib/cam_stereo.bag
+   rosbags-convert --src cam_imu --dst ~/calib/cam_imu.bag
+   ```
+
+   If `~/calib` is not on the host mount, copy the `.bag` files to the host
+   data folder.
+
+### Step 2: Measure The IMU Noise
+
+You find the IMU noise values with an Allan variance analysis. This analysis
+measures the sensor noise from a long still recording.
+
+1. Run [allan_variance_ros](https://github.com/ori-drs/allan_variance_ros) on
+   `imu_static.bag`. Follow the instructions in its README.
+
+2. The tool writes an `imu.yaml` file. Make sure that it has these fields, and
+   set `rostopic` and `update_rate` to your values:
+
+   ```yaml
+   rostopic: /insta/imu/data_raw
+   update_rate: 200.0
+   accelerometer_noise_density: 0.0
+   accelerometer_random_walk: 0.0
+   gyroscope_noise_density: 0.0
+   gyroscope_random_walk: 0.0
+   ```
+
+3. Put `imu.yaml` in the host data folder.
+
+Measured noise values are often too small for OpenVINS, because they do not
+include vibration and temperature changes. If OpenVINS diverges, multiply the
+four noise values by a factor from 2 to 10.
+
+If the IMU vendor publishes noise values, you can use them as a start value
+instead of this step.
+
+### Step 3: Calibrate The Camera Intrinsics And Stereo Extrinsics
+
+1. Start the Kalibr container on the host:
+
+   ```bash
+   xhost +local:root
+   docker run -it -e "DISPLAY" -e "QT_X11_NO_MITSHM=1" \
+     -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+     -v "$HOME/calib:/data" kalibr
+   ```
+
+2. In the container, source the Kalibr workspace:
+
+   ```bash
+   source devel/setup.bash
+   ```
+
+3. Calibrate both cameras with the equidistant fisheye model:
+
+   ```bash
+   rosrun kalibr kalibr_calibrate_cameras \
+     --bag /data/cam_stereo.bag \
+     --target /data/april_6x6.yaml \
+     --models pinhole-equi pinhole-equi \
+     --topics /insta/cam0/image_raw /insta/cam1/image_raw \
+     --bag-freq 10.0
+   ```
+
+   Use `pinhole-equi`. OpenVINS supports only the `radtan` and `equidistant`
+   distortion models, and `radtan` does not fit a 180° fisheye lens.
+
+4. Open the PDF report. Make sure that the reprojection error is below 1
+   pixel for each camera.
+
+Kalibr writes `camchain-cam_stereo.yaml` in `/data`.
+
+If the two lenses do not see the target at the same time, Kalibr cannot
+connect the two cameras. In this case, record a new camera bag with more time
+in the overlap area.
+
+#### Use An Existing OpenCV Stereo Calibration
+
+If you calibrated the stereo pair with the OpenCV ChArUco tool, you do not do
+Step 3 in Kalibr. You convert `stereo_calibration.json` to a Kalibr camchain
+file instead. The OpenCV fisheye model is the Kalibr `pinhole-equi` model, so
+the values do not change. cam0 is the left lens.
+
+1. In the devcontainer, convert the calibration:
+
+   ```bash
+   ros2 run livekit_scarf_slam convert_stereo_calibration.py kalibr \
+     stereo-calibration/stereo_calibration.json ~/calib/camchain-stereo.yaml
+   ```
+
+2. In Step 4, pass `--cam /data/camchain-stereo.yaml`.
+
+The camchain uses the compressed topics by default, because Kalibr reads
+`sensor_msgs/msg/CompressedImage`. With compressed topics, you do not run
+`image_transport republish`, and the bag is much smaller. Record the
+`cam_imu` bag in Step 1 with the compressed topics:
+
+```bash
+ros2 bag record -o cam_imu /insta/cam0/image_raw/compressed \
+  /insta/cam1/image_raw/compressed /insta/imu/data_raw
+```
+
+If you record other topic names, pass `--cam0-topic` and `--cam1-topic` to
+the script. The stereo demo streams one SBS topic, but Kalibr needs one
+topic for each camera. Split the SBS topic into two topics before you record
+the bag. The `cam_stereo` bag is not necessary for this path. The Kalibr
+camera-IMU step needs an AprilGrid target, because Kalibr does not read
+ChArUco boards.
+
+### Step 4: Calibrate The Camera To The IMU
+
+1. In the same Kalibr container, run the camera and IMU calibration:
+
+   ```bash
+   rosrun kalibr kalibr_calibrate_imu_camera \
+     --bag /data/cam_imu.bag \
+     --target /data/april_6x6.yaml \
+     --cam /data/camchain-cam_stereo.yaml \
+     --imu /data/imu.yaml
+   ```
+
+2. Open the PDF report. Make sure that the reprojection error is below 1
+   pixel. Make sure that the gyroscope and accelerometer errors stay inside
+   their bounds.
+
+3. Make sure that the translation in each `T_cam_imu` matches the distance
+   between the IMU and the lens that you measure with a ruler.
+
+Kalibr writes `camchain-imucam-cam_imu.yaml` in `/data`.
+
+### Step 5: Update The Configuration Files
+
+You copy the configuration files from ScaRF-SLAM to `config/` in this
+package, and you put your values in the copies.
+
+1. In `config/open_vins/kalibr_imucam_chain.yaml`, replace the `cam0` and
+   `cam1` blocks with the blocks from `camchain-imucam-cam_imu.yaml`. Keep
+   these fields: `T_cam_imu`, `cam_overlaps`, `camera_model`,
+   `distortion_coeffs`, `distortion_model`, `intrinsics`, `resolution`,
+   `rostopic`, and `timeshift_cam_imu`.
+
+2. Set `rostopic` in each camera block to the raw image topic that OpenVINS
+   reads, `/insta/cam0/image_raw` or `/insta/cam1/image_raw`.
+
+3. In `config/open_vins/kalibr_imu_chain.yaml`, set the four noise values,
+   `rostopic`, and `update_rate` from `imu.yaml`. Keep `T_i_b`, `Tw`,
+   `R_IMUtoGYRO`, `Ta`, `R_IMUtoACC`, and `Tg` at their identity and zero
+   values.
+
+4. In `config/open_vins/estimator_config.yaml`, set `max_cameras: 2`.
+   If the two lenses overlap, set `use_stereo: true`.
+
+5. Replace `mask_cam0.png` and `mask_cam1.png` with masks for your camera.
+   Each mask has the same resolution as its camera image. OpenVINS does not
+   track features in the white areas. Paint the robot body and the black
+   border outside the fisheye circle white.
+
+6. If you do not make masks, set `use_mask: false` in
+   `estimator_config.yaml`.
+
+7. In `config/scarf_slam/indoor_ori_slam.yaml`, replace the `fisheye_cam0`
+   and `fisheye_cam1` blocks with the same values as in step 1. Do not copy
+   `rostopic` or `timeshift_cam_imu` to these blocks.
+
+8. In the same file, set `pinhole_intrinsics` and `pinhole_resolution`.
+   ScaRF-SLAM undistorts each fisheye image to this virtual pinhole camera
+   before depth estimation. Start with the `fx` and `fy` of `cam0`, and put
+   the image center in the middle of `pinhole_resolution`.
+
+### Step 6: Test The Calibration
+
+1. Start the demo with your configuration files. Use the commands in the
+   `README.md` of this package.
+
+2. Hold the camera still for about 2 seconds, so that OpenVINS can
+   initialize.
+
+3. Walk a closed loop of about 10 meters, and stop at the start point.
+
+4. Make sure that the OpenVINS trajectory in RViz ends near the start point.
+   Make sure that the scale matches the distance that you walked.
+
+If the trajectory drifts fast or jumps, do these checks:
+
+- Make sure that the `stamp_fallbacks` diagnostic of `ros_portal` stays at
+  zero. If it increases, the image stamps come from the receive time and not
+  from the robot clock.
+- Make sure that the IMU axes in `T_cam_imu` match the axes of the IMU on the
+  robot.
+- Increase the IMU noise values, as described in step 2.
diff --git a/src/livekit_scarf_slam/config/ros_portal/edge_robot.yaml b/src/livekit_scarf_slam/config/ros_portal/edge_robot.yaml
new file mode 100644
index 00000000..a7c1efd5
--- /dev/null
+++ b/src/livekit_scarf_slam/config/ros_portal/edge_robot.yaml
@@ -0,0 +1,31 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+#
+# ScaRF-SLAM demo, edge robot side (Jetson). See README.md.
+#
+# Sends the side-by-side (SBS) stereo JPEG topic as one LiveKit video track.
+# One track keeps both eyes in one frame with one stamp. The topic
+# /stereo/image_raw/compressed becomes the track /stereo/image_raw.
+
+$schema: https://raw.githubusercontent.com/livekit/ros-portal/main/src/ros_portal_config/schema/ros_portal_config.schema.json
+
+ros_portal:
+  version: "0.0.1"
+  ros_threads: 4
+
+  topics:
+    - topic: "/stereo/image_raw/compressed"
+      direction: "out"
+      video_options:
+        bitrate_kbps: 8000
diff --git a/src/livekit_scarf_slam/config/ros_portal/slam_host.yaml b/src/livekit_scarf_slam/config/ros_portal/slam_host.yaml
new file mode 100644
index 00000000..280c0ef4
--- /dev/null
+++ b/src/livekit_scarf_slam/config/ros_portal/slam_host.yaml
@@ -0,0 +1,29 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+#
+# ScaRF-SLAM demo, SLAM host side (MacBook Pro). See README.md.
+#
+# Receives the SBS stereo video track /stereo/image_raw and republishes it as
+# sensor_msgs/CompressedImage on /stereo/image_raw/compressed. Video tracks
+# match by exact name, so this entry is not a regex.
+
+$schema: https://raw.githubusercontent.com/livekit/ros-portal/main/src/ros_portal_config/schema/ros_portal_config.schema.json
+
+ros_portal:
+  version: "0.0.1"
+  ros_threads: 4
+
+  topics:
+    - topic: "/stereo/image_raw"
+      direction: "in"
diff --git a/src/livekit_scarf_slam/config/rtabmap/stereo.yaml b/src/livekit_scarf_slam/config/rtabmap/stereo.yaml
new file mode 100644
index 00000000..f39ecdc6
--- /dev/null
+++ b/src/livekit_scarf_slam/config/rtabmap/stereo.yaml
@@ -0,0 +1,41 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+#
+# RTAB-Map parameters for stereo-only SLAM on the rectified SBS stream.
+# Both stereo_odometry and rtabmap read this file. RTAB-Map core parameters
+# (the names with a slash) are strings.
+
+/**:
+  ros__parameters:
+    frame_id: stereo_link
+    subscribe_stereo: true
+    subscribe_depth: false
+    subscribe_rgb: false
+    # Both rectified images carry the stamp of one SBS frame.
+    approx_sync: false
+    sync_queue_size: 10
+    wait_for_transform: 0.2
+
+    # Add a graph node at most 3 times per second. This matches the ScaRF-SLAM
+    # keyframe selection (sec_skip: 0.32). RTAB-Map adds no node while the
+    # rover stands still (RGBD/LinearUpdate and RGBD/AngularUpdate).
+    Rtabmap/DetectionRate: "3"
+    # Keep all nodes in working memory, so that MapData has the full graph.
+    Rtabmap/MemoryThr: "0"
+    Mem/IncrementalMemory: "true"
+
+    Odom/Strategy: "0"
+    Vis/MaxFeatures: "1000"
+    Vis/CorType: "0"
+    Reg/Force3DoF: "false"
diff --git a/src/livekit_scarf_slam/config/scarf_slam/cobra_stereo.yaml b/src/livekit_scarf_slam/config/scarf_slam/cobra_stereo.yaml
new file mode 100644
index 00000000..7fc8885c
--- /dev/null
+++ b/src/livekit_scarf_slam/config/scarf_slam/cobra_stereo.yaml
@@ -0,0 +1,54 @@
+# Camera blocks generated by convert_stereo_calibration.py from stereo_calibration.json.
+# To change them, run the script again. Do not edit them by hand.
+model_name: da
+torch_compile: false
+torch_compile_mode: default
+use_slam: true
+is_mono: false
+trajectory: rtabmap
+slam_trajectory_topic: /scarf_slam/input/trajectory
+slam_final_trajectory_topic: /scarf_slam/input/trajectory_final
+slam_odometry_topic: /scarf_slam/input/odometry
+slam_image_topic: /scarf_slam/input/cam0/image_raw/compressed
+image_pose_timestamp_tolerance_sec: 0.005
+frame_scale_opt: true
+submap_scale_opt: true
+point_cloud_fusion: true
+select_kf_on_time: true
+sec_skip: 0.32
+kf_distance: 0.2
+kf_distance_large: 0.2
+kf_angle_deg: 10
+max_distance: 0
+num_ref_poses_per_batch: 6
+num_ref_poses_per_submap: 6
+overlap_ref_views: 1
+publish_ros2_pointcloud: false
+publish_ros2_path: false
+publish_ros2_images: false
+pinhole_intrinsics: [539.0, 539.0, 400.0, 300.0]
+pinhole_resolution: [800, 600]
+fisheye_cam0:
+  T_cam_imu:
+  - [1.0, 0.0, 0.0, 0.0]
+  - [0.0, 1.0, 0.0, 0.0]
+  - [0.0, 0.0, 1.0, 0.0]
+  - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: []
+  camera_model: pinhole
+  distortion_coeffs: [-0.04187668769449636, 0.008842900822683568, -0.018973519833513752, 0.008395592311699627]
+  distortion_model: equidistant
+  intrinsics: [538.9987531735244, 538.4890555274621, 928.8292052974797, 556.6704070822217]
+  resolution: [1920, 1080]
+fisheye_cam1:
+  T_cam_imu:
+  - [0.9997874993076339, -0.0013293142070001272, 0.0205715617279514, -0.05961638740977289]
+  - [0.001561350816807591, 0.9999353003065135, -0.011267536756075829, -9.712534112990536e-05]
+  - [-0.02055525265752531, 0.011297261821420851, 0.9997248888886999, 0.0002070272201636824]
+  - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: []
+  camera_model: pinhole
+  distortion_coeffs: [-0.06364670050007876, 0.11276227546312205, -0.20718265187759605, 0.12174943387929793]
+  distortion_model: equidistant
+  intrinsics: [538.6078309982747, 538.1432657478606, 964.4018726155841, 572.2211790066814]
+  resolution: [1920, 1080]
diff --git a/src/livekit_scarf_slam/launch/edge_robot.launch.py b/src/livekit_scarf_slam/launch/edge_robot.launch.py
new file mode 100644
index 00000000..7911d243
--- /dev/null
+++ b/src/livekit_scarf_slam/launch/edge_robot.launch.py
@@ -0,0 +1,48 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+# ScaRF-SLAM demo, edge robot side (Jetson). See README.md.
+#
+# Starts ROS Portal, which sends /stereo/image_raw/compressed as the LiveKit
+# video track /stereo/image_raw. Start the camera driver separately.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    ros_portal_launch = PathJoinSubstitution([
+        FindPackageShare('ros_portal'), 'launch', 'ros_portal_local.launch.py'])
+    config = PathJoinSubstitution([
+        FindPackageShare('livekit_scarf_slam'), 'config', 'ros_portal', 'edge_robot.yaml'])
+
+    return LaunchDescription([
+        DeclareLaunchArgument('room_name', default_value='robo_room'),
+        DeclareLaunchArgument('identity', default_value='edge_robot'),
+        DeclareLaunchArgument('livekit_url', default_value='ws://host.docker.internal:7880'),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(ros_portal_launch),
+            launch_arguments={
+                'config_path': config,
+                'identity': LaunchConfiguration('identity'),
+                'room_name': LaunchConfiguration('room_name'),
+                'livekit_url': LaunchConfiguration('livekit_url'),
+            }.items(),
+        ),
+    ])
diff --git a/src/livekit_scarf_slam/launch/stereo_slam.launch.py b/src/livekit_scarf_slam/launch/stereo_slam.launch.py
new file mode 100644
index 00000000..cd21c73a
--- /dev/null
+++ b/src/livekit_scarf_slam/launch/stereo_slam.launch.py
@@ -0,0 +1,161 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+# ScaRF-SLAM demo, SLAM host side. See README.md.
+#
+# Receives the SBS stereo stream through ROS Portal, rectifies it, runs
+# RTAB-Map stereo odometry and SLAM, and records the ScaRF-SLAM input bag to
+# <output_dir>/slam_bag. Run ScaRF-SLAM on that bag after the run.
+
+from datetime import datetime
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.actions import LogInfo
+from launch.actions import OpaqueFunction
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+PACKAGE = 'livekit_scarf_slam'
+RECT_NAMESPACE = '/stereo'
+
+
+def _output_dir(context):
+    value = LaunchConfiguration('output_dir').perform(context).strip()
+    if value:
+        return Path(value).expanduser()
+    return Path.home() / 'scarf_slam_runs' / datetime.now().strftime('%Y%m%d_%H%M%S')
+
+
+def _launch_setup(context):
+    output_dir = _output_dir(context)
+    if (output_dir / 'slam_bag').exists():
+        raise RuntimeError(f'{output_dir / "slam_bag"} already exists. Pick a new output_dir.')
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    calibration_path = LaunchConfiguration('calibration_path')
+    sbs_topic = LaunchConfiguration('sbs_topic')
+    rtabmap_params = PathJoinSubstitution([FindPackageShare(PACKAGE), 'config', 'rtabmap',
+                                           'stereo.yaml'])
+    rtabmap_remappings = [
+        (f'{side}/{name}', f'{RECT_NAMESPACE}/{side}/{name}')
+        for side in ('left', 'right') for name in ('image_rect', 'camera_info')]
+
+    return [
+        LogInfo(msg=f'Writing the run output to {output_dir}'),
+        Node(
+            package=PACKAGE,
+            executable='stereo_rectify_node',
+            name='stereo_rectify',
+            output='screen',
+            parameters=[{
+                'calibration_path': calibration_path,
+                'input_topic': sbs_topic,
+                'output_namespace': RECT_NAMESPACE,
+                'decode_reduction': ParameterValue(
+                    LaunchConfiguration('decode_reduction'), value_type=int),
+                'rect_scale': ParameterValue(LaunchConfiguration('rect_scale'), value_type=float),
+            }],
+        ),
+        Node(
+            package='rtabmap_odom',
+            executable='stereo_odometry',
+            namespace='rtabmap',
+            output='screen',
+            parameters=[rtabmap_params],
+            remappings=rtabmap_remappings,
+        ),
+        Node(
+            package='rtabmap_slam',
+            executable='rtabmap',
+            namespace='rtabmap',
+            output='screen',
+            parameters=[rtabmap_params, {'database_path': str(output_dir / 'rtabmap.db')}],
+            remappings=rtabmap_remappings,
+            arguments=['-d'],
+        ),
+        Node(
+            package='rtabmap_viz',
+            executable='rtabmap_viz',
+            namespace='rtabmap',
+            output='screen',
+            parameters=[rtabmap_params],
+            remappings=rtabmap_remappings,
+            condition=IfCondition(LaunchConfiguration('rtabmap_viz')),
+        ),
+        Node(
+            package=PACKAGE,
+            executable='slam_bag_recorder_node',
+            name='slam_bag_recorder',
+            output='screen',
+            parameters=[{
+                'calibration_path': calibration_path,
+                'sbs_topic': sbs_topic,
+                'map_data_topic': '/rtabmap/mapData',
+                'output_path': str(output_dir / 'slam_bag'),
+            }],
+        ),
+    ]
+
+
+def generate_launch_description():
+    ros_portal_launch = PathJoinSubstitution([
+        FindPackageShare('ros_portal'), 'launch', 'ros_portal_local.launch.py'])
+    ros_portal_config = PathJoinSubstitution([
+        FindPackageShare(PACKAGE), 'config', 'ros_portal', 'slam_host.yaml'])
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'calibration_path',
+            default_value='/livekit_ws/stereo-calibration/stereo_calibration.json',
+            description='stereo_calibration.json of the SBS fisheye camera.'),
+        DeclareLaunchArgument(
+            'sbs_topic', default_value='/stereo/image_raw/compressed',
+            description='SBS sensor_msgs/CompressedImage topic.'),
+        DeclareLaunchArgument(
+            'output_dir', default_value='',
+            description='Run output folder. Default: ~/scarf_slam_runs/<date>_<time>.'),
+        DeclareLaunchArgument(
+            'decode_reduction', default_value='2',
+            description='Decode the JPEG at 1/n size: 1, 2, 4, or 8.'),
+        DeclareLaunchArgument(
+            'rect_scale', default_value='0.5',
+            description='Size of the rectified images relative to the calibration.'),
+        DeclareLaunchArgument(
+            'use_ros_portal', default_value='true',
+            description='Start ROS Portal. Set false to play a recorded SBS bag.'),
+        DeclareLaunchArgument('room_name', default_value='robo_room'),
+        DeclareLaunchArgument('identity', default_value='slam_host'),
+        DeclareLaunchArgument('livekit_url', default_value='ws://host.docker.internal:7880'),
+        DeclareLaunchArgument(
+            'rtabmap_viz', default_value='false', description='Start the RTAB-Map GUI.'),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(ros_portal_launch),
+            launch_arguments={
+                'config_path': ros_portal_config,
+                'identity': LaunchConfiguration('identity'),
+                'room_name': LaunchConfiguration('room_name'),
+                'livekit_url': LaunchConfiguration('livekit_url'),
+            }.items(),
+            condition=IfCondition(LaunchConfiguration('use_ros_portal')),
+        ),
+        OpaqueFunction(function=_launch_setup),
+    ])
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/__init__.py b/src/livekit_scarf_slam/livekit_scarf_slam/__init__.py
new file mode 100644
index 00000000..37203f26
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/__init__.py
@@ -0,0 +1,15 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Stereo SLAM host for the ScaRF-SLAM demo."""
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/bag_writer.py b/src/livekit_scarf_slam/livekit_scarf_slam/bag_writer.py
new file mode 100644
index 00000000..6e1857dc
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/bag_writer.py
@@ -0,0 +1,55 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""rosbag2 writer for the ScaRF-SLAM input bag."""
+
+from pathlib import Path
+
+from livekit_scarf_slam.slam_bag import FINAL_TRAJECTORY_TOPIC
+from livekit_scarf_slam.slam_bag import IMAGE_TOPIC
+from livekit_scarf_slam.slam_bag import ODOMETRY_TOPIC
+from livekit_scarf_slam.slam_bag import TRAJECTORY_TOPIC
+from rclpy.serialization import serialize_message
+import rosbag2_py
+
+_TOPIC_TYPES = (
+    (TRAJECTORY_TOPIC, 'nav_msgs/msg/Path'),
+    (FINAL_TRAJECTORY_TOPIC, 'nav_msgs/msg/Path'),
+    (ODOMETRY_TOPIC, 'nav_msgs/msg/Odometry'),
+    (IMAGE_TOPIC, 'sensor_msgs/msg/CompressedImage'),
+)
+
+
+class SlamBagWriter:
+    """Write the four ScaRF-SLAM input topics to a new bag."""
+
+    def __init__(self, uri, storage_id='mcap'):
+        if Path(uri).exists():
+            raise FileExistsError(f'bag path already exists: {uri}')
+        Path(uri).parent.mkdir(parents=True, exist_ok=True)
+        self._writer = rosbag2_py.SequentialWriter()
+        self._writer.open(
+            rosbag2_py.StorageOptions(uri=str(uri), storage_id=storage_id),
+            rosbag2_py.ConverterOptions('cdr', 'cdr'))
+        for topic_id, (name, msg_type) in enumerate(_TOPIC_TYPES):
+            self._writer.create_topic(rosbag2_py.TopicMetadata(
+                id=topic_id, name=name, type=msg_type, serialization_format='cdr'))
+
+    def write(self, topic, msg, stamp_ns):
+        """Write one message with the receive time stamp_ns."""
+        self._writer.write(topic, serialize_message(msg), stamp_ns)
+
+    def close(self):
+        """Flush and close the bag."""
+        self._writer.close()
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/ros_messages.py b/src/livekit_scarf_slam/livekit_scarf_slam/ros_messages.py
new file mode 100644
index 00000000..0ec8a566
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/ros_messages.py
@@ -0,0 +1,121 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Conversions between ROS messages and the numpy types of the core modules."""
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import TransformStamped
+from livekit_scarf_slam.transforms import pose_from_transform
+from livekit_scarf_slam.transforms import transform_from_pose
+from nav_msgs.msg import Odometry
+from nav_msgs.msg import Path
+import numpy as np
+from sensor_msgs.msg import CameraInfo
+
+
+def time_from_ns(stamp_ns):
+    """Return a builtin_interfaces/Time of integer nanoseconds."""
+    return Time(sec=stamp_ns // 1_000_000_000, nanosec=stamp_ns % 1_000_000_000)
+
+
+def ns_from_time(stamp):
+    """Return the integer nanoseconds of a builtin_interfaces/Time."""
+    return stamp.sec * 1_000_000_000 + stamp.nanosec
+
+
+def pose_msg(transform):
+    """Return a geometry_msgs/Pose of a 4x4 transform."""
+    position, orientation = pose_from_transform(transform)
+    msg = Pose()
+    msg.position.x, msg.position.y, msg.position.z = (float(v) for v in position)
+    (msg.orientation.x, msg.orientation.y,
+     msg.orientation.z, msg.orientation.w) = (float(v) for v in orientation)
+    return msg
+
+
+def transform_of_pose_msg(msg):
+    """Return the 4x4 transform of a geometry_msgs/Pose."""
+    return transform_from_pose(
+        (msg.position.x, msg.position.y, msg.position.z),
+        (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w))
+
+
+def transform_of_transform_msg(msg):
+    """Return the 4x4 transform of a geometry_msgs/Transform."""
+    return transform_from_pose(
+        (msg.translation.x, msg.translation.y, msg.translation.z),
+        (msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w))
+
+
+def transform_stamped_msg(transform, parent_frame, child_frame, stamp):
+    """Return a geometry_msgs/TransformStamped of a 4x4 child pose in the parent frame."""
+    position, orientation = pose_from_transform(transform)
+    msg = TransformStamped()
+    msg.header.stamp = stamp
+    msg.header.frame_id = parent_frame
+    msg.child_frame_id = child_frame
+    t = msg.transform
+    t.translation.x, t.translation.y, t.translation.z = (float(v) for v in position)
+    t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w = (float(v) for v in orientation)
+    return msg
+
+
+def odometry_msg(transform, stamp_ns, frame_id, child_frame_id):
+    """Return a nav_msgs/Odometry with the pose of a 4x4 transform."""
+    msg = Odometry()
+    msg.header.stamp = time_from_ns(stamp_ns)
+    msg.header.frame_id = frame_id
+    msg.child_frame_id = child_frame_id
+    msg.pose.pose = pose_msg(transform)
+    return msg
+
+
+def path_msg(poses, frame_id):
+    """
+    Return a nav_msgs/Path of (stamp_ns, 4x4 transform) entries.
+
+    The header stamp is the stamp of the last pose.
+    """
+    msg = Path()
+    msg.header.frame_id = frame_id
+    for stamp_ns, transform in poses:
+        pose = PoseStamped()
+        pose.header.stamp = time_from_ns(stamp_ns)
+        pose.header.frame_id = frame_id
+        pose.pose = pose_msg(transform)
+        msg.poses.append(pose)
+    if msg.poses:
+        msg.header.stamp = msg.poses[-1].header.stamp
+    return msg
+
+
+def rectified_camera_info_msg(projection, size, frame_id):
+    """
+    Return a sensor_msgs/CameraInfo of a rectified image.
+
+    The image has no distortion and no rectification rotation, so K is the
+    left 3x3 block of the projection.
+    """
+    projection = np.asarray(projection, dtype=float)
+    msg = CameraInfo()
+    msg.header.frame_id = frame_id
+    msg.width, msg.height = int(size[0]), int(size[1])
+    msg.distortion_model = 'plumb_bob'
+    msg.d = [0.0] * 5
+    msg.k = [float(v) for v in projection[:, :3].reshape(-1)]
+    msg.r = [float(v) for v in np.eye(3).reshape(-1)]
+    msg.p = [float(v) for v in projection.reshape(-1)]
+    return msg
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/sbs.py b/src/livekit_scarf_slam/livekit_scarf_slam/sbs.py
new file mode 100644
index 00000000..695f29db
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/sbs.py
@@ -0,0 +1,80 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Decode and split side-by-side (SBS) stereo JPEG frames."""
+
+import cv2
+import numpy as np
+
+# libjpeg decodes at 1/2, 1/4, or 1/8 size in the DCT, which is faster than a
+# full decode followed by a resize.
+_GRAYSCALE_DECODE_FLAGS = {
+    1: cv2.IMREAD_GRAYSCALE,
+    2: cv2.IMREAD_REDUCED_GRAYSCALE_2,
+    4: cv2.IMREAD_REDUCED_GRAYSCALE_4,
+    8: cv2.IMREAD_REDUCED_GRAYSCALE_8,
+}
+
+# Relative tolerance for the aspect ratio check. One pixel of rounding on a
+# 1080-pixel axis is about 0.1 percent.
+_ASPECT_TOLERANCE = 0.005
+
+
+def decode_grayscale(data, reduction=1):
+    """
+    Decode a JPEG to a grayscale image, reduced by 1, 2, 4, or 8.
+
+    Returns None if the data does not decode.
+    """
+    if reduction not in _GRAYSCALE_DECODE_FLAGS:
+        raise ValueError(f'reduction must be one of {sorted(_GRAYSCALE_DECODE_FLAGS)}')
+    return cv2.imdecode(np.frombuffer(data, dtype=np.uint8), _GRAYSCALE_DECODE_FLAGS[reduction])
+
+
+def frame_scale(frame_size, sbs_size):
+    """
+    Return the scale of a frame relative to the calibrated SBS size.
+
+    Returns None if the aspect ratio differs, because then the frame is not a
+    uniformly scaled copy of the calibrated mode.
+    """
+    width_scale = frame_size[0] / sbs_size[0]
+    height_scale = frame_size[1] / sbs_size[1]
+    if abs(width_scale - height_scale) > _ASPECT_TOLERANCE * height_scale:
+        return None
+    return height_scale
+
+
+def split_sbs(frame):
+    """Return the (left, right) halves of an SBS frame as views, without a copy."""
+    half = frame.shape[1] // 2
+    return frame[:, :half], frame[:, half:2 * half]
+
+
+def left_eye_jpeg(data, eye_size, quality=95):
+    """
+    Return the left eye of an SBS JPEG as a new JPEG of eye_size (width, height).
+
+    If the frame is a scaled copy of the calibrated mode, the eye is resized to
+    eye_size, so that the calibrated intrinsics apply. Returns None if the data
+    does not decode.
+    """
+    frame = cv2.imdecode(np.frombuffer(data, dtype=np.uint8), cv2.IMREAD_COLOR)
+    if frame is None:
+        return None
+    left, _ = split_sbs(frame)
+    if (left.shape[1], left.shape[0]) != tuple(eye_size):
+        left = cv2.resize(left, tuple(eye_size), interpolation=cv2.INTER_LINEAR)
+    ok, encoded = cv2.imencode('.jpg', left, [cv2.IMWRITE_JPEG_QUALITY, quality])
+    return encoded.tobytes() if ok else None
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/slam_bag.py b/src/livekit_scarf_slam/livekit_scarf_slam/slam_bag.py
new file mode 100644
index 00000000..e2855f66
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/slam_bag.py
@@ -0,0 +1,164 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""
+Keyframe bookkeeping for the ScaRF-SLAM input bag.
+
+ScaRF-SLAM reads one bag with four topics. For each keyframe, the bag has the
+cam0 image, the odometry pose (without loop closure), and a trajectory snapshot
+of all keyframes so far (with loop closure). At the end, the bag has one final
+trajectory. All poses are cam0 poses, stamped with the image stamp.
+
+This module has no ROS imports, so the tests run without a ROS graph.
+"""
+
+import bisect
+from dataclasses import dataclass
+
+import numpy as np
+
+# ScaRF-SLAM default input topics (scarf_slam/integrations/slam_bag.py).
+TRAJECTORY_TOPIC = '/scarf_slam/input/trajectory'
+FINAL_TRAJECTORY_TOPIC = '/scarf_slam/input/trajectory_final'
+ODOMETRY_TOPIC = '/scarf_slam/input/odometry'
+IMAGE_TOPIC = '/scarf_slam/input/cam0/image_raw/compressed'
+
+
+def seconds_to_ns(stamp_sec):
+    """Convert a float stamp in seconds, as RTAB-Map stores it, to integer nanoseconds."""
+    return int(round(stamp_sec * 1e9))
+
+
+class StampBuffer:
+    """Recent items by stamp in nanoseconds, kept for a fixed time horizon."""
+
+    def __init__(self, horizon_ns):
+        self._horizon_ns = horizon_ns
+        self._stamps = []
+        self._items = []
+
+    def __len__(self):
+        return len(self._stamps)
+
+    def add(self, stamp_ns, item):
+        """Add an item, and drop the items older than the horizon before the newest stamp."""
+        index = bisect.bisect_right(self._stamps, stamp_ns)
+        self._stamps.insert(index, stamp_ns)
+        self._items.insert(index, item)
+        cutoff = bisect.bisect_left(self._stamps, self._stamps[-1] - self._horizon_ns)
+        del self._stamps[:cutoff]
+        del self._items[:cutoff]
+
+    def nearest(self, stamp_ns, tolerance_ns):
+        """Return (stamp_ns, item) nearest to stamp_ns within tolerance_ns, or None."""
+        index = bisect.bisect_left(self._stamps, stamp_ns)
+        best = None
+        for candidate in (index - 1, index):
+            if 0 <= candidate < len(self._stamps):
+                delta = abs(self._stamps[candidate] - stamp_ns)
+                if delta <= tolerance_ns and (best is None or delta < best[0]):
+                    best = (delta, candidate)
+        if best is None:
+            return None
+        return self._stamps[best[1]], self._items[best[1]]
+
+
+@dataclass(frozen=True)
+class GraphNode:
+    """One RTAB-Map graph node from MapData."""
+
+    node_id: int
+    stamp_ns: int
+    odom_pose: np.ndarray
+    """4x4 pose of the base frame in the odometry frame when the node was added."""
+
+
+@dataclass(frozen=True)
+class Keyframe:
+    """One keyframe to write to the bag."""
+
+    node_id: int
+    stamp_ns: int
+    """Stamp of the matched image. All records of this keyframe use it."""
+    odom_pose: np.ndarray
+    """4x4 pose of cam0 in the odometry frame."""
+    image: object
+    """The image that image_lookup returned."""
+
+
+class KeyframeTracker:
+    """
+    Turn RTAB-Map graph updates into ScaRF-SLAM keyframes.
+
+    RTAB-Map poses are base frame poses. The tracker multiplies them by the
+    static base-to-cam0 transform, so that all output poses are cam0 poses.
+    """
+
+    def __init__(self, base_to_camera, stamp_tolerance_ns):
+        self._base_to_camera = np.asarray(base_to_camera, dtype=float)
+        self._tolerance_ns = stamp_tolerance_ns
+        self._seen = set()
+        self._stamps = {}
+        self._map_poses = {}
+        self.skipped_nodes = 0
+
+    @property
+    def keyframe_count(self):
+        """Return the number of accepted keyframes."""
+        return len(self._stamps)
+
+    def update(self, nodes, graph_poses, map_to_odom, image_lookup):
+        """
+        Process one MapData message, and return the new keyframes in node order.
+
+        nodes: GraphNode list of the message.
+        graph_poses: dict of node id to the optimized 4x4 base pose in the map frame.
+        map_to_odom: 4x4 transform of the odometry frame in the map frame.
+        image_lookup: callable(stamp_ns, tolerance_ns) that returns
+            (image_stamp_ns, image) or None.
+
+        A new node without a matching image is skipped and is never written.
+        """
+        new_keyframes = []
+        for node in sorted(nodes, key=lambda n: n.node_id):
+            if node.node_id in self._seen:
+                continue
+            self._seen.add(node.node_id)
+            match = image_lookup(node.stamp_ns, self._tolerance_ns)
+            if match is None:
+                self.skipped_nodes += 1
+                continue
+            image_stamp_ns, image = match
+            self._stamps[node.node_id] = image_stamp_ns
+            self._map_poses[node.node_id] = map_to_odom @ node.odom_pose
+            new_keyframes.append(Keyframe(
+                node_id=node.node_id,
+                stamp_ns=image_stamp_ns,
+                odom_pose=node.odom_pose @ self._base_to_camera,
+                image=image))
+        for node_id, pose in graph_poses.items():
+            if node_id in self._stamps:
+                self._map_poses[node_id] = np.asarray(pose, dtype=float)
+        return new_keyframes
+
+    def snapshot(self):
+        """
+        Return the trajectory of all keyframes as (stamp_ns, 4x4 cam0 pose in the map frame).
+
+        A keyframe that is not in the latest graph keeps its last known pose.
+        """
+        return sorted(
+            ((self._stamps[node_id], pose @ self._base_to_camera)
+             for node_id, pose in self._map_poses.items()),
+            key=lambda entry: entry[0])
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/slam_bag_recorder_node.py b/src/livekit_scarf_slam/livekit_scarf_slam/slam_bag_recorder_node.py
new file mode 100644
index 00000000..f94df08c
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/slam_bag_recorder_node.py
@@ -0,0 +1,173 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""
+Record the ScaRF-SLAM input bag from RTAB-Map.
+
+Each new RTAB-Map graph node becomes one keyframe. The node writes the cam0
+fisheye image (the left half of the matching SBS frame), the odometry pose, and
+a trajectory snapshot of all keyframes. On shutdown, it writes the final
+trajectory and closes the bag.
+"""
+
+from livekit_scarf_slam.bag_writer import SlamBagWriter
+from livekit_scarf_slam.ros_messages import ns_from_time
+from livekit_scarf_slam.ros_messages import odometry_msg
+from livekit_scarf_slam.ros_messages import path_msg
+from livekit_scarf_slam.ros_messages import time_from_ns
+from livekit_scarf_slam.ros_messages import transform_of_pose_msg
+from livekit_scarf_slam.ros_messages import transform_of_transform_msg
+from livekit_scarf_slam.sbs import left_eye_jpeg
+from livekit_scarf_slam.slam_bag import FINAL_TRAJECTORY_TOPIC
+from livekit_scarf_slam.slam_bag import GraphNode
+from livekit_scarf_slam.slam_bag import IMAGE_TOPIC
+from livekit_scarf_slam.slam_bag import KeyframeTracker
+from livekit_scarf_slam.slam_bag import ODOMETRY_TOPIC
+from livekit_scarf_slam.slam_bag import seconds_to_ns
+from livekit_scarf_slam.slam_bag import StampBuffer
+from livekit_scarf_slam.slam_bag import TRAJECTORY_TOPIC
+from livekit_scarf_slam.stereo_calibration import load_stereo_calibration
+import rclpy
+from rclpy.duration import Duration
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from rclpy.qos import ReliabilityPolicy
+from rclpy.time import Time
+from rtabmap_msgs.msg import MapData
+from sensor_msgs.msg import CompressedImage
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+
+class SlamBagRecorderNode(Node):
+    """ROS wrapper that feeds RTAB-Map MapData to a KeyframeTracker and writes the bag."""
+
+    def __init__(self):
+        super().__init__('slam_bag_recorder')
+        calibration_path = self.declare_parameter('calibration_path', '').value
+        sbs_topic = self.declare_parameter('sbs_topic', '/stereo/image_raw/compressed').value
+        map_data_topic = self.declare_parameter('map_data_topic', '/rtabmap/mapData').value
+        output_path = self.declare_parameter('output_path', '').value
+        self._base_frame = self.declare_parameter('base_frame', 'stereo_link').value
+        self._camera_frame = self.declare_parameter('camera_frame', 'cam0_optical').value
+        self._map_frame = self.declare_parameter('map_frame', 'map').value
+        self._odom_frame = self.declare_parameter('odom_frame', 'odom').value
+        buffer_sec = self.declare_parameter('image_buffer_sec', 5.0).value
+        tolerance_ms = self.declare_parameter('stamp_tolerance_ms', 1.0).value
+        self._jpeg_quality = self.declare_parameter('jpeg_quality', 95).value
+        if not calibration_path or not output_path:
+            raise ValueError('the calibration_path and output_path parameters are required')
+
+        self._eye_size = load_stereo_calibration(calibration_path).image_size
+        self._tolerance_ns = int(tolerance_ms * 1e6)
+        self._images = StampBuffer(int(buffer_sec * 1e9))
+        self._tracker = None
+        self._writer = SlamBagWriter(output_path)
+        self._output_path = output_path
+
+        self._tf_buffer = Buffer()
+        self._tf_listener = TransformListener(self._tf_buffer, self)
+
+        self.create_subscription(
+            CompressedImage, sbs_topic, self._on_image,
+            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))
+        self.create_subscription(MapData, map_data_topic, self._on_map_data, QoSProfile(depth=10))
+        self.get_logger().info(f'Recording the ScaRF-SLAM input bag to {output_path}')
+
+    def _on_image(self, msg):
+        self._images.add(ns_from_time(msg.header.stamp), msg)
+
+    def _on_map_data(self, msg):
+        if self._tracker is None and not self._create_tracker():
+            return
+        graph = msg.graph
+        nodes = [
+            GraphNode(node_id=n.id, stamp_ns=seconds_to_ns(n.stamp),
+                      odom_pose=transform_of_pose_msg(n.pose))
+            for n in msg.nodes]
+        graph_poses = {
+            node_id: transform_of_pose_msg(pose)
+            for node_id, pose in zip(graph.poses_id, graph.poses)}
+        keyframes = self._tracker.update(
+            nodes, graph_poses, transform_of_transform_msg(graph.map_to_odom),
+            self._images.nearest)
+        if not keyframes:
+            return
+
+        for keyframe in keyframes:
+            jpeg = left_eye_jpeg(keyframe.image.data, self._eye_size, self._jpeg_quality)
+            if jpeg is None:
+                self.get_logger().warn(f'Keyframe {keyframe.node_id}: the JPEG does not decode')
+                continue
+            self._writer.write(
+                ODOMETRY_TOPIC,
+                odometry_msg(keyframe.odom_pose, keyframe.stamp_ns,
+                             self._odom_frame, self._camera_frame),
+                keyframe.stamp_ns)
+            image = CompressedImage()
+            image.header.stamp = time_from_ns(keyframe.stamp_ns)
+            image.header.frame_id = self._camera_frame
+            image.format = 'jpeg'
+            image.data.frombytes(jpeg)
+            self._writer.write(IMAGE_TOPIC, image, keyframe.stamp_ns)
+
+        snapshot = path_msg(self._tracker.snapshot(), self._map_frame)
+        self._writer.write(TRAJECTORY_TOPIC, snapshot, ns_from_time(snapshot.header.stamp))
+        self.get_logger().info(
+            f'{self._tracker.keyframe_count} keyframes, '
+            f'{self._tracker.skipped_nodes} graph nodes without a matching image',
+            throttle_duration_sec=10.0)
+
+    def _create_tracker(self):
+        try:
+            transform = self._tf_buffer.lookup_transform(
+                self._base_frame, self._camera_frame, Time(), Duration(seconds=0.0))
+        except TransformException as error:
+            self.get_logger().warn(
+                f'Waiting for the {self._base_frame} -> {self._camera_frame} transform: {error}',
+                throttle_duration_sec=5.0)
+            return False
+        self._tracker = KeyframeTracker(
+            transform_of_transform_msg(transform.transform), self._tolerance_ns)
+        return True
+
+    def close(self):
+        """Write the final trajectory and close the bag."""
+        if self._tracker is not None and self._tracker.keyframe_count:
+            final = path_msg(self._tracker.snapshot(), self._map_frame)
+            self._writer.write(FINAL_TRAJECTORY_TOPIC, final, ns_from_time(final.header.stamp))
+        self._writer.close()
+        keyframes = self._tracker.keyframe_count if self._tracker is not None else 0
+        self.get_logger().info(f'Closed {self._output_path} with {keyframes} keyframes')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = SlamBagRecorderNode()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        if node is not None:
+            node.close()
+            node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/stereo_calibration.py b/src/livekit_scarf_slam/livekit_scarf_slam/stereo_calibration.py
new file mode 100644
index 00000000..4c1ac433
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/stereo_calibration.py
@@ -0,0 +1,229 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""
+Fisheye stereo calibration from the OpenCV ChArUco calibration tool.
+
+The input is stereo_calibration.json. cam0 is the left lens and cam1 is the
+right lens. The OpenCV fisheye model is the Kalibr pinhole-equi model, which
+OpenVINS and ScaRF-SLAM also use.
+"""
+
+from dataclasses import dataclass
+from dataclasses import replace
+import json
+from pathlib import Path
+
+import cv2
+from livekit_scarf_slam.transforms import make_transform
+import numpy as np
+
+
+@dataclass(frozen=True)
+class FisheyeCamera:
+    """One lens: intrinsics, distortion, and its stereo rectification."""
+
+    camera_matrix: np.ndarray
+    """3x3 intrinsic matrix of the fisheye image."""
+    distortion: np.ndarray
+    """Equidistant distortion coefficients k1 to k4."""
+    rect_rotation: np.ndarray
+    """3x3 rotation from this camera frame to its rectified frame."""
+    rect_projection: np.ndarray
+    """3x4 projection matrix of the rectified image."""
+
+
+@dataclass(frozen=True)
+class StereoCalibration:
+    """Calibration of a side-by-side (SBS) fisheye stereo camera."""
+
+    image_size: tuple
+    """(width, height) of one eye."""
+    rect_size: tuple
+    """(width, height) of the rectified images that the projections use."""
+    left: FisheyeCamera
+    right: FisheyeCamera
+    rotation_left_to_right: np.ndarray
+    translation_left_to_right: np.ndarray
+
+    @property
+    def sbs_size(self):
+        """(width, height) of the SBS frame."""
+        return (2 * self.image_size[0], self.image_size[1])
+
+    @property
+    def baseline_m(self):
+        """Distance between the lenses in meters, from the rectified projections."""
+        return -self.right.rect_projection[0, 3] / self.right.rect_projection[0, 0]
+
+    @property
+    def transform_cam1_cam0(self):
+        """4x4 transform that maps points from the cam0 frame to the cam1 frame."""
+        return make_transform(self.rotation_left_to_right, self.translation_left_to_right)
+
+    def scaled(self, factor):
+        """
+        Return the calibration for input frames that are scaled by factor.
+
+        The fisheye intrinsics scale with the input image. The rectified
+        output (rect_size and the projections) does not change, so the output
+        images stay the same when the input size changes.
+        """
+        def scale_camera(camera):
+            camera_matrix = camera.camera_matrix.copy()
+            camera_matrix[:2] *= factor
+            return replace(camera, camera_matrix=camera_matrix)
+
+        return replace(
+            self,
+            image_size=_scale_size(self.image_size, factor),
+            left=scale_camera(self.left),
+            right=scale_camera(self.right))
+
+
+def _scale_size(size, factor):
+    return (int(round(size[0] * factor)), int(round(size[1] * factor)))
+
+
+def _camera_from_dict(name, camera, rect_rotation, rect_projection):
+    camera_matrix = np.array(camera['camera_matrix'], dtype=float)
+    if camera_matrix[0, 1] != 0.0:
+        raise ValueError(f'{name}: camera matrix skew is not supported')
+    distortion = np.array(camera['distortion'], dtype=float).reshape(-1)
+    if distortion.size != 4:
+        raise ValueError(f'{name}: expected 4 fisheye distortion values')
+    return FisheyeCamera(
+        camera_matrix=camera_matrix,
+        distortion=distortion,
+        rect_rotation=np.array(rect_rotation, dtype=float),
+        rect_projection=np.array(rect_projection, dtype=float))
+
+
+def calibration_from_dict(data):
+    """Return the StereoCalibration of a parsed stereo_calibration.json."""
+    if data.get('model') != 'opencv_fisheye':
+        raise ValueError(f'unsupported model: {data.get("model")}')
+    if not data.get('left_first', True):
+        raise ValueError('left_first is false: the left lens must be the left half of the frame')
+    rect = data['rectification_validation']
+    if rect.get('projection') != 'pinhole':
+        raise ValueError(f'unsupported rectified projection: {rect.get("projection")}')
+    image_size = tuple(data['image_size'])
+    source_size = tuple(data.get('source_stereo_size', (2 * image_size[0], image_size[1])))
+    if source_size != (2 * image_size[0], image_size[1]):
+        raise ValueError(f'source_stereo_size {source_size} is not two eyes of {image_size}')
+    stereo = data['stereo']
+    return StereoCalibration(
+        image_size=image_size,
+        rect_size=tuple(rect['output_size']),
+        left=_camera_from_dict(
+            'left', data['left'], rect['left_rotation'], rect['left_projection']),
+        right=_camera_from_dict(
+            'right', data['right'], rect['right_rotation'], rect['right_projection']),
+        rotation_left_to_right=np.array(stereo['rotation_left_to_right'], dtype=float),
+        translation_left_to_right=np.array(
+            stereo['translation_left_to_right_m'], dtype=float).reshape(3))
+
+
+def load_stereo_calibration(path):
+    """Load stereo_calibration.json."""
+    with Path(path).open(encoding='utf-8') as f:
+        return calibration_from_dict(json.load(f))
+
+
+def rectified_projection(camera, rect_scale):
+    """Return the 3x4 rectified projection for output images scaled by rect_scale."""
+    projection = camera.rect_projection.copy()
+    projection[:2] *= rect_scale
+    return projection
+
+
+def rectify_maps(calibration, rect_scale):
+    """
+    Return the remap tables and the output size for both eyes.
+
+    calibration must match the input frame size, for example after scaled().
+    rect_scale is relative to calibration.rect_size, so pass the same absolute
+    scale for every input size to keep the output images the same.
+
+    Returns ((left_map1, left_map2), (right_map1, right_map2), (width, height)).
+    """
+    size = _scale_size(calibration.rect_size, rect_scale)
+
+    def maps(camera):
+        return cv2.fisheye.initUndistortRectifyMap(
+            camera.camera_matrix, camera.distortion, camera.rect_rotation,
+            rectified_projection(camera, rect_scale)[:, :3], size, cv2.CV_16SC2)
+
+    return maps(calibration.left), maps(calibration.right), size
+
+
+def kalibr_camchain_text(calibration, cam0_topic, cam1_topic):
+    """
+    Return a Kalibr camchain YAML text, the --cam input of kalibr_calibrate_imu_camera.
+
+    T_cn_cnm1 of cam1 is the stereo transform from the cam0 frame to the cam1 frame.
+    """
+    resolution = list(calibration.image_size)
+    lines = ['# Generated by convert_stereo_calibration.py. Do not edit.']
+    for name, camera, topic, overlaps in (
+            ('cam0', calibration.left, cam0_topic, [1]),
+            ('cam1', calibration.right, cam1_topic, [0])):
+        lines += [
+            f'{name}:',
+            f'  cam_overlaps: {overlaps}',
+            '  camera_model: pinhole',
+            f'  distortion_coeffs: {_floats(camera.distortion)}',
+            '  distortion_model: equidistant',
+            f'  intrinsics: {_floats(_intrinsics(camera))}',
+            f'  resolution: {resolution}',
+            f'  rostopic: {topic}',
+        ]
+    lines.append('  T_cn_cnm1:')
+    lines += [f'  - {_floats(row)}' for row in calibration.transform_cam1_cam0]
+    return '\n'.join(lines) + '\n'
+
+
+def scarf_camera_blocks(calibration):
+    """
+    Return the fisheye_cam0 and fisheye_cam1 blocks of a ScaRF-SLAM config.
+
+    ScaRF-SLAM uses each trajectory pose as the cam0 pose, and it reads
+    T_cam_imu only to find the cam0 to cam1 offset. Without an IMU, the pose
+    frame is cam0, so T_cam_imu is identity for cam0 and T_cam1_cam0 for cam1.
+    """
+    def block(camera, transform):
+        return {
+            'T_cam_imu': [_floats(row) for row in transform],
+            'cam_overlaps': [],
+            'camera_model': 'pinhole',
+            'distortion_coeffs': _floats(camera.distortion),
+            'distortion_model': 'equidistant',
+            'intrinsics': _floats(_intrinsics(camera)),
+            'resolution': list(calibration.image_size),
+        }
+
+    return {
+        'fisheye_cam0': block(calibration.left, np.eye(4)),
+        'fisheye_cam1': block(calibration.right, calibration.transform_cam1_cam0),
+    }
+
+
+def _intrinsics(camera):
+    k = camera.camera_matrix
+    return [k[0, 0], k[1, 1], k[0, 2], k[1, 2]]
+
+
+def _floats(values):
+    return [float(v) for v in values]
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/stereo_rectify_node.py b/src/livekit_scarf_slam/livekit_scarf_slam/stereo_rectify_node.py
new file mode 100644
index 00000000..6c4b268b
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/stereo_rectify_node.py
@@ -0,0 +1,183 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""
+Split and rectify an SBS fisheye stereo JPEG stream for RTAB-Map.
+
+Subscribes to one sensor_msgs/CompressedImage SBS topic and publishes, with
+the input stamp:
+
+- <output_namespace>/left/image_rect and right/image_rect (mono8)
+- <output_namespace>/left/camera_info and right/camera_info
+
+It also publishes two static transforms: base_frame -> camera_frame (the
+optical frame of the left fisheye lens) and camera_frame -> rect_frame (the
+rectified left frame). Both rectified images use rect_frame, because RTAB-Map
+reads the stereo baseline from the right projection and not from TF.
+"""
+
+import cv2
+from livekit_scarf_slam.ros_messages import rectified_camera_info_msg
+from livekit_scarf_slam.ros_messages import transform_stamped_msg
+from livekit_scarf_slam.sbs import decode_grayscale
+from livekit_scarf_slam.sbs import frame_scale
+from livekit_scarf_slam.sbs import split_sbs
+from livekit_scarf_slam.stereo_calibration import load_stereo_calibration
+from livekit_scarf_slam.stereo_calibration import rectified_projection
+from livekit_scarf_slam.stereo_calibration import rectify_maps
+from livekit_scarf_slam.transforms import BODY_TO_OPTICAL_ROTATION
+from livekit_scarf_slam.transforms import make_transform
+import numpy as np
+import rclpy
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy
+from rclpy.qos import QoSProfile
+from rclpy.qos import ReliabilityPolicy
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import CompressedImage
+from sensor_msgs.msg import Image
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+
+_STATS_PERIOD_SEC = 10.0
+
+
+class StereoRectifyNode(Node):
+    """ROS wrapper around the SBS split and the fisheye stereo rectification."""
+
+    def __init__(self):
+        super().__init__('stereo_rectify')
+        calibration_path = self.declare_parameter('calibration_path', '').value
+        input_topic = self.declare_parameter(
+            'input_topic', '/stereo/image_raw/compressed').value
+        namespace = self.declare_parameter('output_namespace', '/stereo').value.rstrip('/')
+        self._reduction = self.declare_parameter('decode_reduction', 2).value
+        rect_scale = self.declare_parameter('rect_scale', 0.5).value
+        base_frame = self.declare_parameter('base_frame', 'stereo_link').value
+        camera_frame = self.declare_parameter('camera_frame', 'cam0_optical').value
+        self._rect_frame = self.declare_parameter('rect_frame', 'cam0_rect_optical').value
+        if not calibration_path:
+            raise ValueError('the calibration_path parameter is required')
+
+        self._calibration = load_stereo_calibration(calibration_path)
+        self._rect_scale = rect_scale
+        self._frame_size = None
+        self._maps = None
+        self._frames = 0
+        self._dropped = 0
+
+        _, _, rect_size = rectify_maps(self._calibration, rect_scale)
+        self._camera_infos = [
+            rectified_camera_info_msg(
+                rectified_projection(camera, rect_scale), rect_size, self._rect_frame)
+            for camera in (self._calibration.left, self._calibration.right)]
+
+        output_qos = QoSProfile(depth=5)
+        self._image_pubs = [
+            self.create_publisher(Image, f'{namespace}/{side}/image_rect', output_qos)
+            for side in ('left', 'right')]
+        self._info_pubs = [
+            self.create_publisher(CameraInfo, f'{namespace}/{side}/camera_info', output_qos)
+            for side in ('left', 'right')]
+
+        stamp = self.get_clock().now().to_msg()
+        self._tf_broadcaster = StaticTransformBroadcaster(self)
+        self._tf_broadcaster.sendTransform([
+            transform_stamped_msg(
+                make_transform(BODY_TO_OPTICAL_ROTATION), base_frame, camera_frame, stamp),
+            # The rectification rotation maps camera points to rectified points,
+            # so the pose of the rectified frame in the camera frame is its inverse.
+            transform_stamped_msg(
+                make_transform(self._calibration.left.rect_rotation.T),
+                camera_frame, self._rect_frame, stamp),
+        ])
+
+        # Keep only the newest frame, so that a slow remap never builds a backlog.
+        input_qos = QoSProfile(
+            depth=1, history=HistoryPolicy.KEEP_LAST,
+            reliability=ReliabilityPolicy.BEST_EFFORT)
+        self.create_subscription(CompressedImage, input_topic, self._on_frame, input_qos)
+        self.create_timer(_STATS_PERIOD_SEC, self._log_stats)
+        self.get_logger().info(
+            f'Rectifying {input_topic} to {namespace}/{{left,right}}/image_rect '
+            f'at {rect_size[0]}x{rect_size[1]}, baseline '
+            f'{self._calibration.baseline_m * 1000.0:.1f} mm')
+
+    def _on_frame(self, msg):
+        self._frames += 1
+        frame = decode_grayscale(msg.data, self._reduction)
+        if frame is None:
+            self._drop('the JPEG does not decode')
+            return
+        frame_size = (frame.shape[1], frame.shape[0])
+        if frame_size != self._frame_size:
+            scale = frame_scale(frame_size, self._calibration.sbs_size)
+            if scale is None:
+                self._drop(
+                    f'frame size {frame_size} is not a scaled copy of the calibrated '
+                    f'SBS size {self._calibration.sbs_size}')
+                return
+            left_maps, right_maps, _ = rectify_maps(
+                self._calibration.scaled(scale), self._rect_scale)
+            self._maps = (left_maps, right_maps)
+            self._frame_size = frame_size
+            self.get_logger().info(f'Input frame size is {frame_size}, scale {scale:.3f}')
+
+        for eye, maps, image_pub, info_pub, camera_info in zip(
+                split_sbs(frame), self._maps, self._image_pubs, self._info_pubs,
+                self._camera_infos):
+            rectified = cv2.remap(eye, maps[0], maps[1], cv2.INTER_LINEAR)
+            image_pub.publish(_mono8_msg(rectified, msg.header.stamp, self._rect_frame))
+            camera_info.header.stamp = msg.header.stamp
+            info_pub.publish(camera_info)
+
+    def _drop(self, reason):
+        self._dropped += 1
+        self.get_logger().warn(f'Dropping frame: {reason}', throttle_duration_sec=5.0)
+
+    def _log_stats(self):
+        self.get_logger().info(
+            f'{self._frames} frames in the last {_STATS_PERIOD_SEC:.0f} s, '
+            f'{self._dropped} dropped')
+        self._frames = 0
+        self._dropped = 0
+
+
+def _mono8_msg(image, stamp, frame_id):
+    msg = Image()
+    msg.header.stamp = stamp
+    msg.header.frame_id = frame_id
+    msg.height, msg.width = image.shape
+    msg.encoding = 'mono8'
+    msg.step = msg.width
+    msg.data.frombytes(np.ascontiguousarray(image).tobytes())
+    return msg
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = StereoRectifyNode()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/livekit_scarf_slam/livekit_scarf_slam/transforms.py b/src/livekit_scarf_slam/livekit_scarf_slam/transforms.py
new file mode 100644
index 00000000..6835d548
--- /dev/null
+++ b/src/livekit_scarf_slam/livekit_scarf_slam/transforms.py
@@ -0,0 +1,81 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Rigid transform helpers. Quaternions use the ROS (x, y, z, w) order."""
+
+import numpy as np
+
+# Rotation of an optical frame (z forward, x right, y down) in a body frame
+# (x forward, y left, z up). The columns are the optical axes in the body frame.
+BODY_TO_OPTICAL_ROTATION = np.array([
+    [0.0, 0.0, 1.0],
+    [-1.0, 0.0, 0.0],
+    [0.0, -1.0, 0.0],
+])
+
+
+def matrix_from_quaternion(q):
+    """Return the 3x3 rotation matrix of a unit quaternion (x, y, z, w)."""
+    x, y, z, w = np.asarray(q, dtype=float) / np.linalg.norm(q)
+    return np.array([
+        [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+        [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+        [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+    ])
+
+
+def quaternion_from_matrix(rotation):
+    """Return the unit quaternion (x, y, z, w) of a 3x3 rotation matrix, with w >= 0."""
+    m = np.asarray(rotation, dtype=float)
+    trace = np.trace(m)
+    if trace > 0.0:
+        s = 2.0 * np.sqrt(trace + 1.0)
+        q = [(m[2, 1] - m[1, 2]) / s, (m[0, 2] - m[2, 0]) / s,
+             (m[1, 0] - m[0, 1]) / s, 0.25 * s]
+    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2])
+        q = [0.25 * s, (m[0, 1] + m[1, 0]) / s,
+             (m[0, 2] + m[2, 0]) / s, (m[2, 1] - m[1, 2]) / s]
+    elif m[1, 1] > m[2, 2]:
+        s = 2.0 * np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2])
+        q = [(m[0, 1] + m[1, 0]) / s, 0.25 * s,
+             (m[1, 2] + m[2, 1]) / s, (m[0, 2] - m[2, 0]) / s]
+    else:
+        s = 2.0 * np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1])
+        q = [(m[0, 2] + m[2, 0]) / s, (m[1, 2] + m[2, 1]) / s,
+             0.25 * s, (m[1, 0] - m[0, 1]) / s]
+    q = np.array(q)
+    q /= np.linalg.norm(q)
+    return q if q[3] >= 0.0 else -q
+
+
+def make_transform(rotation=None, translation=None):
+    """Return a 4x4 homogeneous transform."""
+    transform = np.eye(4)
+    if rotation is not None:
+        transform[:3, :3] = rotation
+    if translation is not None:
+        transform[:3, 3] = np.asarray(translation, dtype=float).reshape(3)
+    return transform
+
+
+def transform_from_pose(position, orientation):
+    """Return the 4x4 transform of a position (x, y, z) and quaternion (x, y, z, w)."""
+    return make_transform(matrix_from_quaternion(orientation), position)
+
+
+def pose_from_transform(transform):
+    """Return (position, quaternion) of a 4x4 transform."""
+    transform = np.asarray(transform, dtype=float)
+    return transform[:3, 3].copy(), quaternion_from_matrix(transform[:3, :3])
diff --git a/src/livekit_scarf_slam/package.xml b/src/livekit_scarf_slam/package.xml
new file mode 100644
index 00000000..d3260c4f
--- /dev/null
+++ b/src/livekit_scarf_slam/package.xml
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<!--
+  Copyright 2026 LiveKit
+
+  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.
+-->
+<package format="3">
+  <name>livekit_scarf_slam</name>
+  <version>0.0.0</version>
+  <description>ScaRF-SLAM demo integration for ros_portal: stereo-only RTAB-Map SLAM host that records the ScaRF-SLAM input bag. See README.md.</description>
+  <maintainer email="stephen.derosa@livekit.io">Stephen DeRosa</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>ros_portal</exec_depend>
+  <exec_depend>rosbag2_py</exec_depend>
+  <exec_depend>rosbag2_storage_mcap</exec_depend>
+  <exec_depend>rtabmap_msgs</exec_depend>
+  <exec_depend>rtabmap_odom</exec_depend>
+  <exec_depend>rtabmap_slam</exec_depend>
+  <exec_depend>rtabmap_viz</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/livekit_scarf_slam/scripts/convert_stereo_calibration.py b/src/livekit_scarf_slam/scripts/convert_stereo_calibration.py
new file mode 100755
index 00000000..e5345fab
--- /dev/null
+++ b/src/livekit_scarf_slam/scripts/convert_stereo_calibration.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""
+Convert stereo_calibration.json to Kalibr or ScaRF-SLAM configuration.
+
+kalibr: write a Kalibr camchain file, the --cam input of
+kalibr_calibrate_imu_camera.
+scarf: replace the fisheye_cam0 and fisheye_cam1 blocks of a ScaRF-SLAM
+config. The other keys of the template do not change, but comments are lost.
+"""
+
+import argparse
+from pathlib import Path
+import sys
+
+from livekit_scarf_slam.stereo_calibration import kalibr_camchain_text
+from livekit_scarf_slam.stereo_calibration import load_stereo_calibration
+from livekit_scarf_slam.stereo_calibration import scarf_camera_blocks
+import yaml
+
+_SCARF_HEADER = (
+    '# Camera blocks generated by convert_stereo_calibration.py from {source}.\n'
+    '# To change them, run the script again. Do not edit them by hand.\n'
+)
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__.strip().splitlines()[0])
+    subparsers = parser.add_subparsers(dest='format', required=True)
+
+    kalibr = subparsers.add_parser('kalibr', help='write a Kalibr camchain file')
+    kalibr.add_argument('calibration', type=Path, help='path to stereo_calibration.json')
+    kalibr.add_argument('output', type=Path, help='path of the camchain YAML to write')
+    kalibr.add_argument('--cam0-topic', default='/insta/cam0/image_raw/compressed')
+    kalibr.add_argument('--cam1-topic', default='/insta/cam1/image_raw/compressed')
+
+    scarf = subparsers.add_parser('scarf', help='update the camera blocks of a ScaRF-SLAM config')
+    scarf.add_argument('calibration', type=Path, help='path to stereo_calibration.json')
+    scarf.add_argument('template', type=Path, help='ScaRF-SLAM config to read')
+    scarf.add_argument(
+        'output', type=Path, nargs='?', help='config to write (default: the template)')
+
+    args = parser.parse_args()
+    try:
+        calibration = load_stereo_calibration(args.calibration)
+    except (KeyError, ValueError) as error:
+        sys.exit(f'error: {args.calibration}: {error}')
+
+    if args.format == 'kalibr':
+        output = args.output
+        output.write_text(
+            kalibr_camchain_text(calibration, args.cam0_topic, args.cam1_topic),
+            encoding='utf-8')
+    else:
+        output = args.output or args.template
+        config = yaml.safe_load(args.template.read_text(encoding='utf-8'))
+        config.update(scarf_camera_blocks(calibration))
+        output.write_text(
+            _SCARF_HEADER.format(source=args.calibration.name)
+            + yaml.safe_dump(config, sort_keys=False, default_flow_style=None, width=100),
+            encoding='utf-8')
+    print(f'wrote {output}')
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/livekit_scarf_slam/scripts/slam_bag_recorder_node b/src/livekit_scarf_slam/scripts/slam_bag_recorder_node
new file mode 100755
index 00000000..ed06e0ef
--- /dev/null
+++ b/src/livekit_scarf_slam/scripts/slam_bag_recorder_node
@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Entry point of the slam_bag_recorder_node."""
+
+from livekit_scarf_slam.slam_bag_recorder_node import main
+
+if __name__ == '__main__':
+    main()
diff --git a/src/livekit_scarf_slam/scripts/stereo_rectify_node b/src/livekit_scarf_slam/scripts/stereo_rectify_node
new file mode 100755
index 00000000..44ec401b
--- /dev/null
+++ b/src/livekit_scarf_slam/scripts/stereo_rectify_node
@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Entry point of the stereo_rectify_node."""
+
+from livekit_scarf_slam.stereo_rectify_node import main
+
+if __name__ == '__main__':
+    main()
diff --git a/src/livekit_scarf_slam/test/conftest.py b/src/livekit_scarf_slam/test/conftest.py
new file mode 100644
index 00000000..b355afd5
--- /dev/null
+++ b/src/livekit_scarf_slam/test/conftest.py
@@ -0,0 +1,20 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+"""Make the package importable from the source tree."""
+
+from pathlib import Path
+import sys
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
diff --git a/src/livekit_scarf_slam/test/test_sbs.py b/src/livekit_scarf_slam/test/test_sbs.py
new file mode 100644
index 00000000..d95ce7cb
--- /dev/null
+++ b/src/livekit_scarf_slam/test/test_sbs.py
@@ -0,0 +1,76 @@
+# Copyright 2026 LiveKit
+#
+# 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 cv2
+from livekit_scarf_slam.sbs import decode_grayscale
+from livekit_scarf_slam.sbs import frame_scale
+from livekit_scarf_slam.sbs import left_eye_jpeg
+from livekit_scarf_slam.sbs import split_sbs
+import numpy as np
+import pytest
+
+SBS_SIZE = (3840, 1080)
+
+
+def _sbs_jpeg(width, height, left_value=50, right_value=200):
+    frame = np.empty((height, width, 3), dtype=np.uint8)
+    frame[:, :width // 2] = left_value
+    frame[:, width // 2:] = right_value
+    ok, data = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 95])
+    assert ok
+    return data.tobytes()
+
+
+@pytest.mark.parametrize('reduction', [1, 2, 4])
+def test_decode_and_split(reduction):
+    frame = decode_grayscale(_sbs_jpeg(*SBS_SIZE), reduction)
+    assert frame.shape == (1080 // reduction, 3840 // reduction)
+    left, right = split_sbs(frame)
+    assert left.shape == right.shape == (1080 // reduction, 1920 // reduction)
+    # Skip the columns next to the seam, where JPEG blocks mix both values.
+    assert abs(float(left[:, :-16].mean()) - 50) < 3
+    assert abs(float(right[:, 16:].mean()) - 200) < 3
+
+
+def test_decode_rejects_bad_input():
+    assert decode_grayscale(b'not a jpeg') is None
+    with pytest.raises(ValueError):
+        decode_grayscale(_sbs_jpeg(64, 32), 3)
+
+
+@pytest.mark.parametrize('frame_size, expected', [
+    ((3840, 1080), 1.0),
+    ((1920, 540), 0.5),
+    ((1280, 360), 1.0 / 3.0),
+    ((1920, 1080), None),
+    ((3840, 1088), None),
+])
+def test_frame_scale(frame_size, expected):
+    scale = frame_scale(frame_size, SBS_SIZE)
+    if expected is None:
+        assert scale is None
+    else:
+        assert scale == pytest.approx(expected)
+
+
+@pytest.mark.parametrize('frame_size', [(3840, 1080), (1920, 540)])
+def test_left_eye_jpeg_has_the_calibrated_size(frame_size):
+    data = left_eye_jpeg(_sbs_jpeg(*frame_size), (1920, 1080))
+    eye = cv2.imdecode(np.frombuffer(data, dtype=np.uint8), cv2.IMREAD_COLOR)
+    assert eye.shape == (1080, 1920, 3)
+    assert abs(float(eye[:, :-32].mean()) - 50) < 3
+
+
+def test_left_eye_jpeg_rejects_bad_input():
+    assert left_eye_jpeg(b'not a jpeg', (1920, 1080)) is None
diff --git a/src/livekit_scarf_slam/test/test_slam_bag.py b/src/livekit_scarf_slam/test/test_slam_bag.py
new file mode 100644
index 00000000..67e113ab
--- /dev/null
+++ b/src/livekit_scarf_slam/test/test_slam_bag.py
@@ -0,0 +1,130 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+from livekit_scarf_slam.slam_bag import GraphNode
+from livekit_scarf_slam.slam_bag import KeyframeTracker
+from livekit_scarf_slam.slam_bag import seconds_to_ns
+from livekit_scarf_slam.slam_bag import StampBuffer
+from livekit_scarf_slam.transforms import BODY_TO_OPTICAL_ROTATION
+from livekit_scarf_slam.transforms import make_transform
+import numpy as np
+import pytest
+
+BASE_TO_CAMERA = make_transform(BODY_TO_OPTICAL_ROTATION, [0.1, 0.03, 0.2])
+TOLERANCE_NS = 1_000_000
+T0 = 1_760_000_000_000_000_000
+
+
+def _translation(x, y=0.0):
+    return make_transform(translation=[x, y, 0.0])
+
+
+def _node(node_id, stamp_ns, x):
+    # RTAB-Map stores stamps as float seconds, which loses some nanoseconds.
+    return GraphNode(node_id=node_id, stamp_ns=seconds_to_ns(stamp_ns / 1e9),
+                     odom_pose=_translation(x))
+
+
+def _images(*stamps):
+    buffer = StampBuffer(horizon_ns=10_000_000_000)
+    for stamp in stamps:
+        buffer.add(stamp, f'image@{stamp}')
+    return buffer
+
+
+def test_seconds_to_ns_is_close():
+    assert abs(seconds_to_ns((T0 + 123_456_789) / 1e9) - (T0 + 123_456_789)) < 1000
+
+
+def test_stamp_buffer_nearest_and_horizon():
+    buffer = StampBuffer(horizon_ns=100)
+    for stamp in (10, 50, 30, 120):
+        buffer.add(stamp, stamp)
+    # 10 is older than 120 - 100 and is dropped.
+    assert len(buffer) == 3
+    assert buffer.nearest(10, 5) is None
+    assert buffer.nearest(33, 5) == (30, 30)
+    assert buffer.nearest(45, 10) == (50, 50)
+    assert buffer.nearest(119, 0) is None
+
+
+def test_new_nodes_become_keyframes_with_image_stamps():
+    stamps = [T0, T0 + 333_000_001]
+    images = _images(*stamps)
+    tracker = KeyframeTracker(BASE_TO_CAMERA, TOLERANCE_NS)
+    nodes = [_node(2, stamps[1], 1.0), _node(1, stamps[0], 0.0)]
+    graph = {1: _translation(0.0), 2: _translation(1.0)}
+
+    keyframes = tracker.update(nodes, graph, np.eye(4), images.nearest)
+
+    assert [k.node_id for k in keyframes] == [1, 2]
+    assert [k.stamp_ns for k in keyframes] == stamps
+    assert keyframes[1].image == f'image@{stamps[1]}'
+    np.testing.assert_allclose(keyframes[1].odom_pose, _translation(1.0) @ BASE_TO_CAMERA)
+    snapshot = tracker.snapshot()
+    assert [stamp for stamp, _ in snapshot] == stamps
+    np.testing.assert_allclose(snapshot[1][1], _translation(1.0) @ BASE_TO_CAMERA)
+
+
+def test_loop_closure_updates_the_snapshot_but_not_the_odometry():
+    stamps = [T0, T0 + 400_000_000, T0 + 800_000_000]
+    images = _images(*stamps)
+    tracker = KeyframeTracker(BASE_TO_CAMERA, TOLERANCE_NS)
+    tracker.update([_node(1, stamps[0], 0.0), _node(2, stamps[1], 1.0)],
+                   {1: _translation(0.0), 2: _translation(1.0)}, np.eye(4), images.nearest)
+
+    # MapData repeats node 2 and adds node 3. The optimizer moved node 2.
+    keyframes = tracker.update(
+        [_node(2, stamps[1], 1.0), _node(3, stamps[2], 2.0)],
+        {1: _translation(0.0), 2: _translation(0.9, 0.1), 3: _translation(1.8)},
+        np.eye(4), images.nearest)
+
+    assert [k.node_id for k in keyframes] == [3]
+    np.testing.assert_allclose(keyframes[0].odom_pose, _translation(2.0) @ BASE_TO_CAMERA)
+    snapshot = dict(tracker.snapshot())
+    np.testing.assert_allclose(snapshot[stamps[1]], _translation(0.9, 0.1) @ BASE_TO_CAMERA)
+    np.testing.assert_allclose(snapshot[stamps[2]], _translation(1.8) @ BASE_TO_CAMERA)
+
+
+def test_node_without_image_is_skipped_once():
+    images = _images(T0)
+    tracker = KeyframeTracker(BASE_TO_CAMERA, TOLERANCE_NS)
+    missing = _node(1, T0 + 50_000_000, 0.0)
+
+    assert tracker.update([missing], {1: _translation(0.0)}, np.eye(4), images.nearest) == []
+    images.add(T0 + 50_000_000, 'late image')
+    assert tracker.update([missing], {1: _translation(0.0)}, np.eye(4), images.nearest) == []
+    assert tracker.skipped_nodes == 1
+    assert tracker.keyframe_count == 0
+    assert tracker.snapshot() == []
+
+
+def test_node_outside_the_graph_uses_map_to_odom():
+    images = _images(T0)
+    tracker = KeyframeTracker(BASE_TO_CAMERA, TOLERANCE_NS)
+    map_to_odom = _translation(5.0)
+
+    tracker.update([_node(1, T0, 1.0)], {}, map_to_odom, images.nearest)
+
+    (stamp, pose), = tracker.snapshot()
+    assert stamp == T0
+    np.testing.assert_allclose(pose, _translation(6.0) @ BASE_TO_CAMERA)
+
+
+@pytest.mark.parametrize('offset_ns, accepted', [(900_000, True), (1_100_000, False)])
+def test_stamp_tolerance(offset_ns, accepted):
+    images = _images(T0 + offset_ns)
+    tracker = KeyframeTracker(BASE_TO_CAMERA, TOLERANCE_NS)
+    keyframes = tracker.update([_node(1, T0, 0.0)], {}, np.eye(4), images.nearest)
+    assert bool(keyframes) == accepted
diff --git a/src/livekit_scarf_slam/test/test_stereo_calibration.py b/src/livekit_scarf_slam/test/test_stereo_calibration.py
new file mode 100644
index 00000000..6d01ee59
--- /dev/null
+++ b/src/livekit_scarf_slam/test/test_stereo_calibration.py
@@ -0,0 +1,156 @@
+# Copyright 2026 LiveKit
+#
+# 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 copy
+import json
+import os
+from pathlib import Path
+import subprocess
+import sys
+
+import cv2
+from livekit_scarf_slam.stereo_calibration import calibration_from_dict
+from livekit_scarf_slam.stereo_calibration import kalibr_camchain_text
+from livekit_scarf_slam.stereo_calibration import load_stereo_calibration
+from livekit_scarf_slam.stereo_calibration import rectified_projection
+from livekit_scarf_slam.stereo_calibration import rectify_maps
+from livekit_scarf_slam.stereo_calibration import scarf_camera_blocks
+import numpy as np
+import pytest
+import yaml
+
+PACKAGE_DIR = Path(__file__).resolve().parents[1]
+CALIBRATION_PATH = PACKAGE_DIR / 'test' / 'data' / 'stereo_calibration.json'
+
+
+@pytest.fixture(scope='module')
+def raw():
+    return json.loads(CALIBRATION_PATH.read_text(encoding='utf-8'))
+
+
+@pytest.fixture(scope='module')
+def calibration():
+    return load_stereo_calibration(CALIBRATION_PATH)
+
+
+def test_load(calibration, raw):
+    assert calibration.image_size == (1920, 1080)
+    assert calibration.sbs_size == (3840, 1080)
+    assert calibration.rect_size == (1920, 1080)
+    assert calibration.baseline_m == pytest.approx(raw['stereo']['baseline_m'], abs=2e-4)
+    np.testing.assert_allclose(
+        calibration.transform_cam1_cam0[:3, 3],
+        np.array(raw['stereo']['translation_left_to_right_m']).reshape(3))
+
+
+@pytest.mark.parametrize('change, message', [
+    (lambda d: d.update(model='opencv_pinhole'), 'unsupported model'),
+    (lambda d: d.update(left_first=False), 'left_first'),
+    (lambda d: d['left']['camera_matrix'][0].__setitem__(1, 0.5), 'skew'),
+    (lambda d: d.update(source_stereo_size=[1920, 1080]), 'source_stereo_size'),
+])
+def test_rejects_unsupported_input(raw, change, message):
+    data = copy.deepcopy(raw)
+    change(data)
+    with pytest.raises(ValueError, match=message):
+        calibration_from_dict(data)
+
+
+def test_scaled(calibration):
+    half = calibration.scaled(0.5)
+    assert half.image_size == (960, 540)
+    assert half.sbs_size == (1920, 540)
+    assert half.rect_size == calibration.rect_size
+    np.testing.assert_array_equal(half.left.rect_projection, calibration.left.rect_projection)
+    assert half.left.camera_matrix[0, 0] == pytest.approx(calibration.left.camera_matrix[0, 0] / 2)
+    assert half.left.camera_matrix[2, 2] == 1.0
+    assert half.baseline_m == pytest.approx(calibration.baseline_m)
+    np.testing.assert_array_equal(half.left.distortion, calibration.left.distortion)
+
+
+@pytest.mark.parametrize('input_scale', [1.0, 0.5])
+@pytest.mark.parametrize('pixel', [(480, 270), (100, 60), (850, 500)])
+@pytest.mark.parametrize('depth', [1.0, 4.0])
+def test_rectify_maps_match_the_fisheye_projection(calibration, input_scale, pixel, depth):
+    # A rectified pixel must sample the fisheye pixel where the same 3D point
+    # appears. The right eye checks that the stereo transform matches R2 and P2.
+    rect_scale = 0.5
+    scaled = calibration.scaled(input_scale)
+    left_maps, right_maps, size = rectify_maps(scaled, rect_scale)
+    assert size == (960, 540)
+    u, v = pixel
+
+    for camera, maps in ((scaled.left, left_maps), (scaled.right, right_maps)):
+        projection = rectified_projection(camera, rect_scale)
+        fx, fy, cx, cy = projection[0, 0], projection[1, 1], projection[0, 2], projection[1, 2]
+        # Back-project the pixel to a point in the rectified left frame. The
+        # right projection holds Tx = -fx * baseline.
+        point_rect = np.array([
+            (u - cx) * depth / fx - projection[0, 3] / fx, (v - cy) * depth / fy, depth])
+        point_left = scaled.left.rect_rotation.T @ point_rect
+        point = point_left
+        if camera is scaled.right:
+            point = calibration.rotation_left_to_right @ point_left \
+                + calibration.translation_left_to_right
+        expected, _ = cv2.fisheye.projectPoints(
+            point.reshape(1, 1, 3), np.zeros(3), np.zeros(3),
+            camera.camera_matrix, camera.distortion)
+        map_x, map_y = cv2.convertMaps(maps[0], maps[1], cv2.CV_32FC1)
+        sampled = np.array([map_x[v, u], map_y[v, u]])
+        np.testing.assert_allclose(sampled, expected.reshape(2), atol=0.5 * input_scale)
+
+
+def test_rectified_rows_match(calibration):
+    left = rectified_projection(calibration.left, 0.5)
+    right = rectified_projection(calibration.right, 0.5)
+    np.testing.assert_allclose(left[1], right[1])
+    assert right[0, 3] == pytest.approx(-left[0, 0] * calibration.baseline_m)
+
+
+def test_kalibr_camchain(calibration):
+    chain = yaml.safe_load(kalibr_camchain_text(calibration, '/a', '/b'))
+    assert chain['cam0']['rostopic'] == '/a'
+    assert chain['cam0']['distortion_model'] == 'equidistant'
+    assert chain['cam0']['resolution'] == [1920, 1080]
+    k = calibration.left.camera_matrix
+    assert chain['cam0']['intrinsics'] == [k[0, 0], k[1, 1], k[0, 2], k[1, 2]]
+    np.testing.assert_allclose(chain['cam1']['T_cn_cnm1'], calibration.transform_cam1_cam0)
+
+
+def test_scarf_camera_blocks(calibration):
+    blocks = scarf_camera_blocks(calibration)
+    np.testing.assert_allclose(blocks['fisheye_cam0']['T_cam_imu'], np.eye(4))
+    np.testing.assert_allclose(
+        blocks['fisheye_cam1']['T_cam_imu'], calibration.transform_cam1_cam0)
+    # ScaRF-SLAM derives T_cam0_cam1 = T_cam0_imu @ inv(T_cam1_imu).
+    t_cam0_cam1 = np.array(blocks['fisheye_cam0']['T_cam_imu']) @ np.linalg.inv(
+        blocks['fisheye_cam1']['T_cam_imu'])
+    np.testing.assert_allclose(
+        t_cam0_cam1 @ calibration.transform_cam1_cam0, np.eye(4), atol=1e-12)
+    assert blocks['fisheye_cam1']['distortion_coeffs'] == list(calibration.right.distortion)
+
+
+def test_converter_scarf_keeps_other_keys(tmp_path, calibration):
+    template = tmp_path / 'config.yaml'
+    template.write_text('model_name: da\npinhole_resolution: [800, 600]\n', encoding='utf-8')
+    env = dict(os.environ, PYTHONPATH=os.pathsep.join(
+        [str(PACKAGE_DIR), os.environ.get('PYTHONPATH', '')]))
+    subprocess.run(
+        [sys.executable, str(PACKAGE_DIR / 'scripts' / 'convert_stereo_calibration.py'),
+         'scarf', str(CALIBRATION_PATH), str(template)],
+        check=True, env=env, capture_output=True)
+    config = yaml.safe_load(template.read_text(encoding='utf-8'))
+    assert config['model_name'] == 'da'
+    assert config['pinhole_resolution'] == [800, 600]
+    assert config['fisheye_cam0'] == scarf_camera_blocks(calibration)['fisheye_cam0']
diff --git a/src/livekit_scarf_slam/test/test_transforms.py b/src/livekit_scarf_slam/test/test_transforms.py
new file mode 100644
index 00000000..956f1f00
--- /dev/null
+++ b/src/livekit_scarf_slam/test/test_transforms.py
@@ -0,0 +1,46 @@
+# Copyright 2026 LiveKit
+#
+# 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.
+
+from livekit_scarf_slam.transforms import BODY_TO_OPTICAL_ROTATION
+from livekit_scarf_slam.transforms import make_transform
+from livekit_scarf_slam.transforms import matrix_from_quaternion
+from livekit_scarf_slam.transforms import pose_from_transform
+from livekit_scarf_slam.transforms import quaternion_from_matrix
+from livekit_scarf_slam.transforms import transform_from_pose
+import numpy as np
+import pytest
+
+
+def test_body_to_optical_is_the_ros_optical_rotation():
+    np.testing.assert_allclose(
+        quaternion_from_matrix(BODY_TO_OPTICAL_ROTATION), [-0.5, 0.5, -0.5, 0.5], atol=1e-12)
+    # The optical z axis (forward) is the body x axis.
+    np.testing.assert_allclose(BODY_TO_OPTICAL_ROTATION @ [0, 0, 1], [1, 0, 0])
+
+
+@pytest.mark.parametrize('seed', range(20))
+def test_quaternion_round_trip(seed):
+    rng = np.random.default_rng(seed)
+    q = rng.normal(size=4)
+    q /= np.linalg.norm(q)
+    q = q if q[3] >= 0 else -q
+    rotation = matrix_from_quaternion(q)
+    np.testing.assert_allclose(rotation @ rotation.T, np.eye(3), atol=1e-12)
+    np.testing.assert_allclose(quaternion_from_matrix(rotation), q, atol=1e-12)
+
+
+def test_pose_round_trip():
+    transform = make_transform(matrix_from_quaternion([0.1, -0.2, 0.3, 0.9]), [1.0, 2.0, 3.0])
+    position, orientation = pose_from_transform(transform)
+    np.testing.assert_allclose(transform_from_pose(position, orientation), transform, atol=1e-12)
diff --git a/src/ros_portal/CMakeLists.txt b/src/ros_portal/CMakeLists.txt
index 4fa02203..0fc7b01a 100644
--- a/src/ros_portal/CMakeLists.txt
+++ b/src/ros_portal/CMakeLists.txt
@@ -107,6 +107,8 @@ endif()
 
 find_package(nlohmann_json CONFIG REQUIRED)
 find_package(OpenSSL REQUIRED COMPONENTS Crypto)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(TURBOJPEG REQUIRED IMPORTED_TARGET libturbojpeg)
 find_package(ros2_medkit_cmake REQUIRED)
 include(ROS2MedkitCompat)
 medkit_find_yaml_cpp()
@@ -277,8 +279,11 @@ ament_auto_add_library(ros_portal_lib SHARED
   src/utils/base64.cpp
   src/utils/config_mapping.cpp
   src/utils/image_conversion.cpp
+  src/utils/jpeg_decoder.cpp
+  src/utils/jpeg_encoder.cpp
   src/utils/ros_utils.cpp
   src/utils/topic_matcher.cpp
+  src/utils/video_frame_metadata.cpp
 )
 target_compile_definitions(ros_portal_lib PRIVATE
   LIVEKIT_HAS_ROSBAG2_MESSAGE_DEFINITIONS=${LIVEKIT_HAS_ROSBAG2_MESSAGE_DEFINITIONS}
@@ -292,6 +297,7 @@ target_link_libraries(ros_portal_lib
   "${LIVEKIT_CORE_TARGET}"
   nlohmann_json::nlohmann_json
   OpenSSL::Crypto
+  PkgConfig::TURBOJPEG
   ${LIVEKIT_EXTRA_LINK_LIBS}
 )
 livekit_set_runtime_rpath(ros_portal_lib)
diff --git a/src/ros_portal/include/ros_portal/topic_forwarder.hpp b/src/ros_portal/include/ros_portal/topic_forwarder.hpp
index 6a9ba3f4..822fd4b5 100644
--- a/src/ros_portal/include/ros_portal/topic_forwarder.hpp
+++ b/src/ros_portal/include/ros_portal/topic_forwarder.hpp
@@ -39,9 +39,12 @@
 #include <rclcpp/subscription_base.hpp>
 #include <rclcpp/time.hpp>
 #include <regex>
+#include <std_msgs/msg/header.hpp>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include "ros_portal/diagnostics/diagnostics_fns.hpp"
@@ -49,6 +52,7 @@
 #include "ros_portal/schema/manager.hpp"
 #include "ros_portal/types.hpp"
 #include "ros_portal/utils/image_conversion.hpp"
+#include "ros_portal/utils/jpeg_decoder.hpp"
 
 #ifdef BUILD_TESTING
 #include <gtest/gtest_prod.h>
@@ -62,6 +66,8 @@ namespace ros_portal {
 
 /// @brief ROS type string for sensor image topics forwarded as video tracks.
 inline constexpr const char* kImageMsgType = "sensor_msgs/msg/Image";
+/// @brief ROS type string for JPEG image topics decoded and forwarded as video tracks.
+inline constexpr const char* kCompressedImageMsgType = "sensor_msgs/msg/CompressedImage";
 
 /// @brief Default minimum subscription history depth when no publishers exist.
 inline constexpr std::size_t kDefaultMinQosDepth = 1U;
@@ -84,16 +90,26 @@ class TopicForwarder {
     std::function<livekit::Result<void, std::string>(const std::uint8_t* payload, std::size_t payload_size)> try_push;
   };
 
+  /// @brief ROS header fields sent with one outbound video frame.
+  struct VideoFrameStamp {
+    /// @brief ROS `header.stamp` in microseconds. Non-negative values are also
+    /// sent as frame metadata `user_timestamp_us`.
+    std::int64_t timestamp_us{0};
+    /// @brief ROS `header.frame_id`, sent as frame metadata user data. Unset
+    /// when the frame_id is too long to send.
+    std::optional<std::string_view> frame_id;
+  };
+
   /// @brief Outbound LiveKit video-track sink.
   struct VideoTrackSink {
     /// @brief Track/source width fixed at publication time.
     int width{0};
     /// @brief Track/source height fixed at publication time.
     int height{0};
-    /// @brief Capture one video frame with the ROS message timestamp.
+    /// @brief Capture one video frame with the ROS header stamp and frame_id.
     /// @note Must consume the borrowed frame synchronously. Do not retain a
     /// reference to the frame or its pixel buffer after returning.
-    std::function<void(const livekit::VideoFrame&, std::int64_t)> capture_frame;
+    std::function<void(const livekit::VideoFrame&, const VideoFrameStamp&)> capture_frame;
   };
 
   /// @brief Topic forwarding options derived from ROS Portal configuration.
@@ -204,6 +220,8 @@ class TopicForwarder {
   FRIEND_TEST(TopicForwarderTest, InboundTrackDoesNotBlockLocalOutboundForwardingOrEcho);
   FRIEND_TEST(TopicForwarderTest, DiagnosticsReportOutboundInventoryAndPushFailures);
   FRIEND_TEST(TopicForwarderTest, DiagnosticsReportInboundRejectionsAndStoppedReaders);
+  FRIEND_TEST(TopicForwarderTest, OutboundCompressedImageSkipsNonJpegFormats);
+  FRIEND_TEST(TopicForwarderTest, OutboundRawAndCompressedImagesHaveDistinctTrackNames);
 #endif
 
   /// @brief Resolve the ROS type for an inbound LiveKit track.
@@ -229,6 +247,9 @@ class TopicForwarder {
   void createDataSubscriber(const std::string& topic_name, const std::string& topic_type);
   /// @brief Subscribe to a ROS image topic and forward frames to LiveKit.
   void createImageSubscriber(const std::string& topic_name);
+  /// @brief Subscribe to a JPEG `sensor_msgs/CompressedImage` topic, decode
+  /// each frame, and forward it to a LiveKit video track.
+  void createCompressedImageSubscriber(const std::string& topic_name);
   /// @brief Return whether a ROS sample originated from an inbound LiveKit
   /// publisher owned by this forwarder.
   bool isInboundPublication(const rclcpp::MessageInfo& message_info);
@@ -270,12 +291,21 @@ class TopicForwarder {
     /// @brief Serializes frames of this topic. Held while converting and
     /// capturing, so other outbound topics are not blocked by image work.
     std::mutex mutex;
+    /// @brief LiveKit video track name for this topic.
+    std::string track_name;
     /// @brief LiveKit video sink lazily created on the first frame.
     std::shared_ptr<VideoTrackSink> sink;
-    /// @brief Builds LiveKit frames with the fewest pixel copies.
+    /// @brief Builds LiveKit frames with the fewest pixel copies (raw images).
     utils::ImageFrameBuilder frame_builder;
+    /// @brief Decodes JPEG frames. Set only for compressed image topics.
+    std::unique_ptr<utils::JpegDecoder> jpeg_decoder;
   };
 
+  /// @brief Create the per-topic state and ROS subscription for an outbound
+  /// video topic. The LiveKit track keeps the full ROS topic name.
+  template <typename MessageT, typename CallbackT>
+  void subscribeVideoTopic(const std::string& topic_name, const char* topic_type, bool compressed,
+                           CallbackT&& callback);
   /// @brief Return the state of an outbound video topic, or null once reaped.
   std::shared_ptr<ImageTopicState> findImageTopicState(const std::string& topic_name);
   /// @brief Create the LiveKit video sink on the first frame and reject frames
@@ -283,6 +313,9 @@ class TopicForwarder {
   /// @return True when @p state has a sink that accepts @p width x @p height.
   bool ensureVideoSinkLocked(ImageTopicState& state, const std::string& topic_name, int width, int height,
                              const std::string& source_format);
+  /// @brief Build the stamp and frame_id metadata for one outbound frame.
+  /// @param header ROS header that must outlive the returned value.
+  VideoFrameStamp makeVideoFrameStamp(const std_msgs::msg::Header& header, const std::string& topic_name);
 
   /// @brief Per-topic state for outbound serialized ROS forwarding.
   struct DataTopicState {
diff --git a/src/ros_portal/include/ros_portal/utils/jpeg_decoder.hpp b/src/ros_portal/include/ros_portal/utils/jpeg_decoder.hpp
new file mode 100644
index 00000000..771e10a2
--- /dev/null
+++ b/src/ros_portal/include/ros_portal/utils/jpeg_decoder.hpp
@@ -0,0 +1,80 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <livekit/video_frame.h>
+
+#include <cstddef>
+#include <cstdint>
+#include <string>
+#include <string_view>
+
+namespace ros_portal::utils {
+
+/// @brief Return whether a `sensor_msgs/CompressedImage.format` names JPEG data.
+///
+/// Accepts plain values such as `jpeg` and image_transport values such as
+/// `bgr8; jpeg compressed bgr8`, case-insensitively.
+bool isJpegFormat(std::string_view format);
+
+/// @brief Reusable TurboJPEG decompressor that produces LiveKit video frames.
+///
+/// A 4:2:0 JPEG decodes straight into an I420 frame with no color conversion,
+/// and a grayscale JPEG into the Y plane of an I420 frame with neutral chroma.
+/// Other subsampling decodes to RGB24. The output frame is reused while the
+/// size and layout stay the same. An instance is not thread-safe; use one per
+/// topic.
+class JpegDecoder {
+public:
+  /// @brief Create a decompressor.
+  JpegDecoder();
+
+  /// @brief Release the TurboJPEG handle.
+  ~JpegDecoder();
+
+  JpegDecoder(const JpegDecoder&) = delete;
+  JpegDecoder& operator=(const JpegDecoder&) = delete;
+  JpegDecoder(JpegDecoder&&) = delete;
+  JpegDecoder& operator=(JpegDecoder&&) = delete;
+
+  /// @brief Decode one JPEG image.
+  /// @param jpeg JPEG bytes.
+  /// @param size Number of bytes at @p jpeg.
+  /// @return The decoded frame, valid until the next call, or nullptr when the
+  /// data is not a complete, valid JPEG. A JPEG that decodes only with a
+  /// warning, such as a truncated one, is also rejected. On failure,
+  /// @ref lastError describes the cause.
+  [[nodiscard]] const livekit::VideoFrame* decode(const std::uint8_t* jpeg, std::size_t size);
+
+  /// @brief Return the cause of the most recent decode failure.
+  const std::string& lastError() const noexcept { return last_error_; }
+
+private:
+  /// @brief Return the reused output frame, reallocating it only when the size
+  /// or layout changes.
+  /// @param fresh Set to true when the frame was reallocated.
+  livekit::VideoFrame& reuse(int width, int height, livekit::VideoBufferType type, bool& fresh);
+
+  /// @brief Opaque TurboJPEG decompressor handle (`tjhandle`).
+  void* handle_{nullptr};
+  /// @brief Reused output frame.
+  livekit::VideoFrame frame_;
+  /// @brief Cause of the most recent decode failure.
+  std::string last_error_;
+};
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/include/ros_portal/utils/jpeg_encoder.hpp b/src/ros_portal/include/ros_portal/utils/jpeg_encoder.hpp
new file mode 100644
index 00000000..4434353f
--- /dev/null
+++ b/src/ros_portal/include/ros_portal/utils/jpeg_encoder.hpp
@@ -0,0 +1,84 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <livekit/video_frame.h>
+
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace ros_portal::utils {
+
+/// @brief JPEG quality (1-100) used for inbound video frames.
+inline constexpr int kDefaultJpegQuality = 90;
+
+/// @brief Reusable TurboJPEG compressor for decoded LiveKit video frames.
+///
+/// Each instance owns one TurboJPEG handle and one output buffer. Both are
+/// reused across frames, so steady-state encoding does not allocate unless the
+/// frame size grows. An instance is not thread-safe; use one per reader thread.
+class JpegEncoder {
+public:
+  /// @brief Create a compressor.
+  /// @param quality JPEG quality from 1 to 100. Values outside the range are
+  /// clamped.
+  explicit JpegEncoder(int quality = kDefaultJpegQuality);
+
+  /// @brief Release the TurboJPEG handle and output buffer.
+  ~JpegEncoder();
+
+  JpegEncoder(const JpegEncoder&) = delete;
+  JpegEncoder& operator=(const JpegEncoder&) = delete;
+  JpegEncoder(JpegEncoder&&) = delete;
+  JpegEncoder& operator=(JpegEncoder&&) = delete;
+
+  /// @brief Encode one tightly packed I420 frame as a 4:2:0 JPEG.
+  ///
+  /// The frame must use @ref livekit::VideoBufferType::I420 with the Y, U and V
+  /// planes packed back to back and no row padding, which is the layout that
+  /// @ref livekit::VideoStream delivers.
+  /// @param frame Decoded I420 frame.
+  /// @param out Receives the JPEG bytes. Its capacity is reused across calls.
+  /// @return True on success. On failure, @p out is unchanged and
+  /// @ref lastError describes the cause.
+  [[nodiscard]] bool encodeI420(const livekit::VideoFrame& frame, std::vector<std::uint8_t>& out);
+
+  /// @brief Return the cause of the most recent encode failure.
+  const std::string& lastError() const noexcept { return last_error_; }
+
+private:
+  /// @brief Release a TurboJPEG-allocated output buffer.
+  struct BufferDeleter {
+    /// @brief Free @p buffer with TurboJPEG's allocator.
+    void operator()(unsigned char* buffer) const noexcept;
+  };
+
+  /// @brief Opaque TurboJPEG compressor handle (`tjhandle`).
+  void* handle_{nullptr};
+  /// @brief TurboJPEG-allocated output buffer reused across frames.
+  std::unique_ptr<unsigned char[], BufferDeleter> buffer_;
+  /// @brief Capacity of @ref buffer_ in bytes.
+  unsigned long buffer_capacity_{0U};
+  /// @brief JPEG quality from 1 to 100.
+  int quality_;
+  /// @brief Cause of the most recent encode failure.
+  std::string last_error_;
+};
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/include/ros_portal/utils/ros_utils.hpp b/src/ros_portal/include/ros_portal/utils/ros_utils.hpp
index 0879b355..617df461 100644
--- a/src/ros_portal/include/ros_portal/utils/ros_utils.hpp
+++ b/src/ros_portal/include/ros_portal/utils/ros_utils.hpp
@@ -193,4 +193,5 @@ bool isRosTopicStatisticsTopic(const std::string& topic_name);
 /// @param topic_name Absolute ROS topic name.
 /// @return True when the last segment of @p topic_name starts with `_buf_`.
 bool isRmwBufferTopic(const std::string& topic_name);
+
 } // namespace ros_portal::utils
diff --git a/src/ros_portal/include/ros_portal/utils/video_frame_metadata.hpp b/src/ros_portal/include/ros_portal/utils/video_frame_metadata.hpp
new file mode 100644
index 00000000..434646cc
--- /dev/null
+++ b/src/ros_portal/include/ros_portal/utils/video_frame_metadata.hpp
@@ -0,0 +1,48 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+#include <optional>
+#include <string_view>
+#include <vector>
+
+namespace ros_portal::utils {
+
+/// @brief Version byte that prefixes the ROS frame_id in video frame user data.
+inline constexpr std::uint8_t kFrameIdUserDataVersion = 0x01U;
+/// @brief Maximum ROS frame_id length, in bytes, carried in video frame user data.
+inline constexpr std::size_t kMaxFrameIdUserDataLength = 255U;
+
+/// @brief Encode a ROS frame_id as LiveKit video frame user data.
+///
+/// The layout is one version byte (@ref kFrameIdUserDataVersion) followed by
+/// the frame_id bytes. Receivers use it to restore `header.frame_id`.
+/// @param frame_id ROS `header.frame_id` of the source image.
+/// @return Encoded user data, or std::nullopt when @p frame_id is longer than
+/// @ref kMaxFrameIdUserDataLength.
+std::optional<std::vector<std::uint8_t>> encodeFrameIdUserData(std::string_view frame_id);
+
+/// @brief Decode a ROS frame_id from LiveKit video frame user data.
+/// @param user_data User data received with a video frame.
+/// @return A view of the frame_id inside @p user_data, valid while @p user_data
+/// is unchanged, or std::nullopt when @p user_data is empty, has an unknown
+/// version byte, or is longer than the encoded maximum.
+std::optional<std::string_view> decodeFrameIdUserData(const std::vector<std::uint8_t>& user_data);
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/package.xml b/src/ros_portal/package.xml
index 95104c3b..83372d87 100644
--- a/src/ros_portal/package.xml
+++ b/src/ros_portal/package.xml
@@ -29,6 +29,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
+  <depend>libturbojpeg</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>
   <depend>ros2_medkit_serialization</depend>
diff --git a/src/ros_portal/src/ros_portal.cpp b/src/ros_portal/src/ros_portal.cpp
index 80c42d49..d7f56e6a 100644
--- a/src/ros_portal/src/ros_portal.cpp
+++ b/src/ros_portal/src/ros_portal.cpp
@@ -50,6 +50,7 @@
 #include "ros_portal/topic_forwarder.hpp"
 #include "ros_portal/utils/config_mapping.hpp"
 #include "ros_portal/utils/ros_utils.hpp"
+#include "ros_portal/utils/video_frame_metadata.hpp"
 #include "ros_portal_config/config/config_parser.hpp"
 
 namespace ros_portal {
@@ -753,22 +754,48 @@ bool RosPortal::initializeTopicForwarder(const std::vector<ros_portal_config::To
 
       try {
         auto source = std::make_shared<livekit::VideoSource>(width, height);
-        auto track = participant->publishVideoTrack(topic_name, source, livekit::TrackSource::SOURCE_CAMERA);
+        auto track = livekit::LocalVideoTrack::createLocalVideoTrack(topic_name, source);
         if (!track) {
           return livekit::Result<std::shared_ptr<TopicForwarder::VideoTrackSink>, std::string>::failure(
-              "publishVideoTrack returned null track");
+              "createLocalVideoTrack returned null track");
         }
+        // Receivers restore the ROS header from this metadata, so inbound
+        // CompressedImage stamps stay on the sender's clock (see
+        // docs/configuration.md "Inbound video").
+        livekit::TrackPublishOptions publish_options;
+        publish_options.source = livekit::TrackSource::SOURCE_CAMERA;
+        publish_options.frame_metadata_features = livekit::FrameMetadataFeatures{};
+        publish_options.frame_metadata_features->user_timestamp = true;
+        publish_options.frame_metadata_features->user_data = true;
+        participant->publishTrack(track, publish_options);
+
         auto sink = std::make_shared<TopicForwarder::VideoTrackSink>();
         sink->width = width;
         sink->height = height;
-        sink->capture_frame = [operations_enabled = room_operations_enabled_, source = std::move(source),
-                               track = std::move(track)](const livekit::VideoFrame& frame, std::int64_t timestamp_us) {
-          if (!operations_enabled->load()) {
-            return;
-          }
-          (void)track;
-          source->captureFrame(frame, timestamp_us);
-        };
+        // The forwarder calls capture_frame under the topic's lock, so the
+        // reused capture options and cached frame_id need no extra locking.
+        livekit::VideoCaptureOptions capture_options;
+        capture_options.metadata.emplace();
+        sink->capture_frame =
+            [operations_enabled = room_operations_enabled_, source = std::move(source), track = std::move(track),
+             capture_options = std::move(capture_options), last_frame_id = std::optional<std::string>{}](
+                const livekit::VideoFrame& frame, const TopicForwarder::VideoFrameStamp& stamp) mutable {
+              if (!operations_enabled->load()) {
+                return;
+              }
+              (void)track;
+              auto& metadata = *capture_options.metadata;
+              capture_options.timestamp_us = stamp.timestamp_us;
+              metadata.user_timestamp_us =
+                  stamp.timestamp_us >= 0 ? std::optional<std::uint64_t>(static_cast<std::uint64_t>(stamp.timestamp_us))
+                                          : std::nullopt;
+              // Encode the frame_id only when it changes, so steady frames do not allocate.
+              if (stamp.frame_id != last_frame_id) {
+                last_frame_id = stamp.frame_id ? std::optional<std::string>(*stamp.frame_id) : std::nullopt;
+                metadata.user_data = stamp.frame_id ? utils::encodeFrameIdUserData(*stamp.frame_id) : std::nullopt;
+              }
+              source->captureFrame(frame, capture_options);
+            };
         return livekit::Result<std::shared_ptr<TopicForwarder::VideoTrackSink>, std::string>::success(std::move(sink));
       } catch (const std::exception& error) {
         return livekit::Result<std::shared_ptr<TopicForwarder::VideoTrackSink>, std::string>::failure(error.what());
diff --git a/src/ros_portal/src/topic_forwarder.cpp b/src/ros_portal/src/topic_forwarder.cpp
index 58b0dd9d..028df64e 100644
--- a/src/ros_portal/src/topic_forwarder.cpp
+++ b/src/ros_portal/src/topic_forwarder.cpp
@@ -26,6 +26,7 @@
 #include <exception>
 #include <rclcpp/generic_subscription.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <stdexcept>
 #include <string>
@@ -37,6 +38,7 @@
 #include "ros_portal/utils/image_conversion.hpp"
 #include "ros_portal/utils/ros_utils.hpp"
 #include "ros_portal/utils/topic_matcher.hpp"
+#include "ros_portal/utils/video_frame_metadata.hpp"
 
 namespace ros_portal {
 
@@ -234,6 +236,8 @@ std::optional<std::chrono::steady_clock::time_point> TopicForwarder::nextExpiryD
 void TopicForwarder::createSubscriber(const std::string& topic_name, const std::string& topic_type) {
   if (topic_type == kImageMsgType) {
     createImageSubscriber(topic_name);
+  } else if (topic_type == kCompressedImageMsgType) {
+    createCompressedImageSubscriber(topic_name);
   } else {
     createDataSubscriber(topic_name, topic_type);
   }
@@ -406,22 +410,22 @@ std::shared_ptr<TopicForwarder::ImageTopicState> TopicForwarder::findImageTopicS
 bool TopicForwarder::ensureVideoSinkLocked(ImageTopicState& state, const std::string& topic_name, const int width,
                                            const int height, const std::string& source_format) {
   if (!state.sink) {
-    const auto sink_result = livekit_methods_.publish_video_track(topic_name, width, height);
+    const auto sink_result = livekit_methods_.publish_video_track(state.track_name, width, height);
     if (!sink_result) {
-      RCLCPP_ERROR(logger_, "Failed to create LiveKit video track for '%s': %s", topic_name.c_str(),
-                   sink_result.error().c_str());
+      RCLCPP_ERROR(logger_, "Failed to create LiveKit video track '%s' for '%s': %s", state.track_name.c_str(),
+                   topic_name.c_str(), sink_result.error().c_str());
       return false;
     }
 
     state.sink = sink_result.value();
     if (!state.sink || !state.sink->capture_frame) {
-      RCLCPP_ERROR(logger_, "publish_video_track('%s') returned an invalid sink", topic_name.c_str());
+      RCLCPP_ERROR(logger_, "publish_video_track('%s') returned an invalid sink", state.track_name.c_str());
       state.sink.reset();
       return false;
     }
 
-    RCLCPP_INFO(logger_, "Created LiveKit video track '%s' (%dx%d, %s)", topic_name.c_str(), width, height,
-                source_format.c_str());
+    RCLCPP_INFO(logger_, "Created LiveKit video track '%s' from '%s' (%dx%d, %s)", state.track_name.c_str(),
+                topic_name.c_str(), width, height, source_format.c_str());
   }
 
   if (state.sink->width != width || state.sink->height != height) {
@@ -434,7 +438,25 @@ bool TopicForwarder::ensureVideoSinkLocked(ImageTopicState& state, const std::st
   return true;
 }
 
-void TopicForwarder::createImageSubscriber(const std::string& topic_name) {
+TopicForwarder::VideoFrameStamp TopicForwarder::makeVideoFrameStamp(const std_msgs::msg::Header& header,
+                                                                    const std::string& topic_name) {
+  VideoFrameStamp frame_stamp;
+  frame_stamp.timestamp_us =
+      static_cast<std::int64_t>(header.stamp.sec) * 1'000'000 + static_cast<std::int64_t>(header.stamp.nanosec) / 1'000;
+  if (header.frame_id.size() <= utils::kMaxFrameIdUserDataLength) {
+    frame_stamp.frame_id = header.frame_id;
+  } else {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000,
+                         "Not sending frame_id for '%s' because it is longer than %zu bytes; receivers will use a "
+                         "fallback frame_id",
+                         topic_name.c_str(), utils::kMaxFrameIdUserDataLength);
+  }
+  return frame_stamp;
+}
+
+template <typename MessageT, typename CallbackT>
+void TopicForwarder::subscribeVideoTopic(const std::string& topic_name, const char* topic_type, const bool compressed,
+                                         CallbackT&& callback) {
   const auto qos = determineQoS(topic_name);
   const auto node = node_.lock();
   if (!node) {
@@ -442,6 +464,43 @@ void TopicForwarder::createImageSubscriber(const std::string& topic_name) {
     return;
   }
 
+  const std::lock_guard<std::mutex> lock(outbound_topics_mutex_);
+  if (subscriptions_.count(topic_name) > 0) {
+    return;
+  }
+
+  auto state = std::make_shared<ImageTopicState>();
+  state->track_name = topic_name;
+  if (compressed) {
+    state->jpeg_decoder = std::make_unique<utils::JpegDecoder>();
+  }
+  image_topic_states_[topic_name] = state;
+
+  try {
+    rclcpp::SubscriptionOptions sub_options;
+    sub_options.callback_group = callback_group_;
+    if (shouldEnableRosTopicStats(topic_name, options_.ros_topic_stats_topic_patterns)) {
+      sub_options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
+      sub_options.topic_stats_options.publish_topic = utils::rosTopicStatisticsTopic(topic_name);
+      RCLCPP_INFO(logger_, "Enabled ROS 2 topic statistics for image topic '%s' on '%s'", topic_name.c_str(),
+                  sub_options.topic_stats_options.publish_topic.c_str());
+    }
+    auto subscription =
+        node->create_subscription<MessageT>(topic_name, qos, std::forward<CallbackT>(callback), sub_options);
+    subscriptions_[topic_name] = OutboundSubscription{std::move(subscription), std::nullopt};
+  } catch (const std::exception& e) {
+    image_topic_states_.erase(topic_name);
+    diagnostic_state_.outbound_failures.fetch_add(1, std::memory_order_relaxed);
+    RCLCPP_ERROR(logger_, "Failed to create ROS image subscription for '%s' [%s]: %s", topic_name.c_str(), topic_type,
+                 e.what());
+    return;
+  }
+
+  RCLCPP_INFO(logger_, "Subscribed to ROS image topic '%s' [%s] for LiveKit video track '%s'", topic_name.c_str(),
+              topic_type, state->track_name.c_str());
+}
+
+void TopicForwarder::createImageSubscriber(const std::string& topic_name) {
   // Taking ownership of the message lets packed rgba8/bgra8/rgb8 pixels move
   // into the LiveKit frame without a copy.
   auto callback = [this, topic_name](std::unique_ptr<sensor_msgs::msg::Image> msg,
@@ -459,9 +518,7 @@ void TopicForwarder::createImageSubscriber(const std::string& topic_name) {
       return;
     }
 
-    const auto& stamp = msg->header.stamp;
-    const std::int64_t timestamp_us =
-        static_cast<std::int64_t>(stamp.sec) * 1'000'000 + static_cast<std::int64_t>(stamp.nanosec) / 1'000;
+    const auto frame_stamp = makeVideoFrameStamp(msg->header, topic_name);
     const auto* frame = state->frame_builder.build(*msg);
     if (frame == nullptr) {
       RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000,
@@ -473,42 +530,45 @@ void TopicForwarder::createImageSubscriber(const std::string& topic_name) {
     }
     // state keeps the builder alive; state_lock prevents buffer reuse until
     // synchronous capture returns. LiveKit reads the pixels during this call.
-    state->sink->capture_frame(*frame, timestamp_us);
+    state->sink->capture_frame(*frame, frame_stamp);
   };
+  subscribeVideoTopic<sensor_msgs::msg::Image>(topic_name, kImageMsgType, false, std::move(callback));
+}
 
-  const std::lock_guard<std::mutex> lock(outbound_topics_mutex_);
-  if (subscriptions_.count(topic_name) > 0) {
-    return;
-  }
-
-  image_topic_states_[topic_name] = std::make_shared<ImageTopicState>();
-
-  try {
-    rclcpp::SubscriptionOptions sub_options;
-    sub_options.callback_group = callback_group_;
-    if (shouldEnableRosTopicStats(topic_name, options_.ros_topic_stats_topic_patterns)) {
-      sub_options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
-      sub_options.topic_stats_options.publish_topic = utils::rosTopicStatisticsTopic(topic_name);
-      RCLCPP_INFO(logger_, "Enabled ROS 2 topic statistics for image topic '%s' on '%s'", topic_name.c_str(),
-                  sub_options.topic_stats_options.publish_topic.c_str());
+void TopicForwarder::createCompressedImageSubscriber(const std::string& topic_name) {
+  auto callback = [this, topic_name](const sensor_msgs::msg::CompressedImage::ConstSharedPtr& msg,
+                                     const rclcpp::MessageInfo& message_info) {
+    if (isInboundPublication(message_info) || !livekit_methods_.is_room_available()) {
+      return;
     }
-    auto subscription =
-        node->create_subscription<sensor_msgs::msg::Image>(topic_name, qos, std::move(callback), sub_options);
-    subscriptions_[topic_name] = OutboundSubscription{std::move(subscription), std::nullopt};
-  } catch (const std::exception& e) {
-    image_topic_states_.erase(topic_name);
-    RCLCPP_ERROR(logger_, "Failed to create ROS image subscription for '%s' [%s]: %s", topic_name.c_str(),
-                 kImageMsgType, e.what());
-    return;
-  } catch (...) {
-    image_topic_states_.erase(topic_name);
-    diagnostic_state_.outbound_failures.fetch_add(1, std::memory_order_relaxed);
-    RCLCPP_ERROR(logger_, "Unknown exception creating image subscription for '%s' [%s]", topic_name.c_str(),
-                 kImageMsgType);
-    return;
-  }
+    if (!utils::isJpegFormat(msg->format)) {
+      diagnostic_state_.outbound_failures.fetch_add(1, std::memory_order_relaxed);
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000,
+                           "Skipping compressed image on '%s': format '%s' is not JPEG, the only supported format",
+                           topic_name.c_str(), msg->format.c_str());
+      return;
+    }
+    const auto state = findImageTopicState(topic_name);
+    if (!state || !state->jpeg_decoder) {
+      return;
+    }
+    const std::lock_guard<std::mutex> state_lock(state->mutex);
 
-  RCLCPP_INFO(logger_, "Subscribed to ROS image topic '%s' [%s]", topic_name.c_str(), kImageMsgType);
+    // Decode first: the JPEG header is the only source of the frame size.
+    const auto* frame = state->jpeg_decoder->decode(msg->data.data(), msg->data.size());
+    if (frame == nullptr) {
+      diagnostic_state_.outbound_failures.fetch_add(1, std::memory_order_relaxed);
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Dropping compressed image on '%s': JPEG decode failed: %s",
+                           topic_name.c_str(), state->jpeg_decoder->lastError().c_str());
+      return;
+    }
+    if (!ensureVideoSinkLocked(*state, topic_name, frame->width(), frame->height(), msg->format)) {
+      return;
+    }
+    state->sink->capture_frame(*frame, makeVideoFrameStamp(msg->header, topic_name));
+  };
+  subscribeVideoTopic<sensor_msgs::msg::CompressedImage>(topic_name, kCompressedImageMsgType, true,
+                                                         std::move(callback));
 }
 
 void TopicForwarder::onDataTrackPublished(std::shared_ptr<livekit::RemoteDataTrack> track) {
diff --git a/src/ros_portal/src/utils/jpeg_decoder.cpp b/src/ros_portal/src/utils/jpeg_decoder.cpp
new file mode 100644
index 00000000..e4bf70dd
--- /dev/null
+++ b/src/ros_portal/src/utils/jpeg_decoder.cpp
@@ -0,0 +1,114 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#include "ros_portal/utils/jpeg_decoder.hpp"
+
+#include <turbojpeg.h>
+
+#include <algorithm>
+#include <cctype>
+#include <cstring>
+#include <limits>
+
+namespace ros_portal::utils {
+
+namespace {
+
+/// @brief Neutral chroma value for grayscale I420 frames.
+constexpr std::uint8_t kNeutralChroma = 128U;
+
+bool containsIgnoreCase(std::string_view text, std::string_view needle) {
+  return std::search(text.begin(), text.end(), needle.begin(), needle.end(), [](const char lhs, const char rhs) {
+           return std::tolower(static_cast<unsigned char>(lhs)) == std::tolower(static_cast<unsigned char>(rhs));
+         }) != text.end();
+}
+
+} // namespace
+
+bool isJpegFormat(std::string_view format) {
+  return containsIgnoreCase(format, "jpeg") || containsIgnoreCase(format, "jpg");
+}
+
+JpegDecoder::JpegDecoder() : handle_(tjInitDecompress()) {}
+
+JpegDecoder::~JpegDecoder() {
+  if (handle_ != nullptr) {
+    tjDestroy(handle_);
+  }
+}
+
+const livekit::VideoFrame* JpegDecoder::decode(const std::uint8_t* jpeg, const std::size_t size) {
+  if (handle_ == nullptr) {
+    last_error_ = "TurboJPEG decompressor is unavailable";
+    return nullptr;
+  }
+  if (jpeg == nullptr || size == 0U || size > std::numeric_limits<unsigned long>::max()) {
+    last_error_ = "JPEG data is empty";
+    return nullptr;
+  }
+
+  const auto jpeg_size = static_cast<unsigned long>(size);
+  int width = 0;
+  int height = 0;
+  int subsampling = 0;
+  int colorspace = 0;
+  if (tjDecompressHeader3(handle_, jpeg, jpeg_size, &width, &height, &subsampling, &colorspace) != 0) {
+    last_error_ = tjGetErrorStr2(handle_);
+    return nullptr;
+  }
+  if (width <= 0 || height <= 0) {
+    last_error_ = "JPEG has no pixels";
+    return nullptr;
+  }
+
+  bool fresh = false;
+  int result = 0;
+  if (subsampling == TJSAMP_420 || subsampling == TJSAMP_GRAY) {
+    auto& frame = reuse(width, height, livekit::VideoBufferType::I420, fresh);
+    const int chroma_width = (width + 1) / 2;
+    const auto luma_size = static_cast<std::size_t>(width) * static_cast<std::size_t>(height);
+    const auto chroma_size = static_cast<std::size_t>(chroma_width) * static_cast<std::size_t>((height + 1) / 2);
+    if (fresh && subsampling == TJSAMP_GRAY) {
+      // Grayscale JPEGs only write the Y plane.
+      std::memset(frame.data() + luma_size, kNeutralChroma, 2U * chroma_size);
+    }
+    unsigned char* planes[3] = {frame.data(), frame.data() + luma_size, frame.data() + luma_size + chroma_size};
+    int strides[3] = {width, chroma_width, chroma_width};
+    result = tjDecompressToYUVPlanes(handle_, jpeg, jpeg_size, planes, width, strides, height, 0);
+  } else {
+    auto& frame = reuse(width, height, livekit::VideoBufferType::RGB24, fresh);
+    result = tjDecompress2(handle_, jpeg, jpeg_size, frame.data(), width, width * 3, height, TJPF_RGB, 0);
+  }
+
+  // A warning means the image decoded only partly, for example from a
+  // truncated JPEG. Drop it rather than send gray or garbled rows.
+  if (result != 0) {
+    last_error_ = tjGetErrorStr2(handle_);
+    return nullptr;
+  }
+  return &frame_;
+}
+
+livekit::VideoFrame& JpegDecoder::reuse(const int width, const int height, const livekit::VideoBufferType type,
+                                        bool& fresh) {
+  fresh = frame_.width() != width || frame_.height() != height || frame_.type() != type || frame_.dataSize() == 0U;
+  if (fresh) {
+    frame_ = livekit::VideoFrame::create(width, height, type);
+  }
+  return frame_;
+}
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/src/utils/jpeg_encoder.cpp b/src/ros_portal/src/utils/jpeg_encoder.cpp
new file mode 100644
index 00000000..d5cdffa6
--- /dev/null
+++ b/src/ros_portal/src/utils/jpeg_encoder.cpp
@@ -0,0 +1,96 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#include "ros_portal/utils/jpeg_encoder.hpp"
+
+#include <turbojpeg.h>
+
+#include <algorithm>
+#include <cstddef>
+#include <limits>
+#include <string>
+
+namespace ros_portal::utils {
+
+void JpegEncoder::BufferDeleter::operator()(unsigned char* buffer) const noexcept { tjFree(buffer); }
+
+JpegEncoder::JpegEncoder(const int quality) : handle_(tjInitCompress()), quality_(std::clamp(quality, 1, 100)) {}
+
+JpegEncoder::~JpegEncoder() {
+  if (handle_ != nullptr) {
+    tjDestroy(handle_);
+  }
+}
+
+bool JpegEncoder::encodeI420(const livekit::VideoFrame& frame, std::vector<std::uint8_t>& out) {
+  if (handle_ == nullptr) {
+    last_error_ = "TurboJPEG compressor is unavailable";
+    return false;
+  }
+  if (frame.type() != livekit::VideoBufferType::I420) {
+    last_error_ = "frame is not I420";
+    return false;
+  }
+
+  const int width = frame.width();
+  const int height = frame.height();
+  if (width <= 0 || height <= 0) {
+    last_error_ = "frame has no pixels";
+    return false;
+  }
+
+  const auto luma_size = static_cast<std::size_t>(width) * static_cast<std::size_t>(height);
+  const int chroma_width = (width + 1) / 2;
+  const auto chroma_size = static_cast<std::size_t>(chroma_width) * static_cast<std::size_t>((height + 1) / 2);
+  if (frame.dataSize() != luma_size + 2U * chroma_size) {
+    last_error_ = "frame buffer size " + std::to_string(frame.dataSize()) + " does not match packed I420 " +
+                  std::to_string(width) + "x" + std::to_string(height);
+    return false;
+  }
+
+  const unsigned long required_capacity = tjBufSize(width, height, TJSAMP_420);
+  // tjBufSize() returns (unsigned long)-1 on error, and tjAlloc() takes an int.
+  if (required_capacity > static_cast<unsigned long>(std::numeric_limits<int>::max())) {
+    last_error_ = "frame is too large to encode";
+    return false;
+  }
+  if (buffer_capacity_ < required_capacity) {
+    std::unique_ptr<unsigned char[], BufferDeleter> new_buffer(tjAlloc(static_cast<int>(required_capacity)));
+    if (!new_buffer) {
+      last_error_ = "failed to allocate the JPEG output buffer";
+      return false;
+    }
+    buffer_.swap(new_buffer);
+    buffer_capacity_ = required_capacity;
+  }
+
+  const std::uint8_t* luma = frame.data();
+  const unsigned char* planes[3] = {luma, luma + luma_size, luma + luma_size + chroma_size};
+  const int strides[3] = {width, chroma_width, chroma_width};
+  unsigned char* jpeg = buffer_.get();
+  unsigned long jpeg_size = buffer_capacity_;
+  // NOREALLOC: the buffer already holds tjBufSize() bytes, the worst case for this frame size.
+  if (tjCompressFromYUVPlanes(handle_, planes, width, strides, height, TJSAMP_420, &jpeg, &jpeg_size, quality_,
+                              TJFLAG_NOREALLOC) != 0) {
+    last_error_ = tjGetErrorStr2(handle_);
+    return false;
+  }
+
+  out.assign(jpeg, jpeg + jpeg_size);
+  return true;
+}
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/src/utils/ros_utils.cpp b/src/ros_portal/src/utils/ros_utils.cpp
index 916876d1..71e26aa5 100644
--- a/src/ros_portal/src/utils/ros_utils.cpp
+++ b/src/ros_portal/src/utils/ros_utils.cpp
@@ -21,6 +21,7 @@
 #include <cstring>
 #include <exception>
 #include <rclcpp/rclcpp.hpp>
+#include <string_view>
 
 namespace ros_portal::utils {
 
@@ -228,4 +229,5 @@ bool isRmwBufferTopic(const std::string& topic_name) {
   const auto segment_start = topic_name.rfind('/') + 1U;
   return topic_name.compare(segment_start, sizeof(kBufferSegmentPrefix) - 1U, kBufferSegmentPrefix) == 0;
 }
+
 } // namespace ros_portal::utils
diff --git a/src/ros_portal/src/utils/video_frame_metadata.cpp b/src/ros_portal/src/utils/video_frame_metadata.cpp
new file mode 100644
index 00000000..260b0976
--- /dev/null
+++ b/src/ros_portal/src/utils/video_frame_metadata.cpp
@@ -0,0 +1,41 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#include "ros_portal/utils/video_frame_metadata.hpp"
+
+namespace ros_portal::utils {
+
+std::optional<std::vector<std::uint8_t>> encodeFrameIdUserData(std::string_view frame_id) {
+  if (frame_id.size() > kMaxFrameIdUserDataLength) {
+    return std::nullopt;
+  }
+
+  std::vector<std::uint8_t> user_data;
+  user_data.reserve(frame_id.size() + 1U);
+  user_data.push_back(kFrameIdUserDataVersion);
+  user_data.insert(user_data.end(), frame_id.begin(), frame_id.end());
+  return user_data;
+}
+
+std::optional<std::string_view> decodeFrameIdUserData(const std::vector<std::uint8_t>& user_data) {
+  if (user_data.empty() || user_data.front() != kFrameIdUserDataVersion ||
+      user_data.size() - 1U > kMaxFrameIdUserDataLength) {
+    return std::nullopt;
+  }
+  return std::string_view(reinterpret_cast<const char*>(user_data.data() + 1), user_data.size() - 1U);
+}
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/test/unit/jpeg_decoder_test.cpp b/src/ros_portal/test/unit/jpeg_decoder_test.cpp
new file mode 100644
index 00000000..f0b6a8df
--- /dev/null
+++ b/src/ros_portal/test/unit/jpeg_decoder_test.cpp
@@ -0,0 +1,150 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#include "ros_portal/utils/jpeg_decoder.hpp"
+
+#include <gtest/gtest.h>
+#include <turbojpeg.h>
+
+#include <cstddef>
+#include <cstdint>
+#include <cstdlib>
+#include <cstring>
+#include <vector>
+
+#include "ros_portal/utils/jpeg_encoder.hpp"
+
+namespace ros_portal::utils {
+namespace {
+
+std::vector<std::uint8_t> encodeI420(const int width, const int height, const std::uint8_t y, const std::uint8_t u,
+                                     const std::uint8_t v) {
+  auto frame = livekit::VideoFrame::create(width, height, livekit::VideoBufferType::I420);
+  const auto luma_size = static_cast<std::size_t>(width) * static_cast<std::size_t>(height);
+  const auto chroma_size = static_cast<std::size_t>((width + 1) / 2) * static_cast<std::size_t>((height + 1) / 2);
+  std::memset(frame.data(), y, luma_size);
+  std::memset(frame.data() + luma_size, u, chroma_size);
+  std::memset(frame.data() + luma_size + chroma_size, v, chroma_size);
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg;
+  EXPECT_TRUE(encoder.encodeI420(frame, jpeg));
+  return jpeg;
+}
+
+/// Encode a solid RGB image with the given chroma subsampling.
+std::vector<std::uint8_t> encodeRgb(const int width, const int height, const int subsampling, const std::uint8_t r,
+                                    const std::uint8_t g, const std::uint8_t b) {
+  std::vector<std::uint8_t> rgb(static_cast<std::size_t>(width) * static_cast<std::size_t>(height) * 3U);
+  for (std::size_t i = 0; i < rgb.size(); i += 3U) {
+    rgb[i] = r;
+    rgb[i + 1U] = g;
+    rgb[i + 2U] = b;
+  }
+  tjhandle compressor = tjInitCompress();
+  unsigned char* jpeg = nullptr;
+  unsigned long jpeg_size = 0;
+  EXPECT_EQ(tjCompress2(compressor, rgb.data(), width, 0, height, TJPF_RGB, &jpeg, &jpeg_size, subsampling, 95, 0), 0);
+  std::vector<std::uint8_t> out(jpeg, jpeg + jpeg_size);
+  tjFree(jpeg);
+  tjDestroy(compressor);
+  return out;
+}
+
+} // namespace
+
+TEST(JpegDecoderTest, RecognizesJpegFormats) {
+  EXPECT_TRUE(isJpegFormat("jpeg"));
+  EXPECT_TRUE(isJpegFormat("JPG"));
+  EXPECT_TRUE(isJpegFormat("bgr8; jpeg compressed bgr8"));
+  EXPECT_FALSE(isJpegFormat("png"));
+  EXPECT_FALSE(isJpegFormat("bgr8; png compressed bgr8"));
+  EXPECT_FALSE(isJpegFormat(""));
+}
+
+TEST(JpegDecoderTest, Decodes420JpegToI420WithSourceSize) {
+  const auto jpeg = encodeI420(33, 17, 100, 128, 128);
+  JpegDecoder decoder;
+
+  const auto* frame = decoder.decode(jpeg.data(), jpeg.size());
+
+  ASSERT_NE(frame, nullptr) << decoder.lastError();
+  EXPECT_EQ(frame->type(), livekit::VideoBufferType::I420);
+  EXPECT_EQ(frame->width(), 33);
+  EXPECT_EQ(frame->height(), 17);
+  EXPECT_NEAR(frame->data()[0], 100, 3);
+}
+
+TEST(JpegDecoderTest, Decodes444JpegToRgb24WithColorKept) {
+  const auto jpeg = encodeRgb(16, 16, TJSAMP_444, 255, 0, 0);
+  JpegDecoder decoder;
+
+  const auto* frame = decoder.decode(jpeg.data(), jpeg.size());
+
+  ASSERT_NE(frame, nullptr) << decoder.lastError();
+  EXPECT_EQ(frame->type(), livekit::VideoBufferType::RGB24);
+  const std::size_t center = (8U * 16U + 8U) * 3U;
+  EXPECT_GT(frame->data()[center], 240);
+  EXPECT_LT(frame->data()[center + 1U], 15);
+  EXPECT_LT(frame->data()[center + 2U], 15);
+}
+
+TEST(JpegDecoderTest, DecodesGrayscaleJpegToI420WithNeutralChroma) {
+  std::vector<std::uint8_t> gray(16U * 16U, 90U);
+  tjhandle compressor = tjInitCompress();
+  unsigned char* jpeg = nullptr;
+  unsigned long jpeg_size = 0;
+  ASSERT_EQ(tjCompress2(compressor, gray.data(), 16, 0, 16, TJPF_GRAY, &jpeg, &jpeg_size, TJSAMP_GRAY, 95, 0), 0);
+  const std::vector<std::uint8_t> data(jpeg, jpeg + jpeg_size);
+  tjFree(jpeg);
+  tjDestroy(compressor);
+  JpegDecoder decoder;
+
+  const auto* frame = decoder.decode(data.data(), data.size());
+
+  ASSERT_NE(frame, nullptr) << decoder.lastError();
+  EXPECT_EQ(frame->type(), livekit::VideoBufferType::I420);
+  EXPECT_NEAR(frame->data()[0], 90, 3);
+  EXPECT_EQ(frame->data()[256], 128);
+  EXPECT_EQ(frame->data()[frame->dataSize() - 1U], 128);
+}
+
+TEST(JpegDecoderTest, ReusesOutputFrameForSameSize) {
+  const auto jpeg = encodeI420(32, 32, 100, 128, 128);
+  JpegDecoder decoder;
+
+  const auto* first = decoder.decode(jpeg.data(), jpeg.size());
+  ASSERT_NE(first, nullptr);
+  const auto* first_pixels = first->data();
+  const auto* second = decoder.decode(jpeg.data(), jpeg.size());
+
+  ASSERT_NE(second, nullptr);
+  EXPECT_EQ(second->data(), first_pixels);
+}
+
+TEST(JpegDecoderTest, RejectsInvalidAndTruncatedData) {
+  JpegDecoder decoder;
+  const std::vector<std::uint8_t> garbage{1, 2, 3, 4, 5};
+  EXPECT_EQ(decoder.decode(garbage.data(), garbage.size()), nullptr);
+  EXPECT_FALSE(decoder.lastError().empty());
+  EXPECT_EQ(decoder.decode(nullptr, 0), nullptr);
+
+  // A truncated JPEG decodes only with a warning, so it must be rejected.
+  auto jpeg = encodeRgb(64, 64, TJSAMP_420, 10, 200, 30);
+  jpeg.resize(jpeg.size() / 2U);
+  EXPECT_EQ(decoder.decode(jpeg.data(), jpeg.size()), nullptr);
+}
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/test/unit/jpeg_encoder_test.cpp b/src/ros_portal/test/unit/jpeg_encoder_test.cpp
new file mode 100644
index 00000000..b909eb8d
--- /dev/null
+++ b/src/ros_portal/test/unit/jpeg_encoder_test.cpp
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#include "ros_portal/utils/jpeg_encoder.hpp"
+
+#include <gtest/gtest.h>
+#include <turbojpeg.h>
+
+#include <cstddef>
+#include <cstdint>
+#include <cstring>
+#include <string>
+#include <vector>
+
+namespace ros_portal::utils {
+namespace {
+
+/// Build a packed I420 frame filled with one YUV color.
+livekit::VideoFrame makeI420Frame(const int width, const int height, const std::uint8_t y, const std::uint8_t u,
+                                  const std::uint8_t v) {
+  auto frame = livekit::VideoFrame::create(width, height, livekit::VideoBufferType::I420);
+  const auto luma_size = static_cast<std::size_t>(width) * static_cast<std::size_t>(height);
+  const auto chroma_size = static_cast<std::size_t>((width + 1) / 2) * static_cast<std::size_t>((height + 1) / 2);
+  std::memset(frame.data(), y, luma_size);
+  std::memset(frame.data() + luma_size, u, chroma_size);
+  std::memset(frame.data() + luma_size + chroma_size, v, chroma_size);
+  return frame;
+}
+
+struct JpegHeader {
+  int width{0};
+  int height{0};
+  int subsampling{-1};
+};
+
+JpegHeader readHeader(const std::vector<std::uint8_t>& jpeg) {
+  JpegHeader header;
+  int colorspace = 0;
+  tjhandle decompressor = tjInitDecompress();
+  EXPECT_EQ(tjDecompressHeader3(decompressor, jpeg.data(), static_cast<unsigned long>(jpeg.size()), &header.width,
+                                &header.height, &header.subsampling, &colorspace),
+            0);
+  tjDestroy(decompressor);
+  return header;
+}
+
+std::vector<std::uint8_t> decodeRgb(const std::vector<std::uint8_t>& jpeg, const int width, const int height) {
+  std::vector<std::uint8_t> rgb(static_cast<std::size_t>(width) * static_cast<std::size_t>(height) * 3U);
+  tjhandle decompressor = tjInitDecompress();
+  EXPECT_EQ(tjDecompress2(decompressor, jpeg.data(), static_cast<unsigned long>(jpeg.size()), rgb.data(), width, 0,
+                          height, TJPF_RGB, 0),
+            0);
+  tjDestroy(decompressor);
+  return rgb;
+}
+
+} // namespace
+
+TEST(JpegEncoderTest, EncodesI420FrameAsJpegWithSourceGeometry) {
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg;
+
+  ASSERT_TRUE(encoder.encodeI420(makeI420Frame(64, 48, 128, 128, 128), jpeg)) << encoder.lastError();
+
+  ASSERT_GE(jpeg.size(), 4U);
+  EXPECT_EQ(jpeg[0], 0xFF);
+  EXPECT_EQ(jpeg[1], 0xD8);
+  EXPECT_EQ(jpeg[jpeg.size() - 2U], 0xFF);
+  EXPECT_EQ(jpeg[jpeg.size() - 1U], 0xD9);
+  const auto header = readHeader(jpeg);
+  EXPECT_EQ(header.width, 64);
+  EXPECT_EQ(header.height, 48);
+  EXPECT_EQ(header.subsampling, TJSAMP_420);
+}
+
+// BT.601 YUV (81, 90, 240) is red. Swapped U and V planes would decode as blue.
+TEST(JpegEncoderTest, KeepsChromaPlanesInOrder) {
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg;
+
+  ASSERT_TRUE(encoder.encodeI420(makeI420Frame(32, 32, 81, 90, 240), jpeg)) << encoder.lastError();
+
+  const auto rgb = decodeRgb(jpeg, 32, 32);
+  const std::size_t center = (std::size_t{16} * 32U + 16U) * 3U;
+  EXPECT_GT(rgb[center], 200);
+  EXPECT_LT(rgb[center + 1U], 60);
+  EXPECT_LT(rgb[center + 2U], 60);
+}
+
+TEST(JpegEncoderTest, EncodesOddDimensions) {
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg;
+
+  ASSERT_TRUE(encoder.encodeI420(makeI420Frame(33, 17, 100, 128, 128), jpeg)) << encoder.lastError();
+
+  const auto header = readHeader(jpeg);
+  EXPECT_EQ(header.width, 33);
+  EXPECT_EQ(header.height, 17);
+}
+
+TEST(JpegEncoderTest, RejectsNonI420FrameAndLeavesOutputUnchanged) {
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg{1, 2, 3};
+
+  EXPECT_FALSE(encoder.encodeI420(livekit::VideoFrame::create(8, 8, livekit::VideoBufferType::RGBA), jpeg));
+
+  EXPECT_EQ(jpeg, (std::vector<std::uint8_t>{1, 2, 3}));
+  EXPECT_FALSE(encoder.lastError().empty());
+}
+
+TEST(JpegEncoderTest, RejectsI420BufferWithUnexpectedSize) {
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg;
+  // Row padding would make the buffer larger than the packed layout.
+  const livekit::VideoFrame padded(8, 8, livekit::VideoBufferType::I420, std::vector<std::uint8_t>(200U, 0U));
+
+  EXPECT_FALSE(encoder.encodeI420(padded, jpeg));
+  EXPECT_TRUE(jpeg.empty());
+  EXPECT_NE(encoder.lastError().find("does not match"), std::string::npos);
+}
+
+TEST(JpegEncoderTest, ReusesOutputStorageForSameSizeFrames) {
+  JpegEncoder encoder;
+  std::vector<std::uint8_t> jpeg;
+  const auto frame = makeI420Frame(64, 64, 128, 128, 128);
+
+  ASSERT_TRUE(encoder.encodeI420(frame, jpeg));
+  const auto* first_storage = jpeg.data();
+  const auto first_size = jpeg.size();
+  ASSERT_TRUE(encoder.encodeI420(frame, jpeg));
+
+  EXPECT_EQ(jpeg.size(), first_size);
+  EXPECT_EQ(jpeg.data(), first_storage);
+}
+
+TEST(JpegEncoderTest, HandlesBufferGrowthAndRejectedFramesAcrossLifetimes) {
+  const auto small_frame = makeI420Frame(32, 32, 100, 128, 128);
+  const auto large_frame = makeI420Frame(128, 96, 100, 128, 128);
+  const auto invalid_frame = livekit::VideoFrame::create(8, 8, livekit::VideoBufferType::RGBA);
+
+  for (int iteration = 0; iteration < 8; ++iteration) {
+    SCOPED_TRACE(iteration);
+    JpegEncoder encoder;
+    std::vector<std::uint8_t> jpeg;
+
+    ASSERT_TRUE(encoder.encodeI420(small_frame, jpeg)) << encoder.lastError();
+    ASSERT_TRUE(encoder.encodeI420(large_frame, jpeg)) << encoder.lastError();
+    const auto large_header = readHeader(jpeg);
+    EXPECT_EQ(large_header.width, 128);
+    EXPECT_EQ(large_header.height, 96);
+
+    const auto previous_jpeg = jpeg;
+    EXPECT_FALSE(encoder.encodeI420(invalid_frame, jpeg));
+    EXPECT_EQ(jpeg, previous_jpeg);
+
+    ASSERT_TRUE(encoder.encodeI420(small_frame, jpeg)) << encoder.lastError();
+    const auto small_header = readHeader(jpeg);
+    EXPECT_EQ(small_header.width, 32);
+    EXPECT_EQ(small_header.height, 32);
+  }
+}
+
+} // namespace ros_portal::utils
diff --git a/src/ros_portal/test/unit/ros_utils_test.cpp b/src/ros_portal/test/unit/ros_utils_test.cpp
index fbb6467b..45ac413f 100644
--- a/src/ros_portal/test/unit/ros_utils_test.cpp
+++ b/src/ros_portal/test/unit/ros_utils_test.cpp
@@ -411,5 +411,6 @@ TEST(RosUtilsTest, IsRmwBufferTopicMatchesBufferBackendSegment) {
   EXPECT_FALSE(isRmwBufferTopic("/fibonacci/_action/feedback"));
   EXPECT_FALSE(isRmwBufferTopic(""));
 }
+
 } // namespace
 } // namespace ros_portal::utils
diff --git a/src/ros_portal/test/unit/topic_forwarder_test.cpp b/src/ros_portal/test/unit/topic_forwarder_test.cpp
index cb8c180c..16d1e13d 100644
--- a/src/ros_portal/test/unit/topic_forwarder_test.cpp
+++ b/src/ros_portal/test/unit/topic_forwarder_test.cpp
@@ -31,6 +31,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/serialization.hpp>
 #include <rclcpp/serialized_message.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <stdexcept>
 #include <string>
@@ -41,7 +43,9 @@
 #include "diagnostics_test_utils.hpp"
 #include "ros_portal/diagnostics/diagnostics_fns.hpp"
 #include "ros_portal/schema/manager.hpp"
+#include "ros_portal/utils/jpeg_encoder.hpp"
 #include "ros_portal/utils/topic_matcher.hpp"
+#include "ros_portal/utils/video_frame_metadata.hpp"
 
 // TopicForwarder now creates its subscriptions and publishers directly on the
 // ROS node it is given, so these unit tests cover only the node-independent
@@ -872,4 +876,164 @@ TEST_F(TopicForwarderTest, DiagnosticsReportInboundRejectionsAndStoppedReaders)
   EXPECT_FALSE(valueFor(status, "inbound.last_terminal_error").has_value());
 }
 
+namespace {
+
+// Records what the outbound video sink receives for each captured frame.
+struct VideoSinkRecord {
+  std::mutex mutex;
+  int captures{0};
+  std::int64_t timestamp_us{0};
+  std::optional<std::string> frame_id;
+  std::vector<std::string> track_names;
+  int width{0};
+  int height{0};
+  livekit::VideoBufferType frame_type{livekit::VideoBufferType::RGBA};
+};
+
+TopicForwarder::LiveKitMethods makeVideoRecordingLiveKitMethods(std::shared_ptr<VideoSinkRecord> record) {
+  auto livekit_methods = makeLiveKitMethods();
+  livekit_methods.publish_video_track =
+      [record](const std::string& track_name, int width,
+               int height) -> livekit::Result<std::shared_ptr<TopicForwarder::VideoTrackSink>, std::string> {
+    {
+      const std::lock_guard<std::mutex> lock(record->mutex);
+      record->track_names.push_back(track_name);
+      record->width = width;
+      record->height = height;
+    }
+    auto sink = std::make_shared<TopicForwarder::VideoTrackSink>();
+    sink->width = width;
+    sink->height = height;
+    sink->capture_frame = [record](const livekit::VideoFrame& frame, const TopicForwarder::VideoFrameStamp& stamp) {
+      const std::lock_guard<std::mutex> lock(record->mutex);
+      ++record->captures;
+      record->frame_type = frame.type();
+      record->timestamp_us = stamp.timestamp_us;
+      record->frame_id = stamp.frame_id ? std::optional<std::string>(*stamp.frame_id) : std::nullopt;
+    };
+    return livekit::Result<std::shared_ptr<TopicForwarder::VideoTrackSink>, std::string>::success(std::move(sink));
+  };
+  return livekit_methods;
+}
+
+sensor_msgs::msg::Image makeRgbImage(const std::string& frame_id) {
+  sensor_msgs::msg::Image image;
+  image.header.stamp.sec = 12;
+  image.header.stamp.nanosec = 345'678'000U;
+  image.header.frame_id = frame_id;
+  image.width = 2;
+  image.height = 2;
+  image.encoding = "rgb8";
+  image.step = 6;
+  image.data.assign(12U, 128U);
+  return image;
+}
+
+} // namespace
+
+TEST_F(TopicForwarderTest, OutboundVideoSendsImageStampAndFrameId) {
+  auto record = std::make_shared<VideoSinkRecord>();
+  TopicForwarder forwarder(makeOptions(), node_, makeVideoRecordingLiveKitMethods(record), diagnostics_fns_);
+
+  rclcpp::QoS pub_qos{rclcpp::KeepLast(10)};
+  pub_qos.reliable();
+  auto publisher = node_->create_publisher<sensor_msgs::msg::Image>("/allowed/image", pub_qos);
+  ASSERT_TRUE(waitForPublishers("/allowed/image", 1U));
+  reconcileFromGraph(forwarder);
+  ASSERT_TRUE(spinUntil([&]() { return publisher->get_subscription_count() >= 1U; }));
+
+  publisher->publish(makeRgbImage("camera_optical_frame"));
+  ASSERT_TRUE(spinUntil([&]() {
+    const std::lock_guard<std::mutex> lock(record->mutex);
+    return record->captures == 1;
+  }));
+  {
+    const std::lock_guard<std::mutex> lock(record->mutex);
+    EXPECT_EQ(record->timestamp_us, 12'345'678);
+    EXPECT_EQ(record->frame_id, "camera_optical_frame");
+  }
+
+  // A frame_id too long for the metadata is left out, not truncated.
+  publisher->publish(makeRgbImage(std::string(utils::kMaxFrameIdUserDataLength + 1U, 'a')));
+  ASSERT_TRUE(spinUntil([&]() {
+    const std::lock_guard<std::mutex> lock(record->mutex);
+    return record->captures == 2;
+  }));
+  const std::lock_guard<std::mutex> lock(record->mutex);
+  EXPECT_FALSE(record->frame_id.has_value());
+}
+
+namespace {
+
+// A JPEG CompressedImage of a 16x8 gray frame, as image_transport publishes it.
+sensor_msgs::msg::CompressedImage makeJpegImage(const std::string& format) {
+  auto i420 = livekit::VideoFrame::create(16, 8, livekit::VideoBufferType::I420);
+  std::fill(i420.data(), i420.data() + i420.dataSize(), std::uint8_t{128});
+  utils::JpegEncoder encoder;
+  sensor_msgs::msg::CompressedImage image;
+  EXPECT_TRUE(encoder.encodeI420(i420, image.data));
+  image.header.stamp.sec = 1'769'886'345;
+  image.header.stamp.nanosec = 745'156'000U;
+  image.header.frame_id = "cam0";
+  image.format = format;
+  return image;
+}
+
+} // namespace
+
+TEST_F(TopicForwarderTest, OutboundCompressedImagePreservesTopicName) {
+  auto record = std::make_shared<VideoSinkRecord>();
+  TopicForwarder forwarder(makeOptions(), node_, makeVideoRecordingLiveKitMethods(record), diagnostics_fns_);
+
+  auto publisher = node_->create_publisher<sensor_msgs::msg::CompressedImage>("/allowed/cam/image_raw/compressed", 10);
+  ASSERT_TRUE(waitForPublishers("/allowed/cam/image_raw/compressed", 1U));
+  reconcileFromGraph(forwarder);
+  ASSERT_TRUE(spinUntil([&]() { return publisher->get_subscription_count() >= 1U; }));
+
+  publisher->publish(makeJpegImage("bgr8; jpeg compressed bgr8"));
+  ASSERT_TRUE(spinUntil([&]() {
+    const std::lock_guard<std::mutex> lock(record->mutex);
+    return record->captures == 1;
+  }));
+  const std::lock_guard<std::mutex> lock(record->mutex);
+  EXPECT_EQ(record->track_names, (std::vector<std::string>{"/allowed/cam/image_raw/compressed"}));
+  EXPECT_EQ(record->width, 16);
+  EXPECT_EQ(record->height, 8);
+  // 4:2:0 JPEG decodes straight to I420.
+  EXPECT_EQ(record->frame_type, livekit::VideoBufferType::I420);
+  EXPECT_EQ(record->timestamp_us, 1'769'886'345'745'156);
+  EXPECT_EQ(record->frame_id, "cam0");
+}
+
+TEST_F(TopicForwarderTest, OutboundCompressedImageSkipsNonJpegFormats) {
+  auto record = std::make_shared<VideoSinkRecord>();
+  TopicForwarder forwarder(makeOptions(), node_, makeVideoRecordingLiveKitMethods(record), diagnostics_fns_);
+
+  auto publisher = node_->create_publisher<sensor_msgs::msg::CompressedImage>("/allowed/cam/compressed", 10);
+  ASSERT_TRUE(waitForPublishers("/allowed/cam/compressed", 1U));
+  reconcileFromGraph(forwarder);
+  ASSERT_TRUE(spinUntil([&]() { return publisher->get_subscription_count() >= 1U; }));
+
+  publisher->publish(makeJpegImage("png"));
+  ASSERT_TRUE(spinUntil([&]() { return forwarder.diagnostic_state_.outbound_failures.load() >= 1U; }));
+  const std::lock_guard<std::mutex> lock(record->mutex);
+  EXPECT_EQ(record->captures, 0);
+  EXPECT_TRUE(record->track_names.empty());
+}
+
+TEST_F(TopicForwarderTest, OutboundRawAndCompressedImagesHaveDistinctTrackNames) {
+  TopicForwarder forwarder(makeOptions(), node_, makeLiveKitMethods(), diagnostics_fns_);
+
+  forwarder.createImageSubscriber("/allowed/cam");
+  forwarder.createCompressedImageSubscriber("/allowed/cam/compressed");
+  forwarder.createCompressedImageSubscriber("/allowed/cam/compressed");
+
+  const std::lock_guard<std::mutex> lock(forwarder.outbound_topics_mutex_);
+  EXPECT_EQ(forwarder.subscriptions_.count("/allowed/cam"), 1U);
+  EXPECT_EQ(forwarder.subscriptions_.count("/allowed/cam/compressed"), 1U);
+  EXPECT_EQ(forwarder.image_topic_states_.at("/allowed/cam")->track_name, "/allowed/cam");
+  EXPECT_EQ(forwarder.image_topic_states_.at("/allowed/cam/compressed")->track_name, "/allowed/cam/compressed");
+  EXPECT_EQ(forwarder.diagnostic_state_.outbound_failures.load(), 0U);
+}
+
 } // namespace ros_portal
diff --git a/src/ros_portal/test/unit/video_frame_metadata_test.cpp b/src/ros_portal/test/unit/video_frame_metadata_test.cpp
new file mode 100644
index 00000000..3d8e9395
--- /dev/null
+++ b/src/ros_portal/test/unit/video_frame_metadata_test.cpp
@@ -0,0 +1,57 @@
+/*
+ * Copyright 2026 LiveKit
+ *
+ * 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.
+ */
+
+#include "ros_portal/utils/video_frame_metadata.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace ros_portal::utils {
+
+TEST(VideoFrameMetadataTest, FrameIdRoundTrips) {
+  const auto user_data = encodeFrameIdUserData("camera_optical_frame");
+
+  ASSERT_TRUE(user_data.has_value());
+  EXPECT_EQ(user_data->front(), kFrameIdUserDataVersion);
+  EXPECT_EQ(decodeFrameIdUserData(*user_data), "camera_optical_frame");
+}
+
+TEST(VideoFrameMetadataTest, EmptyFrameIdRoundTrips) {
+  const auto user_data = encodeFrameIdUserData("");
+
+  ASSERT_TRUE(user_data.has_value());
+  EXPECT_EQ(*user_data, (std::vector<std::uint8_t>{kFrameIdUserDataVersion}));
+  EXPECT_EQ(decodeFrameIdUserData(*user_data), "");
+}
+
+TEST(VideoFrameMetadataTest, EncodeRejectsFrameIdOverLengthLimit) {
+  EXPECT_TRUE(encodeFrameIdUserData(std::string(kMaxFrameIdUserDataLength, 'a')).has_value());
+  EXPECT_FALSE(encodeFrameIdUserData(std::string(kMaxFrameIdUserDataLength + 1U, 'a')).has_value());
+}
+
+TEST(VideoFrameMetadataTest, DecodeRejectsInvalidUserData) {
+  EXPECT_FALSE(decodeFrameIdUserData({}).has_value());
+  EXPECT_FALSE(decodeFrameIdUserData({0x02, 'a'}).has_value());
+
+  std::vector<std::uint8_t> oversized(kMaxFrameIdUserDataLength + 2U, 'a');
+  oversized.front() = kFrameIdUserDataVersion;
+  EXPECT_FALSE(decodeFrameIdUserData(oversized).has_value());
+}
+
+} // namespace ros_portal::utils