RGB-D SLAM with Masks

[fmuehlis]

System setup
Hardware:

• RTX 3070
• Ryzen 5600X (32GB RAM)

GPU Info:

• Driver 580.95.05
• CUDA 13.0

OS Info:

• Ubuntu 24.04
• Docker 29.1.3
• Container Toolkit 1.18.1

PyCuVSLAM is running in a Docker container based on the nvidia/cuda:12.6.1-devel-ubuntu22.04 image.

Camera setup
Camera data produced by Toyota HSR Robot (RGB-D Camera) via ROS bag file.

Description
I am trying to use RGB-D SLAM with PyCuVSLAM and masks to cut out dynamic objects in the scene. This leads to almost all of the features to be discarded from the landmarks. Altheigh observations seem to be correctly masked out of the image.

Image without mask (last observations in red):

Features are detected on the chair.

Image with mask:

Features are properly discarded.

Pointcloud without any masking:

Example result:

last_observations: 217
last_landmarks: 130
final_landmarks: 512

Pointcloud with an all 0 mask:

Example result:

last_observations: 321
last_landmarks: 0
final_landmarks: 0

Pointcloud with an all 255 mask:

Result on every frame:

last_observations: 0
last_landmarks: 0
final_landmarks: 0

Pointcloud with a real mask:

Example result:

last_observations: 316
last_landmarks: 0
final_landmarks: 0

To Reproduce

1. Use PyCuVSLAM in RGBD Mode with a SLAM Config

rgbd_settings = vslam.Tracker.OdometryRGBDSettings(
    depth_scale_factor=1.0 / 0.001,
    depth_camera_id=0,
    enable_depth_stereo_tracking=False,
)
odom_config = vslam.Tracker.OdometryConfig(
    async_sba=True,
    enable_final_landmarks_export=True,
    odometry_mode=vslam.Tracker.OdometryMode.RGBD,
    rgbd_settings=rgbd_settings,
    use_denoising=True,
    use_motion_model=True, 
    use_gpu=True,
)
slam_config = vslam.Tracker.SlamConfig(
    use_gpu=True,
    sync_mode=False,
)
loc_settings = vslam.Tracker.SlamLocalizationSettings(
    horizontal_search_radius=8.0,
    vertical_search_radius=2.0,
    horizontal_step=0.5,
    vertical_step=0.2,
    angular_step_rads=0.03,
)

rig = vslam.Rig()

cam = vslam.Camera()
cam.distortion = vslam.Distortion(vslam.Distortion.Model.Pinhole)
cam.focal = fx, fy
cam.principal = cx, cy
cam.size = width, height
cam.rig_from_camera.rotation = [-0.500, 0.500, -0.500, 0.500]

rig.cameras = [cam]

tracker = vslam.Tracker(rig, odom_config, slam_config)

2. Provide any mask to the track method:

H, W = image.shape[:2]

assert np.all(image.shape[:2] == depth.shape[:2])

mask = np.zeros((H, W), dtype=np.uint8)  # or np.ones((H, W), dtype=np.uint8)

odom_pose_estimate, slam_pose_raw = tracker.track(
    timestamp_ns,
    images=[image],
    masks=[mask],
    depths=[depth],
)

3. Realize that all points are discarded from the final landmarks

landmarks = tracker.get_final_landmarks().values()
print(f"number of landmarks: {len(landmarks)}")
# number of landmarks: 0

Expected behavior
I would expect that only features inside the mask are discarded. If I provide an all zero mask all features should be kept. If I provide an all 255 mask all features should be discarded.

What you have tried
I have tried

• providing a real-world example mask created with segmentation models like YOLOE and SAM
• providing a mask with np.zeros
• providing a mask with np.ones
• providing a mask with np.ones * 255

Additional information
N/A

[zwdoescode]

@fmuehlis Could you please try our tum example with SLAM mode?

We tried on our side and cannot replicate this issue. We are able to observe increasing map landmarks. Please check out the attached video. In this video, we applied a dummy mask per frame:

    dummy_mask = np.zeros((color_frame.shape[0], color_frame.shape[1]), dtype=np.uint8)
    dummy_mask[120:-120, 180:-180] = 255
    
    # Track frame
    odom_pose_estimate, _ = tracker.track(
        timestamp, images=[color_frame], depths=[depth_frame], 
        masks=[dummy_mask]
    )

track_tum_slam_small.webm

Here is a screenshot at the end of the dataset

[fmuehlis]

@zwdoescode thank you for the quick reply. It seems to me that you were indeed able to reproduce my exact issue. As seen in my original report there are some landmarks being kept when not providing a fully white or fully black mask. I have tried the tum example in the x86 Docker image provided in this repository.

This is the result after the full dataset with the mask you used in your example (some landmarks are kept, mask is working correctly on observations in the left images):

Final landmarks size after last frame: number of landmarks: 440

However, this is the result after the full dataset without any masks applied (dummy_mask = None, everything else is the same):

Final landmarks size after last frame: number of landmarks: 42364

As you can see the trajectory is completely off and the Point cloud is significantly reduced. This is also the case in your provided example.

When I now use a completely empty mask by removing this line dummy_mask[120:-120, 180:-180] = 255 I even get:

Final landmarks size after last frame: number of landmarks: 0

My exact setup

import os
import yaml

import numpy as np
import rerun as rr
import rerun.blueprint as rrb
import cuvslam
from scipy.spatial.transform import Rotation as R

# Local imports
from dataset_utils import load_frame, get_matched_rgbd_pairs

# Lambda to convert quaternion [x, y, z, w] to 3x3 rotation matrix (as list of lists)
quaternion_to_rotation_matrix = lambda q: R.from_quat(q).as_matrix().tolist()

# Lambda to multiply two quaternions [x, y, z, w] * [x, y, z, w]
quaternion_multiply = lambda q1, q2: (R.from_quat(q1) * R.from_quat(q2)).as_quat().tolist()

# Lambda to rotate a 3D vector using a 3x3 rotation matrix
rotate_vector = lambda vector, rotation_matrix: R.from_matrix(rotation_matrix).apply(vector).tolist()

def transform_landmarks(landmarks, initial_pose):
    """
    Transform landmarks by initial pose (rotation + translation).

    Args:
        landmarks: list of 3D landmark coordinates
        initial_pose: cuvslam.Pose object representing initial pose

    Returns:
        List of transformed 3D landmark coordinates
    """
    rotation_matrix = quaternion_to_rotation_matrix(initial_pose.rotation)
    transformed_landmarks = []

    for landmark in landmarks:
        # Rotate landmark by initial pose rotation
        rotated_landmark = rotate_vector(landmark, rotation_matrix)

        # Add initial translation
        transformed_landmark = [
            initial_pose.translation[0] + rotated_landmark[0],
            initial_pose.translation[1] + rotated_landmark[1],
            initial_pose.translation[2] + rotated_landmark[2],
            ]
        transformed_landmarks.append(transformed_landmark)

    return transformed_landmarks

# Set up dataset path
tum_dataset_path = os.path.join(
    os.path.dirname(__file__), "dataset/rgbd_dataset_freiburg3_long_office_household"
)


def color_from_id(identifier):
    """Generate a color from an identifier."""
    return [(identifier * 17) % 256, (identifier * 31) % 256, (identifier * 47) % 256]


# Constants
IMAGE_JITTER_THRESHOLD_MS = 40 * 1e6  # 40ms in nanoseconds

# Setup rerun visualizer
rr.init("tum_dataset", strict=True, spawn=True)
rr.send_blueprint(
    rrb.Blueprint(
        rrb.TimePanel(state="collapsed"),
        rrb.Horizontal(
            contents=[
                rrb.Vertical(
                    contents=[
                        rrb.Spatial2DView(
                            origin="world/camera/image", name="RGB Camera"
                        ),
                        rrb.Spatial2DView(
                            origin="world/camera/depth", name="Depth Camera"
                        ),
                    ]
                ),
                rrb.Spatial3DView(
                    name="3D", defaults=[rr.components.ImagePlaneDistance(0.5)]
                ),
            ]
        ),
    ),
    make_active=True,
)

# Setup coordinate basis for root
# cuvslam uses right-hand system with X-right, Y-down, Z-forward
rr.log("/", rr.ViewCoordinates.RIGHT_HAND_Y_DOWN, static=True)

# Get matched RGB-D pairs from dataset
rgbd_pairs = get_matched_rgbd_pairs(tum_dataset_path, max_time_diff=0.02, max_gap=0.5)
print(f"Found {len(rgbd_pairs)} matched RGB-D pairs")

# Load tum configuration from YAML
print("Loading tum configuration...")
config_path = os.path.join(tum_dataset_path, "freiburg3_rig.yaml")
with open(config_path, "r") as file:
    config_data = yaml.safe_load(file)

# Set up camera parameters
camera = cuvslam.Camera()
camera.size = (
    config_data["rgb_camera"]["image_width"],
    config_data["rgb_camera"]["image_height"],
)
camera.principal = config_data["rgb_camera"]["principal_point"]
camera.focal = config_data["rgb_camera"]["focal_length"]
camera.border_top = 20
camera.border_bottom = 20
camera.border_left = 10
camera.border_right = 50

rgbd_settings = cuvslam.Tracker.OdometryRGBDSettings(
    depth_scale_factor=1.0 / 0.001,
    depth_camera_id=0,
    enable_depth_stereo_tracking=False,
)
cfg = cuvslam.Tracker.OdometryConfig(
    async_sba=True,
    enable_final_landmarks_export=True,
    odometry_mode=cuvslam.Tracker.OdometryMode.RGBD,
    rgbd_settings=rgbd_settings,
    use_denoising=True,
    use_motion_model=True,
    use_gpu=True,
)
slam_config = cuvslam.Tracker.SlamConfig(
    use_gpu=True,
    sync_mode=False,
)
loc_settings = cuvslam.Tracker.SlamLocalizationSettings(
    horizontal_search_radius=8.0,
    vertical_search_radius=2.0,
    horizontal_step=0.5,
    vertical_step=0.2,
    angular_step_rads=0.03,
)

slam_initial_pose = cuvslam.Pose(translation=[0, 0, 0], rotation=[0, 0, 0, 1])

# Initialize tracker
tracker = cuvslam.Tracker(cuvslam.Rig([camera]), cfg, slam_config)

frame_id = 0
prev_timestamp = None
trajectory = []

# Process each matched RGB-D pair
for rgb_time, rgb_path, depth_path in rgbd_pairs:
    color_frame = load_frame(rgb_path)
    depth_frame = load_frame(depth_path)

    if color_frame is None or depth_frame is None:
        print(f"Warning: Failed to read image files: {rgb_path} or {depth_path}")
        continue

    # Convert timestamp to nanoseconds for tracker
    timestamp = int(rgb_time * 1e9)

    # Check timestamp difference with previous frame
    if prev_timestamp is not None:
        timestamp_diff = timestamp - prev_timestamp
        if timestamp_diff > IMAGE_JITTER_THRESHOLD_MS:
            print(
                f"Warning: Camera stream message delayed: timestap gap "
                f"({timestamp_diff / 1e6:.2f} ms) exceeds threshold "
                f"{IMAGE_JITTER_THRESHOLD_MS / 1e6:.2f} ms"
            )

    # Use for Result 1: Apply no mask
    dummy_mask = None

    # Use for Result 2: Apply mask
    # dummy_mask = np.zeros((color_frame.shape[0], color_frame.shape[1]), dtype=np.uint8)
    # dummy_mask[120:-120, 180:-180] = 255

    # Track frame
    odom_pose_estimate, _ = tracker.track(
        timestamp, images=[color_frame], depths=[depth_frame], masks=[dummy_mask]
    )

    raw_final_landmarks = list(tracker.get_final_landmarks().values())
    final_landmarks = transform_landmarks(raw_final_landmarks, slam_initial_pose)
    print(f"number of landmarks: {len(final_landmarks)}")

    if odom_pose_estimate.world_from_rig is None:
        print(f"Warning: Failed to track frame {frame_id}")
        continue

    # Get current pose and observations for the main camera and gravity in rig frame
    odom_pose = odom_pose_estimate.world_from_rig.pose
    trajectory.append(odom_pose.translation)

    # Get observations
    observations = [tracker.get_last_observations(0)]

    # Extract observation points and colors
    obs_uv = [[o.u, o.v] for o in observations[0]]
    obs_colors = [color_from_id(o.id) for o in observations[0]]

    # Log visualization data
    rr.set_time_sequence("frame", frame_id)
    rr.log("trajectory", rr.LineStrips3D(trajectory), static=True)

    rr.log(
        "world/camera/rgb",
        rr.Transform3D(
            translation=odom_pose.translation, quaternion=odom_pose.rotation
        ),
        rr.Arrows3D(
            vectors=np.eye(3) * 0.2,
            colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]],  # RGB for XYZ axes
        ),
    )

    # Log RGB and depth images with observations
    rr.log("world/camera/image", rr.Image(color_frame).compress(jpeg_quality=80))
    rr.log(
        "world/camera/image/observations",
        rr.Points2D(obs_uv, radii=5, colors=obs_colors),
    )
    rr.log("world/camera/depth", rr.Image(depth_frame))
    rr.log(
        "world/camera/depth/observations",
        rr.Points2D(obs_uv, radii=5, colors=obs_colors),
    )
    rr.log("final_landmarks", rr.Points3D(final_landmarks, radii=0.1))

    # Update for next iteration
    frame_id += 1
    prev_timestamp = timestamp

Edit: Correct the number of landmarks for their respective images

[zwdoescode]

Thanks @fmuehlis. This is indeed a bug. We will root cause it and fix it in future version.

[fmuehlis]

@zwdoescode awesome, thank you!

[Spyder156]

Hey @zwdoescode, If it’s something I can access in the repo, I'd be interested to work on this.

[zwdoescode]

@Spyder156 thanks for your interests. This bug is in our private codebase.
We have already root caused it and merged a fix in the private codebase. We will release the fix in the future