From 5a5a80f5120578ada5e09a53255411f68c056d52 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 17 Apr 2026 17:19:20 +0200 Subject: [PATCH 01/59] [fix] - update PyTorch version mismatch and adjust build commands for realsense2_ros_mqtt_bridge --- docker/Jazzy.Dockerfile | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docker/Jazzy.Dockerfile b/docker/Jazzy.Dockerfile index 7aa138b..ce55ab3 100644 --- a/docker/Jazzy.Dockerfile +++ b/docker/Jazzy.Dockerfile @@ -65,7 +65,7 @@ RUN groupadd --gid $USER_GID $USERNAME \ # --- Python environment setup --- RUN pip3 install networkx==3.1 -RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu121 \ +RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu129 \ torch \ torchvision RUN apt remove --purge python3-typing-extensions -y @@ -161,7 +161,8 @@ RUN apt-get update && apt-get install -y \ # Build the workspace WORKDIR /home/$USERNAME/workspace/ RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +run /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" # --- Miscalleanous --- RUN ldconfig From 5630cf9afe18e65d306183332edac28cdd4962f0 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 20 Apr 2026 12:41:18 +0200 Subject: [PATCH 02/59] [fix] - update SSH_AUTH_SOCK path and correct workspace paths in docker-compose.yml --- docker/Jazzy.Dockerfile | 2 +- docker/docker-compose.yml | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/docker/Jazzy.Dockerfile b/docker/Jazzy.Dockerfile index ce55ab3..d75c062 100644 --- a/docker/Jazzy.Dockerfile +++ b/docker/Jazzy.Dockerfile @@ -162,7 +162,7 @@ RUN apt-get update && apt-get install -y \ WORKDIR /home/$USERNAME/workspace/ RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" -run /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" # --- Miscalleanous --- RUN ldconfig diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 74429dc..cfca8e3 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -14,7 +14,7 @@ services: environment: DISPLAY: $DISPLAY XAUTHORITY: $XAUTHORITY - SSH_AUTH_SOCK: $SSH_AUTH_SOCK + SSH_AUTH_SOCK: /ssh-agent NVIDIA_VISIBLE_DEVICES: all NVIDIA_DRIVER_CAPABILITIES: all TERM: $TERM @@ -30,7 +30,7 @@ services: # - ~/.Xauthority:/home/user/.Xauthority:rw - $XAUTHORITY:$XAUTHORITY:rw # SSH - - $SSH_AUTH_SOCK:$SSH_AUTH_SOCK + - $SSH_AUTH_SOCK:/ssh-agent # TMUX - ~/.tmux:/home/$USERNAME/.tmux:ro - ~/.tmux.conf:/home/$USERNAME/.tmux.conf:ro @@ -42,13 +42,13 @@ services: # - ~/.bash_history:/root/.bash_history # - container_home_cache:/root/.cache # Optional: persist cache # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" # Data Directory (** MODIFY **) - - "/media/$USERNAME/AT_SSD/Datasets:/home/$USERNAME/datasets:rw" + - "/media/$USERNAME/X9\ Pro1/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" command: tail -f /dev/null @@ -57,13 +57,13 @@ services: - volume-init volume-init: image: busybox - command: ["chown", "-R", "1000:1000", "/workspace/src/visual_sgraphs"] + command: ["chown", "-R", "1000:1000", "/home/$USERNAME/workspace/src/visual_sgraphs"] volumes: # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" entrypoint: "" restart: "no" From a7f73089b55e86e47107648476dacfdc2a8d1f16 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 22 Apr 2026 13:46:56 +0200 Subject: [PATCH 03/59] [fix] - correct PyTorch version in Dockerfile to match CUDA 12.6 --- docker/Jazzy.Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Jazzy.Dockerfile b/docker/Jazzy.Dockerfile index d75c062..4a7763f 100644 --- a/docker/Jazzy.Dockerfile +++ b/docker/Jazzy.Dockerfile @@ -65,7 +65,7 @@ RUN groupadd --gid $USER_GID $USERNAME \ # --- Python environment setup --- RUN pip3 install networkx==3.1 -RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu129 \ +RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \ torch \ torchvision RUN apt remove --purge python3-typing-extensions -y From 5d3a4afd56c11167b88bb1eadd646301beb2673b Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 4 Jun 2026 17:43:45 +0200 Subject: [PATCH 04/59] add eomt scene segmenter support --- launch/rgbd.launch.py | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/launch/rgbd.launch.py b/launch/rgbd.launch.py index e4e4343..04ded81 100644 --- a/launch/rgbd.launch.py +++ b/launch/rgbd.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): "semantic_scene_segmenter", default_value="yoso", description="The method to segment the semantic scene (if off, the baseline)", - choices=["yoso", "pfcn", "off"], + choices=["yoso", "pfcn", "eomt", "off"], ), # Topics DeclareLaunchArgument("camera_frame", default_value="camera"), @@ -215,6 +215,28 @@ def generate_launch_description(): ], ], ), + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "eomt" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="segmenter_eomt.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_eomt.yaml", + ], + ], + ), # Structural Element Detectors # Node( # name="situational_graphs_reasoning", From be8f8903b569727fabaa66eb09f6f8f24511eaf8 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 4 Jun 2026 17:57:32 +0200 Subject: [PATCH 05/59] migrate mono-inertial to ros2 --- CMakeLists.txt | 1 + docker/Jazzy.Dockerfile | 7 +- docker/Jazzy_new.Dockerfile | 229 ++++++++++++++++++++ docker/docker-compose.yml | 6 +- launch/mono-imu.launch.py | 219 +++++++++++++++++++ src/ros_mono_inertial.cc | 419 ++++++++++++++++++++---------------- 6 files changed, 699 insertions(+), 182 deletions(-) create mode 100644 docker/Jazzy_new.Dockerfile create mode 100644 launch/mono-imu.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 8365d21..ff4080f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -254,6 +254,7 @@ endfunction() # ROS2 nodes add_ros2_node(ros_rgbd src/ros_rgbd.cc) add_ros2_node(ros_rgbd_inertial src/ros_rgbd_inertial.cc) +add_ros2_node(ros_mono_inertial src/ros_mono_inertial.cc) # Install other resources install( diff --git a/docker/Jazzy.Dockerfile b/docker/Jazzy.Dockerfile index 4a7763f..9686bc8 100644 --- a/docker/Jazzy.Dockerfile +++ b/docker/Jazzy.Dockerfile @@ -121,6 +121,7 @@ RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/realsense-ros.git +RUN --mount=type=ssh git clone git@github.com:snt-arg/object_tracker_3d_ros.git # RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git # Repositories for GNN-based room detection and reasoning @@ -133,6 +134,10 @@ RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/real WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt +# Install object tracker dependencies +RUN pip3 install --break-system-packages --ignore-installed \ + -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt + # [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy # (Read more: https://github.com/ros2/ros2/issues/1702) # RUN pip3 install --break-system-packages setuptools==79.0.1 @@ -212,4 +217,4 @@ RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/c ENTRYPOINT ["/entrypoint.sh"] USER $USERNAME CMD ["/bin/bash"] -SHELL ["/bin/bash"] \ No newline at end of file +SHELL ["/bin/bash"] diff --git a/docker/Jazzy_new.Dockerfile b/docker/Jazzy_new.Dockerfile new file mode 100644 index 0000000..f646f36 --- /dev/null +++ b/docker/Jazzy_new.Dockerfile @@ -0,0 +1,229 @@ +FROM nvcr.io/nvidia/cuda-dl-base:25.04-cuda12.9-devel-ubuntu24.04 + +# Arguments +ARG USERNAME=user +ARG USER_UID=1000 +ARG USER_GID=$USER_UID +ARG DEBIAN_FRONTEND=noninteractive + +# Environment variables +ENV CUDA_HOME=/usr/local/cuda \ + LANG=en_US.UTF-8 \ + LC_ALL=en_US.UTF-8 \ + ROS_DISTRO=jazzy \ + PIP_BREAK_SYSTEM_PACKAGES=1 + +# --- Fix MPI issue --- +RUN mkdir -p /opt/hpcx/ompi/lib/x86_64-linux-gnu \ + && ln -s /opt/hpcx/ompi /opt/hpcx/ompi/lib/x86_64-linux-gnu/openmpi \ + && dpkg-reconfigure libc-bin + +# --- Handle user creation --- +RUN if id -u $USER_UID ; then userdel "$(id -un $USER_UID)" ; fi + +# --- System setup --- +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-pip \ + python-is-python3 \ + git \ + openssh-client \ + wget \ + vim \ + curl \ + libeigen3-dev \ + build-essential \ + locales \ + software-properties-common \ + lsb-release \ + gnupg2 && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +RUN add-apt-repository universe + +# --- ROS 2 Jazzy APT source setup --- +RUN ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ + curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" && \ + apt install -y /tmp/ros2-apt-source.deb + +# --- Install ROS 2 Jazzy development tools --- +RUN apt update && apt upgrade -y && \ + apt install -y ros-dev-tools ros-${ROS_DISTRO}-desktop ros-${ROS_DISTRO}-rqt-tf-tree + +# --- Source ROS globally --- +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc + +# --- Initialize rosdep --- +RUN rosdep init && rosdep update + +# --- Clean up --- +RUN rm -rf /var/lib/apt/lists/* /tmp/* + +# --- Create user --- +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# --- Python environment setup --- +RUN pip3 install networkx==3.1 +RUN pip3 uninstall -y torch torchvision torchaudio || true +RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \ + torch==2.6.0+cu126 \ + torchvision==0.21.0+cu126 +RUN apt remove --purge python3-typing-extensions -y +RUN pip3 install typing-extensions==4.11.0 + +# --- CLIP and Detectron2 setup --- +ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX" +ENV FORCE_CUDA="1" +RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git' +RUN pip3 install 'git+https://github.com/openai/CLIP.git' + + +# --- SSH keys --- +# Define the SSH keys as build arguments for latter mounting +RUN mkdir -p -m 0600 ~/.ssh && ssh-keyscan github.com >> ~/.ssh/known_hosts + +# --- Clone repositories --- +RUN apt-get update && apt-get install -y \ + libepoxy-dev \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + freeglut3-dev \ + libglew-dev \ + cmake \ + build-essential \ + git + +# Pangolin +WORKDIR /opt/ +RUN git clone --branch v0.9.1 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \ + cd Pangolin && \ + mkdir build && cd build && \ + cmake .. && \ + make -j && \ + make install + +# Cmake +ARG version=3.22 +ARG build=1 +WORKDIR /tmp +RUN wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz + +RUN tar -xzvf cmake-$version.$build.tar.gz +WORKDIR /tmp/cmake-$version.$build +RUN ./bootstrap +RUN make -j8 +RUN make install + +WORKDIR /home/$USERNAME/workspace/src + +# Mount the SSH keys and clone the vS-Graphs repositories +RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git +RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git +RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git +RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/realsense-ros.git +# RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git + +# Repositories for GNN-based room detection and reasoning +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_wrapper.git +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_datasets.git +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_reasoning.git +# RUN --mount=type=ssh git clone -b main git@github.com:snt-arg/situational_graphs_reasoning_msgs.git + +# Install the vS-Graphs dependencies +WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker +RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt + +# Install object tracker dependencies when the package is present in the workspace +RUN if [ -f /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ]; then \ + pip3 install --break-system-packages --ignore-installed \ + -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ; \ + fi + +# [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy +# (Read more: https://github.com/ros2/ros2/issues/1702) +# RUN pip3 install --break-system-packages setuptools==79.0.1 + +# Install reasoning dependencies +# RUN pip3 install --break-system-packages shapely==2.1.1 torch-geometric==2.6.1 transforms3d==0.4.2 +# RUN mkdir -p /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports \ +# && chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports + +# Install the EOMT dependencies +WORKDIR /home/$USERNAME/workspace/src/scene_segment_ros/src/ +RUN git clone https://github.com/tue-mps/eomt.git +RUN pip3 install --ignore-installed -r /home/${USERNAME}/workspace/src/scene_segment_ros/src/eomt/requirements.txt +RUN pip3 install "numpy<2.0" --force-reinstall + +# WORKDIR /home/$USERNAME/workspace/src/ + +# Download the yoso checkpoint +RUN wget https://github.com/hujiecpp/YOSO/releases/download/v0.1/yoso_res50_coco.pth +RUN mv yoso_res50_coco.pth /home/$USERNAME/workspace/src/scene_segment_ros/include/ + +# USER root +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-rviz-visual-tools \ + ros-${ROS_DISTRO}-depth-image-proc \ + ros-${ROS_DISTRO}-backward-ros \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + ros-${ROS_DISTRO}-diagnostic-updater \ + ros-${ROS_DISTRO}-pcl-ros + +# Build the workspace +WORKDIR /home/$USERNAME/workspace/ +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" + +# --- Miscalleanous --- +RUN ldconfig + +# --- Clean up --- +# Remove the apt list files +RUN rm -rf /var/lib/apt/lists/* + +# Remove packages no longer needed +RUN apt-get clean && apt-get autoremove -y + +# Remove the ssh keys +RUN rm -rf /root/.ssh/ + +# --- Build entrypoint --- +RUN echo "#!/bin/bash" >> /entrypoint.sh \ + && echo "echo \"source /opt/ros/$ROS_DISTRO/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ + && echo "echo \"source /home/$USERNAME/workspace/install/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ + && echo 'exec "$@"' >> /entrypoint.sh \ + && chmod a+x /entrypoint.sh + +# ------------------------------------ +# Download Vox2Ros Toolkit for Voxblox +# ------------------------------------ +WORKDIR /home/$USERNAME/workspace/vsgraphs_tools +RUN curl -L https://raw.githubusercontent.com/snt-arg/vsgraphs_tools/refs/heads/main/Voxblox/relay_jazzy.py -o /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py +RUN chmod +x /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py + +USER $USERNAME +RUN sudo chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace +WORKDIR /home/$USERNAME/workspace/ + +# --------------------------- +# Download and Install mprocs +# --------------------------- +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | bash -s -- -y \ + && . "$HOME/.cargo/env" \ + && cargo install mprocs + +# -------------------------- +# Aliases and Environment Setup +# -------------------------- +RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/config/mprocs.yml'" >> ~/.bashrc && \ + echo "alias rel_vox='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode voxblox_client'" >> ~/.bashrc && \ + echo "alias rel_pcl='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode pc_server'" >> ~/.bashrc + +ENTRYPOINT ["/entrypoint.sh"] +USER $USERNAME +CMD ["/bin/bash"] +SHELL ["/bin/bash"] +# note: you might have to reinstall numpy and opencv python pkgs and rebuilding cause the system packages are pointed instead \ No newline at end of file diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index cfca8e3..e9bbb81 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -47,8 +47,10 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + # Working Directory - 3D object tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" # Data Directory (** MODIFY **) - - "/media/$USERNAME/X9\ Pro1/datasets:/home/$USERNAME/datasets:rw" + - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" command: tail -f /dev/null @@ -65,5 +67,7 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + # Working Directory - 3D object tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" entrypoint: "" restart: "no" diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py new file mode 100644 index 0000000..a828552 --- /dev/null +++ b/launch/mono-imu.launch.py @@ -0,0 +1,219 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import ComposableNodeContainer +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration, EqualsSubstitution + + +def generate_launch_description(): + return LaunchDescription( + [ + # Global arguments declarations + DeclareLaunchArgument("offline", default_value="true"), + DeclareLaunchArgument("launch_rviz", default_value="true"), + DeclareLaunchArgument("colored_pointcloud", default_value="true"), + DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), + DeclareLaunchArgument( + "semantic_scene_segmenter", + default_value="yoso", + description="The method to segment the semantic scene (if off, the baseline)", + choices=["yoso", "pfcn", "off"], + ), + # Topics + DeclareLaunchArgument("camera_frame", default_value="camera"), + DeclareLaunchArgument("sensor_config", default_value="RealSense_D435i"), + DeclareLaunchArgument( + "rgb_image_topic", default_value="/camera/realsense/color/image_raw" + ), + DeclareLaunchArgument( + "rgb_camera_info_topic", + default_value="/camera/realsense/color/camera_info", + ), + DeclareLaunchArgument( + "depth_image_topic", + default_value="/camera/realsense/aligned_depth_to_color/image_raw", + ), + DeclareLaunchArgument( + "imu_topic", + default_value="/camera/realsense/imu", + ), + # VS-Graphs Node + Node( + name="vs_graphs", + package="vs_graphs", + executable="ros_mono_inertial", + output="screen", + parameters=[ + {"use_sim_time": LaunchConfiguration("offline")}, + { + "voc_file": LaunchConfiguration( + "voc_file", + default=[ + get_package_share_directory("vs_graphs"), + "/Vocabulary/ORBvoc.txt.bin", + ], + ) + }, + { + "settings_file": LaunchConfiguration( + "settings_file", + default=[ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + ) + }, + { + "sys_params_file": LaunchConfiguration( + "sys_params_file", + default=[ + get_package_share_directory("vs_graphs"), + "/config/system_params.yaml", + ], + ) + }, + {"yaw": 0.0}, + {"roll": 0.0}, + {"pitch": 0.0}, + {"frame_map": "map"}, + {"frame_imu": "imu"}, + {"frame_world": "world"}, + {"frame_camera": "camera"}, + {"enable_pangolin": False}, + {"static_transform": True}, + {"colored_pointcloud": False}, + {"publish_pointclouds": True}, + ], + remappings=[ + ("/imu", LaunchConfiguration("imu_topic")), + ("/camera/image_raw", LaunchConfiguration("rgb_image_topic")), + ], + ), + # Static Transforms + Node( + package="tf2_ros", + name="map_to_map_elevated", # For Voxblox Skeleton + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "map", "map_elevated"], + ), + Node( + name="bc_to_se", + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "3", "0", "0", "0", "build_comp", "struc_elem"], + ), + Node( + package="tf2_ros", + name="world_to_bc", + executable="static_transform_publisher", + arguments=["0", "0", "3", "0", "0", "0", "world", "build_comp"], + ), + Node( + package="tf2_ros", + name="camera_to_imu", + executable="static_transform_publisher", + arguments=["0", "0", "0", "1.5708", "0", "1.5708", "camera", "imu"], + ), + Node( + package="tf2_ros", + name="camera_to_camera_optical", + executable="static_transform_publisher", + arguments=[ + "0", + "0", + "0", + "0", + "0", + "0", + "camera", + "camera_color_optical_frame", + # RealSense: camera_color_optical_frame, OpenLoris: d400_color + ], + ), + # RViz + Node( + condition=IfCondition(LaunchConfiguration("launch_rviz")), + package="rviz2", + executable="rviz2", + name="rviz", + arguments=[ + "-d", + [ + get_package_share_directory("vs_graphs"), + "/config/Visualization/vsgraphs_rgbd.rviz", + ], + ], + output="screen", + ), + # Semantic Scene Segmenter Node (based on semantic_scene_segmenter argument) + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "yoso" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="segmenter_yoso.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_yoso.yaml", + ], + ], + ), + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "pfcn" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="segmenter_pFCN.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_pFCN.yaml", + ], + ], + ), + # Structural Element Detectors + # Node( + # name="situational_graphs_reasoning", + # package="situational_graphs_reasoning", + # executable="situational_graphs_reasoning", + # output="screen", + # # parameters=[ + # # os.path.join( + # # get_package_share_directory("situational_graphs_reasoning"), + # # "config", + # # "params.yaml", + # # ) + # # ], + # # remappings=[ + # # ( + # # "situational_graphs_reasoning/graphs", + # # "/s_graphs/graph_structure", + # # ), + # # ], + # ), + ] + ) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 58d329d..caffcce 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -22,139 +22,71 @@ #include "common.h" +#include +#include + using namespace std; -class ImuGrabber +class ImuGrabber : public rclcpp::Node { public: - ImuGrabber() {}; + ImuGrabber() : rclcpp::Node("imu_grabber") + { + tfBroadcaster = std::make_shared(this); + staticTfBroadcaster = std::make_shared(this); + } - void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg); + void GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &imu_msg); std::mutex mBufMutex; - queue imuBuf; + std::queue imuBuf; }; -class ImageGrabber +class ImageGrabber : public rclcpp::Node { public: - ImageGrabber(ImuGrabber *pImuGb) : mpImuGb(pImuGb) {} + ImageGrabber(std::shared_ptr imuGrabber) + : rclcpp::Node("image_grabber"), + mpImuGb(std::move(imuGrabber)) + { + tfBroadcaster = std::make_shared(this); + staticTfBroadcaster = std::make_shared(this); + } void SyncWithImu(); - void GrabImage(const sensor_msgs::ImageConstPtr &msg); + void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg); - cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg); - void GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage); - void GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraph); + cv::Mat GetImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg); + void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage); + void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraph); - ImuGrabber *mpImuGb; std::mutex mBufMutex; - queue img0Buf; + std::atomic mustStop{false}; + std::shared_ptr mpImuGb; + std::queue img0Buf; private: - // Marker detection - double minMarkerTimeDiff; + double minMarkerTimeDiff = std::numeric_limits::max(); std::vector matchedMarkers; }; -int main(int argc, char **argv) +void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &imu_msg) { - ros::init(argc, argv, "Mono_Inertial"); - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); - - if (argc > 1) - ROS_WARN("Arguments supplied via command line are ignored."); - - std::string node_name = ros::this_node::getName(); - - ros::NodeHandle nodeHandler; - image_transport::ImageTransport image_transport(nodeHandler); - - std::string voc_file, settings_file, sys_params_file; - nodeHandler.param(node_name + "/sys_params_file", sys_params_file, "file_not_set"); - nodeHandler.param(node_name + "/voc_file", voc_file, "file_not_set"); - nodeHandler.param(node_name + "/settings_file", settings_file, "file_not_set"); - - if (voc_file == "file_not_set" || settings_file == "file_not_set") - { - ROS_ERROR("Please provide voc_file and settings_file in the launch file"); - ros::shutdown(); - return 1; - } - - if (sys_params_file == "file_not_set") - { - ROS_ERROR("Please provide the YAML file containing system parameters in the launch file!"); - ros::shutdown(); - return 1; - } - - bool enable_pangolin; - nodeHandler.param(node_name + "/enable_pangolin", enable_pangolin, true); - - nodeHandler.param(node_name + "/yaw", yaw, 0.0); - nodeHandler.param(node_name + "/roll", roll, 0.0); - nodeHandler.param(node_name + "/pitch", pitch, 0.0); - - nodeHandler.param(node_name + "/frame_map", frameMap, "map"); - nodeHandler.param(node_name + "/frame_imu", frameImu, "imu"); - nodeHandler.param(node_name + "/frame_camera", frameCamera, "camera"); - nodeHandler.param(node_name + "/frame_world", frameWorld, "world"); - nodeHandler.param(node_name + "/static_transform", pubStaticTransform, false); - nodeHandler.param(node_name + "/frame_building_component", frameBC, "build_comp"); - nodeHandler.param(node_name + "/frame_structural_element", frameSE, "struc_elem"); - - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ImuGrabber imugb; - ImageGrabber igb(&imugb); - sensorType = ORB_SLAM3::System::IMU_MONOCULAR; - - pSLAM = new ORB_SLAM3::System(voc_file, settings_file, sys_params_file, sensorType, enable_pangolin); - - // Subscribe to get raw images and IMU data - ros::Subscriber sub_imu = nodeHandler.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); - ros::Subscriber sub_img = nodeHandler.subscribe("/camera/image_raw", 500, &ImageGrabber::GrabImage, &igb); - - // Subscribe to the markers detected by `aruco_ros` library - // ros::Subscriber sub_aruco = nodeHandler.subscribe("/aruco_marker_publisher/markers", 1, - // &ImageGrabber::GrabArUcoMarker, &igb); - - // Subscriber for images obtained from the Semantic Segmentater - ros::Subscriber sub_segmented_img = nodeHandler.subscribe("/camera/color/image_segment", 50, - &ImageGrabber::GrabSegmentation, &igb); - - // Subscriber to get the mesh from voxblox - ros::Subscriber voxblox_skeleton_mesh = nodeHandler.subscribe("/voxblox_skeletonizer/sparse_graph", 1, - &ImageGrabber::GrabVoxbloxSkeletonGraph, &igb); - - setupPublishers(nodeHandler, image_transport, node_name); - setupServices(nodeHandler, node_name); - - // Syncing images with IMU - std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb); - - ros::spin(); - - // Stop all threads - pSLAM->Shutdown(); - ros::shutdown(); - - return 0; + std::lock_guard lock(mBufMutex); + imuBuf.push(imu_msg); } -void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg) +void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg) { - mBufMutex.lock(); + std::lock_guard lock(mBufMutex); if (!img0Buf.empty()) img0Buf.pop(); img0Buf.push(img_msg); - mBufMutex.unlock(); } -cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) +cv::Mat ImageGrabber::GetImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg) { - // Copy the ros image message to cv::Mat. cv_bridge::CvImageConstPtr cv_ptr; try { @@ -162,126 +94,253 @@ cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "[Error] `cv_bridge` exception: %s", e.what()); + return cv::Mat(); } - // Find the marker with the minimum time difference compared to the current frame std::pair> result = - findNearestMarker(cv_ptr->header.stamp.toSec()); + findNearestMarker(rclcpp::Time(cv_ptr->header.stamp).seconds()); minMarkerTimeDiff = result.first; matchedMarkers = result.second; - if (cv_ptr->image.type() == 0) - { - return cv_ptr->image.clone(); - } - else - { - std::cout << "Error type" << std::endl; - return cv_ptr->image.clone(); - } + return cv_ptr->image.clone(); } void ImageGrabber::SyncWithImu() { - while (1) + if (!mpImuGb) { - if (!img0Buf.empty() && !mpImuGb->imuBuf.empty()) + RCLCPP_ERROR(this->get_logger(), "[Error] IMU Grabber not initialized!"); + return; + } + + while (!mustStop) + { + bool hasData = false; { - cv::Mat im; - double tIm = 0; + std::lock_guard lock(mBufMutex); + std::lock_guard lock2(mpImuGb->mBufMutex); + hasData = !img0Buf.empty() && !mpImuGb->imuBuf.empty(); + } - tIm = img0Buf.front()->header.stamp.toSec(); - if (tIm > mpImuGb->imuBuf.back()->header.stamp.toSec()) + if (!hasData) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + + cv::Mat im; + double tIm = 0.0; + rclcpp::Time msgTime; + Eigen::Vector3f Wbb = Eigen::Vector3f::Zero(); + std::vector vImuMeas; + + { + std::lock_guard lock(mBufMutex); + std::lock_guard lock2(mpImuGb->mBufMutex); + + if (img0Buf.empty() || mpImuGb->imuBuf.empty()) continue; - this->mBufMutex.lock(); - im = GetImage(img0Buf.front()); - rclcpp::Time msg_time = img0Buf.front()->header.stamp; - img0Buf.pop(); - this->mBufMutex.unlock(); + tIm = rclcpp::Time(img0Buf.front()->header.stamp).seconds(); + const double latestImuTime = rclcpp::Time(mpImuGb->imuBuf.back()->header.stamp).seconds(); - vector vImuMeas; - Eigen::Vector3f Wbb; - mpImuGb->mBufMutex.lock(); - if (!mpImuGb->imuBuf.empty()) + if (tIm > latestImuTime) { - // Load imu measurements from buffer - vImuMeas.clear(); - while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec() <= tIm) - { - double t = mpImuGb->imuBuf.front()->header.stamp.toSec(); - cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z); - cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z); - vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t)); - Wbb << mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z; - mpImuGb->imuBuf.pop(); - } + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "[Warning] Waiting for IMU data ... image time (%.3f) is ahead of latest IMU time (%.3f)!", + tIm, latestImuTime); + continue; } - mpImuGb->mBufMutex.unlock(); - // ORB-SLAM3 runs in TrackMonocular() - if (minMarkerTimeDiff < 0.05) + msgTime = rclcpp::Time(img0Buf.front()->header.stamp); + im = GetImage(img0Buf.front()); + img0Buf.pop(); + + while (!mpImuGb->imuBuf.empty() && + rclcpp::Time(mpImuGb->imuBuf.front()->header.stamp).seconds() <= tIm) { - Sophus::SE3f Tcw = pSLAM->TrackMonocular(im, tIm, vImuMeas, - "", matchedMarkers); - markersBuffer.clear(); + const auto imuMsg = mpImuGb->imuBuf.front(); + const double t = rclcpp::Time(imuMsg->header.stamp).seconds(); + const cv::Point3f acc(imuMsg->linear_acceleration.x, + imuMsg->linear_acceleration.y, + imuMsg->linear_acceleration.z); + const cv::Point3f gyr(imuMsg->angular_velocity.x, + imuMsg->angular_velocity.y, + imuMsg->angular_velocity.z); + + vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t)); + Wbb << imuMsg->angular_velocity.x, imuMsg->angular_velocity.y, imuMsg->angular_velocity.z; + mpImuGb->imuBuf.pop(); } - else - Sophus::SE3f Tcw = pSLAM->TrackMonocular(im, tIm, vImuMeas); - - publishTopics(msg_time, Wbb); } - std::chrono::milliseconds tSleep(1); - std::this_thread::sleep_for(tSleep); - } -} + if (im.empty()) + continue; -void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg) -{ - mBufMutex.lock(); - imuBuf.push(imu_msg); - mBufMutex.unlock(); + if (minMarkerTimeDiff < 0.05) + { + pSLAM->TrackMonocular(im, tIm, vImuMeas, "", matchedMarkers); + markersBuffer.clear(); + } + else + pSLAM->TrackMonocular(im, tIm, vImuMeas); - return; + publishTopics(msgTime, Wbb); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } } -// void ImageGrabber::GrabArUcoMarker(const aruco_msgs::MarkerArray &markerArray) -// { -// // Pass the visited markers to a buffer to be processed later -// // addMarkersToBuffer(markerArray); -// } - -void ImageGrabber::GrabSegmentation(const segmenter_ros::SegmenterDataMsg &msgSegImage) +/** + * @brief Callback function to get scene segmentation results from the SemanticSegmenter module + * + * @param msgSegImage The segmentation results from the SemanticSegmenter + */ +void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage) { - // Fetch the segmentation results - cv_bridge::CvImageConstPtr cv_imgSeg; - uint64_t keyFrameId = msgSegImage.keyFrameId.data; + cv_bridge::CvImageConstPtr cvImgSeg; + uint64_t keyFrameId = msgSegImage.key_frame_id.data; try { - cv_imgSeg = cv_bridge::toCvCopy(msgSegImage.segmentedImage, sensor_msgs::image_encodings::BGR8); + cvImgSeg = cv_bridge::toCvCopy(std::make_shared(msgSegImage.segmented_image), + sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "[Error] `cv_bridge` exception: %s", e.what()); return; } - // convert to PCL PointCloud2 from sensor_msgs PointCloud2 pcl::PCLPointCloud2::Ptr pclPc2SegPrb(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(msgSegImage.segmentedImageProbability, *pclPc2SegPrb); - - // Create the tuple to be appended to the segmentedImageBuffer - std::tuple tuple(keyFrameId, cv_imgSeg->image, pclPc2SegPrb); + pcl_conversions::toPCL(msgSegImage.segmented_image_probability, *pclPc2SegPrb); - // Add the segmented image to a buffer to be processed in the SemanticSegmentation thread + std::tuple tuple(keyFrameId, cvImgSeg->image, pclPc2SegPrb); pSLAM->addSegmentedImage(&tuple); } -void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::MarkerArray &msgSkeletonGraphs) +/** + * @brief Callback function to get the skeleton graph from the `voxblox` module + * + * @param msgSkeletonGraphs The skeleton graph from the `voxblox` module + */ +void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraphs) { - // Pass the skeleton graph to a buffer to be processed by the SemanticSegmentation thread setVoxbloxSkeletonCluster(msgSkeletonGraphs); -} \ No newline at end of file +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("vs_graphs"); + + if (argc > 1) + RCLCPP_WARN(node->get_logger(), "Arguments supplied via command line are ignored."); + + std::string nodeName = node->get_name(); + + node->declare_parameter("yaw", 0.0); + node->declare_parameter("roll", 0.0); + node->declare_parameter("pitch", 0.0); + node->declare_parameter("enable_pangolin", true); + node->declare_parameter("static_transform", false); + node->declare_parameter("frame_imu", "imu"); + node->declare_parameter("frame_map", "map"); + node->declare_parameter("colored_pointcloud", true); + node->declare_parameter("publish_pointclouds", true); + node->declare_parameter("frame_world", "world"); + node->declare_parameter("frame_camera", "camera"); + node->declare_parameter("voc_file", "file_not_set"); + node->declare_parameter("settings_file", "file_not_set"); + node->declare_parameter("sys_params_file", "file_not_set"); + node->declare_parameter("frame_structural_element", "struc_elem"); + node->declare_parameter("frame_building_component", "build_comp"); + + std::string vocFile = node->get_parameter("voc_file").as_string(); + std::string settingsFile = node->get_parameter("settings_file").as_string(); + std::string sysParamsFile = node->get_parameter("sys_params_file").as_string(); + + if (vocFile == "file_not_set" || settingsFile == "file_not_set") + { + RCLCPP_ERROR(node->get_logger(), "[Error] 'voc_file' and 'settings_file' are not provided in the launch file! Exiting..."); + rclcpp::shutdown(); + return 1; + } + + if (sysParamsFile == "file_not_set") + { + RCLCPP_ERROR(node->get_logger(), "[Error] The `YAML` file containing system parameters is not provided in the launch file! Exiting..."); + rclcpp::shutdown(); + return 1; + } + + yaw = node->get_parameter("yaw").as_double(); + roll = node->get_parameter("roll").as_double(); + pitch = node->get_parameter("pitch").as_double(); + frameImu = node->get_parameter("frame_imu").as_string(); + frameMap = node->get_parameter("frame_map").as_string(); + frameWorld = node->get_parameter("frame_world").as_string(); + frameCamera = node->get_parameter("frame_camera").as_string(); + colorPointcloud = node->get_parameter("colored_pointcloud").as_bool(); + pubPointClouds = node->get_parameter("publish_pointclouds").as_bool(); + frameBC = node->get_parameter("frame_building_component").as_string(); + frameSE = node->get_parameter("frame_structural_element").as_string(); + pubStaticTransform = node->get_parameter("static_transform").as_bool(); + bool enablePangolin = node->get_parameter("enable_pangolin").as_bool(); + + auto imugb = std::make_shared(); + auto igb = std::make_shared(imugb); + + sensorType = ORB_SLAM3::System::IMU_MONOCULAR; + pSLAM = new ORB_SLAM3::System(vocFile, settingsFile, sysParamsFile, sensorType, enablePangolin); + + using sensor_msgs::msg::Image; + using sensor_msgs::msg::Imu; + + rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); + sensorQos.reliability(rclcpp::ReliabilityPolicy::BestEffort); + sensorQos.durability(rclcpp::DurabilityPolicy::Volatile); + + auto subImu = node->create_subscription( + "/imu", sensorQos, + [imugb](const Imu::ConstSharedPtr msg) + { imugb->GrabImu(msg); }); + + auto subImg = node->create_subscription( + "/camera/image_raw", sensorQos, + [igb](const Image::ConstSharedPtr msg) + { igb->GrabImage(msg); }); + + // Subscribe to the markers detected by `aruco_ros` library + // auto subAruco = node->create_subscription( + // "/aruco_marker_publisher/markers", 1, + // [igb](const aruco_msgs::msg::MarkerArray::SharedPtr msg) + // { igb->GrabArUcoMarker(*msg); }); + + auto subSegmentedImage = node->create_subscription( + "/camera/color/image_segment", 50, + [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) + { igb->GrabSegmentation(*msg); }); + + auto subVoxbloxSkeletonMesh = node->create_subscription( + "/voxblox_skeletonizer/sparse_graph", 1, + [igb](const visualization_msgs::msg::MarkerArray::SharedPtr msg) + { igb->GrabVoxbloxSkeletonGraph(*msg); }); + + static std::shared_ptr imageTransport = + std::make_shared(node); + setupPublishers(node, imageTransport, nodeName); + setupServices(node, nodeName); + + std::thread syncThread(&ImageGrabber::SyncWithImu, igb); + + rclcpp::spin(node); + + pSLAM->Shutdown(); + igb->mustStop = true; + syncThread.join(); + + rclcpp::shutdown(); + + return 0; +} From 12acb01ff96365c86260722b75da4f1e2d31f8c7 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 4 Jun 2026 18:00:36 +0200 Subject: [PATCH 06/59] [fix] add IMU.Threshold parameter to configuration files and disable global arguments in node constructors --- config/Monocular-Inertial/EuRoC.yaml | 2 +- config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml | 1 + config/Monocular-Inertial/RealSense_D435i.yaml | 1 + config/Monocular-Inertial/RealSense_T265.yaml | 1 + config/Monocular-Inertial/TUM-VI.yaml | 1 + config/Monocular-Inertial/TUM-VI_far.yaml | 1 + config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml | 1 + src/ros_mono_inertial.cc | 4 ++-- 8 files changed, 9 insertions(+), 3 deletions(-) diff --git a/config/Monocular-Inertial/EuRoC.yaml b/config/Monocular-Inertial/EuRoC.yaml index 4728453..4ca8b32 100644 --- a/config/Monocular-Inertial/EuRoC.yaml +++ b/config/Monocular-Inertial/EuRoC.yaml @@ -42,6 +42,7 @@ IMU.T_b_c1: !!opencv-matrix 0.0, 0.0, 0.0, 1.0] # IMU noise +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1.7e-4 #1.6968e-04 IMU.NoiseAcc: 2.0000e-3 #2.0e-3 IMU.GyroWalk: 1.9393e-05 @@ -81,4 +82,3 @@ Viewer.ViewpointX: 0.0 Viewer.ViewpointY: -1.7 Viewer.ViewpointZ: 0.8 Viewer.ViewpointF: -500.0 - diff --git a/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml b/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml index 7239455..1549c31 100755 --- a/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml +++ b/config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 diff --git a/config/Monocular-Inertial/RealSense_D435i.yaml b/config/Monocular-Inertial/RealSense_D435i.yaml index 7b67ddd..ed46c4f 100755 --- a/config/Monocular-Inertial/RealSense_D435i.yaml +++ b/config/Monocular-Inertial/RealSense_D435i.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 diff --git a/config/Monocular-Inertial/RealSense_T265.yaml b/config/Monocular-Inertial/RealSense_T265.yaml index e9043c6..089d3fd 100755 --- a/config/Monocular-Inertial/RealSense_T265.yaml +++ b/config/Monocular-Inertial/RealSense_T265.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 0.000005148030141 # rad/s^0.5 IMU.NoiseAcc: 0.000066952452471 # m/s^1.5 IMU.GyroWalk: 0.000000499999999 # rad/s^1.5 diff --git a/config/Monocular-Inertial/TUM-VI.yaml b/config/Monocular-Inertial/TUM-VI.yaml index b882f33..ae825d4 100755 --- a/config/Monocular-Inertial/TUM-VI.yaml +++ b/config/Monocular-Inertial/TUM-VI.yaml @@ -41,6 +41,7 @@ IMU.T_b_c1: !!opencv-matrix 0.0, 0.0, 0.0, 1.0] # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 0.00016 # rad/s^0.5 IMU.NoiseAcc: 0.0028 # m/s^1.5 IMU.GyroWalk: 0.000022 # rad/s^1.5 diff --git a/config/Monocular-Inertial/TUM-VI_far.yaml b/config/Monocular-Inertial/TUM-VI_far.yaml index 8ad355a..dcfe82f 100755 --- a/config/Monocular-Inertial/TUM-VI_far.yaml +++ b/config/Monocular-Inertial/TUM-VI_far.yaml @@ -42,6 +42,7 @@ IMU.T_b_c1: !!opencv-matrix # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 0.00016 # 0.004 (VINS) # 0.00016 (TUM) # 0.00016 # rad/s^0.5 IMU.NoiseAcc: 0.0028 # 0.04 (VINS) # 0.0028 (TUM) # 0.0028 # m/s^1.5 IMU.GyroWalk: 0.000022 # 0.000022 (VINS and TUM) rad/s^1.5 diff --git a/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml b/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml index ba056d3..77b0f39 100755 --- a/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml +++ b/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml @@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index caffcce..0375358 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -30,7 +30,7 @@ using namespace std; class ImuGrabber : public rclcpp::Node { public: - ImuGrabber() : rclcpp::Node("imu_grabber") + ImuGrabber() : rclcpp::Node("imu_grabber", rclcpp::NodeOptions().use_global_arguments(false)) { tfBroadcaster = std::make_shared(this); staticTfBroadcaster = std::make_shared(this); @@ -46,7 +46,7 @@ class ImageGrabber : public rclcpp::Node { public: ImageGrabber(std::shared_ptr imuGrabber) - : rclcpp::Node("image_grabber"), + : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)), mpImuGb(std::move(imuGrabber)) { tfBroadcaster = std::make_shared(this); From 6b177bb37b541865ea57961c94aa833fce056960 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 4 Jun 2026 20:38:25 +0200 Subject: [PATCH 07/59] add mono-inertial sensor setup --- launch/mono-imu.launch.py | 8 +- src/ros_mono_inertial.cc | 520 ++++++++++++++++++++++++-------------- 2 files changed, 338 insertions(+), 190 deletions(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index a828552..4642aff 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): ), # Topics DeclareLaunchArgument("camera_frame", default_value="camera"), - DeclareLaunchArgument("sensor_config", default_value="RealSense_D435i"), + DeclareLaunchArgument("sensor_config", default_value="UniLu_RealSense_D435i_640"), DeclareLaunchArgument( "rgb_image_topic", default_value="/camera/realsense/color/image_raw" ), @@ -32,10 +32,6 @@ def generate_launch_description(): "rgb_camera_info_topic", default_value="/camera/realsense/color/camera_info", ), - DeclareLaunchArgument( - "depth_image_topic", - default_value="/camera/realsense/aligned_depth_to_color/image_raw", - ), DeclareLaunchArgument( "imu_topic", default_value="/camera/realsense/imu", @@ -83,7 +79,7 @@ def generate_launch_description(): {"frame_map": "map"}, {"frame_imu": "imu"}, {"frame_world": "world"}, - {"frame_camera": "camera"}, + {"frame_camera": "camera_color_optical_frame"}, {"enable_pangolin": False}, {"static_transform": True}, {"colored_pointcloud": False}, diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 0375358..7df0609 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -11,224 +11,371 @@ * This file is part of vS-Graphs, which is free software: you can redistribute it * and/or modify it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - * - * vS-Graphs is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS - * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with this program. - * If not, see . */ #include "common.h" #include +#include #include +#include +#include +#include using namespace std; -class ImuGrabber : public rclcpp::Node +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +/** + * Linearly interpolate a single accel sample to @target_time +*/ +static ORB_SLAM3::IMU::Point interpolateAccel( + const double target_time, + const Eigen::Vector3f &cur_acc, const double cur_t, + const Eigen::Vector3f &prev_acc, const double prev_t, + const Eigen::Vector3f &gyr) { -public: - ImuGrabber() : rclcpp::Node("imu_grabber", rclcpp::NodeOptions().use_global_arguments(false)) + Eigen::Vector3f acc; + + if (prev_t == 0.0) + { + acc = cur_acc; + } + else if (target_time >= cur_t) { - tfBroadcaster = std::make_shared(this); - staticTfBroadcaster = std::make_shared(this); + acc = cur_acc; } + else if (target_time > prev_t) + { + const double factor = (target_time - prev_t) / (cur_t - prev_t); + acc = prev_acc + static_cast(factor) * (cur_acc - prev_acc); + } + else + { + acc = prev_acc; + } + + return ORB_SLAM3::IMU::Point( + acc.x(), acc.y(), acc.z(), + gyr.x(), gyr.y(), gyr.z(), + target_time); +} - void GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &imu_msg); +// --------------------------------------------------------------------------- +// Shared IMU + Image state +// --------------------------------------------------------------------------- - std::mutex mBufMutex; - std::queue imuBuf; +struct SharedState +{ + std::mutex mtx; + std::condition_variable image_ready_cv; + + // Raw gyro — collected at ~200 Hz + std::vector gyro_timestamps; + std::vector gyro_data; + + // Accel interpolated to gyro timestamps + std::vector accel_timestamps_sync; + std::vector accel_data_sync; + + // Running accel state for interpolation + Eigen::Vector3f prev_accel_data; + double prev_accel_timestamp = 0.0; + Eigen::Vector3f cur_accel_data; + double cur_accel_timestamp = 0.0; + + // Image state + cv::Mat image; + double image_timestamp = -1.0; + bool image_ready = false; + int dropped_frames = 0; + + // Marker state (populated inside the image callback path) + double min_marker_time_diff = std::numeric_limits::max(); + std::vector matched_markers; }; -class ImageGrabber : public rclcpp::Node +// --------------------------------------------------------------------------- +// ImuGrabber — thin ROS2 subscriber node, pushes directly into SharedStat +// --------------------------------------------------------------------------- + +class ImuGrabber : public rclcpp::Node { public: - ImageGrabber(std::shared_ptr imuGrabber) - : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)), - mpImuGb(std::move(imuGrabber)) + explicit ImuGrabber(std::shared_ptr state) + : rclcpp::Node("imu_grabber", rclcpp::NodeOptions().use_global_arguments(false)) + , state_(std::move(state)) + {} + + void GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg); + +private: + std::shared_ptr state_; +}; + +void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) +{ + const double t = rclcpp::Time(msg->header.stamp).seconds(); + + const Eigen::Vector3f acc(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + const Eigen::Vector3f gyr(msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z); + + std::lock_guard lock(state_->mtx); + + // --- Gyro path (high-rate, ~200 Hz) --- + // Just buffer; accel will be interpolated to these timestamps + state_->gyro_data.push_back(gyr); + state_->gyro_timestamps.push_back(t); + + // --- Accel path (lower-rate, ~60 Hz or same rate depending on IMU) --- + // ROS Imu message carries both in the same message, so we treat every + // message as potentially carrying a new accel sample. We detect a new + // accel sample by checking if t > cur_accel_timestamp + if (t > state_->cur_accel_timestamp) { - tfBroadcaster = std::make_shared(this); - staticTfBroadcaster = std::make_shared(this); + state_->prev_accel_data = state_->cur_accel_data; + state_->prev_accel_timestamp = state_->cur_accel_timestamp; + state_->cur_accel_data = acc; + state_->cur_accel_timestamp = t; } - void SyncWithImu(); + // Interpolate accel to cover any gyro timestamps not yet synced + while (state_->gyro_timestamps.size() > state_->accel_timestamps_sync.size()) + { + const int idx = static_cast(state_->accel_timestamps_sync.size()); + const double target_time = state_->gyro_timestamps[idx]; + const Eigen::Vector3f &gyro_at_idx = state_->gyro_data[idx]; + + ORB_SLAM3::IMU::Point pt = interpolateAccel( + target_time, + state_->cur_accel_data, state_->cur_accel_timestamp, + state_->prev_accel_data, state_->prev_accel_timestamp, + gyro_at_idx); + + state_->accel_data_sync.push_back(pt.a); // Eigen::Vector3f member + state_->accel_timestamps_sync.push_back(target_time); + } +} + +// --------------------------------------------------------------------------- +// ImageGrabber — handles image callback and the main SyncWithImu loop +// --------------------------------------------------------------------------- + +class ImageGrabber : public rclcpp::Node +{ +public: + ImageGrabber(std::shared_ptr state, + ORB_SLAM3::System *slam) + : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)) + , state_(std::move(state)) + , slam_(slam) + {} void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); - // void GrabArUcoMarker(const aruco_msgs::MarkerArray &msg); - cv::Mat GetImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg); - void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage); - void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraph); + void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msg); + void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msg); - std::mutex mBufMutex; - std::atomic mustStop{false}; - std::shared_ptr mpImuGb; - std::queue img0Buf; + // Entry point for the sync thread + void SyncWithImu(); + std::atomic mustStop{false}; private: - double minMarkerTimeDiff = std::numeric_limits::max(); - std::vector matchedMarkers; + std::shared_ptr state_; + ORB_SLAM3::System *slam_; }; -void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &imu_msg) -{ - std::lock_guard lock(mBufMutex); - imuBuf.push(imu_msg); -} +// --------------------------------------------------------------------------- +// GrabImage +// --------------------------------------------------------------------------- -void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg) -{ - std::lock_guard lock(mBufMutex); - if (!img0Buf.empty()) - img0Buf.pop(); - img0Buf.push(img_msg); -} - -cv::Mat ImageGrabber::GetImage(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg) +void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { cv_bridge::CvImageConstPtr cv_ptr; try { - cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8); + cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - RCLCPP_ERROR(this->get_logger(), "[Error] `cv_bridge` exception: %s", e.what()); - return cv::Mat(); + RCLCPP_ERROR(this->get_logger(), "[Error] cv_bridge exception: %s", e.what()); + return; } - std::pair> result = - findNearestMarker(rclcpp::Time(cv_ptr->header.stamp).seconds()); - minMarkerTimeDiff = result.first; - matchedMarkers = result.second; + const double new_ts = rclcpp::Time(cv_ptr->header.stamp).seconds(); - return cv_ptr->image.clone(); -} + std::unique_lock lock(state_->mtx); -void ImageGrabber::SyncWithImu() -{ - if (!mpImuGb) - { - RCLCPP_ERROR(this->get_logger(), "[Error] IMU Grabber not initialized!"); + // Duplicate-timestamp guard + if (std::abs(state_->image_timestamp - new_ts) < 1e-3) return; - } - while (!mustStop) - { - bool hasData = false; - { - std::lock_guard lock(mBufMutex); - std::lock_guard lock2(mpImuGb->mBufMutex); - hasData = !img0Buf.empty() && !mpImuGb->imuBuf.empty(); - } + state_->dropped_frames++; - if (!hasData) - { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - continue; - } + state_->image = cv_ptr->image.clone(); + state_->image_timestamp = new_ts; + state_->image_ready = true; - cv::Mat im; - double tIm = 0.0; - rclcpp::Time msgTime; - Eigen::Vector3f Wbb = Eigen::Vector3f::Zero(); - std::vector vImuMeas; + // Resolve marker association for this timestamp + auto [min_diff, markers] = findNearestMarker(new_ts); + state_->min_marker_time_diff = min_diff; + state_->matched_markers = markers; - { - std::lock_guard lock(mBufMutex); - std::lock_guard lock2(mpImuGb->mBufMutex); + // Flush any remaining gyro→accel sync gaps, using the last known accel + while (state_->gyro_timestamps.size() > state_->accel_timestamps_sync.size()) + { + const int idx = static_cast(state_->accel_timestamps_sync.size()); + const double target_time = state_->gyro_timestamps[idx]; + const Eigen::Vector3f &gyro_at_idx = state_->gyro_data[idx]; + + ORB_SLAM3::IMU::Point pt = interpolateAccel( + target_time, + state_->cur_accel_data, state_->cur_accel_timestamp, + state_->prev_accel_data, state_->prev_accel_timestamp, + gyro_at_idx); + + state_->accel_data_sync.push_back(pt.a); + state_->accel_timestamps_sync.push_back(target_time); + } - if (img0Buf.empty() || mpImuGb->imuBuf.empty()) - continue; + lock.unlock(); + state_->image_ready_cv.notify_all(); +} - tIm = rclcpp::Time(img0Buf.front()->header.stamp).seconds(); - const double latestImuTime = rclcpp::Time(mpImuGb->imuBuf.back()->header.stamp).seconds(); +// --------------------------------------------------------------------------- +// SyncWithImu +// --------------------------------------------------------------------------- - if (tIm > latestImuTime) +void ImageGrabber::SyncWithImu() +{ + while (!mustStop && !slam_->isShutDown()) + { + cv::Mat im; + double tIm = 0.0; + rclcpp::Time msgTime; + Eigen::Vector3f Wbb = Eigen::Vector3f::Zero(); + std::vector vImuMeas; + double min_marker_diff; + std::vector matched_markers; + + // --- Critical section: wait, copy, clear --- + { + std::unique_lock lk(state_->mtx); + state_->image_ready_cv.wait(lk, [this]{ return state_->image_ready || mustStop; }); + + if (mustStop) + break; + + // Log dropped frames + if (state_->dropped_frames > 1) + RCLCPP_WARN(this->get_logger(), "%d dropped frames", state_->dropped_frames - 1); + state_->dropped_frames = 0; + + // Copy image + im = state_->image.clone(); + tIm = state_->image_timestamp; + msgTime = rclcpp::Time(static_cast(tIm * 1e9)); + + // Copy marker state + min_marker_diff = state_->min_marker_time_diff; + matched_markers = state_->matched_markers; + + // Build IMU measurement vector from synced accel + gyro + const size_t n = state_->gyro_timestamps.size(); + vImuMeas.reserve(n); + for (size_t i = 0; i < n; ++i) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "[Warning] Waiting for IMU data ... image time (%.3f) is ahead of latest IMU time (%.3f)!", - tIm, latestImuTime); - continue; + if (i < state_->accel_data_sync.size()) + { + const Eigen::Vector3f &acc = state_->accel_data_sync[i]; + const Eigen::Vector3f &gyr = state_->gyro_data[i]; + + vImuMeas.emplace_back( + acc.x(), acc.y(), acc.z(), + gyr.x(), gyr.y(), gyr.z(), + state_->gyro_timestamps[i]); + + // Wbb = angular velocity of the IMU sample closest to image timestamp. + // Pick the last sample before or at tIm + if (state_->gyro_timestamps[i] <= tIm) + Wbb = state_->gyro_data[i]; + } } - msgTime = rclcpp::Time(img0Buf.front()->header.stamp); - im = GetImage(img0Buf.front()); - img0Buf.pop(); - - while (!mpImuGb->imuBuf.empty() && - rclcpp::Time(mpImuGb->imuBuf.front()->header.stamp).seconds() <= tIm) - { - const auto imuMsg = mpImuGb->imuBuf.front(); - const double t = rclcpp::Time(imuMsg->header.stamp).seconds(); - const cv::Point3f acc(imuMsg->linear_acceleration.x, - imuMsg->linear_acceleration.y, - imuMsg->linear_acceleration.z); - const cv::Point3f gyr(imuMsg->angular_velocity.x, - imuMsg->angular_velocity.y, - imuMsg->angular_velocity.z); - - vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t)); - Wbb << imuMsg->angular_velocity.x, imuMsg->angular_velocity.y, imuMsg->angular_velocity.z; - mpImuGb->imuBuf.pop(); - } + // Clear shared buffers + state_->gyro_data.clear(); + state_->gyro_timestamps.clear(); + state_->accel_data_sync.clear(); + state_->accel_timestamps_sync.clear(); + state_->image_ready = false; } + // Lock released — TrackMonocular runs outside lock if (im.empty()) continue; - if (minMarkerTimeDiff < 0.05) + // Image scaling + const float imageScale = slam_->GetImageScale(); + if (imageScale != 1.f) + { + const int w = static_cast(im.cols * imageScale); + const int h = static_cast(im.rows * imageScale); + cv::resize(im, im, cv::Size(w, h)); + } + + // Track + if (min_marker_diff < 0.05) { - pSLAM->TrackMonocular(im, tIm, vImuMeas, "", matchedMarkers); + slam_->TrackMonocular(im, tIm, vImuMeas, "", matched_markers); markersBuffer.clear(); } else - pSLAM->TrackMonocular(im, tIm, vImuMeas); + { + slam_->TrackMonocular(im, tIm, vImuMeas); + } publishTopics(msgTime, Wbb); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } -/** - * @brief Callback function to get scene segmentation results from the SemanticSegmenter module - * - * @param msgSegImage The segmentation results from the SemanticSegmenter - */ void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage) { cv_bridge::CvImageConstPtr cvImgSeg; - uint64_t keyFrameId = msgSegImage.key_frame_id.data; + const uint64_t keyFrameId = msgSegImage.key_frame_id.data; try { - cvImgSeg = cv_bridge::toCvCopy(std::make_shared(msgSegImage.segmented_image), - sensor_msgs::image_encodings::BGR8); + cvImgSeg = cv_bridge::toCvCopy( + std::make_shared(msgSegImage.segmented_image), + sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception &e) { - RCLCPP_ERROR(this->get_logger(), "[Error] `cv_bridge` exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "[Error] cv_bridge exception: %s", e.what()); return; } pcl::PCLPointCloud2::Ptr pclPc2SegPrb(new pcl::PCLPointCloud2); pcl_conversions::toPCL(msgSegImage.segmented_image_probability, *pclPc2SegPrb); - std::tuple tuple(keyFrameId, cvImgSeg->image, pclPc2SegPrb); - pSLAM->addSegmentedImage(&tuple); + auto tuple = std::make_tuple(keyFrameId, cvImgSeg->image, pclPc2SegPrb); + slam_->addSegmentedImage(&tuple); } -/** - * @brief Callback function to get the skeleton graph from the `voxblox` module - * - * @param msgSkeletonGraphs The skeleton graph from the `voxblox` module - */ void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraphs) { setVoxbloxSkeletonCluster(msgSkeletonGraphs); } +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + int main(int argc, char **argv) { rclcpp::init(argc, argv); @@ -237,85 +384,89 @@ int main(int argc, char **argv) if (argc > 1) RCLCPP_WARN(node->get_logger(), "Arguments supplied via command line are ignored."); - std::string nodeName = node->get_name(); - - node->declare_parameter("yaw", 0.0); - node->declare_parameter("roll", 0.0); + // --- Parameters --- + node->declare_parameter("yaw", 0.0); + node->declare_parameter("roll", 0.0); node->declare_parameter("pitch", 0.0); - node->declare_parameter("enable_pangolin", true); - node->declare_parameter("static_transform", false); - node->declare_parameter("frame_imu", "imu"); - node->declare_parameter("frame_map", "map"); + node->declare_parameter("enable_pangolin", true); + node->declare_parameter("static_transform", false); node->declare_parameter("colored_pointcloud", true); node->declare_parameter("publish_pointclouds", true); - node->declare_parameter("frame_world", "world"); - node->declare_parameter("frame_camera", "camera"); - node->declare_parameter("voc_file", "file_not_set"); - node->declare_parameter("settings_file", "file_not_set"); - node->declare_parameter("sys_params_file", "file_not_set"); + node->declare_parameter("frame_imu", "imu"); + node->declare_parameter("frame_map", "map"); + node->declare_parameter("frame_world", "world"); + node->declare_parameter("frame_camera", "camera"); node->declare_parameter("frame_structural_element", "struc_elem"); node->declare_parameter("frame_building_component", "build_comp"); + node->declare_parameter("voc_file", "file_not_set"); + node->declare_parameter("settings_file", "file_not_set"); + node->declare_parameter("sys_params_file", "file_not_set"); - std::string vocFile = node->get_parameter("voc_file").as_string(); - std::string settingsFile = node->get_parameter("settings_file").as_string(); - std::string sysParamsFile = node->get_parameter("sys_params_file").as_string(); + const std::string vocFile = node->get_parameter("voc_file").as_string(); + const std::string settingsFile = node->get_parameter("settings_file").as_string(); + const std::string sysParamsFile = node->get_parameter("sys_params_file").as_string(); if (vocFile == "file_not_set" || settingsFile == "file_not_set") { - RCLCPP_ERROR(node->get_logger(), "[Error] 'voc_file' and 'settings_file' are not provided in the launch file! Exiting..."); + RCLCPP_ERROR(node->get_logger(), "[Error] 'voc_file' and 'settings_file' not set. Exiting."); rclcpp::shutdown(); return 1; } - if (sysParamsFile == "file_not_set") { - RCLCPP_ERROR(node->get_logger(), "[Error] The `YAML` file containing system parameters is not provided in the launch file! Exiting..."); + RCLCPP_ERROR(node->get_logger(), "[Error] 'sys_params_file' not set. Exiting."); rclcpp::shutdown(); return 1; } - - yaw = node->get_parameter("yaw").as_double(); - roll = node->get_parameter("roll").as_double(); - pitch = node->get_parameter("pitch").as_double(); - frameImu = node->get_parameter("frame_imu").as_string(); - frameMap = node->get_parameter("frame_map").as_string(); - frameWorld = node->get_parameter("frame_world").as_string(); - frameCamera = node->get_parameter("frame_camera").as_string(); - colorPointcloud = node->get_parameter("colored_pointcloud").as_bool(); - pubPointClouds = node->get_parameter("publish_pointclouds").as_bool(); - frameBC = node->get_parameter("frame_building_component").as_string(); - frameSE = node->get_parameter("frame_structural_element").as_string(); + + // Populate globals required by common.h helpers (publishTopics, etc.) + yaw = node->get_parameter("yaw").as_double(); + roll = node->get_parameter("roll").as_double(); + pitch = node->get_parameter("pitch").as_double(); + frameImu = node->get_parameter("frame_imu").as_string(); + frameMap = node->get_parameter("frame_map").as_string(); + frameWorld = node->get_parameter("frame_world").as_string(); + frameCamera = node->get_parameter("frame_camera").as_string(); + colorPointcloud = node->get_parameter("colored_pointcloud").as_bool(); + pubPointClouds = node->get_parameter("publish_pointclouds").as_bool(); + frameBC = node->get_parameter("frame_building_component").as_string(); + frameSE = node->get_parameter("frame_structural_element").as_string(); pubStaticTransform = node->get_parameter("static_transform").as_bool(); - bool enablePangolin = node->get_parameter("enable_pangolin").as_bool(); + const bool enablePangolin = node->get_parameter("enable_pangolin").as_bool(); - auto imugb = std::make_shared(); - auto igb = std::make_shared(imugb); + // --- SLAM system (owned here, not a global) --- + ORB_SLAM3::System slam(vocFile, settingsFile, sysParamsFile, + ORB_SLAM3::System::IMU_MONOCULAR, enablePangolin); + pSLAM = &slam; // common.h helpers still need this pointer - sensorType = ORB_SLAM3::System::IMU_MONOCULAR; - pSLAM = new ORB_SLAM3::System(vocFile, settingsFile, sysParamsFile, sensorType, enablePangolin); + // --- Shared state --- + auto state = std::make_shared(); - using sensor_msgs::msg::Image; - using sensor_msgs::msg::Imu; + // --- TF broadcasters (one pair, shared) --- + tfBroadcaster = + std::make_shared(node); + staticTfBroadcaster = + std::make_shared(node); + + // --- Grabber nodes --- + auto imugb = std::make_shared(state); + auto igb = std::make_shared(state, &slam); + // --- QoS (sensor data profile — same as before) --- rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); sensorQos.reliability(rclcpp::ReliabilityPolicy::BestEffort); sensorQos.durability(rclcpp::DurabilityPolicy::Volatile); + using sensor_msgs::msg::Image; + using sensor_msgs::msg::Imu; + auto subImu = node->create_subscription( "/imu", sensorQos, - [imugb](const Imu::ConstSharedPtr msg) - { imugb->GrabImu(msg); }); + [imugb](const Imu::ConstSharedPtr msg) { imugb->GrabImu(msg); }); auto subImg = node->create_subscription( "/camera/image_raw", sensorQos, - [igb](const Image::ConstSharedPtr msg) - { igb->GrabImage(msg); }); - - // Subscribe to the markers detected by `aruco_ros` library - // auto subAruco = node->create_subscription( - // "/aruco_marker_publisher/markers", 1, - // [igb](const aruco_msgs::msg::MarkerArray::SharedPtr msg) - // { igb->GrabArUcoMarker(*msg); }); + [igb](const Image::ConstSharedPtr msg) { igb->GrabImage(msg); }); auto subSegmentedImage = node->create_subscription( "/camera/color/image_segment", 50, @@ -329,18 +480,19 @@ int main(int argc, char **argv) static std::shared_ptr imageTransport = std::make_shared(node); - setupPublishers(node, imageTransport, nodeName); - setupServices(node, nodeName); + setupPublishers(node, imageTransport, node->get_name()); + setupServices(node, node->get_name()); + // --- Sync thread --- std::thread syncThread(&ImageGrabber::SyncWithImu, igb); rclcpp::spin(node); - pSLAM->Shutdown(); + slam.Shutdown(); igb->mustStop = true; + state->image_ready_cv.notify_all(); // unblock SyncWithImu if it's waiting syncThread.join(); rclcpp::shutdown(); - return 0; -} +} \ No newline at end of file From 4f26daba9df2e05ac0af83a75c34278fa01eee48 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 8 Jun 2026 18:52:18 +0200 Subject: [PATCH 08/59] fix param initialization bug --- core/include/Types/SystemParams.h | 5 +++-- core/src/SemanticsManager.cc | 5 +++-- core/src/Types/SystemParams.cc | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/core/include/Types/SystemParams.h b/core/include/Types/SystemParams.h index 0bceab7..45362af 100644 --- a/core/include/Types/SystemParams.h +++ b/core/include/Types/SystemParams.h @@ -160,7 +160,8 @@ namespace ORB_SLAM3 { GEOMETRIC = 0, FREE_SPACE = 1, - GNN = 2 + GNN = 2, + GNN_NEW = 3 }; Method method = FREE_SPACE; @@ -185,4 +186,4 @@ namespace ORB_SLAM3 }; } -#endif \ No newline at end of file +#endif diff --git a/core/src/SemanticsManager.cc b/core/src/SemanticsManager.cc index 206332c..7c4efc0 100644 --- a/core/src/SemanticsManager.cc +++ b/core/src/SemanticsManager.cc @@ -52,7 +52,8 @@ namespace ORB_SLAM3 // Check for possible room candidates if (sysParams->room_seg.method == SystemParams::room_seg::Method::FREE_SPACE) detectRoom_FreeSpaceCluster(); - else if (sysParams->room_seg.method == SystemParams::room_seg::Method::GNN) + else if (sysParams->room_seg.method == SystemParams::room_seg::Method::GNN || + sysParams->room_seg.method == SystemParams::room_seg::Method::GNN_NEW) detectRoom_GNN(); // Re-associate rooms based on walls and clusters @@ -653,4 +654,4 @@ namespace ORB_SLAM3 } } } -} \ No newline at end of file +} diff --git a/core/src/Types/SystemParams.cc b/core/src/Types/SystemParams.cc index 4b79d27..0b6eeb5 100644 --- a/core/src/Types/SystemParams.cc +++ b/core/src/Types/SystemParams.cc @@ -50,7 +50,7 @@ namespace ORB_SLAM3 try { // General Parameters - general.env_database = mConfig["general"]["env_database"].as(); + general.env_database = mConfig["general"]["env_database"].as(""); general.mode_of_operation = static_cast(mConfig["general"]["mode_of_operation"].as()); // Marker Parameters From 29405fe06b278e33535cf8ac5367b53cbabeff57 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 8 Jun 2026 19:14:50 +0200 Subject: [PATCH 09/59] refactor image handling in SharedState to use a queue for better timestamp management --- src/ros_mono_inertial.cc | 75 +++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 40 deletions(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 7df0609..dff6b23 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -72,6 +72,14 @@ struct SharedState std::mutex mtx; std::condition_variable image_ready_cv; + struct ImagePacket + { + cv::Mat image; + double timestamp = -1.0; + double min_marker_time_diff = std::numeric_limits::max(); + std::vector matched_markers; + }; + // Raw gyro — collected at ~200 Hz std::vector gyro_timestamps; std::vector gyro_data; @@ -86,11 +94,10 @@ struct SharedState Eigen::Vector3f cur_accel_data; double cur_accel_timestamp = 0.0; - // Image state - cv::Mat image; - double image_timestamp = -1.0; - bool image_ready = false; - int dropped_frames = 0; + // Image queue keeps timestamps monotonic even if tracking briefly lags. + std::queue image_queue; + std::size_t max_image_queue_size = 60; + int dropped_frames = 0; // Marker state (populated inside the image callback path) double min_marker_time_diff = std::numeric_limits::max(); @@ -209,41 +216,27 @@ void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) const double new_ts = rclcpp::Time(cv_ptr->header.stamp).seconds(); + // Resolve marker association for this timestamp. + auto [min_diff, markers] = findNearestMarker(new_ts); + std::unique_lock lock(state_->mtx); - // Duplicate-timestamp guard - if (std::abs(state_->image_timestamp - new_ts) < 1e-3) + if (!state_->image_queue.empty() && std::abs(state_->image_queue.back().timestamp - new_ts) < 1e-3) return; - state_->dropped_frames++; - - state_->image = cv_ptr->image.clone(); - state_->image_timestamp = new_ts; - state_->image_ready = true; - - // Resolve marker association for this timestamp - auto [min_diff, markers] = findNearestMarker(new_ts); - state_->min_marker_time_diff = min_diff; - state_->matched_markers = markers; - - // Flush any remaining gyro→accel sync gaps, using the last known accel - while (state_->gyro_timestamps.size() > state_->accel_timestamps_sync.size()) + if (state_->image_queue.size() >= state_->max_image_queue_size) { - const int idx = static_cast(state_->accel_timestamps_sync.size()); - const double target_time = state_->gyro_timestamps[idx]; - const Eigen::Vector3f &gyro_at_idx = state_->gyro_data[idx]; - - ORB_SLAM3::IMU::Point pt = interpolateAccel( - target_time, - state_->cur_accel_data, state_->cur_accel_timestamp, - state_->prev_accel_data, state_->prev_accel_timestamp, - gyro_at_idx); - - state_->accel_data_sync.push_back(pt.a); - state_->accel_timestamps_sync.push_back(target_time); + state_->image_queue.pop(); + state_->dropped_frames++; } - lock.unlock(); + SharedState::ImagePacket packet; + packet.image = cv_ptr->image.clone(); + packet.timestamp = new_ts; + packet.min_marker_time_diff = min_diff; + packet.matched_markers = std::move(markers); + + state_->image_queue.push(std::move(packet)); state_->image_ready_cv.notify_all(); } @@ -266,7 +259,7 @@ void ImageGrabber::SyncWithImu() // --- Critical section: wait, copy, clear --- { std::unique_lock lk(state_->mtx); - state_->image_ready_cv.wait(lk, [this]{ return state_->image_ready || mustStop; }); + state_->image_ready_cv.wait(lk, [this]{ return !state_->image_queue.empty() || mustStop; }); if (mustStop) break; @@ -276,14 +269,17 @@ void ImageGrabber::SyncWithImu() RCLCPP_WARN(this->get_logger(), "%d dropped frames", state_->dropped_frames - 1); state_->dropped_frames = 0; - // Copy image - im = state_->image.clone(); - tIm = state_->image_timestamp; + // Copy next queued image in timestamp order. + SharedState::ImagePacket packet = std::move(state_->image_queue.front()); + state_->image_queue.pop(); + + im = std::move(packet.image); + tIm = packet.timestamp; msgTime = rclcpp::Time(static_cast(tIm * 1e9)); // Copy marker state - min_marker_diff = state_->min_marker_time_diff; - matched_markers = state_->matched_markers; + min_marker_diff = packet.min_marker_time_diff; + matched_markers = std::move(packet.matched_markers); // Build IMU measurement vector from synced accel + gyro const size_t n = state_->gyro_timestamps.size(); @@ -312,7 +308,6 @@ void ImageGrabber::SyncWithImu() state_->gyro_timestamps.clear(); state_->accel_data_sync.clear(); state_->accel_timestamps_sync.clear(); - state_->image_ready = false; } // Lock released — TrackMonocular runs outside lock From bce94dfb75ae9ac0f00545d08a5a18f9b1d3ff8c Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 9 Jun 2026 17:25:44 +0200 Subject: [PATCH 10/59] refactor SharedState to use deque for gyro and accel data, increase max image queue size to 300, and improve synchronization logic --- src/ros_mono_inertial.cc | 74 ++++++++++++++++++++++------------------ 1 file changed, 41 insertions(+), 33 deletions(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index dff6b23..9a3e73c 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -81,12 +82,12 @@ struct SharedState }; // Raw gyro — collected at ~200 Hz - std::vector gyro_timestamps; - std::vector gyro_data; + std::deque gyro_timestamps; + std::deque gyro_data; // Accel interpolated to gyro timestamps - std::vector accel_timestamps_sync; - std::vector accel_data_sync; + std::deque accel_timestamps_sync; + std::deque accel_data_sync; // Running accel state for interpolation Eigen::Vector3f prev_accel_data; @@ -96,7 +97,7 @@ struct SharedState // Image queue keeps timestamps monotonic even if tracking briefly lags. std::queue image_queue; - std::size_t max_image_queue_size = 60; + std::size_t max_image_queue_size = 300; int dropped_frames = 0; // Marker state (populated inside the image callback path) @@ -151,10 +152,12 @@ void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) state_->cur_accel_data = acc; state_->cur_accel_timestamp = t; } + // std::cout << "[ROS] Received IMU message at time " << t << std::endl; // Interpolate accel to cover any gyro timestamps not yet synced while (state_->gyro_timestamps.size() > state_->accel_timestamps_sync.size()) { + // std::cout << "[ROS] Interpolating accel for gyro timestamp " << state_->gyro_timestamps[state_->accel_timestamps_sync.size()] << std::endl; const int idx = static_cast(state_->accel_timestamps_sync.size()); const double target_time = state_->gyro_timestamps[idx]; const Eigen::Vector3f &gyro_at_idx = state_->gyro_data[idx]; @@ -168,6 +171,8 @@ void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) state_->accel_data_sync.push_back(pt.a); // Eigen::Vector3f member state_->accel_timestamps_sync.push_back(target_time); } + + state_->image_ready_cv.notify_all(); } // --------------------------------------------------------------------------- @@ -216,6 +221,8 @@ void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) const double new_ts = rclcpp::Time(cv_ptr->header.stamp).seconds(); + // std::cout << "[ROS] Received image message at time " << new_ts << std::endl; + // Resolve marker association for this timestamp. auto [min_diff, markers] = findNearestMarker(new_ts); @@ -259,7 +266,12 @@ void ImageGrabber::SyncWithImu() // --- Critical section: wait, copy, clear --- { std::unique_lock lk(state_->mtx); - state_->image_ready_cv.wait(lk, [this]{ return !state_->image_queue.empty() || mustStop; }); + state_->image_ready_cv.wait(lk, [this] { + return mustStop || + (!state_->image_queue.empty() && + !state_->gyro_timestamps.empty() && + state_->gyro_timestamps.back() >= state_->image_queue.front().timestamp); + }); if (mustStop) break; @@ -269,7 +281,8 @@ void ImageGrabber::SyncWithImu() RCLCPP_WARN(this->get_logger(), "%d dropped frames", state_->dropped_frames - 1); state_->dropped_frames = 0; - // Copy next queued image in timestamp order. + // Copy next queued image in timestamp order. Do this only after + // the IMU buffer has reached this image timestamp. SharedState::ImagePacket packet = std::move(state_->image_queue.front()); state_->image_queue.pop(); @@ -281,33 +294,28 @@ void ImageGrabber::SyncWithImu() min_marker_diff = packet.min_marker_time_diff; matched_markers = std::move(packet.matched_markers); - // Build IMU measurement vector from synced accel + gyro - const size_t n = state_->gyro_timestamps.size(); - vImuMeas.reserve(n); - for (size_t i = 0; i < n; ++i) + // Build IMU measurement vector from synced accel + gyro, consuming + // only samples up to this image timestamp. Future IMU samples stay + // buffered for the next frame, matching the ORB-SLAM3 ROS1 wrapper. + while (!state_->gyro_timestamps.empty() && + !state_->accel_data_sync.empty() && + state_->gyro_timestamps.front() <= tIm) { - if (i < state_->accel_data_sync.size()) - { - const Eigen::Vector3f &acc = state_->accel_data_sync[i]; - const Eigen::Vector3f &gyr = state_->gyro_data[i]; - - vImuMeas.emplace_back( - acc.x(), acc.y(), acc.z(), - gyr.x(), gyr.y(), gyr.z(), - state_->gyro_timestamps[i]); - - // Wbb = angular velocity of the IMU sample closest to image timestamp. - // Pick the last sample before or at tIm - if (state_->gyro_timestamps[i] <= tIm) - Wbb = state_->gyro_data[i]; - } + const double imuTime = state_->gyro_timestamps.front(); + const Eigen::Vector3f acc = state_->accel_data_sync.front(); + const Eigen::Vector3f gyr = state_->gyro_data.front(); + + vImuMeas.emplace_back( + acc.x(), acc.y(), acc.z(), + gyr.x(), gyr.y(), gyr.z(), + imuTime); + Wbb = gyr; + + state_->gyro_data.pop_front(); + state_->gyro_timestamps.pop_front(); + state_->accel_data_sync.pop_front(); + state_->accel_timestamps_sync.pop_front(); } - - // Clear shared buffers - state_->gyro_data.clear(); - state_->gyro_timestamps.clear(); - state_->accel_data_sync.clear(); - state_->accel_timestamps_sync.clear(); } // Lock released — TrackMonocular runs outside lock @@ -460,7 +468,7 @@ int main(int argc, char **argv) [imugb](const Imu::ConstSharedPtr msg) { imugb->GrabImu(msg); }); auto subImg = node->create_subscription( - "/camera/image_raw", sensorQos, + "/camera/image_raw", 1, [igb](const Image::ConstSharedPtr msg) { igb->GrabImage(msg); }); auto subSegmentedImage = node->create_subscription( From 58354f8758626f68c1fcb2bfa0826a63e5cbf0cd Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 10 Jun 2026 12:22:32 +0200 Subject: [PATCH 11/59] enable rgb input support --- include/common.h | 1 + src/common.cc | 17 +++++++++++++++-- src/ros_mono_inertial.cc | 16 +++++++++++++--- 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/include/common.h b/include/common.h index f7ab45c..0a7ec47 100644 --- a/include/common.h +++ b/include/common.h @@ -57,6 +57,7 @@ #include #include #include +#include #include // #include // #include diff --git a/src/common.cc b/src/common.cc index 8103cb2..08b9b38 100644 --- a/src/common.cc +++ b/src/common.cc @@ -415,6 +415,9 @@ void publishKeyFrameImages(std::vector keyframe_vec, rclc if (keyframe->isPublished) continue; + if (keyframe->mImage.empty()) + continue; + // Create an object of VSGraphDataMsg segmenter_ros::msg::VSGraphDataMsg vsGraphPublisher = segmenter_ros::msg::VSGraphDataMsg(); std_msgs::msg::Header header; @@ -422,8 +425,18 @@ void publishKeyFrameImages(std::vector keyframe_vec, rclc header.frame_id = frameWorld; std_msgs::msg::UInt64 kfId; kfId.data = keyframe->mnId; + cv::Mat keyframeImageBgr; + if (keyframe->mImage.channels() == 1) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_GRAY2BGR); + else if (keyframe->mImage.channels() == 4) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_BGRA2BGR); + else if (keyframe->mImage.channels() == 3) + keyframeImageBgr = keyframe->mImage; + else + continue; + const sensor_msgs::msg::Image::SharedPtr rendered_image_msg = - cv_bridge::CvImage(header, "bgr8", keyframe->mImage).toImageMsg(); + cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, keyframeImageBgr).toImageMsg(); vsGraphPublisher.header = header; vsGraphPublisher.key_frame_id = kfId; @@ -1555,4 +1568,4 @@ void setGNNBasedRoomCandidates(const vs_graphs::msg::VSGraphsAllDetectdetRooms & // [TODO] Define a 'setGNNRoomCandidates' in System.h pSLAM->setGNNRoomCandidates(gnnRoomCandidates); -} \ No newline at end of file +} diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 9a3e73c..5410aba 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -211,7 +211,7 @@ void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) cv_bridge::CvImageConstPtr cv_ptr; try { - cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8); + cv_ptr = cv_bridge::toCvShare(msg); } catch (cv_bridge::Exception &e) { @@ -238,7 +238,17 @@ void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) } SharedState::ImagePacket packet; - packet.image = cv_ptr->image.clone(); + if (cv_ptr->image.channels() == 1 || cv_ptr->image.channels() == 3 || cv_ptr->image.channels() == 4) + { + packet.image = cv_ptr->image.clone(); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "[Error] Unsupported image encoding '%s' with %d channels.", + msg->encoding.c_str(), cv_ptr->image.channels()); + return; + } packet.timestamp = new_ts; packet.min_marker_time_diff = min_diff; packet.matched_markers = std::move(markers); @@ -498,4 +508,4 @@ int main(int argc, char **argv) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From e3ee25e486568110cbe3a9d427ae319c3d7baac6 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 10 Jun 2026 12:36:25 +0200 Subject: [PATCH 12/59] refactor to remove private slam instance to shared pSLAM --- src/ros_mono_inertial.cc | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 5410aba..9da3745 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -182,11 +182,9 @@ void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) class ImageGrabber : public rclcpp::Node { public: - ImageGrabber(std::shared_ptr state, - ORB_SLAM3::System *slam) + ImageGrabber(std::shared_ptr state) : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)) , state_(std::move(state)) - , slam_(slam) {} void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); @@ -199,7 +197,6 @@ class ImageGrabber : public rclcpp::Node std::atomic mustStop{false}; private: std::shared_ptr state_; - ORB_SLAM3::System *slam_; }; // --------------------------------------------------------------------------- @@ -263,7 +260,7 @@ void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) void ImageGrabber::SyncWithImu() { - while (!mustStop && !slam_->isShutDown()) + while (!mustStop && !pSLAM->isShutDown()) { cv::Mat im; double tIm = 0.0; @@ -333,7 +330,7 @@ void ImageGrabber::SyncWithImu() continue; // Image scaling - const float imageScale = slam_->GetImageScale(); + const float imageScale = pSLAM->GetImageScale(); if (imageScale != 1.f) { const int w = static_cast(im.cols * imageScale); @@ -344,12 +341,12 @@ void ImageGrabber::SyncWithImu() // Track if (min_marker_diff < 0.05) { - slam_->TrackMonocular(im, tIm, vImuMeas, "", matched_markers); + pSLAM->TrackMonocular(im, tIm, vImuMeas, "", matched_markers); markersBuffer.clear(); } else { - slam_->TrackMonocular(im, tIm, vImuMeas); + pSLAM->TrackMonocular(im, tIm, vImuMeas); } publishTopics(msgTime, Wbb); @@ -377,7 +374,7 @@ void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg & pcl_conversions::toPCL(msgSegImage.segmented_image_probability, *pclPc2SegPrb); auto tuple = std::make_tuple(keyFrameId, cvImgSeg->image, pclPc2SegPrb); - slam_->addSegmentedImage(&tuple); + pSLAM->addSegmentedImage(&tuple); } void ImageGrabber::GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msgSkeletonGraphs) @@ -447,10 +444,9 @@ int main(int argc, char **argv) pubStaticTransform = node->get_parameter("static_transform").as_bool(); const bool enablePangolin = node->get_parameter("enable_pangolin").as_bool(); - // --- SLAM system (owned here, not a global) --- - ORB_SLAM3::System slam(vocFile, settingsFile, sysParamsFile, - ORB_SLAM3::System::IMU_MONOCULAR, enablePangolin); - pSLAM = &slam; // common.h helpers still need this pointer + // --- SLAM system --- + sensorType = ORB_SLAM3::System::IMU_MONOCULAR; + pSLAM = new ORB_SLAM3::System(vocFile, settingsFile, sysParamsFile, sensorType, enablePangolin); // --- Shared state --- auto state = std::make_shared(); @@ -463,9 +459,9 @@ int main(int argc, char **argv) // --- Grabber nodes --- auto imugb = std::make_shared(state); - auto igb = std::make_shared(state, &slam); + auto igb = std::make_shared(state); - // --- QoS (sensor data profile — same as before) --- + // --- QoS (sensor data profile) --- rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); sensorQos.reliability(rclcpp::ReliabilityPolicy::BestEffort); sensorQos.durability(rclcpp::DurabilityPolicy::Volatile); @@ -501,11 +497,11 @@ int main(int argc, char **argv) rclcpp::spin(node); - slam.Shutdown(); + pSLAM->Shutdown(); igb->mustStop = true; state->image_ready_cv.notify_all(); // unblock SyncWithImu if it's waiting syncThread.join(); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file From ea5a79819e5f82a9a420cc4cc6b6709197dbf991 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 10 Jun 2026 16:08:17 +0200 Subject: [PATCH 13/59] add checks to skip frames with insufficient IMU measurements or future timestamps --- src/ros_mono_inertial.cc | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 9da3745..46ccc68 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -329,6 +329,21 @@ void ImageGrabber::SyncWithImu() if (im.empty()) continue; + if (vImuMeas.size() < 2) + { + RCLCPP_WARN(this->get_logger(), + "Skipping frame t=%.9f because vImuMeas has only %zu samples", + tIm, vImuMeas.size()); + continue; + } + if (vImuMeas.back().t > tIm) + { + RCLCPP_WARN(this->get_logger(), + "Skipping frame t=%.9f because last IMU t=%.9f is newer than image", + tIm, vImuMeas.back().t); + continue; + } + // Image scaling const float imageScale = pSLAM->GetImageScale(); if (imageScale != 1.f) From 2b0087c6f7ba4c548a092d2081d8a8d908da3341 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 11 Jun 2026 12:49:36 +0200 Subject: [PATCH 14/59] add auxiliary depth support to KeyFrame and System, update ImageGrabber for depth handling --- core/include/KeyFrame.h | 13 ++ core/include/System.h | 4 + core/src/GeometricSegmentation.cc | 5 +- core/src/KeyFrame.cc | 125 ++++++++++++- core/src/SemanticSegmentation.cc | 8 +- core/src/System.cc | 18 ++ src/ros_mono_inertial.cc | 291 ++++++++++++++++++++++++++---- 7 files changed, 425 insertions(+), 39 deletions(-) diff --git a/core/include/KeyFrame.h b/core/include/KeyFrame.h index 819806d..12c60ed 100644 --- a/core/include/KeyFrame.h +++ b/core/include/KeyFrame.h @@ -447,6 +447,19 @@ namespace ORB_SLAM3 // For Semantic Segmentation cv::Mat mImage; bool isPublished; + cv::Mat mAuxDepth; + bool mbHasAuxDepth = false; + double mAuxDepthTimestamp = 0.0; + std::string mAuxDepthFrameId; + float mAuxDepthMin = 0.2f; + float mAuxDepthMax = 20.0f; + int mAuxDepthStride = 2; + std::string mAuxDepthScaleMode = "none"; + void SetAuxDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, + const std::string &auxDepthFrameId, float auxDepthMin, + float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode); + pcl::PointCloud::Ptr getAuxDepthPointCloud(); // The following variables need to be accessed trough a mutex to be thread safe. protected: diff --git a/core/include/System.h b/core/include/System.h index 437e522..9a7768b 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -270,6 +270,10 @@ namespace ORB_SLAM3 * @param tuple the address of the tuple of segmented image and pointcloud */ void addSegmentedImage(std::tuple *tuple); + void AttachAuxDepthToKeyFrame(uint64_t keyFrameId, const cv::Mat &auxDepth, + double auxDepthTimestamp, const std::string &auxDepthFrameId, + float auxDepthMin, float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode); /** * @brief Get the skeleton cluster coming from the current map diff --git a/core/src/GeometricSegmentation.cc b/core/src/GeometricSegmentation.cc index 6323580..03d423a 100644 --- a/core/src/GeometricSegmentation.cc +++ b/core/src/GeometricSegmentation.cc @@ -130,6 +130,9 @@ namespace ORB_SLAM3 // the depth from points using Machine Learning to get a better plane estimate. pointcloud = getCloudFromSparsePoints(pKF->getCurrentFrameMapPoints()); // mCurrentFrame.mvpMapPoints + if (pointcloud == nullptr) + return extractedPlanes; + // Convert the pointcloud to one with PointXYZRGBA for consistency pcl::PointCloud::Ptr cloudRGBA(new pcl::PointCloud); pcl::copyPointCloud(*pointcloud, *cloudRGBA); @@ -183,4 +186,4 @@ namespace ORB_SLAM3 return Eigen::Vector4d(normal.x(), normal.y(), normal.z(), D); } -} \ No newline at end of file +} diff --git a/core/src/KeyFrame.cc b/core/src/KeyFrame.cc index 829f76f..7800a46 100644 --- a/core/src/KeyFrame.cc +++ b/core/src/KeyFrame.cc @@ -23,6 +23,9 @@ #include "KeyFrame.h" #include "Converter.h" #include "ImuTypes.h" +#include +#include +#include #include namespace ORB_SLAM3 @@ -208,13 +211,131 @@ namespace ORB_SLAM3 return mCurrentFramePointClouds; } + void KeyFrame::SetAuxDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, + const std::string &auxDepthFrameId, float auxDepthMin, + float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode) + { + if (auxDepth.empty()) + return; + + mAuxDepth = auxDepth.clone(); + mbHasAuxDepth = true; + mAuxDepthTimestamp = auxDepthTimestamp; + mAuxDepthFrameId = auxDepthFrameId; + mAuxDepthMin = auxDepthMin; + mAuxDepthMax = auxDepthMax; + mAuxDepthStride = std::max(1, auxDepthStride); + mAuxDepthScaleMode = auxDepthScaleMode; + + std::cout << "AuxDepth: copied depth into keyframe ID " << mnId + << " for segmented image, depth timestamp " + << mAuxDepthTimestamp << std::endl; + } + + pcl::PointCloud::Ptr KeyFrame::getAuxDepthPointCloud() + { + if (!mbHasAuxDepth || mAuxDepth.empty() || mAuxDepth.type() != CV_32FC1) + return nullptr; + + cv::Mat depth = mAuxDepth; + float depthScale = 1.0f; + + if (mAuxDepthScaleMode == "map_median") + { + std::vector mapDepths; + std::vector auxDepths; + const Sophus::SE3f Tcw = GetPose(); + + for (size_t i = 0; i < mCurrentFrameMapPoints.size() && i < mvKeysUn.size(); ++i) + { + MapPoint *pMP = mCurrentFrameMapPoints[i]; + if (!pMP || pMP->isBad()) + continue; + + const cv::KeyPoint &kp = mvKeysUn[i]; + const int u = cvRound(kp.pt.x); + const int v = cvRound(kp.pt.y); + if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) + continue; + + const float aux = depth.at(v, u); + if (!std::isfinite(aux) || aux <= 1e-6f) + continue; + + const Eigen::Vector3f Pc = Tcw * pMP->GetWorldPos(); + if (Pc.z() <= mAuxDepthMin || Pc.z() >= mAuxDepthMax) + continue; + + mapDepths.push_back(Pc.z()); + auxDepths.push_back(aux); + } + + if (!mapDepths.empty() && mapDepths.size() == auxDepths.size()) + { + std::nth_element(mapDepths.begin(), mapDepths.begin() + mapDepths.size() / 2, mapDepths.end()); + std::nth_element(auxDepths.begin(), auxDepths.begin() + auxDepths.size() / 2, auxDepths.end()); + const float medMap = mapDepths[mapDepths.size() / 2]; + const float medAux = auxDepths[auxDepths.size() / 2]; + if (medAux > 1e-6f) + depthScale = medMap / medAux; + } + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->width = depth.cols; + cloud->height = depth.rows; + cloud->is_dense = false; + cloud->points.resize(static_cast(cloud->width) * cloud->height); + + const int stride = std::max(1, mAuxDepthStride); + const float nan = std::numeric_limits::quiet_NaN(); + + for (int v = 0; v < depth.rows; ++v) + { + for (int u = 0; u < depth.cols; ++u) + { + pcl::PointXYZRGB &pt = cloud->at(u, v); + pt.x = nan; + pt.y = nan; + pt.z = nan; + + if ((u % stride) != 0 || (v % stride) != 0) + continue; + + const float z = depth.at(v, u) * depthScale; + if (!std::isfinite(z) || z < mAuxDepthMin || z > mAuxDepthMax) + continue; + + pt.x = (static_cast(u) - cx) * z * invfx; + pt.y = (static_cast(v) - cy) * z * invfy; + pt.z = z; + + if (!mImage.empty() && u < mImage.cols && v < mImage.rows) + { + const cv::Vec3b bgr = mImage.channels() == 3 ? mImage.at(v, u) : cv::Vec3b(0, 0, 0); + pt.r = bgr[2]; + pt.g = bgr[1]; + pt.b = bgr[0]; + } + } + } + + return cloud; + } + void KeyFrame::clearPointCloud() { - mCurrentFramePointClouds->clear(); - mCurrentFramePointClouds = nullptr; + if (mCurrentFramePointClouds) + { + mCurrentFramePointClouds->clear(); + mCurrentFramePointClouds = nullptr; + } // clear images mImage.release(); + mAuxDepth.release(); + mbHasAuxDepth = false; } std::vector::Ptr> KeyFrame::getClsCloudPtrs() const diff --git a/core/src/SemanticSegmentation.cc b/core/src/SemanticSegmentation.cc index 317eddd..07dcf58 100644 --- a/core/src/SemanticSegmentation.cc +++ b/core/src/SemanticSegmentation.cc @@ -49,11 +49,13 @@ namespace ORB_SLAM3 KeyFrame *thisKF = mpAtlas->GetKeyFrameById(std::get<0>(segImgTuple)); if (thisKF == nullptr || thisKF->isBad()) continue; - const pcl::PointCloud::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud(); + pcl::PointCloud::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud(); + if (thisKFPointCloud == nullptr && thisKF->mbHasAuxDepth) + thisKFPointCloud = thisKF->getAuxDepthPointCloud(); if (thisKFPointCloud == nullptr) { std::cout << "SemSeg: skipping KF ID: " << thisKF->mnId << ". Missing pointcloud..." << std::endl; - exit(1); + // exit(1); continue; } @@ -305,4 +307,4 @@ namespace ORB_SLAM3 // cast a vote for the plane semantics matchedPlane->castWeightedVote(planeType, confidence); } -} \ No newline at end of file +} diff --git a/core/src/System.cc b/core/src/System.cc index bdaa7bd..d96b7dc 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -258,6 +258,24 @@ namespace ORB_SLAM3 mpSemanticSegmentation->AddSegmentedFrameToBuffer(tuple); } + void System::AttachAuxDepthToKeyFrame(uint64_t keyFrameId, const cv::Mat &auxDepth, + double auxDepthTimestamp, const std::string &auxDepthFrameId, + float auxDepthMin, float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode) + { + ORB_SLAM3::KeyFrame *pKF = mpAtlas->GetKeyFrameById(keyFrameId); + if (pKF == nullptr || pKF->isBad()) + { + std::cout << "AuxDepth: keyframe ID " << keyFrameId + << " not available for segmented depth attachment" << std::endl; + return; + } + + pKF->SetAuxDepth(auxDepth, auxDepthTimestamp, auxDepthFrameId, + auxDepthMin, auxDepthMax, auxDepthStride, + auxDepthScaleMode); + } + std::vector> System::getSkeletonCluster() { return mpAtlas->GetSkeletoClusterPoints(); diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 46ccc68..1274a3d 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -15,13 +15,16 @@ #include "common.h" +#include #include #include #include +#include #include #include #include #include +#include using namespace std; @@ -81,6 +84,14 @@ struct SharedState std::vector matched_markers; }; + struct AuxDepthFrame + { + rclcpp::Time stamp; + double timestamp_sec = 0.0; + std::string frame_id; + sensor_msgs::msg::Image::ConstSharedPtr msg; + }; + // Raw gyro — collected at ~200 Hz std::deque gyro_timestamps; std::deque gyro_data; @@ -100,6 +111,9 @@ struct SharedState std::size_t max_image_queue_size = 300; int dropped_frames = 0; + std::deque aux_depth_buffer; + std::size_t max_aux_depth_buffer_size = 120; + // Marker state (populated inside the image callback path) double min_marker_time_diff = std::numeric_limits::max(); std::vector matched_markers; @@ -182,12 +196,25 @@ void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) class ImageGrabber : public rclcpp::Node { public: - ImageGrabber(std::shared_ptr state) + struct AuxDepthOptions + { + bool use_aux_depth = true; + bool encoding_is_metric = true; + double time_tolerance = 0.05; + float min_depth = 0.2f; + float max_depth = 20.0f; + int stride = 2; + std::string scale_mode = "none"; + }; + + ImageGrabber(std::shared_ptr state, AuxDepthOptions aux_depth_options) : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)) , state_(std::move(state)) + , aux_depth_options_(std::move(aux_depth_options)) {} void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + void GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg); void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msg); void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msg); @@ -196,7 +223,11 @@ class ImageGrabber : public rclcpp::Node std::atomic mustStop{false}; private: + bool FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFrame &depth_frame, double &dt_abs); + bool ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m); + std::shared_ptr state_; + AuxDepthOptions aux_depth_options_; }; // --------------------------------------------------------------------------- @@ -254,6 +285,112 @@ void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) state_->image_ready_cv.notify_all(); } +void ImageGrabber::GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg) +{ + if (!aux_depth_options_.use_aux_depth) + return; + + SharedState::AuxDepthFrame frame; + frame.stamp = rclcpp::Time(msg->header.stamp); + frame.timestamp_sec = frame.stamp.seconds(); + frame.frame_id = msg->header.frame_id; + frame.msg = msg; + + std::lock_guard lock(state_->mtx); + state_->aux_depth_buffer.push_back(std::move(frame)); + while (state_->aux_depth_buffer.size() > state_->max_aux_depth_buffer_size) + state_->aux_depth_buffer.pop_front(); +} + +bool ImageGrabber::ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m) +{ + if (!msg) + return false; + + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(msg); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(this->get_logger(), "[Error] AuxDepth cv_bridge exception: %s", e.what()); + return false; + } + + bool depth_is_metric = aux_depth_options_.encoding_is_metric; + if (msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 || cv_ptr->image.type() == CV_32FC1) + { + depth_m = cv_ptr->image.clone(); + } + else if (msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1 || cv_ptr->image.type() == CV_16UC1) + { + cv_ptr->image.convertTo(depth_m, CV_32FC1, 0.001); + depth_is_metric = true; + } + else + { + RCLCPP_WARN(this->get_logger(), "AuxDepth: unsupported encoding '%s'", msg->encoding.c_str()); + return false; + } + + for (int y = 0; y < depth_m.rows; ++y) + { + float *row = depth_m.ptr(y); + for (int x = 0; x < depth_m.cols; ++x) + { + const float d = row[x]; + if (!std::isfinite(d) || + (depth_is_metric && (d < aux_depth_options_.min_depth || d > aux_depth_options_.max_depth))) + row[x] = std::numeric_limits::quiet_NaN(); + } + } + + return true; +} + +bool ImageGrabber::FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFrame &depth_frame, double &dt_abs) +{ + { + std::lock_guard lock(state_->mtx); + + if (!aux_depth_options_.use_aux_depth || state_->aux_depth_buffer.empty()) + return false; + + while (!state_->aux_depth_buffer.empty() && + state_->aux_depth_buffer.front().timestamp_sec < rgb_time - 2.0 * aux_depth_options_.time_tolerance) + { + state_->aux_depth_buffer.pop_front(); + } + + if (state_->aux_depth_buffer.empty()) + return false; + + auto best_it = state_->aux_depth_buffer.end(); + dt_abs = std::numeric_limits::max(); + + for (auto it = state_->aux_depth_buffer.begin(); it != state_->aux_depth_buffer.end(); ++it) + { + const double dt = std::abs(rgb_time - it->timestamp_sec); + if (dt < dt_abs) + { + dt_abs = dt; + best_it = it; + } + } + + if (best_it == state_->aux_depth_buffer.end() || dt_abs > aux_depth_options_.time_tolerance) + return false; + + depth_frame.stamp = best_it->stamp; + depth_frame.timestamp_sec = best_it->timestamp_sec; + depth_frame.frame_id = best_it->frame_id; + depth_frame.msg = best_it->msg; + } + + return true; +} + // --------------------------------------------------------------------------- // SyncWithImu // --------------------------------------------------------------------------- @@ -269,6 +406,8 @@ void ImageGrabber::SyncWithImu() std::vector vImuMeas; double min_marker_diff; std::vector matched_markers; + bool skip_due_to_insufficient_imu = false; + std::size_t ready_imu_count = 0; // --- Critical section: wait, copy, clear --- { @@ -288,40 +427,55 @@ void ImageGrabber::SyncWithImu() RCLCPP_WARN(this->get_logger(), "%d dropped frames", state_->dropped_frames - 1); state_->dropped_frames = 0; - // Copy next queued image in timestamp order. Do this only after - // the IMU buffer has reached this image timestamp. - SharedState::ImagePacket packet = std::move(state_->image_queue.front()); - state_->image_queue.pop(); - - im = std::move(packet.image); - tIm = packet.timestamp; + tIm = state_->image_queue.front().timestamp; msgTime = rclcpp::Time(static_cast(tIm * 1e9)); - // Copy marker state - min_marker_diff = packet.min_marker_time_diff; - matched_markers = std::move(packet.matched_markers); + const std::size_t synced_imu_size = + std::min(state_->gyro_timestamps.size(), state_->accel_data_sync.size()); + while (ready_imu_count < synced_imu_size && + state_->gyro_timestamps[ready_imu_count] <= tIm) + { + ++ready_imu_count; + } - // Build IMU measurement vector from synced accel + gyro, consuming - // only samples up to this image timestamp. Future IMU samples stay - // buffered for the next frame, matching the ORB-SLAM3 ROS1 wrapper. - while (!state_->gyro_timestamps.empty() && - !state_->accel_data_sync.empty() && - state_->gyro_timestamps.front() <= tIm) + if (ready_imu_count < 2) { - const double imuTime = state_->gyro_timestamps.front(); - const Eigen::Vector3f acc = state_->accel_data_sync.front(); - const Eigen::Vector3f gyr = state_->gyro_data.front(); - - vImuMeas.emplace_back( - acc.x(), acc.y(), acc.z(), - gyr.x(), gyr.y(), gyr.z(), - imuTime); - Wbb = gyr; - - state_->gyro_data.pop_front(); - state_->gyro_timestamps.pop_front(); - state_->accel_data_sync.pop_front(); - state_->accel_timestamps_sync.pop_front(); + // state_->image_queue.pop(); + skip_due_to_insufficient_imu = true; + } + else + { + // Copy next queued image in timestamp order. Do this only after + // the IMU buffer has reached this image timestamp. + SharedState::ImagePacket packet = std::move(state_->image_queue.front()); + state_->image_queue.pop(); + + im = std::move(packet.image); + + // Copy marker state + min_marker_diff = packet.min_marker_time_diff; + matched_markers = std::move(packet.matched_markers); + + // Build IMU measurement vector from synced accel + gyro, consuming + // only samples up to this image timestamp. Future IMU samples stay + // buffered for the next frame, matching the ORB-SLAM3 ROS1 wrapper. + for (std::size_t i = 0; i < ready_imu_count; ++i) + { + const double imuTime = state_->gyro_timestamps.front(); + const Eigen::Vector3f acc = state_->accel_data_sync.front(); + const Eigen::Vector3f gyr = state_->gyro_data.front(); + + vImuMeas.emplace_back( + acc.x(), acc.y(), acc.z(), + gyr.x(), gyr.y(), gyr.z(), + imuTime); + Wbb = gyr; + + state_->gyro_data.pop_front(); + state_->gyro_timestamps.pop_front(); + state_->accel_data_sync.pop_front(); + state_->accel_timestamps_sync.pop_front(); + } } } // Lock released — TrackMonocular runs outside lock @@ -388,6 +542,39 @@ void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg & pcl::PCLPointCloud2::Ptr pclPc2SegPrb(new pcl::PCLPointCloud2); pcl_conversions::toPCL(msgSegImage.segmented_image_probability, *pclPc2SegPrb); + if (aux_depth_options_.use_aux_depth) + { + const double segTimestamp = rclcpp::Time(msgSegImage.header.stamp).seconds(); // Use msgSegImage timestamp (submsgs have empty timestamps) + SharedState::AuxDepthFrame auxDepthFrame; + double auxDepthDt = std::numeric_limits::max(); + + if (FindClosestAuxDepth(segTimestamp, auxDepthFrame, auxDepthDt)) + { + cv::Mat auxDepthForSeg; + if (ConvertAuxDepthImage(auxDepthFrame.msg, auxDepthForSeg)) + { + if (!auxDepthForSeg.empty() && auxDepthForSeg.size() != cvImgSeg->image.size()) + { + cv::Mat resizedDepth; + cv::resize(auxDepthForSeg, resizedDepth, cvImgSeg->image.size(), 0, 0, cv::INTER_NEAREST); + auxDepthForSeg = resizedDepth; + } + + RCLCPP_INFO(this->get_logger(), "AuxDepth: matched depth dt = %.6f for segmented keyframe ID %lu", + auxDepthDt, static_cast(keyFrameId)); + pSLAM->AttachAuxDepthToKeyFrame(keyFrameId, auxDepthForSeg, + auxDepthFrame.timestamp_sec, auxDepthFrame.frame_id, + aux_depth_options_.min_depth, aux_depth_options_.max_depth, + aux_depth_options_.stride, aux_depth_options_.scale_mode); + } + } + else + { + RCLCPP_WARN(this->get_logger(), "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu", + segTimestamp, static_cast(keyFrameId)); + } + } + auto tuple = std::make_tuple(keyFrameId, cvImgSeg->image, pclPc2SegPrb); pSLAM->addSegmentedImage(&tuple); } @@ -426,10 +613,39 @@ int main(int argc, char **argv) node->declare_parameter("voc_file", "file_not_set"); node->declare_parameter("settings_file", "file_not_set"); node->declare_parameter("sys_params_file", "file_not_set"); + node->declare_parameter("use_aux_depth", true); + node->declare_parameter("aux_depth_topic", "/camera/depth_da3/image_rect"); + node->declare_parameter("aux_depth_encoding_is_metric", true); + node->declare_parameter("aux_depth_time_tolerance", 0.05); + node->declare_parameter("aux_depth_min", 0.2); + node->declare_parameter("aux_depth_max", 20.0); + node->declare_parameter("aux_depth_stride", 2); + node->declare_parameter("aux_depth_scale_mode", "none"); const std::string vocFile = node->get_parameter("voc_file").as_string(); const std::string settingsFile = node->get_parameter("settings_file").as_string(); const std::string sysParamsFile = node->get_parameter("sys_params_file").as_string(); + const std::string auxDepthTopic = node->get_parameter("aux_depth_topic").as_string(); + + ImageGrabber::AuxDepthOptions auxDepthOptions; + auxDepthOptions.use_aux_depth = node->get_parameter("use_aux_depth").as_bool(); + auxDepthOptions.encoding_is_metric = node->get_parameter("aux_depth_encoding_is_metric").as_bool(); + auxDepthOptions.time_tolerance = node->get_parameter("aux_depth_time_tolerance").as_double(); + auxDepthOptions.min_depth = static_cast(node->get_parameter("aux_depth_min").as_double()); + auxDepthOptions.max_depth = static_cast(node->get_parameter("aux_depth_max").as_double()); + auxDepthOptions.stride = std::max(1, static_cast(node->get_parameter("aux_depth_stride").as_int())); + auxDepthOptions.scale_mode = node->get_parameter("aux_depth_scale_mode").as_string(); + if (auxDepthOptions.scale_mode != "none" && auxDepthOptions.scale_mode != "map_median") + { + RCLCPP_WARN(node->get_logger(), "AuxDepth: unsupported aux_depth_scale_mode '%s', using 'none'", + auxDepthOptions.scale_mode.c_str()); + auxDepthOptions.scale_mode = "none"; + } + if (auxDepthOptions.use_aux_depth && !auxDepthOptions.encoding_is_metric && auxDepthOptions.scale_mode == "none") + { + RCLCPP_WARN(node->get_logger(), + "AuxDepth: aux_depth_encoding_is_metric is false with scale_mode 'none'; depth will be interpreted as meters by segmentation"); + } if (vocFile == "file_not_set" || settingsFile == "file_not_set") { @@ -474,7 +690,7 @@ int main(int argc, char **argv) // --- Grabber nodes --- auto imugb = std::make_shared(state); - auto igb = std::make_shared(state); + auto igb = std::make_shared(state, auxDepthOptions); // --- QoS (sensor data profile) --- rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); @@ -492,6 +708,15 @@ int main(int argc, char **argv) "/camera/image_raw", 1, [igb](const Image::ConstSharedPtr msg) { igb->GrabImage(msg); }); + rclcpp::Subscription::SharedPtr subAuxDepth; + if (auxDepthOptions.use_aux_depth) + { + subAuxDepth = node->create_subscription( + auxDepthTopic, sensorQos, + [igb](const Image::ConstSharedPtr msg) { igb->GrabAuxDepth(msg); }); + RCLCPP_INFO(node->get_logger(), "AuxDepth: subscribed to %s", auxDepthTopic.c_str()); + } + auto subSegmentedImage = node->create_subscription( "/camera/color/image_segment", 50, [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) @@ -519,4 +744,4 @@ int main(int argc, char **argv) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From afd349ef793441faf8c9c07157cf0d3a2662eae3 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 11 Jun 2026 13:39:28 +0200 Subject: [PATCH 15/59] fix time inconsistency --- include/common.h | 5 +++-- src/common.cc | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/common.h b/include/common.h index 0a7ec47..ffa4b50 100644 --- a/include/common.h +++ b/include/common.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -143,7 +144,7 @@ extern std::vector gnnRoomCandidates; extern rclcpp::Publisher::SharedPtr pubAllWalls_new; extern rclcpp::Publisher::SharedPtr pubAllWalls_legacy; -extern rclcpp::Time lastPlanePublishTime; +extern double lastPlanePublishTime; extern std::shared_ptr pubTrackingImage; extern rclcpp::Publisher::SharedPtr pubOdometry; extern rclcpp::Publisher::SharedPtr pubKFImage; @@ -274,4 +275,4 @@ void setGNNBasedRoomCandidates(const situational_graphs_msgs::msg::RoomsData &ms * Mainly designed for the new version of the GNN-based room detector * @param msgGNNRooms The message containing the detected room candidates */ -void setGNNBasedRoomCandidates(const vs_graphs::msg::VSGraphsAllDetectdetRooms &msgGNNRooms); \ No newline at end of file +void setGNNBasedRoomCandidates(const vs_graphs::msg::VSGraphsAllDetectdetRooms &msgGNNRooms); diff --git a/src/common.cc b/src/common.cc index 08b9b38..58c2bdd 100644 --- a/src/common.cc +++ b/src/common.cc @@ -43,7 +43,7 @@ std::vector> skeletonClusterPoints; std::shared_ptr staticTfBroadcaster; std::string frameWorld, frameCamera, frameImu, frameMap, frameBC, frameSE; -rclcpp::Time lastPlanePublishTime(0, 0, RCL_ROS_TIME); +double lastPlanePublishTime = -std::numeric_limits::infinity(); rclcpp::Publisher::SharedPtr pubKeyFrameList; rclcpp::Publisher::SharedPtr pubOdometry; rclcpp::Publisher::SharedPtr pubDoor; @@ -857,10 +857,10 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) return; // Check if sufficient time has passed since the last plane publication - if ((msgTime - lastPlanePublishTime).seconds() < 3.0) + if (msgTime.seconds() - lastPlanePublishTime < 3.0) return; - lastPlanePublishTime = msgTime; + lastPlanePublishTime = msgTime.seconds(); // Variables visualization_msgs::msg::MarkerArray planeLabelArray; From 99c458f590bc9d01933a15ad2df0904e58010cd1 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 11 Jun 2026 13:40:04 +0200 Subject: [PATCH 16/59] prevent to accumulate stale frames in image queue --- src/ros_mono_inertial.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 1274a3d..e6490ae 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -440,7 +440,7 @@ void ImageGrabber::SyncWithImu() if (ready_imu_count < 2) { - // state_->image_queue.pop(); + state_->image_queue.pop(); skip_due_to_insufficient_imu = true; } else From a6b227560b9a42a16df9a9ef35c7a95cd6e5171f Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 11 Jun 2026 18:42:44 +0200 Subject: [PATCH 17/59] convert and store pointcloud instead of depth frame to keyframe --- core/include/KeyFrame.h | 18 ++++------ core/src/KeyFrame.cc | 61 +++++++++++++++----------------- core/src/SemanticSegmentation.cc | 4 +-- core/src/System.cc | 8 ++--- 4 files changed, 42 insertions(+), 49 deletions(-) diff --git a/core/include/KeyFrame.h b/core/include/KeyFrame.h index 12c60ed..bb8b459 100644 --- a/core/include/KeyFrame.h +++ b/core/include/KeyFrame.h @@ -447,19 +447,15 @@ namespace ORB_SLAM3 // For Semantic Segmentation cv::Mat mImage; bool isPublished; - cv::Mat mAuxDepth; - bool mbHasAuxDepth = false; + pcl::PointCloud::Ptr mAuxPointCloud; + bool mbHasAuxPointCloud = false; double mAuxDepthTimestamp = 0.0; std::string mAuxDepthFrameId; - float mAuxDepthMin = 0.2f; - float mAuxDepthMax = 20.0f; - int mAuxDepthStride = 2; - std::string mAuxDepthScaleMode = "none"; - void SetAuxDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, - const std::string &auxDepthFrameId, float auxDepthMin, - float auxDepthMax, int auxDepthStride, - const std::string &auxDepthScaleMode); - pcl::PointCloud::Ptr getAuxDepthPointCloud(); + void SetAuxPointCloudFromDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, + const std::string &auxDepthFrameId, float auxDepthMin, + float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode); + pcl::PointCloud::Ptr getAuxPointCloud(); // The following variables need to be accessed trough a mutex to be thread safe. protected: diff --git a/core/src/KeyFrame.cc b/core/src/KeyFrame.cc index 7800a46..6fb77a7 100644 --- a/core/src/KeyFrame.cc +++ b/core/src/KeyFrame.cc @@ -211,37 +211,18 @@ namespace ORB_SLAM3 return mCurrentFramePointClouds; } - void KeyFrame::SetAuxDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, - const std::string &auxDepthFrameId, float auxDepthMin, - float auxDepthMax, int auxDepthStride, - const std::string &auxDepthScaleMode) + void KeyFrame::SetAuxPointCloudFromDepth(const cv::Mat &auxDepth, double auxDepthTimestamp, + const std::string &auxDepthFrameId, float auxDepthMin, + float auxDepthMax, int auxDepthStride, + const std::string &auxDepthScaleMode) { - if (auxDepth.empty()) + if (auxDepth.empty() || auxDepth.type() != CV_32FC1) return; - mAuxDepth = auxDepth.clone(); - mbHasAuxDepth = true; - mAuxDepthTimestamp = auxDepthTimestamp; - mAuxDepthFrameId = auxDepthFrameId; - mAuxDepthMin = auxDepthMin; - mAuxDepthMax = auxDepthMax; - mAuxDepthStride = std::max(1, auxDepthStride); - mAuxDepthScaleMode = auxDepthScaleMode; - - std::cout << "AuxDepth: copied depth into keyframe ID " << mnId - << " for segmented image, depth timestamp " - << mAuxDepthTimestamp << std::endl; - } - - pcl::PointCloud::Ptr KeyFrame::getAuxDepthPointCloud() - { - if (!mbHasAuxDepth || mAuxDepth.empty() || mAuxDepth.type() != CV_32FC1) - return nullptr; - - cv::Mat depth = mAuxDepth; + cv::Mat depth = auxDepth; float depthScale = 1.0f; - if (mAuxDepthScaleMode == "map_median") + if (auxDepthScaleMode == "map_median") { std::vector mapDepths; std::vector auxDepths; @@ -264,7 +245,7 @@ namespace ORB_SLAM3 continue; const Eigen::Vector3f Pc = Tcw * pMP->GetWorldPos(); - if (Pc.z() <= mAuxDepthMin || Pc.z() >= mAuxDepthMax) + if (Pc.z() <= auxDepthMin || Pc.z() >= auxDepthMax) continue; mapDepths.push_back(Pc.z()); @@ -288,7 +269,7 @@ namespace ORB_SLAM3 cloud->is_dense = false; cloud->points.resize(static_cast(cloud->width) * cloud->height); - const int stride = std::max(1, mAuxDepthStride); + const int stride = std::max(1, auxDepthStride); const float nan = std::numeric_limits::quiet_NaN(); for (int v = 0; v < depth.rows; ++v) @@ -304,7 +285,7 @@ namespace ORB_SLAM3 continue; const float z = depth.at(v, u) * depthScale; - if (!std::isfinite(z) || z < mAuxDepthMin || z > mAuxDepthMax) + if (!std::isfinite(z) || z < auxDepthMin || z > auxDepthMax) continue; pt.x = (static_cast(u) - cx) * z * invfx; @@ -321,7 +302,19 @@ namespace ORB_SLAM3 } } - return cloud; + mAuxPointCloud = cloud; + mbHasAuxPointCloud = true; + mAuxDepthTimestamp = auxDepthTimestamp; + mAuxDepthFrameId = auxDepthFrameId; + + std::cout << "AuxDepth: projected and copied pointcloud into keyframe ID " << mnId + << " for segmented image, depth timestamp " + << mAuxDepthTimestamp << std::endl; + } + + pcl::PointCloud::Ptr KeyFrame::getAuxPointCloud() + { + return mbHasAuxPointCloud ? mAuxPointCloud : nullptr; } void KeyFrame::clearPointCloud() @@ -334,8 +327,12 @@ namespace ORB_SLAM3 // clear images mImage.release(); - mAuxDepth.release(); - mbHasAuxDepth = false; + if (mAuxPointCloud) + { + mAuxPointCloud->clear(); + mAuxPointCloud = nullptr; + } + mbHasAuxPointCloud = false; } std::vector::Ptr> KeyFrame::getClsCloudPtrs() const diff --git a/core/src/SemanticSegmentation.cc b/core/src/SemanticSegmentation.cc index 07dcf58..c3701e6 100644 --- a/core/src/SemanticSegmentation.cc +++ b/core/src/SemanticSegmentation.cc @@ -50,8 +50,8 @@ namespace ORB_SLAM3 if (thisKF == nullptr || thisKF->isBad()) continue; pcl::PointCloud::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud(); - if (thisKFPointCloud == nullptr && thisKF->mbHasAuxDepth) - thisKFPointCloud = thisKF->getAuxDepthPointCloud(); + if (thisKFPointCloud == nullptr && thisKF->mbHasAuxPointCloud) + thisKFPointCloud = thisKF->getAuxPointCloud(); if (thisKFPointCloud == nullptr) { std::cout << "SemSeg: skipping KF ID: " << thisKF->mnId << ". Missing pointcloud..." << std::endl; diff --git a/core/src/System.cc b/core/src/System.cc index d96b7dc..446fad6 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -267,13 +267,13 @@ namespace ORB_SLAM3 if (pKF == nullptr || pKF->isBad()) { std::cout << "AuxDepth: keyframe ID " << keyFrameId - << " not available for segmented depth attachment" << std::endl; + << " not available for segmented pointcloud attachment" << std::endl; return; } - pKF->SetAuxDepth(auxDepth, auxDepthTimestamp, auxDepthFrameId, - auxDepthMin, auxDepthMax, auxDepthStride, - auxDepthScaleMode); + pKF->SetAuxPointCloudFromDepth(auxDepth, auxDepthTimestamp, auxDepthFrameId, + auxDepthMin, auxDepthMax, auxDepthStride, + auxDepthScaleMode); } std::vector> System::getSkeletonCluster() From 07bb298c94c5d0282f13ce51feeaaa5376f14689 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 12 Jun 2026 09:58:57 +0200 Subject: [PATCH 18/59] reduce depth processing computational overweight on ros callbacks --- core/src/KeyFrame.cc | 6 +- core/src/System.cc | 4 +- src/ros_mono_inertial.cc | 247 ++++++++++++++++++++++++++++++++++----- 3 files changed, 226 insertions(+), 31 deletions(-) diff --git a/core/src/KeyFrame.cc b/core/src/KeyFrame.cc index 6fb77a7..3dfdca8 100644 --- a/core/src/KeyFrame.cc +++ b/core/src/KeyFrame.cc @@ -307,9 +307,9 @@ namespace ORB_SLAM3 mAuxDepthTimestamp = auxDepthTimestamp; mAuxDepthFrameId = auxDepthFrameId; - std::cout << "AuxDepth: projected and copied pointcloud into keyframe ID " << mnId - << " for segmented image, depth timestamp " - << mAuxDepthTimestamp << std::endl; + // std::cout << "AuxDepth: projected and copied pointcloud into keyframe ID " << mnId + // << " for segmented image, depth timestamp " + // << mAuxDepthTimestamp << std::endl; } pcl::PointCloud::Ptr KeyFrame::getAuxPointCloud() diff --git a/core/src/System.cc b/core/src/System.cc index 446fad6..8d974a9 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -266,8 +266,8 @@ namespace ORB_SLAM3 ORB_SLAM3::KeyFrame *pKF = mpAtlas->GetKeyFrameById(keyFrameId); if (pKF == nullptr || pKF->isBad()) { - std::cout << "AuxDepth: keyframe ID " << keyFrameId - << " not available for segmented pointcloud attachment" << std::endl; + // std::cout << "AuxDepth: keyframe ID " << keyFrameId + // << " not available for segmented pointcloud attachment" << std::endl; return; } diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index e6490ae..8c17ea5 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,19 @@ using namespace std; +namespace +{ +constexpr double kMaxImuGap = 0.5; +constexpr double kMaxFrameImuDt = 0.1; +constexpr float kMaxAccelNorm = 150.0f; +constexpr float kMaxGyroNorm = 50.0f; + +bool isFiniteVector(const Eigen::Vector3f &v) +{ + return std::isfinite(v.x()) && std::isfinite(v.y()) && std::isfinite(v.z()); +} +} + // --------------------------------------------------------------------------- // Helpers // --------------------------------------------------------------------------- @@ -148,8 +162,37 @@ void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::ConstSharedPtr &msg) msg->angular_velocity.y, msg->angular_velocity.z); + if (!std::isfinite(t) || !isFiniteVector(acc) || !isFiniteVector(gyr) || + acc.norm() > kMaxAccelNorm || gyr.norm() > kMaxGyroNorm) + { + RCLCPP_WARN(this->get_logger(), "Dropping invalid IMU sample at t=%.9f", t); + return; + } + std::lock_guard lock(state_->mtx); + if (!state_->gyro_timestamps.empty()) + { + const double last_t = state_->gyro_timestamps.back(); + if (t <= last_t) + { + RCLCPP_WARN(this->get_logger(), "Dropping non-monotonic IMU sample t=%.9f after %.9f", t, last_t); + return; + } + if (t - last_t > kMaxImuGap) + { + RCLCPP_WARN(this->get_logger(), "Large IMU gap %.6f s; clearing stale IMU buffers", t - last_t); + state_->gyro_timestamps.clear(); + state_->gyro_data.clear(); + state_->accel_timestamps_sync.clear(); + state_->accel_data_sync.clear(); + state_->prev_accel_data = Eigen::Vector3f::Zero(); + state_->cur_accel_data = Eigen::Vector3f::Zero(); + state_->prev_accel_timestamp = 0.0; + state_->cur_accel_timestamp = 0.0; + } + } + // --- Gyro path (high-rate, ~200 Hz) --- // Just buffer; accel will be interpolated to these timestamps state_->gyro_data.push_back(gyr); @@ -204,6 +247,7 @@ class ImageGrabber : public rclcpp::Node float min_depth = 0.2f; float max_depth = 20.0f; int stride = 2; + std::size_t max_segmentation_queue_size = 100; std::string scale_mode = "none"; }; @@ -211,23 +255,44 @@ class ImageGrabber : public rclcpp::Node : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)) , state_(std::move(state)) , aux_depth_options_(std::move(aux_depth_options)) - {} + { + segmentation_worker_thread_ = std::thread(&ImageGrabber::SegmentationWorkerLoop, this); + } + + ~ImageGrabber() + { + StopSegmentationWorker(); + } void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); void GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg); - void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msg); + void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr &msg); void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msg); + void StopSegmentationWorker(); // Entry point for the sync thread void SyncWithImu(); std::atomic mustStop{false}; private: + struct SegmentationJob + { + segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr msg; + std::chrono::steady_clock::time_point received_time; + }; + bool FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFrame &depth_frame, double &dt_abs); bool ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m); + void SegmentationWorkerLoop(); + void ProcessSegmentationJob(const SegmentationJob &job); std::shared_ptr state_; AuxDepthOptions aux_depth_options_; + std::mutex segmentation_jobs_mtx_; + std::condition_variable segmentation_jobs_cv_; + std::deque segmentation_jobs_; + std::thread segmentation_worker_thread_; + bool stop_segmentation_worker_ = false; }; // --------------------------------------------------------------------------- @@ -357,15 +422,6 @@ bool ImageGrabber::FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFra if (!aux_depth_options_.use_aux_depth || state_->aux_depth_buffer.empty()) return false; - while (!state_->aux_depth_buffer.empty() && - state_->aux_depth_buffer.front().timestamp_sec < rgb_time - 2.0 * aux_depth_options_.time_tolerance) - { - state_->aux_depth_buffer.pop_front(); - } - - if (state_->aux_depth_buffer.empty()) - return false; - auto best_it = state_->aux_depth_buffer.end(); dt_abs = std::numeric_limits::max(); @@ -406,7 +462,6 @@ void ImageGrabber::SyncWithImu() std::vector vImuMeas; double min_marker_diff; std::vector matched_markers; - bool skip_due_to_insufficient_imu = false; std::size_t ready_imu_count = 0; // --- Critical section: wait, copy, clear --- @@ -441,7 +496,6 @@ void ImageGrabber::SyncWithImu() if (ready_imu_count < 2) { state_->image_queue.pop(); - skip_due_to_insufficient_imu = true; } else { @@ -498,6 +552,33 @@ void ImageGrabber::SyncWithImu() continue; } + bool invalidImuPacket = false; + for (std::size_t i = 0; i < vImuMeas.size(); ++i) + { + const ORB_SLAM3::IMU::Point &imu = vImuMeas[i]; + if (!std::isfinite(imu.t) || !isFiniteVector(imu.a) || !isFiniteVector(imu.w) || + imu.a.norm() > kMaxAccelNorm || imu.w.norm() > kMaxGyroNorm) + { + invalidImuPacket = true; + break; + } + + if (i > 0) + { + const double dt = imu.t - vImuMeas[i - 1].t; + if (dt <= 0.0 || dt > kMaxFrameImuDt) + { + invalidImuPacket = true; + break; + } + } + } + if (invalidImuPacket) + { + RCLCPP_WARN(this->get_logger(), "Skipping frame t=%.9f because IMU packet is invalid", tIm); + continue; + } + // Image scaling const float imageScale = pSLAM->GetImageScale(); if (imageScale != 1.f) @@ -522,9 +603,82 @@ void ImageGrabber::SyncWithImu() } } -void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage) +void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr &msgSegImage) +{ + if (!msgSegImage) + return; + + SegmentationJob droppedJob; + bool droppedOldest = false; + { + SegmentationJob job; + job.msg = msgSegImage; + job.received_time = std::chrono::steady_clock::now(); + + std::lock_guard lock(segmentation_jobs_mtx_); + segmentation_jobs_.push_back(job); + if (segmentation_jobs_.size() > aux_depth_options_.max_segmentation_queue_size) + { + droppedJob = segmentation_jobs_.front(); + segmentation_jobs_.pop_front(); + droppedOldest = true; + } + } + + if (droppedOldest && droppedJob.msg) + { + RCLCPP_WARN(this->get_logger(), + "Segmentation worker queue exceeded %zu jobs; dropping oldest keyframe ID %lu", + aux_depth_options_.max_segmentation_queue_size, + static_cast(droppedJob.msg->key_frame_id.data)); + } + + segmentation_jobs_cv_.notify_one(); +} + +void ImageGrabber::StopSegmentationWorker() +{ + { + std::lock_guard lock(segmentation_jobs_mtx_); + if (stop_segmentation_worker_) + return; + stop_segmentation_worker_ = true; + segmentation_jobs_.clear(); + } + segmentation_jobs_cv_.notify_all(); + if (segmentation_worker_thread_.joinable()) + segmentation_worker_thread_.join(); +} + +void ImageGrabber::SegmentationWorkerLoop() +{ + while (true) + { + SegmentationJob job; + { + std::unique_lock lock(segmentation_jobs_mtx_); + segmentation_jobs_cv_.wait(lock, [this] { + return stop_segmentation_worker_ || !segmentation_jobs_.empty(); + }); + + if (stop_segmentation_worker_ && segmentation_jobs_.empty()) + break; + + job = segmentation_jobs_.front(); + segmentation_jobs_.pop_front(); + } + + ProcessSegmentationJob(job); + } +} + +void ImageGrabber::ProcessSegmentationJob(const SegmentationJob &job) { cv_bridge::CvImageConstPtr cvImgSeg; + if (!job.msg) + return; + + const auto &msgSegImage = *job.msg; const uint64_t keyFrameId = msgSegImage.key_frame_id.data; try @@ -560,8 +714,8 @@ void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg & auxDepthForSeg = resizedDepth; } - RCLCPP_INFO(this->get_logger(), "AuxDepth: matched depth dt = %.6f for segmented keyframe ID %lu", - auxDepthDt, static_cast(keyFrameId)); + // RCLCPP_INFO(this->get_logger(), "AuxDepth: matched depth dt = %.6f for segmented keyframe ID %lu", + // auxDepthDt, static_cast(keyFrameId)); pSLAM->AttachAuxDepthToKeyFrame(keyFrameId, auxDepthForSeg, auxDepthFrame.timestamp_sec, auxDepthFrame.frame_id, aux_depth_options_.min_depth, aux_depth_options_.max_depth, @@ -570,8 +724,18 @@ void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg & } else { - RCLCPP_WARN(this->get_logger(), "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu", - segTimestamp, static_cast(keyFrameId)); + if (std::isfinite(auxDepthDt)) + { + RCLCPP_WARN(this->get_logger(), + "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu; nearest dt = %.9f", + segTimestamp, static_cast(keyFrameId), auxDepthDt); + } + else + { + RCLCPP_WARN(this->get_logger(), + "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu; depth buffer empty", + segTimestamp, static_cast(keyFrameId)); + } } } @@ -620,6 +784,8 @@ int main(int argc, char **argv) node->declare_parameter("aux_depth_min", 0.2); node->declare_parameter("aux_depth_max", 20.0); node->declare_parameter("aux_depth_stride", 2); + node->declare_parameter("aux_depth_buffer_size", 600); + node->declare_parameter("aux_depth_job_queue_size", 100); node->declare_parameter("aux_depth_scale_mode", "none"); const std::string vocFile = node->get_parameter("voc_file").as_string(); @@ -634,6 +800,8 @@ int main(int argc, char **argv) auxDepthOptions.min_depth = static_cast(node->get_parameter("aux_depth_min").as_double()); auxDepthOptions.max_depth = static_cast(node->get_parameter("aux_depth_max").as_double()); auxDepthOptions.stride = std::max(1, static_cast(node->get_parameter("aux_depth_stride").as_int())); + auxDepthOptions.max_segmentation_queue_size = + static_cast(std::max(1, static_cast(node->get_parameter("aux_depth_job_queue_size").as_int()))); auxDepthOptions.scale_mode = node->get_parameter("aux_depth_scale_mode").as_string(); if (auxDepthOptions.scale_mode != "none" && auxDepthOptions.scale_mode != "map_median") { @@ -681,6 +849,8 @@ int main(int argc, char **argv) // --- Shared state --- auto state = std::make_shared(); + state->max_aux_depth_buffer_size = + static_cast(std::max(1, static_cast(node->get_parameter("aux_depth_buffer_size").as_int()))); // --- TF broadcasters (one pair, shared) --- tfBroadcaster = @@ -700,32 +870,54 @@ int main(int argc, char **argv) using sensor_msgs::msg::Image; using sensor_msgs::msg::Imu; + auto imuCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto imageCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto auxDepthCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto semanticCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto voxbloxCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::SubscriptionOptions imuOptions; + imuOptions.callback_group = imuCallbackGroup; + rclcpp::SubscriptionOptions imageOptions; + imageOptions.callback_group = imageCallbackGroup; + rclcpp::SubscriptionOptions auxDepthOptionsSub; + auxDepthOptionsSub.callback_group = auxDepthCallbackGroup; + rclcpp::SubscriptionOptions semanticOptions; + semanticOptions.callback_group = semanticCallbackGroup; + rclcpp::SubscriptionOptions voxbloxOptions; + voxbloxOptions.callback_group = voxbloxCallbackGroup; + auto subImu = node->create_subscription( "/imu", sensorQos, - [imugb](const Imu::ConstSharedPtr msg) { imugb->GrabImu(msg); }); + [imugb](const Imu::ConstSharedPtr msg) { imugb->GrabImu(msg); }, + imuOptions); auto subImg = node->create_subscription( "/camera/image_raw", 1, - [igb](const Image::ConstSharedPtr msg) { igb->GrabImage(msg); }); + [igb](const Image::ConstSharedPtr msg) { igb->GrabImage(msg); }, + imageOptions); rclcpp::Subscription::SharedPtr subAuxDepth; if (auxDepthOptions.use_aux_depth) { subAuxDepth = node->create_subscription( - auxDepthTopic, sensorQos, - [igb](const Image::ConstSharedPtr msg) { igb->GrabAuxDepth(msg); }); + auxDepthTopic, 1, + [igb](const Image::ConstSharedPtr msg) { igb->GrabAuxDepth(msg); }, + auxDepthOptionsSub); RCLCPP_INFO(node->get_logger(), "AuxDepth: subscribed to %s", auxDepthTopic.c_str()); } auto subSegmentedImage = node->create_subscription( "/camera/color/image_segment", 50, [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) - { igb->GrabSegmentation(*msg); }); + { igb->GrabSegmentation(msg); }, + semanticOptions); auto subVoxbloxSkeletonMesh = node->create_subscription( "/voxblox_skeletonizer/sparse_graph", 1, [igb](const visualization_msgs::msg::MarkerArray::SharedPtr msg) - { igb->GrabVoxbloxSkeletonGraph(*msg); }); + { igb->GrabVoxbloxSkeletonGraph(*msg); }, + voxbloxOptions); static std::shared_ptr imageTransport = std::make_shared(node); @@ -735,12 +927,15 @@ int main(int argc, char **argv) // --- Sync thread --- std::thread syncThread(&ImageGrabber::SyncWithImu, igb); - rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); + executor.add_node(node); + executor.spin(); - pSLAM->Shutdown(); igb->mustStop = true; state->image_ready_cv.notify_all(); // unblock SyncWithImu if it's waiting + igb->StopSegmentationWorker(); syncThread.join(); + pSLAM->Shutdown(); rclcpp::shutdown(); return 0; From 85d40e37382f48a8b1938f204888e6691af0b6ab Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 12 Jun 2026 10:06:25 +0200 Subject: [PATCH 19/59] minor updates --- README.md | 1 + docker/Jazzy_new.Dockerfile | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 5578940..02ddfd3 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,7 @@ Once you have installed the required dependencies and configured the parameters, > 🛎️ Note: The current version of vS-Graphs supports **ROS2 Jazzy** and is primarily tested on Ubuntu 24.04.2 LTS. +> 🛎️ Note: mono-inertial vS-Graphs is currently under development, having a stable version at commit ea5a79819e5f82a9a420cc4cc6b6709197dbf991 ## 🐋 Docker For a fully reproducible, environment-independent setup, see the [Docker](/docker) section. diff --git a/docker/Jazzy_new.Dockerfile b/docker/Jazzy_new.Dockerfile index f646f36..044ec4a 100644 --- a/docker/Jazzy_new.Dockerfile +++ b/docker/Jazzy_new.Dockerfile @@ -226,4 +226,5 @@ ENTRYPOINT ["/entrypoint.sh"] USER $USERNAME CMD ["/bin/bash"] SHELL ["/bin/bash"] -# note: you might have to reinstall numpy and opencv python pkgs and rebuilding cause the system packages are pointed instead \ No newline at end of file +# note: you might have to reinstall numpy and opencv python pkgs and rebuilding cause the system packages are pointed instead +# note: torch version might be wrong (cu130 wheel), reinstall torch and torchvision with the correct versions if you encounter issues related to torch. \ No newline at end of file From f8c4316c0d9b9caffca2fbc7efbe06369b28c8cc Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 12 Jun 2026 10:06:52 +0200 Subject: [PATCH 20/59] add launch aurgument for optional use of aux depth --- launch/mono-imu.launch.py | 6 ++++++ src/ros_mono_inertial.cc | 2 ++ 2 files changed, 8 insertions(+) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 4642aff..cf6f9c3 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -16,6 +16,10 @@ def generate_launch_description(): DeclareLaunchArgument("launch_rviz", default_value="true"), DeclareLaunchArgument("colored_pointcloud", default_value="true"), DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), + DeclareLaunchArgument("use_aux_depth", default_value="true"), + DeclareLaunchArgument( + "aux_depth_topic", default_value="/camera/depth_da3/image_rect" + ), DeclareLaunchArgument( "semantic_scene_segmenter", default_value="yoso", @@ -84,6 +88,8 @@ def generate_launch_description(): {"static_transform": True}, {"colored_pointcloud": False}, {"publish_pointclouds": True}, + {"use_aux_depth": LaunchConfiguration("use_aux_depth")}, + {"aux_depth_topic": LaunchConfiguration("aux_depth_topic")}, ], remappings=[ ("/imu", LaunchConfiguration("imu_topic")), diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 8c17ea5..4ca4ccb 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -814,6 +814,8 @@ int main(int argc, char **argv) RCLCPP_WARN(node->get_logger(), "AuxDepth: aux_depth_encoding_is_metric is false with scale_mode 'none'; depth will be interpreted as meters by segmentation"); } + RCLCPP_INFO(node->get_logger(), "AuxDepth: use_aux_depth = %s", + auxDepthOptions.use_aux_depth ? "true" : "false"); if (vocFile == "file_not_set" || settingsFile == "file_not_set") { From 8c045e46b49b2e5a314bfd89215a6a8a4d218b6b Mon Sep 17 00:00:00 2001 From: Marco Giberna <102148514+mgiberna@users.noreply.github.com> Date: Fri, 12 Jun 2026 15:55:26 +0200 Subject: [PATCH 21/59] Delete docker/Jazzy_new.Dockerfile --- docker/Jazzy_new.Dockerfile | 230 ------------------------------------ 1 file changed, 230 deletions(-) delete mode 100644 docker/Jazzy_new.Dockerfile diff --git a/docker/Jazzy_new.Dockerfile b/docker/Jazzy_new.Dockerfile deleted file mode 100644 index 044ec4a..0000000 --- a/docker/Jazzy_new.Dockerfile +++ /dev/null @@ -1,230 +0,0 @@ -FROM nvcr.io/nvidia/cuda-dl-base:25.04-cuda12.9-devel-ubuntu24.04 - -# Arguments -ARG USERNAME=user -ARG USER_UID=1000 -ARG USER_GID=$USER_UID -ARG DEBIAN_FRONTEND=noninteractive - -# Environment variables -ENV CUDA_HOME=/usr/local/cuda \ - LANG=en_US.UTF-8 \ - LC_ALL=en_US.UTF-8 \ - ROS_DISTRO=jazzy \ - PIP_BREAK_SYSTEM_PACKAGES=1 - -# --- Fix MPI issue --- -RUN mkdir -p /opt/hpcx/ompi/lib/x86_64-linux-gnu \ - && ln -s /opt/hpcx/ompi /opt/hpcx/ompi/lib/x86_64-linux-gnu/openmpi \ - && dpkg-reconfigure libc-bin - -# --- Handle user creation --- -RUN if id -u $USER_UID ; then userdel "$(id -un $USER_UID)" ; fi - -# --- System setup --- -RUN apt-get update && apt-get install -y --no-install-recommends \ - python3-pip \ - python-is-python3 \ - git \ - openssh-client \ - wget \ - vim \ - curl \ - libeigen3-dev \ - build-essential \ - locales \ - software-properties-common \ - lsb-release \ - gnupg2 && \ - locale-gen en_US en_US.UTF-8 && \ - update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 -RUN add-apt-repository universe - -# --- ROS 2 Jazzy APT source setup --- -RUN ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ - curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" && \ - apt install -y /tmp/ros2-apt-source.deb - -# --- Install ROS 2 Jazzy development tools --- -RUN apt update && apt upgrade -y && \ - apt install -y ros-dev-tools ros-${ROS_DISTRO}-desktop ros-${ROS_DISTRO}-rqt-tf-tree - -# --- Source ROS globally --- -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc - -# --- Initialize rosdep --- -RUN rosdep init && rosdep update - -# --- Clean up --- -RUN rm -rf /var/lib/apt/lists/* /tmp/* - -# --- Create user --- -RUN groupadd --gid $USER_GID $USERNAME \ - && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ - && echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers - -# --- Python environment setup --- -RUN pip3 install networkx==3.1 -RUN pip3 uninstall -y torch torchvision torchaudio || true -RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \ - torch==2.6.0+cu126 \ - torchvision==0.21.0+cu126 -RUN apt remove --purge python3-typing-extensions -y -RUN pip3 install typing-extensions==4.11.0 - -# --- CLIP and Detectron2 setup --- -ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX" -ENV FORCE_CUDA="1" -RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git' -RUN pip3 install 'git+https://github.com/openai/CLIP.git' - - -# --- SSH keys --- -# Define the SSH keys as build arguments for latter mounting -RUN mkdir -p -m 0600 ~/.ssh && ssh-keyscan github.com >> ~/.ssh/known_hosts - -# --- Clone repositories --- -RUN apt-get update && apt-get install -y \ - libepoxy-dev \ - libgl1-mesa-dev \ - libglu1-mesa-dev \ - freeglut3-dev \ - libglew-dev \ - cmake \ - build-essential \ - git - -# Pangolin -WORKDIR /opt/ -RUN git clone --branch v0.9.1 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \ - cd Pangolin && \ - mkdir build && cd build && \ - cmake .. && \ - make -j && \ - make install - -# Cmake -ARG version=3.22 -ARG build=1 -WORKDIR /tmp -RUN wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz - -RUN tar -xzvf cmake-$version.$build.tar.gz -WORKDIR /tmp/cmake-$version.$build -RUN ./bootstrap -RUN make -j8 -RUN make install - -WORKDIR /home/$USERNAME/workspace/src - -# Mount the SSH keys and clone the vS-Graphs repositories -RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git -RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git -RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git -RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/realsense-ros.git -# RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git - -# Repositories for GNN-based room detection and reasoning -# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_wrapper.git -# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_datasets.git -# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_reasoning.git -# RUN --mount=type=ssh git clone -b main git@github.com:snt-arg/situational_graphs_reasoning_msgs.git - -# Install the vS-Graphs dependencies -WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker -RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt - -# Install object tracker dependencies when the package is present in the workspace -RUN if [ -f /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ]; then \ - pip3 install --break-system-packages --ignore-installed \ - -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ; \ - fi - -# [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy -# (Read more: https://github.com/ros2/ros2/issues/1702) -# RUN pip3 install --break-system-packages setuptools==79.0.1 - -# Install reasoning dependencies -# RUN pip3 install --break-system-packages shapely==2.1.1 torch-geometric==2.6.1 transforms3d==0.4.2 -# RUN mkdir -p /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports \ -# && chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports - -# Install the EOMT dependencies -WORKDIR /home/$USERNAME/workspace/src/scene_segment_ros/src/ -RUN git clone https://github.com/tue-mps/eomt.git -RUN pip3 install --ignore-installed -r /home/${USERNAME}/workspace/src/scene_segment_ros/src/eomt/requirements.txt -RUN pip3 install "numpy<2.0" --force-reinstall - -# WORKDIR /home/$USERNAME/workspace/src/ - -# Download the yoso checkpoint -RUN wget https://github.com/hujiecpp/YOSO/releases/download/v0.1/yoso_res50_coco.pth -RUN mv yoso_res50_coco.pth /home/$USERNAME/workspace/src/scene_segment_ros/include/ - -# USER root -ENV DEBIAN_FRONTEND=noninteractive -RUN apt-get update && apt-get install -y \ - ros-${ROS_DISTRO}-rviz-visual-tools \ - ros-${ROS_DISTRO}-depth-image-proc \ - ros-${ROS_DISTRO}-backward-ros \ - ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ - ros-${ROS_DISTRO}-diagnostic-updater \ - ros-${ROS_DISTRO}-pcl-ros - -# Build the workspace -WORKDIR /home/$USERNAME/workspace/ -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" - -# --- Miscalleanous --- -RUN ldconfig - -# --- Clean up --- -# Remove the apt list files -RUN rm -rf /var/lib/apt/lists/* - -# Remove packages no longer needed -RUN apt-get clean && apt-get autoremove -y - -# Remove the ssh keys -RUN rm -rf /root/.ssh/ - -# --- Build entrypoint --- -RUN echo "#!/bin/bash" >> /entrypoint.sh \ - && echo "echo \"source /opt/ros/$ROS_DISTRO/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ - && echo "echo \"source /home/$USERNAME/workspace/install/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ - && echo 'exec "$@"' >> /entrypoint.sh \ - && chmod a+x /entrypoint.sh - -# ------------------------------------ -# Download Vox2Ros Toolkit for Voxblox -# ------------------------------------ -WORKDIR /home/$USERNAME/workspace/vsgraphs_tools -RUN curl -L https://raw.githubusercontent.com/snt-arg/vsgraphs_tools/refs/heads/main/Voxblox/relay_jazzy.py -o /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py -RUN chmod +x /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py - -USER $USERNAME -RUN sudo chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace -WORKDIR /home/$USERNAME/workspace/ - -# --------------------------- -# Download and Install mprocs -# --------------------------- -RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | bash -s -- -y \ - && . "$HOME/.cargo/env" \ - && cargo install mprocs - -# -------------------------- -# Aliases and Environment Setup -# -------------------------- -RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/config/mprocs.yml'" >> ~/.bashrc && \ - echo "alias rel_vox='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode voxblox_client'" >> ~/.bashrc && \ - echo "alias rel_pcl='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode pc_server'" >> ~/.bashrc - -ENTRYPOINT ["/entrypoint.sh"] -USER $USERNAME -CMD ["/bin/bash"] -SHELL ["/bin/bash"] -# note: you might have to reinstall numpy and opencv python pkgs and rebuilding cause the system packages are pointed instead -# note: torch version might be wrong (cu130 wheel), reinstall torch and torchvision with the correct versions if you encounter issues related to torch. \ No newline at end of file From f5f7cb94e2a3677b5307eb9080a0e1ff2a3d1b2f Mon Sep 17 00:00:00 2001 From: Marco Giberna <102148514+mgiberna@users.noreply.github.com> Date: Fri, 12 Jun 2026 16:05:07 +0200 Subject: [PATCH 22/59] Modify docker-compose.yml Updated paths and SSH_AUTH_SOCK variable in docker-compose. --- docker/docker-compose.yml | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index e9bbb81..74429dc 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -14,7 +14,7 @@ services: environment: DISPLAY: $DISPLAY XAUTHORITY: $XAUTHORITY - SSH_AUTH_SOCK: /ssh-agent + SSH_AUTH_SOCK: $SSH_AUTH_SOCK NVIDIA_VISIBLE_DEVICES: all NVIDIA_DRIVER_CAPABILITIES: all TERM: $TERM @@ -30,7 +30,7 @@ services: # - ~/.Xauthority:/home/user/.Xauthority:rw - $XAUTHORITY:$XAUTHORITY:rw # SSH - - $SSH_AUTH_SOCK:/ssh-agent + - $SSH_AUTH_SOCK:$SSH_AUTH_SOCK # TMUX - ~/.tmux:/home/$USERNAME/.tmux:ro - ~/.tmux.conf:/home/$USERNAME/.tmux.conf:ro @@ -42,15 +42,13 @@ services: # - ~/.bash_history:/root/.bash_history # - container_home_cache:/root/.cache # Optional: persist cache # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" - # Working Directory - 3D object tracker (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" # Data Directory (** MODIFY **) - - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" + - "/media/$USERNAME/AT_SSD/Datasets:/home/$USERNAME/datasets:rw" network_mode: "host" command: tail -f /dev/null @@ -59,15 +57,13 @@ services: - volume-init volume-init: image: busybox - command: ["chown", "-R", "1000:1000", "/home/$USERNAME/workspace/src/visual_sgraphs"] + command: ["chown", "-R", "1000:1000", "/workspace/src/visual_sgraphs"] volumes: # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" - # Working Directory - 3D object tracker (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" entrypoint: "" restart: "no" From 46a850c96a7508dae48c8c7db1f9093c3399597d Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 16 Jun 2026 18:49:17 +0200 Subject: [PATCH 23/59] add instance mask handling in ImageGrabber --- src/ros_mono_inertial.cc | 125 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 122 insertions(+), 3 deletions(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 4ca4ccb..51f5035 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace { constexpr double kMaxImuGap = 0.5; constexpr double kMaxFrameImuDt = 0.1; +constexpr double kDefaultInstanceMaskTimeTolerance = 0.03; constexpr float kMaxAccelNorm = 150.0f; constexpr float kMaxGyroNorm = 50.0f; @@ -94,6 +96,8 @@ struct SharedState { cv::Mat image; double timestamp = -1.0; + cv::Mat instance_mask; + double instance_mask_time_diff = std::numeric_limits::max(); double min_marker_time_diff = std::numeric_limits::max(); std::vector matched_markers; }; @@ -106,6 +110,14 @@ struct SharedState sensor_msgs::msg::Image::ConstSharedPtr msg; }; + struct InstanceMaskFrame + { + rclcpp::Time stamp; + double timestamp_sec = 0.0; + std::string frame_id; + cv::Mat mask; + }; + // Raw gyro — collected at ~200 Hz std::deque gyro_timestamps; std::deque gyro_data; @@ -128,6 +140,10 @@ struct SharedState std::deque aux_depth_buffer; std::size_t max_aux_depth_buffer_size = 120; + std::deque instance_mask_buffer; + std::size_t max_instance_mask_buffer_size = 300; + double instance_mask_time_tolerance = kDefaultInstanceMaskTimeTolerance; + // Marker state (populated inside the image callback path) double min_marker_time_diff = std::numeric_limits::max(); std::vector matched_markers; @@ -251,11 +267,24 @@ class ImageGrabber : public rclcpp::Node std::string scale_mode = "none"; }; - ImageGrabber(std::shared_ptr state, AuxDepthOptions aux_depth_options) + struct SegmentOptions + { + std::string instance_mask_topic = "/camera/color/image_instance_masks"; + double instance_mask_time_tolerance = kDefaultInstanceMaskTimeTolerance; + std::size_t max_instance_mask_buffer_size = 300; + }; + + ImageGrabber( + std::shared_ptr state, + AuxDepthOptions aux_depth_options, + SegmentOptions segment_options) : rclcpp::Node("image_grabber", rclcpp::NodeOptions().use_global_arguments(false)) , state_(std::move(state)) , aux_depth_options_(std::move(aux_depth_options)) + , segment_options_(std::move(segment_options)) { + state_->max_instance_mask_buffer_size = segment_options_.max_instance_mask_buffer_size; + state_->instance_mask_time_tolerance = segment_options_.instance_mask_time_tolerance; segmentation_worker_thread_ = std::thread(&ImageGrabber::SegmentationWorkerLoop, this); } @@ -266,6 +295,7 @@ class ImageGrabber : public rclcpp::Node void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); void GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + void GrabInstanceMask(const sensor_msgs::msg::Image::ConstSharedPtr &msg); void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr &msg); void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msg); void StopSegmentationWorker(); @@ -288,6 +318,7 @@ class ImageGrabber : public rclcpp::Node std::shared_ptr state_; AuxDepthOptions aux_depth_options_; + SegmentOptions segment_options_; std::mutex segmentation_jobs_mtx_; std::condition_variable segmentation_jobs_cv_; std::deque segmentation_jobs_; @@ -367,6 +398,43 @@ void ImageGrabber::GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &m state_->aux_depth_buffer.pop_front(); } +void ImageGrabber::GrabInstanceMask(const sensor_msgs::msg::Image::ConstSharedPtr &msg) +{ + if (!msg) + return; + + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(msg); + } + catch (cv_bridge::Exception &e) + { + RCLCPP_ERROR(this->get_logger(), "[Error] Instance mask cv_bridge exception: %s", e.what()); + return; + } + + if (cv_ptr->image.type() != CV_16UC1 && + cv_ptr->image.type() != CV_32SC1 && + cv_ptr->image.type() != CV_8UC1) + { + RCLCPP_WARN(this->get_logger(), "Instance mask: unsupported encoding '%s'", msg->encoding.c_str()); + return; + } + + SharedState::InstanceMaskFrame frame; + frame.stamp = rclcpp::Time(msg->header.stamp); + frame.timestamp_sec = frame.stamp.seconds(); + frame.frame_id = msg->header.frame_id; + frame.mask = cv_ptr->image.clone(); + + std::lock_guard lock(state_->mtx); + state_->instance_mask_buffer.push_back(std::move(frame)); + while (state_->instance_mask_buffer.size() > state_->max_instance_mask_buffer_size) + state_->instance_mask_buffer.pop_front(); + state_->image_ready_cv.notify_all(); +} + bool ImageGrabber::ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m) { if (!msg) @@ -456,6 +524,7 @@ void ImageGrabber::SyncWithImu() while (!mustStop && !pSLAM->isShutDown()) { cv::Mat im; + cv::Mat instanceMask; double tIm = 0.0; rclcpp::Time msgTime; Eigen::Vector3f Wbb = Eigen::Vector3f::Zero(); @@ -506,6 +575,33 @@ void ImageGrabber::SyncWithImu() im = std::move(packet.image); + auto best_mask_it = state_->instance_mask_buffer.end(); + double best_mask_dt = std::numeric_limits::max(); + while (!state_->instance_mask_buffer.empty() && + state_->instance_mask_buffer.front().timestamp_sec < + tIm - state_->instance_mask_time_tolerance) + { + state_->instance_mask_buffer.pop_front(); + } + for (auto it = state_->instance_mask_buffer.begin(); + it != state_->instance_mask_buffer.end(); ++it) + { + const double dt = std::abs(tIm - it->timestamp_sec); + if (dt < best_mask_dt) + { + best_mask_dt = dt; + best_mask_it = it; + } + } + if (best_mask_it != state_->instance_mask_buffer.end() && + best_mask_dt <= state_->instance_mask_time_tolerance) + { + instanceMask = best_mask_it->mask.clone(); + packet.instance_mask_time_diff = best_mask_dt; + state_->instance_mask_buffer.erase( + state_->instance_mask_buffer.begin(), std::next(best_mask_it)); + } + // Copy marker state min_marker_diff = packet.min_marker_time_diff; matched_markers = std::move(packet.matched_markers); @@ -586,8 +682,13 @@ void ImageGrabber::SyncWithImu() const int w = static_cast(im.cols * imageScale); const int h = static_cast(im.rows * imageScale); cv::resize(im, im, cv::Size(w, h)); + if (!instanceMask.empty()) + cv::resize(instanceMask, instanceMask, cv::Size(w, h), 0, 0, cv::INTER_NEAREST); } + // instanceMask is synchronized with im and vImuMeas here for future + // processing before or after TrackMonocular. + // Track if (min_marker_diff < 0.05) { @@ -787,12 +888,14 @@ int main(int argc, char **argv) node->declare_parameter("aux_depth_buffer_size", 600); node->declare_parameter("aux_depth_job_queue_size", 100); node->declare_parameter("aux_depth_scale_mode", "none"); + node->declare_parameter("instance_mask_topic", "/camera/color/image_instance_masks"); + node->declare_parameter("instance_mask_time_tolerance", kDefaultInstanceMaskTimeTolerance); + node->declare_parameter("instance_mask_buffer_size", 300); const std::string vocFile = node->get_parameter("voc_file").as_string(); const std::string settingsFile = node->get_parameter("settings_file").as_string(); const std::string sysParamsFile = node->get_parameter("sys_params_file").as_string(); const std::string auxDepthTopic = node->get_parameter("aux_depth_topic").as_string(); - ImageGrabber::AuxDepthOptions auxDepthOptions; auxDepthOptions.use_aux_depth = node->get_parameter("use_aux_depth").as_bool(); auxDepthOptions.encoding_is_metric = node->get_parameter("aux_depth_encoding_is_metric").as_bool(); @@ -803,6 +906,13 @@ int main(int argc, char **argv) auxDepthOptions.max_segmentation_queue_size = static_cast(std::max(1, static_cast(node->get_parameter("aux_depth_job_queue_size").as_int()))); auxDepthOptions.scale_mode = node->get_parameter("aux_depth_scale_mode").as_string(); + + ImageGrabber::SegmentOptions segmentOptions; + segmentOptions.instance_mask_topic = node->get_parameter("instance_mask_topic").as_string(); + segmentOptions.instance_mask_time_tolerance = + std::max(0.0, node->get_parameter("instance_mask_time_tolerance").as_double()); + segmentOptions.max_instance_mask_buffer_size = + static_cast(std::max(1, static_cast(node->get_parameter("instance_mask_buffer_size").as_int()))); if (auxDepthOptions.scale_mode != "none" && auxDepthOptions.scale_mode != "map_median") { RCLCPP_WARN(node->get_logger(), "AuxDepth: unsupported aux_depth_scale_mode '%s', using 'none'", @@ -862,7 +972,7 @@ int main(int argc, char **argv) // --- Grabber nodes --- auto imugb = std::make_shared(state); - auto igb = std::make_shared(state, auxDepthOptions); + auto igb = std::make_shared(state, auxDepthOptions, segmentOptions); // --- QoS (sensor data profile) --- rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); @@ -875,6 +985,7 @@ int main(int argc, char **argv) auto imuCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto imageCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto auxDepthCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto instanceMaskCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto semanticCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto voxbloxCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -884,6 +995,8 @@ int main(int argc, char **argv) imageOptions.callback_group = imageCallbackGroup; rclcpp::SubscriptionOptions auxDepthOptionsSub; auxDepthOptionsSub.callback_group = auxDepthCallbackGroup; + rclcpp::SubscriptionOptions instanceMaskOptions; + instanceMaskOptions.callback_group = instanceMaskCallbackGroup; rclcpp::SubscriptionOptions semanticOptions; semanticOptions.callback_group = semanticCallbackGroup; rclcpp::SubscriptionOptions voxbloxOptions; @@ -909,6 +1022,12 @@ int main(int argc, char **argv) RCLCPP_INFO(node->get_logger(), "AuxDepth: subscribed to %s", auxDepthTopic.c_str()); } + auto subInstanceMask = node->create_subscription( + segmentOptions.instance_mask_topic, 10, + [igb](const Image::ConstSharedPtr msg) { igb->GrabInstanceMask(msg); }, + instanceMaskOptions); + RCLCPP_INFO(node->get_logger(), "Instance masks: subscribed to %s", segmentOptions.instance_mask_topic.c_str()); + auto subSegmentedImage = node->create_subscription( "/camera/color/image_segment", 50, [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) From e87ec8794375fa9ea3065e69ecfe350af53fa126 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 16 Jun 2026 18:49:38 +0200 Subject: [PATCH 24/59] integrate yolo26 scene segmenter --- launch/mono-imu.launch.py | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index cf6f9c3..7ec2df5 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -16,15 +16,15 @@ def generate_launch_description(): DeclareLaunchArgument("launch_rviz", default_value="true"), DeclareLaunchArgument("colored_pointcloud", default_value="true"), DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), - DeclareLaunchArgument("use_aux_depth", default_value="true"), + DeclareLaunchArgument("use_aux_depth", default_value="false"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" ), DeclareLaunchArgument( "semantic_scene_segmenter", - default_value="yoso", + default_value="yolo26", description="The method to segment the semantic scene (if off, the baseline)", - choices=["yoso", "pfcn", "off"], + choices=["yoso", "pfcn", "yolo26", "off"], ), # Topics DeclareLaunchArgument("camera_frame", default_value="camera"), @@ -197,6 +197,28 @@ def generate_launch_description(): ], ], ), + Node( + condition=IfCondition( + EqualsSubstitution( + LaunchConfiguration("semantic_scene_segmenter"), "yolo26" + ) + ), + name="segmenter_ros", + package="segmenter_ros", + executable="frame_segmenter_yolo26.py", + output="screen", + parameters=[ + {"visualize": LaunchConfiguration("visualize_segmented_scene")} + ], + arguments=[ + "--ros-args", + "--params-file", + [ + get_package_share_directory("segmenter_ros"), + "/config/cfg_yolo26.yaml", + ], + ], + ), # Structural Element Detectors # Node( # name="situational_graphs_reasoning", From 418e7f2f9355e6b11ce1ac53a0c13c74c21b66e3 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 17 Jun 2026 11:39:59 +0200 Subject: [PATCH 25/59] add mask parameter to Frame, System, Tracking classes for ORB feature extraction and update related methods --- core/include/Frame.h | 5 +++-- core/include/System.h | 2 +- core/include/Tracking.h | 2 +- core/src/Frame.cc | 8 ++++---- core/src/System.cc | 7 +++++-- core/src/Tracking.cc | 10 +++++----- src/ros_mono_inertial.cc | 10 ++++++---- 7 files changed, 25 insertions(+), 19 deletions(-) diff --git a/core/include/Frame.h b/core/include/Frame.h index 5832cb7..ba6411d 100644 --- a/core/include/Frame.h +++ b/core/include/Frame.h @@ -92,7 +92,8 @@ namespace ORB_SLAM3 Frame(const cv::Mat &imColor, const cv::Mat &imGray, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame *pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib(), - const std::vector markers = std::vector{}); + const std::vector markers = std::vector{}, + const cv::Mat &mask = cv::Mat()); /** * @brief Extract ORB features from the given grayscale image @@ -102,7 +103,7 @@ namespace ORB_SLAM3 * @param x0 The x-coordinate of the top-left corner of the ROI * @param x1 The x-coordinate of the bottom-right corner of the ROI */ - void ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1); + void ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1, const cv::Mat &mask = cv::Mat()); // Compute Bag of Words representation. void ComputeBoW(); diff --git a/core/include/System.h b/core/include/System.h index 9a7768b..5011c8f 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -167,7 +167,7 @@ namespace ORB_SLAM3 */ Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector &vImuMeas = vector(), string filename = "", - const vector markers = vector{}); + const vector markers = vector{}, const cv::Mat &mask = cv::Mat()); // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); diff --git a/core/include/Tracking.h b/core/include/Tracking.h index 2d0c6be..1ae28ac 100644 --- a/core/include/Tracking.h +++ b/core/include/Tracking.h @@ -95,7 +95,7 @@ namespace ORB_SLAM3 const std::vector doors, const std::vector rooms); Sophus::SE3f GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename, const std::vector markers, const std::vector doors, - const std::vector rooms); + const std::vector rooms, const cv::Mat &mask = cv::Mat()); void GrabImuData(const IMU::Point &imuMeasurement); diff --git a/core/src/Frame.cc b/core/src/Frame.cc index b42a364..115db5a 100644 --- a/core/src/Frame.cc +++ b/core/src/Frame.cc @@ -417,7 +417,7 @@ namespace ORB_SLAM3 // Monocular Frames Processing Frame::Frame(const cv::Mat &imColor, const cv::Mat &imGray, const double &timeStamp, ORBextractor *extractor, ORBVocabulary *voc, GeometricCamera *pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame *pPrevF, - const IMU::Calib &ImuCalib, const std::vector markers) + const IMU::Calib &ImuCalib, const std::vector markers, const cv::Mat &mask) : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(static_cast(pCamera)->toK()), mK_(static_cast(pCamera)->toK_()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mpCamera(pCamera), @@ -442,7 +442,7 @@ namespace ORB_SLAM3 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - ExtractORB(0, imGray, 0, 1000); + ExtractORB(0, imGray, 0, 1000, mask); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -552,12 +552,12 @@ namespace ORB_SLAM3 } } - void Frame::ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1) + void Frame::ExtractORB(int flag, const cv::Mat &imageGray, const int x0, const int x1, const cv::Mat &mask) { vector vLapping = {x0, x1}; // Compute ORB based on the flag (0: left, 1: right) if (flag == 0) - monoLeft = (*mpORBextractorLeft)(imageGray, cv::Mat(), mvKeys, mDescriptors, vLapping); + monoLeft = (*mpORBextractorLeft)(imageGray, mask, mvKeys, mDescriptors, vLapping); else monoRight = (*mpORBextractorRight)(imageGray, cv::Mat(), mvKeysRight, mDescriptorsRight, vLapping); } diff --git a/core/src/System.cc b/core/src/System.cc index 8d974a9..1c624e2 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -453,7 +453,7 @@ namespace ORB_SLAM3 } Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, const vector &vImuMeas, - string filename, const std::vector markers) + string filename, const std::vector markers, const cv::Mat &mask) { // Multi-thread to prevent race conditions { @@ -471,11 +471,14 @@ namespace ORB_SLAM3 // Obtain the images cv::Mat imToFeed = im.clone(); + cv::Mat maskToFeed = mask.clone(); if (settings_ && settings_->needToResize()) { cv::Mat resizedImage; cv::resize(im, resizedImage, settings_->newImSize()); imToFeed = resizedImage; + if (!mask.empty()) + cv::resize(mask, maskToFeed, settings_->newImSize(), 0, 0, cv::INTER_NEAREST); } // Check mode change @@ -522,7 +525,7 @@ namespace ORB_SLAM3 for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) mpTracker->GrabImuData(vImuMeas[i_imu]); - Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, markers, envDoors, envRooms); + Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, markers, envDoors, envRooms, maskToFeed); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; diff --git a/core/src/Tracking.cc b/core/src/Tracking.cc index 6ea666a..a08ad22 100644 --- a/core/src/Tracking.cc +++ b/core/src/Tracking.cc @@ -1636,7 +1636,7 @@ namespace ORB_SLAM3 Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename, const std::vector markers, const std::vector doors, - const std::vector rooms) + const std::vector rooms, const cv::Mat &mask) { // Set arguments to local variables env_doors = doors; @@ -1662,21 +1662,21 @@ namespace ORB_SLAM3 { if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET || (lastID - initID) < mMaxFrames) mCurrentFrame = Frame(im, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers); + mpCamera, mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers, mask); else mCurrentFrame = Frame(im, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mpCamera, - mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers); + mDistCoef, mbf, mThDepth, NULL, IMU::Calib(), markers, mask); } else if (mSensor == System::IMU_MONOCULAR) { if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) { mCurrentFrame = Frame(im, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers); + mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers, mask); } else mCurrentFrame = Frame(im, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers); + mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib, markers, mask); } if (mState == NO_IMAGES_YET) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 51f5035..a2df3f4 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -686,18 +686,20 @@ void ImageGrabber::SyncWithImu() cv::resize(instanceMask, instanceMask, cv::Size(w, h), 0, 0, cv::INTER_NEAREST); } - // instanceMask is synchronized with im and vImuMeas here for future - // processing before or after TrackMonocular. + // ORB extractor masks are CV_8UC1; non-zero pixels allow feature extraction. + cv::Mat orbMask; + if (!instanceMask.empty()) + cv::compare(instanceMask, 0, orbMask, cv::CMP_GT); // Track if (min_marker_diff < 0.05) { - pSLAM->TrackMonocular(im, tIm, vImuMeas, "", matched_markers); + pSLAM->TrackMonocular(im, tIm, vImuMeas, "", matched_markers, orbMask); markersBuffer.clear(); } else { - pSLAM->TrackMonocular(im, tIm, vImuMeas); + pSLAM->TrackMonocular(im, tIm, vImuMeas, "", std::vector{}, orbMask); } publishTopics(msgTime, Wbb); From de35cc772a76b9447dd555b91b34158b7ef6d02f Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 17 Jun 2026 11:52:48 +0200 Subject: [PATCH 26/59] fix thread default argument --- core/src/Frame.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/core/src/Frame.cc b/core/src/Frame.cc index 115db5a..22bc762 100644 --- a/core/src/Frame.cc +++ b/core/src/Frame.cc @@ -130,8 +130,8 @@ namespace ORB_SLAM3 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0); - thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0); + thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0, cv::Mat()); + thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0, cv::Mat()); threadLeft.join(); threadRight.join(); #ifdef REGISTER_TIMES @@ -243,8 +243,8 @@ namespace ORB_SLAM3 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, static_cast(mpCamera)->mvLappingArea[0], static_cast(mpCamera)->mvLappingArea[1]); - thread threadRight(&Frame::ExtractORB, this, 1, imRight, static_cast(mpCamera2)->mvLappingArea[0], static_cast(mpCamera2)->mvLappingArea[1]); + thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, static_cast(mpCamera)->mvLappingArea[0], static_cast(mpCamera)->mvLappingArea[1], cv::Mat()); + thread threadRight(&Frame::ExtractORB, this, 1, imRight, static_cast(mpCamera2)->mvLappingArea[0], static_cast(mpCamera2)->mvLappingArea[1], cv::Mat()); threadLeft.join(); threadRight.join(); #ifdef REGISTER_TIMES From c67414145fee685d6c6d48c18643950fc3947925 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 17 Jun 2026 15:06:22 +0200 Subject: [PATCH 27/59] fix inverse mask bug and add safeguard for low feature frames --- src/ros_mono_inertial.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index a2df3f4..3d68013 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -35,6 +35,7 @@ namespace constexpr double kMaxImuGap = 0.5; constexpr double kMaxFrameImuDt = 0.1; constexpr double kDefaultInstanceMaskTimeTolerance = 0.03; +constexpr double kMinOrbMaskAllowedRatio = 0.05; constexpr float kMaxAccelNorm = 150.0f; constexpr float kMaxGyroNorm = 50.0f; @@ -689,7 +690,14 @@ void ImageGrabber::SyncWithImu() // ORB extractor masks are CV_8UC1; non-zero pixels allow feature extraction. cv::Mat orbMask; if (!instanceMask.empty()) - cv::compare(instanceMask, 0, orbMask, cv::CMP_GT); + { + cv::compare(instanceMask, 0, orbMask, cv::CMP_EQ); + const double allowedRatio = + static_cast(cv::countNonZero(orbMask)) / + static_cast(orbMask.total()); + if (allowedRatio < kMinOrbMaskAllowedRatio) + orbMask.release(); + } // Track if (min_marker_diff < 0.05) From 75f319e2ab5847e551c96107ab8e540660dc0ff4 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 17 Jun 2026 16:24:29 +0200 Subject: [PATCH 28/59] refactor ImageGrabber class by removing unused segmentation worker and related methods --- src/ros_mono_inertial.cc | 124 +++------------------------------------ 1 file changed, 8 insertions(+), 116 deletions(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 3d68013..9adf122 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -17,7 +17,6 @@ #include #include -#include #include #include #include @@ -264,7 +263,6 @@ class ImageGrabber : public rclcpp::Node float min_depth = 0.2f; float max_depth = 20.0f; int stride = 2; - std::size_t max_segmentation_queue_size = 100; std::string scale_mode = "none"; }; @@ -286,45 +284,27 @@ class ImageGrabber : public rclcpp::Node { state_->max_instance_mask_buffer_size = segment_options_.max_instance_mask_buffer_size; state_->instance_mask_time_tolerance = segment_options_.instance_mask_time_tolerance; - segmentation_worker_thread_ = std::thread(&ImageGrabber::SegmentationWorkerLoop, this); } - ~ImageGrabber() - { - StopSegmentationWorker(); - } + ~ImageGrabber() = default; void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); void GrabAuxDepth(const sensor_msgs::msg::Image::ConstSharedPtr &msg); void GrabInstanceMask(const sensor_msgs::msg::Image::ConstSharedPtr &msg); - void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr &msg); + void GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msg); void GrabVoxbloxSkeletonGraph(const visualization_msgs::msg::MarkerArray &msg); - void StopSegmentationWorker(); // Entry point for the sync thread void SyncWithImu(); std::atomic mustStop{false}; private: - struct SegmentationJob - { - segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr msg; - std::chrono::steady_clock::time_point received_time; - }; - bool FindClosestAuxDepth(double rgb_time, SharedState::AuxDepthFrame &depth_frame, double &dt_abs); bool ConvertAuxDepthImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg, cv::Mat &depth_m); - void SegmentationWorkerLoop(); - void ProcessSegmentationJob(const SegmentationJob &job); std::shared_ptr state_; AuxDepthOptions aux_depth_options_; SegmentOptions segment_options_; - std::mutex segmentation_jobs_mtx_; - std::condition_variable segmentation_jobs_cv_; - std::deque segmentation_jobs_; - std::thread segmentation_worker_thread_; - bool stop_segmentation_worker_ = false; }; // --------------------------------------------------------------------------- @@ -709,87 +689,13 @@ void ImageGrabber::SyncWithImu() { pSLAM->TrackMonocular(im, tIm, vImuMeas, "", std::vector{}, orbMask); } - publishTopics(msgTime, Wbb); } } -void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg::ConstSharedPtr &msgSegImage) -{ - if (!msgSegImage) - return; - - SegmentationJob droppedJob; - bool droppedOldest = false; - { - SegmentationJob job; - job.msg = msgSegImage; - job.received_time = std::chrono::steady_clock::now(); - - std::lock_guard lock(segmentation_jobs_mtx_); - segmentation_jobs_.push_back(job); - if (segmentation_jobs_.size() > aux_depth_options_.max_segmentation_queue_size) - { - droppedJob = segmentation_jobs_.front(); - segmentation_jobs_.pop_front(); - droppedOldest = true; - } - } - - if (droppedOldest && droppedJob.msg) - { - RCLCPP_WARN(this->get_logger(), - "Segmentation worker queue exceeded %zu jobs; dropping oldest keyframe ID %lu", - aux_depth_options_.max_segmentation_queue_size, - static_cast(droppedJob.msg->key_frame_id.data)); - } - - segmentation_jobs_cv_.notify_one(); -} - -void ImageGrabber::StopSegmentationWorker() -{ - { - std::lock_guard lock(segmentation_jobs_mtx_); - if (stop_segmentation_worker_) - return; - stop_segmentation_worker_ = true; - segmentation_jobs_.clear(); - } - segmentation_jobs_cv_.notify_all(); - if (segmentation_worker_thread_.joinable()) - segmentation_worker_thread_.join(); -} - -void ImageGrabber::SegmentationWorkerLoop() -{ - while (true) - { - SegmentationJob job; - { - std::unique_lock lock(segmentation_jobs_mtx_); - segmentation_jobs_cv_.wait(lock, [this] { - return stop_segmentation_worker_ || !segmentation_jobs_.empty(); - }); - - if (stop_segmentation_worker_ && segmentation_jobs_.empty()) - break; - - job = segmentation_jobs_.front(); - segmentation_jobs_.pop_front(); - } - - ProcessSegmentationJob(job); - } -} - -void ImageGrabber::ProcessSegmentationJob(const SegmentationJob &job) +void ImageGrabber::GrabSegmentation(const segmenter_ros::msg::SegmenterDataMsg &msgSegImage) { cv_bridge::CvImageConstPtr cvImgSeg; - if (!job.msg) - return; - - const auto &msgSegImage = *job.msg; const uint64_t keyFrameId = msgSegImage.key_frame_id.data; try @@ -825,8 +731,8 @@ void ImageGrabber::ProcessSegmentationJob(const SegmentationJob &job) auxDepthForSeg = resizedDepth; } - // RCLCPP_INFO(this->get_logger(), "AuxDepth: matched depth dt = %.6f for segmented keyframe ID %lu", - // auxDepthDt, static_cast(keyFrameId)); + RCLCPP_INFO(this->get_logger(), "AuxDepth: matched depth dt = %.6f for segmented keyframe ID %lu", + auxDepthDt, static_cast(keyFrameId)); pSLAM->AttachAuxDepthToKeyFrame(keyFrameId, auxDepthForSeg, auxDepthFrame.timestamp_sec, auxDepthFrame.frame_id, aux_depth_options_.min_depth, aux_depth_options_.max_depth, @@ -835,18 +741,8 @@ void ImageGrabber::ProcessSegmentationJob(const SegmentationJob &job) } else { - if (std::isfinite(auxDepthDt)) - { - RCLCPP_WARN(this->get_logger(), - "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu; nearest dt = %.9f", - segTimestamp, static_cast(keyFrameId), auxDepthDt); - } - else - { - RCLCPP_WARN(this->get_logger(), - "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu; depth buffer empty", - segTimestamp, static_cast(keyFrameId)); - } + RCLCPP_WARN(this->get_logger(), "AuxDepth: no depth found for segmented frame timestamp %.9f keyframe ID %lu", + segTimestamp, static_cast(keyFrameId)); } } @@ -896,7 +792,6 @@ int main(int argc, char **argv) node->declare_parameter("aux_depth_max", 20.0); node->declare_parameter("aux_depth_stride", 2); node->declare_parameter("aux_depth_buffer_size", 600); - node->declare_parameter("aux_depth_job_queue_size", 100); node->declare_parameter("aux_depth_scale_mode", "none"); node->declare_parameter("instance_mask_topic", "/camera/color/image_instance_masks"); node->declare_parameter("instance_mask_time_tolerance", kDefaultInstanceMaskTimeTolerance); @@ -913,8 +808,6 @@ int main(int argc, char **argv) auxDepthOptions.min_depth = static_cast(node->get_parameter("aux_depth_min").as_double()); auxDepthOptions.max_depth = static_cast(node->get_parameter("aux_depth_max").as_double()); auxDepthOptions.stride = std::max(1, static_cast(node->get_parameter("aux_depth_stride").as_int())); - auxDepthOptions.max_segmentation_queue_size = - static_cast(std::max(1, static_cast(node->get_parameter("aux_depth_job_queue_size").as_int()))); auxDepthOptions.scale_mode = node->get_parameter("aux_depth_scale_mode").as_string(); ImageGrabber::SegmentOptions segmentOptions; @@ -1041,7 +934,7 @@ int main(int argc, char **argv) auto subSegmentedImage = node->create_subscription( "/camera/color/image_segment", 50, [igb](const segmenter_ros::msg::SegmenterDataMsg::SharedPtr msg) - { igb->GrabSegmentation(msg); }, + { igb->GrabSegmentation(*msg); }, semanticOptions); auto subVoxbloxSkeletonMesh = node->create_subscription( @@ -1064,7 +957,6 @@ int main(int argc, char **argv) igb->mustStop = true; state->image_ready_cv.notify_all(); // unblock SyncWithImu if it's waiting - igb->StopSegmentationWorker(); syncThread.join(); pSLAM->Shutdown(); From b463f33361d335fdf57ddfa59ba7f381c122cebb Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 17 Jun 2026 17:34:13 +0200 Subject: [PATCH 29/59] add mask-based keypoints rejection --- core/include/Frame.h | 2 ++ core/src/Frame.cc | 41 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/core/include/Frame.h b/core/include/Frame.h index ba6411d..dc96dc6 100644 --- a/core/include/Frame.h +++ b/core/include/Frame.h @@ -413,6 +413,8 @@ namespace ORB_SLAM3 } Sophus::SE3 T_test; + + void FilterKeysAndDescriptorsByMask(std::vector& keys, cv::Mat& descriptors, const cv::Mat& mask); }; } // namespace ORB_SLAM diff --git a/core/src/Frame.cc b/core/src/Frame.cc index 22bc762..549d99c 100644 --- a/core/src/Frame.cc +++ b/core/src/Frame.cc @@ -557,9 +557,14 @@ namespace ORB_SLAM3 vector vLapping = {x0, x1}; // Compute ORB based on the flag (0: left, 1: right) if (flag == 0) + { monoLeft = (*mpORBextractorLeft)(imageGray, mask, mvKeys, mDescriptors, vLapping); + FilterKeysAndDescriptorsByMask(mvKeys, mDescriptors, mask); + } else + { monoRight = (*mpORBextractorRight)(imageGray, cv::Mat(), mvKeysRight, mDescriptorsRight, vLapping); + } } bool Frame::isSet() const @@ -1312,4 +1317,40 @@ namespace ORB_SLAM3 return mRwc * mvStereo3Dpoints[i] + mOw; } + void Frame::FilterKeysAndDescriptorsByMask( + std::vector& keys, + cv::Mat& descriptors, + const cv::Mat& mask) + { + if (mask.empty()) + return; + + CV_Assert(mask.type() == CV_8UC1); + + std::vector keptKeys; + cv::Mat keptDescriptors; + + keptKeys.reserve(keys.size()); + + for (int i = 0; i < static_cast(keys.size()); ++i) + { + const int x = cvRound(keys[i].pt.x); + const int y = cvRound(keys[i].pt.y); + + if (x < 0 || x >= mask.cols || y < 0 || y >= mask.rows) + continue; + + if (mask.at(y, x) == 0) + continue; + + keptKeys.push_back(keys[i]); + + if (!descriptors.empty()) + keptDescriptors.push_back(descriptors.row(i)); + } + + keys.swap(keptKeys); + descriptors = keptDescriptors.clone(); + } + } // namespace ORB_SLAM From 6e8ae668155aa019f47f5b19b651ca7677abf05d Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 17 Jun 2026 17:35:36 +0200 Subject: [PATCH 30/59] enhance ImageGrabber synchronization by adding instance mask buffer check --- src/ros_mono_inertial.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index 9adf122..f52b26f 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -520,6 +520,7 @@ void ImageGrabber::SyncWithImu() state_->image_ready_cv.wait(lk, [this] { return mustStop || (!state_->image_queue.empty() && + !state_->instance_mask_buffer.empty() && !state_->gyro_timestamps.empty() && state_->gyro_timestamps.back() >= state_->image_queue.front().timestamp); }); @@ -670,11 +671,11 @@ void ImageGrabber::SyncWithImu() // ORB extractor masks are CV_8UC1; non-zero pixels allow feature extraction. cv::Mat orbMask; if (!instanceMask.empty()) - { + { cv::compare(instanceMask, 0, orbMask, cv::CMP_EQ); const double allowedRatio = static_cast(cv::countNonZero(orbMask)) / - static_cast(orbMask.total()); + static_cast(orbMask.total()); if (allowedRatio < kMinOrbMaskAllowedRatio) orbMask.release(); } From 413bdd49a61aa36a478ceebe59c15325e7e755f9 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 22 Jun 2026 14:35:18 +0200 Subject: [PATCH 31/59] add support to keyframe_depth_estimator --- CMakeLists.txt | 6 ++- config/Visualization/vsgraphs_rgbd.rviz | 69 +++++++++++++++++-------- core/include/LocalMapping.h | 5 ++ core/include/System.h | 4 ++ core/src/LocalMapping.cc | 8 +++ core/src/System.cc | 6 +++ docker/docker-compose.yml | 34 ++++++++---- include/common.h | 2 + launch/mono-imu.launch.py | 49 +++++++++++++++++- package.xml | 3 +- src/common.cc | 44 ++++++++++++++++ 11 files changed, 195 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ff4080f..e2433b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,7 @@ find_package(Eigen3 3.1.0 REQUIRED) find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(segmenter_ros REQUIRED) +find_package(keyframe_depth_estimator REQUIRED) find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) find_package(rviz_visual_tools REQUIRED) @@ -83,6 +84,7 @@ include_directories( ${Pangolin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${segmenter_ros_INCLUDE_DIRS} + ${keyframe_depth_estimator_INCLUDE_DIRS} ) include_directories(${OPENGL_INCLUDE_DIRS}) @@ -210,6 +212,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib image_transport rviz_visual_tools situational_graphs_msgs + keyframe_depth_estimator ) # Common source files @@ -228,6 +231,7 @@ set(COMMON_DEPENDENCIES sensor_msgs geometry_msgs segmenter_ros + keyframe_depth_estimator message_filters situational_graphs_msgs ) @@ -268,4 +272,4 @@ install(TARGETS ${PROJECT_NAME}_lib ) # Ament package configuration -ament_package() \ No newline at end of file +ament_package() diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 56edc5b..074c557 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -5,8 +5,9 @@ Panels: Property Tree Widget: Expanded: - /IMU1/Shape1 + - /Segmented PC1 Splitter Ratio: 0.679411768913269 - Tree Height: 621 + Tree Height: 443 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -348,10 +349,10 @@ Visualization Manager: Axis: X Channel Name: intensity Class: rviz_default_plugins/PointCloud2 - Color: 255; 170; 0 + Color: 255; 255; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: true Max Color: 255; 255; 255 Max Intensity: 4096 @@ -362,7 +363,7 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.019999999552965164 - Style: Flat Squares + Style: Boxes Topic: Depth: 5 Durability Policy: Volatile @@ -371,9 +372,9 @@ Visualization Manager: Value: /vs_graphs/segmented_point_clouds Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Name: Plane Labels Namespaces: {} @@ -383,7 +384,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vs_graphs/plane_labels - Value: false + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Voxblox Skeleton @@ -434,12 +435,6 @@ Visualization Manager: Name: Structural Elements Namespaces: "": true - floorLabels: true - floorRoomEdges: true - floors: true - room: true - roomLabel: true - roomWallLine: true Topic: Depth: 5 Durability Policy: Volatile @@ -480,6 +475,34 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Depth Estimate + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/depth_da3/image_rect + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Instance Masks + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/color/image_instance_masks + Value: true Enabled: true Global Options: Background Color: 255; 255; 255 @@ -526,33 +549,37 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 12 + Distance: 11.482182502746582 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.9686291813850403 - Y: -0.3703233003616333 - Z: -0.6169102191925049 + X: 0.7986353635787964 + Y: 3.4789421558380127 + Z: 2.6508219242095947 Focal Shape Fixed Size: false Focal Shape Size: 0.0010000000474974513 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.919999897480011 + Pitch: 1.065000295639038 Target Frame: camera Value: Orbit (rviz_default_plugins) - Yaw: 4.145030975341797 + Yaw: 4.460034370422363 Saved: ~ Window Geometry: + Depth Estimate: + collapsed: false Displays: collapsed: false - Height: 991 + Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000345fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000345000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002d2000000c70000000000000000000000010000012900000345fc0200000008fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb000000140052006100770020004600720061006d00650073000000003b000000c20000001600fffffffb0000000a0049006d00610067006500000000e5000000a50000000000000000fb0000001c0054007200610063006b006500640020004600720061006d00650073010000003b000000ed0000001600fffffffb0000000a0049006d006100670065010000010d000000d40000000000000000fb00000026005300650067006d0065006e0074006500640020004b00650079004600720061006d00650073010000012e000000e10000001600fffffffb0000000a0056006900650077007301000002150000016b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004f50000034500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Instance Masks: + collapsed: false + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: diff --git a/core/include/LocalMapping.h b/core/include/LocalMapping.h index cb25462..95fdb13 100644 --- a/core/include/LocalMapping.h +++ b/core/include/LocalMapping.h @@ -31,6 +31,7 @@ #include "Settings.h" #include "Types/SystemParams.h" +#include #include namespace ORB_SLAM3 @@ -45,11 +46,14 @@ namespace ORB_SLAM3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using KeyFrameCreatedCallback = std::function; + LocalMapping(System *pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName = std::string()); void SetLoopCloser(LoopClosing *pLoopCloser); void SetTracker(Tracking *pTracker); + void SetKeyFrameCreatedCallback(KeyFrameCreatedCallback callback); // Main function void Run(); @@ -166,6 +170,7 @@ namespace ORB_SLAM3 std::list mlNewKeyFrames; KeyFrame *mpCurrentKeyFrame; + KeyFrameCreatedCallback mKeyFrameCreatedCallback; std::list mlpRecentAddedMapPoints; diff --git a/core/include/System.h b/core/include/System.h index 5011c8f..f0ed531 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ namespace ORB_SLAM3 class Viewer; class FrameDrawer; class MapDrawer; + class KeyFrame; class Atlas; class Tracking; class LocalMapping; @@ -169,6 +171,8 @@ namespace ORB_SLAM3 const vector &vImuMeas = vector(), string filename = "", const vector markers = vector{}, const cv::Mat &mask = cv::Mat()); + void SetKeyFrameCreatedCallback(std::function callback); + // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); diff --git a/core/src/LocalMapping.cc b/core/src/LocalMapping.cc index c13a731..ccf7313 100644 --- a/core/src/LocalMapping.cc +++ b/core/src/LocalMapping.cc @@ -59,6 +59,11 @@ namespace ORB_SLAM3 mpTracker = pTracker; } + void LocalMapping::SetKeyFrameCreatedCallback(KeyFrameCreatedCallback callback) + { + mKeyFrameCreatedCallback = std::move(callback); + } + void LocalMapping::Run() { mbFinished = false; @@ -339,6 +344,9 @@ namespace ORB_SLAM3 // Insert Keyframe in Map mpAtlas->AddKeyFrame(mpCurrentKeyFrame); + + if (mKeyFrameCreatedCallback) + mKeyFrameCreatedCallback(mpCurrentKeyFrame); } void LocalMapping::EmptyQueue() diff --git a/core/src/System.cc b/core/src/System.cc index 1c624e2..d378450 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -546,6 +546,12 @@ namespace ORB_SLAM3 mbDeactivateLocalizationMode = true; } + void System::SetKeyFrameCreatedCallback(std::function callback) + { + if (mpLocalMapper) + mpLocalMapper->SetKeyFrameCreatedCallback(std::move(callback)); + } + bool System::MapChanged() { static int n = 0; diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 74429dc..c960464 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -3,7 +3,7 @@ services: image: vsgraphs_ros2 build: context: . - dockerfile: Jazzy.Dockerfile + dockerfile: Jazzy_new.Dockerfile ssh: - default args: @@ -14,7 +14,7 @@ services: environment: DISPLAY: $DISPLAY XAUTHORITY: $XAUTHORITY - SSH_AUTH_SOCK: $SSH_AUTH_SOCK + SSH_AUTH_SOCK: /ssh-agent NVIDIA_VISIBLE_DEVICES: all NVIDIA_DRIVER_CAPABILITIES: all TERM: $TERM @@ -30,7 +30,7 @@ services: # - ~/.Xauthority:/home/user/.Xauthority:rw - $XAUTHORITY:$XAUTHORITY:rw # SSH - - $SSH_AUTH_SOCK:$SSH_AUTH_SOCK + - $SSH_AUTH_SOCK:/ssh-agent # TMUX - ~/.tmux:/home/$USERNAME/.tmux:ro - ~/.tmux.conf:/home/$USERNAME/.tmux.conf:ro @@ -42,13 +42,19 @@ services: # - ~/.bash_history:/root/.bash_history # - container_home_cache:/root/.cache # Optional: persist cache # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + # Working Directory - 3D object tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + # Working Directory - Dynamic keypoint tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" + # Working Directory - Keyframe depth estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" # Data Directory (** MODIFY **) - - "/media/$USERNAME/AT_SSD/Datasets:/home/$USERNAME/datasets:rw" + - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" command: tail -f /dev/null @@ -57,13 +63,19 @@ services: - volume-init volume-init: image: busybox - command: ["chown", "-R", "1000:1000", "/workspace/src/visual_sgraphs"] + command: ["chown", "-R", "1000:1000", "/home/$USERNAME/workspace/src/visual_sgraphs"] volumes: # Working Directory - vS-Graphs (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/visual_sgraphs:/home/$USERNAME/workspace/src/visual_sgraphs:rw" # Working Directory - Scene Segmenter (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/scene_segment_ros:/home/$USERNAME/workspace/src/scene_segment_ros:rw" # Working Directory - Situational Graphs Messages (** MODIFY **) - - "/home/$USERNAME/[PATH]/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" + # Working Directory - 3D object tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + # Working Directory - Dynamic keypoint tracker (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" + # Working Directory - Keyframe depth estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" entrypoint: "" restart: "no" diff --git a/include/common.h b/include/common.h index ffa4b50..b2c6142 100644 --- a/include/common.h +++ b/include/common.h @@ -78,6 +78,7 @@ #include #include +#include #include #include #include @@ -148,6 +149,7 @@ extern double lastPlanePublishTime; extern std::shared_ptr pubTrackingImage; extern rclcpp::Publisher::SharedPtr pubOdometry; extern rclcpp::Publisher::SharedPtr pubKFImage; +extern rclcpp::Publisher::SharedPtr pubKeyFrameCreated; extern rclcpp::Publisher::SharedPtr pubCameraPose; extern rclcpp::Publisher::SharedPtr pubAllMappoints; extern rclcpp::Publisher::SharedPtr pubTrackedMappoints; diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 7ec2df5..752a829 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -1,3 +1,5 @@ +import os + from launch import LaunchDescription from launch_ros.actions import Node from launch.conditions import IfCondition @@ -6,9 +8,16 @@ from launch_ros.actions import ComposableNodeContainer from ament_index_python.packages import get_package_share_directory from launch.substitutions import LaunchConfiguration, EqualsSubstitution +from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): + keyframe_depth_config = os.path.join( + get_package_share_directory("keyframe_depth_estimator"), + "config", + "config.yaml", + ) + return LaunchDescription( [ # Global arguments declarations @@ -17,12 +26,20 @@ def generate_launch_description(): DeclareLaunchArgument("colored_pointcloud", default_value="true"), DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), DeclareLaunchArgument("use_aux_depth", default_value="false"), + DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="true"), + DeclareLaunchArgument("keyframe_depth_model_path", default_value=""), + DeclareLaunchArgument( + "keyframe_depth_metric_topic", default_value="/keyframe_depth/metric" + ), + DeclareLaunchArgument( + "keyframe_depth_publish_debug_image", default_value="true" + ), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" ), DeclareLaunchArgument( "semantic_scene_segmenter", - default_value="yolo26", + default_value="off", description="The method to segment the semantic scene (if off, the baseline)", choices=["yoso", "pfcn", "yolo26", "off"], ), @@ -96,6 +113,36 @@ def generate_launch_description(): ("/camera/image_raw", LaunchConfiguration("rgb_image_topic")), ], ), + # Keyframe Metric Depth Estimator + Node( + condition=IfCondition( + LaunchConfiguration("launch_keyframe_depth_estimator") + ), + package="keyframe_depth_estimator", + executable="keyframe_depth_estimator_node", + name="keyframe_depth_estimator", + output="screen", + parameters=[ + keyframe_depth_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "keyframe_created_topic": "/orbslam3/keyframe_created", + "camera_info_topic": LaunchConfiguration( + "rgb_camera_info_topic" + ), + "metric_depth_topic": LaunchConfiguration( + "keyframe_depth_metric_topic" + ), + "model_path": LaunchConfiguration( + "keyframe_depth_model_path" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("keyframe_depth_publish_debug_image"), + value_type=bool, + ), + }, + ], + ), # Static Transforms Node( package="tf2_ros", diff --git a/package.xml b/package.xml index 87915a2..9497f8e 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,7 @@ rosidl_default_generators backward_ros segmenter_ros + keyframe_depth_estimator rviz_visual_tools situational_graphs_msgs rosidl_default_runtime @@ -37,4 +38,4 @@ ament_cmake - \ No newline at end of file + diff --git a/src/common.cc b/src/common.cc index 58c2bdd..6b6034a 100644 --- a/src/common.cc +++ b/src/common.cc @@ -50,6 +50,7 @@ rclcpp::Publisher::SharedPtr pubDoor; rclcpp::Publisher::SharedPtr pubAllMappoints; rclcpp::Publisher::SharedPtr pubCameraPose; rclcpp::Publisher::SharedPtr pubKFImage; +rclcpp::Publisher::SharedPtr pubKeyFrameCreated; rclcpp::Publisher::SharedPtr pubBuildingComponents; rclcpp::Publisher::SharedPtr pubTrackedMappoints; rclcpp::Publisher::SharedPtr pubFreespaceCluster; @@ -125,6 +126,43 @@ void setupServices(std::shared_ptr node, const std::string &node_n node_name + "/save_traj", &saveTrajectoryService); } +void publishKeyFrameCreatedEvent(const ORB_SLAM3::KeyFrame *keyframe) +{ + if (!pubKeyFrameCreated || keyframe == nullptr || keyframe->mImage.empty()) + return; + + try + { + cv::Mat keyframeImageBgr; + if (keyframe->mImage.channels() == 1) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_GRAY2BGR); + else if (keyframe->mImage.channels() == 4) + cv::cvtColor(keyframe->mImage, keyframeImageBgr, cv::COLOR_BGRA2BGR); + else if (keyframe->mImage.channels() == 3) + keyframeImageBgr = keyframe->mImage; + else + return; + + std_msgs::msg::Header header; + header.stamp = rclcpp::Time(static_cast(keyframe->mTimeStamp * 1e9)); + header.frame_id = frameCamera; + + keyframe_depth_estimator::msg::KeyFrameCreated event; + event.header = header; + event.keyframe_id = keyframe->mnId; + auto image_msg = cv_bridge::CvImage( + header, sensor_msgs::image_encodings::BGR8, keyframeImageBgr).toImageMsg(); + event.image = *image_msg; + pubKeyFrameCreated->publish(event); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(rclcpp::get_logger("visual_sgraphs"), + "Failed to publish KeyFrameCreated event for keyframe %lu: %s", + keyframe->mnId, e.what()); + } +} + void setupPublishers(std::shared_ptr node, std::shared_ptr image_transport, const std::string &node_name) { // Basic @@ -132,6 +170,9 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher(node_name + "/all_points", 1); pubCameraPose = node->create_publisher(node_name + "/camera_pose", 1); pubKFImage = node->create_publisher(node_name + "/keyframe_image", 50); + pubKeyFrameCreated = + node->create_publisher( + "/orbslam3/keyframe_created", 10); pubTrackedMappoints = node->create_publisher(node_name + "/tracked_points", 1); pubWorldFramePointCloud = node->create_publisher(node_name + "/points_map", 1); pubKeyFrameMarker = node->create_publisher(node_name + "/kf_markers", 1); @@ -160,6 +201,9 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher(node_name + "/body_odom", 1); + if (pSLAM) + pSLAM->SetKeyFrameCreatedCallback(publishKeyFrameCreatedEvent); + tfBuffer_ = std::make_shared(node->get_clock()); tfListener_ = std::make_shared(*tfBuffer_); } From cdcb01d3369c6ed67668c2dea4290213d5900db1 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 22 Jun 2026 18:42:57 +0200 Subject: [PATCH 32/59] add sky_handling flag for keyframe depth estimator --- launch/mono-imu.launch.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 752a829..4ac9553 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -27,13 +27,20 @@ def generate_launch_description(): DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), DeclareLaunchArgument("use_aux_depth", default_value="false"), DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="true"), - DeclareLaunchArgument("keyframe_depth_model_path", default_value=""), + DeclareLaunchArgument( + "keyframe_depth_model_path", + default_value=( + "/home/marco/workspace/src/keyframe_depth_estimator/include/models/" + "engines/da3metric_280x504.engine" + ), + ), DeclareLaunchArgument( "keyframe_depth_metric_topic", default_value="/keyframe_depth/metric" ), DeclareLaunchArgument( "keyframe_depth_publish_debug_image", default_value="true" ), + DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" ), @@ -140,6 +147,10 @@ def generate_launch_description(): LaunchConfiguration("keyframe_depth_publish_debug_image"), value_type=bool, ), + "sky_handling": ParameterValue( + LaunchConfiguration("keyframe_depth_sky_handling"), + value_type=bool, + ), }, ], ), From 2ac0aae2c050c8fe21f55ff8e8b30e531915e636 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 22 Jun 2026 18:45:49 +0200 Subject: [PATCH 33/59] minor updates --- .gitignore | 3 ++- config/Visualization/vsgraphs_rgbd.rviz | 22 +++++++++++++--- docker/README.md | 35 ++++++++++++++++++++++++- docker/requirements.txt | 6 ++--- 4 files changed, 58 insertions(+), 8 deletions(-) diff --git a/.gitignore b/.gitignore index 6ae5d2a..ac75783 100644 --- a/.gitignore +++ b/.gitignore @@ -48,4 +48,5 @@ borrar/* */ExecMean.txt !Dockerfile -docker/docker-compose.yml \ No newline at end of file +docker/docker-compose.yml +docker/Jazzy_new.Dockerfile \ No newline at end of file diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 074c557..8f87e88 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -7,7 +7,7 @@ Panels: - /IMU1/Shape1 - /Segmented PC1 Splitter Ratio: 0.679411768913269 - Tree Height: 443 + Tree Height: 174 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -503,6 +503,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /camera/color/image_instance_masks Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /keyframe_depth/debug_image + Value: true Enabled: true Global Options: Background Color: 255; 255; 255 @@ -577,9 +591,11 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false + Image: + collapsed: false Instance Masks: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: @@ -595,5 +611,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 1920 + X: 3840 Y: 0 diff --git a/docker/README.md b/docker/README.md index 09afaf9..f769ccb 100644 --- a/docker/README.md +++ b/docker/README.md @@ -63,4 +63,37 @@ docker exec -it vsgraphs_ros2 bash ros2 launch vs_graphs rgbd.launch.py ``` -You can even use the **mprocs** tool provided inside the Docker image, by simply running `mprocs` (an alias for `mprocs -c [path]/visual_sgraphs/config/mprocs.yml`) and choose the prepared command sets there. \ No newline at end of file +You can even use the **mprocs** tool provided inside the Docker image, by simply running `mprocs` (an alias for `mprocs -c [path]/visual_sgraphs/config/mprocs.yml`) and choose the prepared command sets there. + +### Solve cuda mismatch after build +Remove wrong versions and Reinstall torch / numpy: +```bash +sudo rm -rf \ + /usr/local/lib/python3.12/dist-packages/torch \ + /usr/local/lib/python3.12/dist-packages/torch-* \ + /usr/local/lib/python3.12/dist-packages/torchvision \ + /usr/local/lib/python3.12/dist-packages/torchvision-* \ + /usr/local/lib/python3.12/dist-packages/torchaudio \ + /usr/local/lib/python3.12/dist-packages/torchaudio-* \ + /usr/local/lib/python3.12/dist-packages/functorch \ + /usr/local/lib/python3.12/dist-packages/torchgen + +python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall \ + torch torchvision torchaudio \ + --index-url https://download.pytorch.org/whl/cu126 + +rm -rf ~/.local/lib/python3.12/site-packages/numpy \ + ~/.local/lib/python3.12/site-packages/numpy-*.dist-info \ + ~/.local/lib/python3.12/site-packages/numpy.libs + +python3 -m pip install --user --break-system-packages --no-cache-dir "numpy==1.26.4" +``` +verify: +```bash +python3 - <<'PY' +import torch, numpy +print("torch:", torch.__version__, torch.__file__, torch.version.cuda, torch.cuda.is_available()) +print("numpy:", numpy.__version__, numpy.__file__) +PY +``` +and build the workspace. \ No newline at end of file diff --git a/docker/requirements.txt b/docker/requirements.txt index 108b326..89716ff 100644 --- a/docker/requirements.txt +++ b/docker/requirements.txt @@ -1,11 +1,11 @@ networkx==3.1 -ultralytics == 8.0.120 +ultralytics==8.4.68 matplotlib>=3.2.2 opencv-python==4.6.0.66 Pillow>=7.1.2 PyYAML>=5.3.1 requests>=2.23.0 -scipy>=1.4.1 +scipy==1.13.1 tqdm>=4.64.0 pandas>=1.1.4 seaborn>=0.11.0 @@ -15,4 +15,4 @@ transformers ftfy regex timm -evo \ No newline at end of file +evo From 99687e0d0212993e1ce37e16cb56c8658351f7bf Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 24 Jun 2026 14:52:06 +0200 Subject: [PATCH 34/59] add integration with keyframe depth validator --- CMakeLists.txt | 4 +++ config/Visualization/vsgraphs_rgbd.rviz | 38 +++++++++++++++------ docker/docker-compose.yml | 4 +++ include/common.h | 2 ++ launch/mono-imu.launch.py | 43 +++++++++++++++++++++++ package.xml | 1 + src/common.cc | 45 +++++++++++++++++++++++++ 7 files changed, 127 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e2433b2..339b994 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,6 +47,7 @@ find_package(backward_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(segmenter_ros REQUIRED) find_package(keyframe_depth_estimator REQUIRED) +find_package(keyframe_depth_validator REQUIRED) find_package(image_transport REQUIRED) find_package(message_filters REQUIRED) find_package(rviz_visual_tools REQUIRED) @@ -85,6 +86,7 @@ include_directories( ${PCL_INCLUDE_DIRS} ${segmenter_ros_INCLUDE_DIRS} ${keyframe_depth_estimator_INCLUDE_DIRS} + ${keyframe_depth_validator_INCLUDE_DIRS} ) include_directories(${OPENGL_INCLUDE_DIRS}) @@ -213,6 +215,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib rviz_visual_tools situational_graphs_msgs keyframe_depth_estimator + keyframe_depth_validator ) # Common source files @@ -232,6 +235,7 @@ set(COMMON_DEPENDENCIES geometry_msgs segmenter_ros keyframe_depth_estimator + keyframe_depth_validator message_filters situational_graphs_msgs ) diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 8f87e88..41c3604 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -1,13 +1,15 @@ Panels: - Class: rviz_common/Displays - Help Height: 157 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /IMU1/Shape1 - /Segmented PC1 + - /Keyframe Depth Debug1 + - /Keyframe Depth Validator Debug1 Splitter Ratio: 0.679411768913269 - Tree Height: 174 + Tree Height: 140 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -25,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Tracked Points + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -137,7 +139,7 @@ Visualization Manager: Enabled: true Name: KeyFrames Namespaces: - kf_markers: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -149,7 +151,7 @@ Visualization Manager: Enabled: true Name: Camera Pose Namespaces: - camera_pose: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -434,7 +436,7 @@ Visualization Manager: Enabled: true Name: Structural Elements Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -508,7 +510,7 @@ Visualization Manager: Max Value: 1 Median window: 5 Min Value: 0 - Name: Image + Name: Keyframe Depth Debug Normalize Range: true Topic: Depth: 5 @@ -517,6 +519,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /keyframe_depth/debug_image Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Keyframe Depth Validator Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /keyframe_depth_validator/debug_image + Value: true Enabled: true Global Options: Background Color: 255; 255; 255 @@ -591,11 +607,13 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - Image: - collapsed: false Instance Masks: collapsed: false - QMainWindow State: 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 + Keyframe Depth Debug: + collapsed: false + Keyframe Depth Validator Debug: + collapsed: false + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index c960464..e975a02 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -53,6 +53,8 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" # Working Directory - Keyframe depth estimator (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" + # Working Directory - Keyframe depth validator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" # Data Directory (** MODIFY **) - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" @@ -77,5 +79,7 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" # Working Directory - Keyframe depth estimator (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" + # Working Directory - Keyframe depth validator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" entrypoint: "" restart: "no" diff --git a/include/common.h b/include/common.h index b2c6142..e374385 100644 --- a/include/common.h +++ b/include/common.h @@ -79,6 +79,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +151,7 @@ extern std::shared_ptr pubTrackingImage; extern rclcpp::Publisher::SharedPtr pubOdometry; extern rclcpp::Publisher::SharedPtr pubKFImage; extern rclcpp::Publisher::SharedPtr pubKeyFrameCreated; +extern rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; extern rclcpp::Publisher::SharedPtr pubCameraPose; extern rclcpp::Publisher::SharedPtr pubAllMappoints; extern rclcpp::Publisher::SharedPtr pubTrackedMappoints; diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 4ac9553..6b730de 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -17,6 +17,11 @@ def generate_launch_description(): "config", "config.yaml", ) + keyframe_depth_validator_config = os.path.join( + get_package_share_directory("keyframe_depth_validator"), + "config", + "config.yaml", + ) return LaunchDescription( [ @@ -27,6 +32,7 @@ def generate_launch_description(): DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), DeclareLaunchArgument("use_aux_depth", default_value="false"), DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="true"), + DeclareLaunchArgument("launch_keyframe_depth_validator", default_value="true"), DeclareLaunchArgument( "keyframe_depth_model_path", default_value=( @@ -37,9 +43,15 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_metric_topic", default_value="/keyframe_depth/metric" ), + DeclareLaunchArgument( + "keyframe_depth_corrected_topic", default_value="/keyframe_depth/corrected" + ), DeclareLaunchArgument( "keyframe_depth_publish_debug_image", default_value="true" ), + DeclareLaunchArgument( + "keyframe_depth_validator_publish_debug_image", default_value="true" + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -154,6 +166,37 @@ def generate_launch_description(): }, ], ), + # Keyframe Depth Validator + Node( + condition=IfCondition( + LaunchConfiguration("launch_keyframe_depth_validator") + ), + package="keyframe_depth_validator", + executable="keyframe_depth_validator_node", + name="keyframe_depth_validator", + output="screen", + parameters=[ + keyframe_depth_validator_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "metric_depth_topic": LaunchConfiguration( + "keyframe_depth_metric_topic" + ), + "static_correspondences_topic": ( + "/orbslam3/keyframe_static_map_points" + ), + "corrected_depth_topic": LaunchConfiguration( + "keyframe_depth_corrected_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_publish_debug_image" + ), + value_type=bool, + ), + }, + ], + ), # Static Transforms Node( package="tf2_ros", diff --git a/package.xml b/package.xml index 9497f8e..75ac5bf 100644 --- a/package.xml +++ b/package.xml @@ -26,6 +26,7 @@ backward_ros segmenter_ros keyframe_depth_estimator + keyframe_depth_validator rviz_visual_tools situational_graphs_msgs rosidl_default_runtime diff --git a/src/common.cc b/src/common.cc index 6b6034a..bcf051c 100644 --- a/src/common.cc +++ b/src/common.cc @@ -21,6 +21,7 @@ */ #include "common.h" +#include #include // Variables for ORB-SLAM3 @@ -51,6 +52,7 @@ rclcpp::Publisher::SharedPtr pubAllMappoints; rclcpp::Publisher::SharedPtr pubCameraPose; rclcpp::Publisher::SharedPtr pubKFImage; rclcpp::Publisher::SharedPtr pubKeyFrameCreated; +rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; rclcpp::Publisher::SharedPtr pubBuildingComponents; rclcpp::Publisher::SharedPtr pubTrackedMappoints; rclcpp::Publisher::SharedPtr pubFreespaceCluster; @@ -154,6 +156,46 @@ void publishKeyFrameCreatedEvent(const ORB_SLAM3::KeyFrame *keyframe) header, sensor_msgs::image_encodings::BGR8, keyframeImageBgr).toImageMsg(); event.image = *image_msg; pubKeyFrameCreated->publish(event); + + if (pubKeyFrameStaticMapPoints) + { + keyframe_depth_validator::msg::StaticMapPointCorrespondences points_msg; + points_msg.header = header; + points_msg.keyframe_id = keyframe->mnId; + + ORB_SLAM3::KeyFrame *mutable_keyframe = const_cast(keyframe); + const std::vector map_points = + mutable_keyframe->GetMapPointMatches(); + const Sophus::SE3f Tcw = mutable_keyframe->GetPose(); + points_msg.u.reserve(map_points.size()); + points_msg.v.reserve(map_points.size()); + points_msg.z_orb.reserve(map_points.size()); + points_msg.map_point_ids.reserve(map_points.size()); + + for (std::size_t i = 0; i < map_points.size() && i < keyframe->mvKeysUn.size(); ++i) + { + ORB_SLAM3::MapPoint *map_point = map_points[i]; + if (map_point == nullptr || map_point->isBad()) + continue; + + const cv::Point2f pixel = keyframe->mvKeysUn[i].pt; + if (pixel.x < 0.0F || pixel.y < 0.0F || + pixel.x >= static_cast(keyframeImageBgr.cols) || + pixel.y >= static_cast(keyframeImageBgr.rows)) + continue; + + const Eigen::Vector3f Xc = Tcw * map_point->GetWorldPos(); + if (!std::isfinite(Xc.z()) || Xc.z() <= 0.0F) + continue; + + points_msg.u.push_back(pixel.x); + points_msg.v.push_back(pixel.y); + points_msg.z_orb.push_back(Xc.z()); + points_msg.map_point_ids.push_back(static_cast(map_point->mnId)); + } + + pubKeyFrameStaticMapPoints->publish(points_msg); + } } catch (const std::exception &e) { @@ -173,6 +215,9 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher( "/orbslam3/keyframe_created", 10); + pubKeyFrameStaticMapPoints = + node->create_publisher( + "/orbslam3/keyframe_static_map_points", 10); pubTrackedMappoints = node->create_publisher(node_name + "/tracked_points", 1); pubWorldFramePointCloud = node->create_publisher(node_name + "/points_map", 1); pubKeyFrameMarker = node->create_publisher(node_name + "/kf_markers", 1); From f9a775f5b643f43381ad86c2a03840e44b9609ab Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 24 Jun 2026 17:13:41 +0200 Subject: [PATCH 35/59] add debug param --- launch/mono-imu.launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 6b730de..46503a8 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -52,6 +52,9 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_validator_publish_debug_image", default_value="true" ), + DeclareLaunchArgument( + "keyframe_depth_validator_debug_match_logging", default_value="true" + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -194,6 +197,12 @@ def generate_launch_description(): ), value_type=bool, ), + "debug_match_logging": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_debug_match_logging" + ), + value_type=bool, + ), }, ], ), From d61ceac50970e3a59bed47d1c7c7e1a6d0625965 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 25 Jun 2026 11:28:57 +0200 Subject: [PATCH 36/59] add offline depth support --- README.md | 10 ++++++++++ config/Visualization/vsgraphs_rgbd.rviz | 14 +++++++------- launch/mono-imu.launch.py | 18 ++++++++++++++++++ 3 files changed, 35 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 02ddfd3..2cd3f31 100644 --- a/README.md +++ b/README.md @@ -74,3 +74,13 @@ To evaluate vS-Graphs against other visual SLAM frameworks, read the [evaluation ## 🔑 License This project is licensed under the GPL-3.0 license - see the [LICENSE](/LICENSE) for more details. + + +## Usage + +```bash +ros2 launch vs_graphs mono-imu.launch.py \ + launch_keyframe_depth_estimator:=false \ + keyframe_depth_validator_use_offline_metric_depth:=true \ + keyframe_depth_validator_offline_sync_tolerance_ms:=20.0 +``` \ No newline at end of file diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 41c3604..44dc4a8 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -9,7 +9,7 @@ Panels: - /Keyframe Depth Debug1 - /Keyframe Depth Validator Debug1 Splitter Ratio: 0.679411768913269 - Tree Height: 140 + Tree Height: 176 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: All Points Visualization Manager: Class: "" Displays: @@ -139,7 +139,7 @@ Visualization Manager: Enabled: true Name: KeyFrames Namespaces: - {} + kf_markers: true Topic: Depth: 5 Durability Policy: Volatile @@ -151,7 +151,7 @@ Visualization Manager: Enabled: true Name: Camera Pose Namespaces: - {} + camera_pose: true Topic: Depth: 5 Durability Policy: Volatile @@ -436,7 +436,7 @@ Visualization Manager: Enabled: true Name: Structural Elements Namespaces: - {} + "": true Topic: Depth: 5 Durability Policy: Volatile @@ -613,7 +613,7 @@ Window Geometry: collapsed: false Keyframe Depth Validator Debug: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: @@ -629,5 +629,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 3840 + X: 1920 Y: 0 diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 46503a8..8535707 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -55,6 +55,12 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_validator_debug_match_logging", default_value="true" ), + DeclareLaunchArgument( + "keyframe_depth_validator_use_offline_metric_depth", default_value="false" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_offline_sync_tolerance_ms", default_value="20.0" + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -203,6 +209,18 @@ def generate_launch_description(): ), value_type=bool, ), + "use_offline_metric_depth": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_use_offline_metric_depth" + ), + value_type=bool, + ), + "offline_metric_depth_sync_tolerance_ms": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_offline_sync_tolerance_ms" + ), + value_type=float, + ), }, ], ), From 7b91b75f3fd7bebc631b9c6f3fd4d2f99ef0f5e8 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 25 Jun 2026 12:01:26 +0200 Subject: [PATCH 37/59] add launch arguments for keyframe depth validator tuning and rviz configuration --- launch/mono-imu.launch.py | 48 +++++++++++++++++++++++++++++++++++---- 1 file changed, 44 insertions(+), 4 deletions(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 8535707..1c03c10 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -28,6 +28,13 @@ def generate_launch_description(): # Global arguments declarations DeclareLaunchArgument("offline", default_value="true"), DeclareLaunchArgument("launch_rviz", default_value="true"), + DeclareLaunchArgument( + "rviz_config", + default_value=[ + get_package_share_directory("vs_graphs"), + "/config/Visualization/vsgraphs_rgbd.rviz", + ], + ), DeclareLaunchArgument("colored_pointcloud", default_value="true"), DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), DeclareLaunchArgument("use_aux_depth", default_value="false"), @@ -52,6 +59,9 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_validator_publish_debug_image", default_value="true" ), + DeclareLaunchArgument( + "keyframe_depth_validator_queue_depth", default_value="4" + ), DeclareLaunchArgument( "keyframe_depth_validator_debug_match_logging", default_value="true" ), @@ -61,6 +71,15 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_validator_offline_sync_tolerance_ms", default_value="20.0" ), + DeclareLaunchArgument( + "keyframe_depth_validator_tuning_mode", default_value="false" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_tuning_history_size", default_value="200" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_tuning_visualization_period_s", default_value="2.0" + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -197,6 +216,12 @@ def generate_launch_description(): "corrected_depth_topic": LaunchConfiguration( "keyframe_depth_corrected_topic" ), + "queue_depth": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_queue_depth" + ), + value_type=int, + ), "publish_debug_image": ParameterValue( LaunchConfiguration( "keyframe_depth_validator_publish_debug_image" @@ -221,6 +246,24 @@ def generate_launch_description(): ), value_type=float, ), + "tuning_mode": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_tuning_mode" + ), + value_type=bool, + ), + "tuning_history_size": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_tuning_history_size" + ), + value_type=int, + ), + "tuning_visualization_period_s": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_tuning_visualization_period_s" + ), + value_type=float, + ), }, ], ), @@ -273,10 +316,7 @@ def generate_launch_description(): name="rviz", arguments=[ "-d", - [ - get_package_share_directory("vs_graphs"), - "/config/Visualization/vsgraphs_rgbd.rviz", - ], + LaunchConfiguration("rviz_config"), ], output="screen", ), From c3652b0f24ffb6dac46de98cead2c78f55f00d12 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Thu, 25 Jun 2026 16:35:29 +0200 Subject: [PATCH 38/59] add depth validator tuning related parameters --- launch/mono-imu.launch.py | 41 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 1c03c10..bf0dced 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -80,6 +80,23 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_validator_tuning_visualization_period_s", default_value="2.0" ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_depth_topic", + default_value="/camera/realsense/aligned_depth_to_color/image_raw", + ), + DeclareLaunchArgument( + "keyframe_depth_validator_dynamic_masks_topic", + default_value="/camera/color/instance_masks", + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_valid_range_min_m", default_value="0.3" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_valid_range_max_m", default_value="3.0" + ), + DeclareLaunchArgument( + "keyframe_depth_validator_rgbd_heatmap_max_abs_error_m", default_value="1.0" + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -264,6 +281,30 @@ def generate_launch_description(): ), value_type=float, ), + "rgbd_depth_topic": LaunchConfiguration( + "keyframe_depth_validator_rgbd_depth_topic" + ), + "dynamic_masks_topic": LaunchConfiguration( + "keyframe_depth_validator_dynamic_masks_topic" + ), + "rgbd_valid_range_min_m": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_rgbd_valid_range_min_m" + ), + value_type=float, + ), + "rgbd_valid_range_max_m": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_rgbd_valid_range_max_m" + ), + value_type=float, + ), + "rgbd_heatmap_max_abs_error_m": ParameterValue( + LaunchConfiguration( + "keyframe_depth_validator_rgbd_heatmap_max_abs_error_m" + ), + value_type=float, + ), }, ], ), From 337a3e625785939933b7a7248463a802cc7b9d0f Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 26 Jun 2026 14:00:58 +0200 Subject: [PATCH 39/59] minor --- docker/docker-compose.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index e975a02..bf25088 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -55,6 +55,8 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" # Working Directory - Keyframe depth validator (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" + # Working Directory - Dynamic keypoint 3D lifter (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" # Data Directory (** MODIFY **) - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" @@ -81,5 +83,7 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_estimator:/home/$USERNAME/workspace/src/keyframe_depth_estimator:rw" # Working Directory - Keyframe depth validator (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" + # Working Directory - Dynamic keypoint 3D lifter (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" entrypoint: "" restart: "no" From f6e9d232f2f34eafcc3c5b2e4e550a263a18ad39 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 26 Jun 2026 14:01:29 +0200 Subject: [PATCH 40/59] add dynamic keypoints lifting pipeline to launch file --- launch/mono-imu.launch.py | 203 +++++++++++++++++++++++++++++++++++++- 1 file changed, 199 insertions(+), 4 deletions(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index bf0dced..f68fb4b 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -22,6 +22,16 @@ def generate_launch_description(): "config", "config.yaml", ) + dynamic_keypoint_tracker_config = os.path.join( + get_package_share_directory("dynamic_keypoint_tracker"), + "config", + "config.yaml", + ) + dynamic_keypoint_3d_lifter_config = os.path.join( + get_package_share_directory("dynamic_keypoint_3d_lifter"), + "config", + "config.yaml", + ) return LaunchDescription( [ @@ -38,8 +48,12 @@ def generate_launch_description(): DeclareLaunchArgument("colored_pointcloud", default_value="true"), DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), DeclareLaunchArgument("use_aux_depth", default_value="false"), - DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="true"), + DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="false"), DeclareLaunchArgument("launch_keyframe_depth_validator", default_value="true"), + DeclareLaunchArgument("launch_dynamic_keypoint_tracker", default_value="true"), + DeclareLaunchArgument("launch_object_track_manager", default_value="true"), + DeclareLaunchArgument("launch_track_outlier_rejector", default_value="true"), + DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="true"), DeclareLaunchArgument( "keyframe_depth_model_path", default_value=( @@ -63,10 +77,10 @@ def generate_launch_description(): "keyframe_depth_validator_queue_depth", default_value="4" ), DeclareLaunchArgument( - "keyframe_depth_validator_debug_match_logging", default_value="true" + "keyframe_depth_validator_debug_match_logging", default_value="false" ), DeclareLaunchArgument( - "keyframe_depth_validator_use_offline_metric_depth", default_value="false" + "keyframe_depth_validator_use_offline_metric_depth", default_value="true" ), DeclareLaunchArgument( "keyframe_depth_validator_offline_sync_tolerance_ms", default_value="20.0" @@ -97,6 +111,50 @@ def generate_launch_description(): DeclareLaunchArgument( "keyframe_depth_validator_rgbd_heatmap_max_abs_error_m", default_value="1.0" ), + DeclareLaunchArgument( + "dynamic_keypoint_instance_masks_topic", + default_value="/camera/color/image_instance_masks_array", + ), + DeclareLaunchArgument( + "dynamic_keypoint_tracks_topic", + default_value="/dynamic_keypoint_tracks", + ), + DeclareLaunchArgument( + "dynamic_keypoint_object_tracks_topic", + default_value="/object_tracks", + ), + DeclareLaunchArgument( + "dynamic_keypoint_filtered_object_tracks_topic", + default_value="/object_tracks/filtered", + ), + DeclareLaunchArgument( + "dynamic_keypoint_publish_debug_image", + default_value="true", + ), + DeclareLaunchArgument( + "dynamic_keypoint_debug_image_width", + default_value="0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_debug_image_height", + default_value="0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_output_topic", + default_value="/dynamic_object_points_3d", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_low_confidence_behavior", + default_value="lift_and_flag", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_low_confidence_max_residual_m", + default_value="0.25", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_publish_debug_image", + default_value="true", + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -308,6 +366,138 @@ def generate_launch_description(): }, ], ), + # Dynamic Keypoint Tracker + Node( + condition=IfCondition( + LaunchConfiguration("launch_dynamic_keypoint_tracker") + ), + package="dynamic_keypoint_tracker", + executable="dynamic_keypoint_tracker_node", + name="dynamic_keypoint_tracker", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("offline"), + "image_topic": LaunchConfiguration("rgb_image_topic"), + "instance_masks_topic": LaunchConfiguration( + "dynamic_keypoint_instance_masks_topic" + ), + "tracks_topic": LaunchConfiguration( + "dynamic_keypoint_tracks_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("dynamic_keypoint_publish_debug_image"), + value_type=bool, + ), + }, + ], + ), + # Dynamic Object Track Manager + Node( + condition=IfCondition( + LaunchConfiguration("launch_object_track_manager") + ), + package="dynamic_keypoint_tracker", + executable="object_track_manager_node", + name="object_track_manager", + output="screen", + parameters=[ + dynamic_keypoint_tracker_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "instance_masks_topic": LaunchConfiguration( + "dynamic_keypoint_instance_masks_topic" + ), + "dynamic_keypoint_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_tracks_topic" + ), + "image_topic": LaunchConfiguration("rgb_image_topic"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_object_tracks_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("dynamic_keypoint_publish_debug_image"), + value_type=bool, + ), + }, + ], + ), + # Dynamic Track Outlier Rejector + Node( + condition=IfCondition( + LaunchConfiguration("launch_track_outlier_rejector") + ), + package="dynamic_keypoint_tracker", + executable="track_outlier_rejector_node", + name="track_outlier_rejector", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_object_tracks_topic" + ), + "filtered_object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("dynamic_keypoint_publish_debug_image"), + value_type=bool, + ), + "debug_image_width": ParameterValue( + LaunchConfiguration("dynamic_keypoint_debug_image_width"), + value_type=int, + ), + "debug_image_height": ParameterValue( + LaunchConfiguration("dynamic_keypoint_debug_image_height"), + value_type=int, + ), + }, + ], + ), + # Dynamic Keypoint 3D Lifter + Node( + condition=IfCondition( + LaunchConfiguration("launch_dynamic_keypoint_3d_lifter") + ), + package="dynamic_keypoint_3d_lifter", + executable="dynamic_keypoint_3d_lifter_node", + name="dynamic_keypoint_3d_lifter", + output="screen", + parameters=[ + dynamic_keypoint_3d_lifter_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "corrected_depth_topic": LaunchConfiguration( + "keyframe_depth_corrected_topic" + ), + "keyframe_pose_topic": "/vs_graphs/camera_pose", + "keyframe_created_topic": "/orbslam3/keyframe_created", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "output_topic": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_output_topic" + ), + "low_confidence_behavior": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_low_confidence_behavior" + ), + "low_confidence_max_residual_m": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_low_confidence_max_residual_m" + ), + value_type=float, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_publish_debug_image" + ), + value_type=bool, + ), + }, + ], + ), # Static Transforms Node( package="tf2_ros", @@ -417,7 +607,12 @@ def generate_launch_description(): executable="frame_segmenter_yolo26.py", output="screen", parameters=[ - {"visualize": LaunchConfiguration("visualize_segmented_scene")} + {"visualize": LaunchConfiguration("visualize_segmented_scene")}, + { + "params.ros_topics.raw_image_topic": LaunchConfiguration( + "rgb_image_topic" + ) + }, ], arguments=[ "--ros-args", From 71ca6dd17d2e0df0e8c5693f9a2d59498449d716 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 29 Jun 2026 17:05:31 +0200 Subject: [PATCH 41/59] fix static tf publisher --- launch/mono-imu.launch.py | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index f68fb4b..6570335 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -501,7 +501,7 @@ def generate_launch_description(): # Static Transforms Node( package="tf2_ros", - name="map_to_map_elevated", # For Voxblox Skeleton + name="map_to_map_elevated", # For Voxblox Skeleton executable="static_transform_publisher", arguments=["0", "0", "0", "0", "0", "0", "map", "map_elevated"], ), @@ -521,19 +521,28 @@ def generate_launch_description(): package="tf2_ros", name="camera_to_imu", executable="static_transform_publisher", - arguments=["0", "0", "0", "1.5708", "0", "1.5708", "camera", "imu"], + arguments=[ + "-0.011739999987185001", + "-0.005520000122487545", + "0.005100000184029341", + "0", + "0", + "0", + "camera", + "imu", + ], ), Node( package="tf2_ros", name="camera_to_camera_optical", executable="static_transform_publisher", arguments=[ - "0", - "0", - "0", - "0", - "0", - "0", + "0.00011983246804447845", + "0.014999180100858212", + "0.00015637179603800178", + "-1.5730284322450263", + "0.019080586334886032", + "-1.574238659946616", "camera", "camera_color_optical_frame", # RealSense: camera_color_optical_frame, OpenLoris: d400_color From 9c23a8124c9c30cd0d1b58e0131caf428ab7fc6e Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 30 Jun 2026 13:03:59 +0200 Subject: [PATCH 42/59] add dynamic keypoint interpolator launch and arguments --- config/Visualization/vsgraphs_rgbd.rviz | 86 +++++++++++++++++++--- launch/mono-imu.launch.py | 95 +++++++++++++++++++++++-- 2 files changed, 165 insertions(+), 16 deletions(-) diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 44dc4a8..ef0bd1a 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -1,6 +1,6 @@ Panels: - Class: rviz_common/Displays - Help Height: 0 + Help Height: 70 Name: Displays Property Tree Widget: Expanded: @@ -9,7 +9,7 @@ Panels: - /Keyframe Depth Debug1 - /Keyframe Depth Validator Debug1 Splitter Ratio: 0.679411768913269 - Tree Height: 176 + Tree Height: 128 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: All Points + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -139,7 +139,7 @@ Visualization Manager: Enabled: true Name: KeyFrames Namespaces: - kf_markers: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -151,7 +151,7 @@ Visualization Manager: Enabled: true Name: Camera Pose Namespaces: - camera_pose: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -436,7 +436,7 @@ Visualization Manager: Enabled: true Name: Structural Elements Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -506,7 +506,7 @@ Visualization Manager: Value: /camera/color/image_instance_masks Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -518,9 +518,9 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /keyframe_depth/debug_image - Value: true + Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -532,6 +532,62 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /keyframe_depth_validator/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Tuning Debug Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /keyframe_depth_validator/tuning_debug_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Dynamic Keypoint Lifter Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynamic_keypoint_3d_lifter/debug_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Dynamic Keypoint Tracker Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynamic_keypoint_tracker/debug_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Object Track Manager Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_track_manager/debug_image Value: true Enabled: true Global Options: @@ -604,6 +660,10 @@ Window Geometry: collapsed: false Displays: collapsed: false + Dynamic Keypoint Lifter Debug: + collapsed: false + Dynamic Keypoint Tracker Debug: + collapsed: false Height: 1043 Hide Left Dock: false Hide Right Dock: false @@ -613,7 +673,9 @@ Window Geometry: collapsed: false Keyframe Depth Validator Debug: collapsed: false - QMainWindow State: 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 + Object Track Manager Debug: + collapsed: false + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: @@ -626,8 +688,10 @@ Window Geometry: collapsed: false Tracked Frames: collapsed: false + Tuning Debug Image: + collapsed: false Views: collapsed: false Width: 1920 - X: 1920 + X: 3840 Y: 0 diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 6570335..7081f2b 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -54,6 +54,7 @@ def generate_launch_description(): DeclareLaunchArgument("launch_object_track_manager", default_value="true"), DeclareLaunchArgument("launch_track_outlier_rejector", default_value="true"), DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="true"), + DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="false"), DeclareLaunchArgument( "keyframe_depth_model_path", default_value=( @@ -68,13 +69,13 @@ def generate_launch_description(): "keyframe_depth_corrected_topic", default_value="/keyframe_depth/corrected" ), DeclareLaunchArgument( - "keyframe_depth_publish_debug_image", default_value="true" + "keyframe_depth_publish_debug_image", default_value="false" ), DeclareLaunchArgument( - "keyframe_depth_validator_publish_debug_image", default_value="true" + "keyframe_depth_validator_publish_debug_image", default_value="false" ), DeclareLaunchArgument( - "keyframe_depth_validator_queue_depth", default_value="4" + "keyframe_depth_validator_queue_depth", default_value="100" ), DeclareLaunchArgument( "keyframe_depth_validator_debug_match_logging", default_value="false" @@ -106,7 +107,7 @@ def generate_launch_description(): "keyframe_depth_validator_rgbd_valid_range_min_m", default_value="0.3" ), DeclareLaunchArgument( - "keyframe_depth_validator_rgbd_valid_range_max_m", default_value="3.0" + "keyframe_depth_validator_rgbd_valid_range_max_m", default_value="8.0" ), DeclareLaunchArgument( "keyframe_depth_validator_rgbd_heatmap_max_abs_error_m", default_value="1.0" @@ -155,6 +156,34 @@ def generate_launch_description(): "dynamic_keypoint_3d_lifter_publish_debug_image", default_value="true", ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_queue_depth", + default_value="100", + ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_debug_match_logging", + default_value="true", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_output_topic", + default_value="/dynamic_object_points_3d/interpolated", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_queue_depth", + default_value="300", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_gap_timeout_ms", + default_value="2000", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_min_residual_path_length_px", + default_value="1.0", + ), + DeclareLaunchArgument( + "dynamic_keypoint_interpolator_publish_debug_image", + default_value="true", + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -475,11 +504,14 @@ def generate_launch_description(): "keyframe_depth_corrected_topic" ), "keyframe_pose_topic": "/vs_graphs/camera_pose", - "keyframe_created_topic": "/orbslam3/keyframe_created", "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), "output_topic": LaunchConfiguration( "dynamic_keypoint_3d_lifter_output_topic" ), + "queue_depth": ParameterValue( + LaunchConfiguration("dynamic_keypoint_3d_lifter_queue_depth"), + value_type=int, + ), "low_confidence_behavior": LaunchConfiguration( "dynamic_keypoint_3d_lifter_low_confidence_behavior" ), @@ -495,6 +527,59 @@ def generate_launch_description(): ), value_type=bool, ), + "debug_match_logging": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_debug_match_logging" + ), + value_type=bool, + ), + }, + ], + ), + # Dynamic Keypoint Interpolator + Node( + condition=IfCondition( + LaunchConfiguration("launch_dynamic_keypoint_interpolator") + ), + package="dynamic_keypoint_3d_lifter", + executable="dynamic_keypoint_interpolator_node", + name="dynamic_keypoint_interpolator", + output="screen", + parameters=[ + dynamic_keypoint_3d_lifter_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "lifted_points_topic": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_output_topic" + ), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "output_topic": LaunchConfiguration( + "dynamic_keypoint_interpolator_output_topic" + ), + "queue_depth": ParameterValue( + LaunchConfiguration("dynamic_keypoint_interpolator_queue_depth"), + value_type=int, + ), + "gap_timeout_ms": ParameterValue( + LaunchConfiguration("dynamic_keypoint_interpolator_gap_timeout_ms"), + value_type=int, + ), + "min_residual_path_length_px": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_interpolator_min_residual_path_length_px" + ), + value_type=float, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_interpolator_publish_debug_image" + ), + value_type=bool, + ), }, ], ), From a0da6bb52da22184e107e9ff1ca9aafc4942f8b2 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 1 Jul 2026 11:40:28 +0200 Subject: [PATCH 43/59] update gitignore --- .gitignore | 4 +- docker/Jazzy_new.Dockerfile | 261 ++++++++++++++++++++++++++++++++++++ docker/docker-compose.yml | 4 +- 3 files changed, 264 insertions(+), 5 deletions(-) create mode 100644 docker/Jazzy_new.Dockerfile diff --git a/.gitignore b/.gitignore index ac75783..cf09f08 100644 --- a/.gitignore +++ b/.gitignore @@ -47,6 +47,4 @@ borrar/* */ExecMean.txt -!Dockerfile -docker/docker-compose.yml -docker/Jazzy_new.Dockerfile \ No newline at end of file +!Dockerfile \ No newline at end of file diff --git a/docker/Jazzy_new.Dockerfile b/docker/Jazzy_new.Dockerfile new file mode 100644 index 0000000..afef1fd --- /dev/null +++ b/docker/Jazzy_new.Dockerfile @@ -0,0 +1,261 @@ +FROM nvcr.io/nvidia/cuda-dl-base:25.04-cuda12.9-devel-ubuntu24.04 + +# Arguments +ARG USERNAME=user +ARG USER_UID=1000 +ARG USER_GID=$USER_UID +ARG DEBIAN_FRONTEND=noninteractive + +# Environment variables +ENV CUDA_HOME=/usr/local/cuda \ + LANG=en_US.UTF-8 \ + LC_ALL=en_US.UTF-8 \ + ROS_DISTRO=jazzy \ + PIP_BREAK_SYSTEM_PACKAGES=1 + +ENV PATH="/usr/src/tensorrt/bin:${PATH}" + +# --- Fix MPI issue --- +RUN mkdir -p /opt/hpcx/ompi/lib/x86_64-linux-gnu \ + && ln -s /opt/hpcx/ompi /opt/hpcx/ompi/lib/x86_64-linux-gnu/openmpi \ + && dpkg-reconfigure libc-bin + +# --- Handle user creation --- +RUN if id -u $USER_UID ; then userdel "$(id -un $USER_UID)" ; fi + +# --- System setup --- +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-pip \ + python-is-python3 \ + git \ + openssh-client \ + wget \ + vim \ + curl \ + libeigen3-dev \ + build-essential \ + locales \ + software-properties-common \ + lsb-release \ + gnupg2 && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +RUN add-apt-repository universe + +# --- ROS 2 Jazzy APT source setup --- +RUN ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ + curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" && \ + apt install -y /tmp/ros2-apt-source.deb + +# --- Install ROS 2 Jazzy development tools --- +RUN apt update && apt upgrade -y && \ + apt install -y ros-dev-tools ros-${ROS_DISTRO}-desktop ros-${ROS_DISTRO}-rqt-tf-tree + +# --- Source ROS globally --- +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc + +# --- Initialize rosdep --- +RUN rosdep init && rosdep update + +# --- Clean up --- +RUN rm -rf /var/lib/apt/lists/* /tmp/* + +# --- Create user --- +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# --- Python environment setup --- +RUN pip3 install networkx==3.1 +RUN pip3 uninstall -y torch torchvision torchaudio || true +RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \ + torch==2.6.0+cu126 \ + torchvision==0.21.0+cu126 +RUN apt remove --purge python3-typing-extensions -y +RUN pip3 install typing-extensions==4.11.0 + +# --- CLIP and Detectron2 setup --- +ARG TORCH_CUDA_ARCH_LIST="7.5;7.0+PTX" +ENV FORCE_CUDA="1" +RUN pip3 install 'git+https://github.com/facebookresearch/detectron2.git' +RUN pip3 install 'git+https://github.com/openai/CLIP.git' + + +# --- SSH keys --- +# Define the SSH keys as build arguments for latter mounting +RUN mkdir -p -m 0600 ~/.ssh && ssh-keyscan github.com >> ~/.ssh/known_hosts + +# --- Clone repositories --- +RUN apt-get update && apt-get install -y \ + libepoxy-dev \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + freeglut3-dev \ + libglew-dev \ + cmake \ + build-essential \ + git + +# Pangolin +WORKDIR /opt/ +RUN git clone --branch v0.9.1 --depth 1 https://github.com/stevenlovegrove/Pangolin.git && \ + cd Pangolin && \ + mkdir build && cd build && \ + cmake .. && \ + make -j && \ + make install + +# Cmake +ARG version=3.22 +ARG build=1 +WORKDIR /tmp +RUN wget https://cmake.org/files/v$version/cmake-$version.$build.tar.gz + +RUN tar -xzvf cmake-$version.$build.tar.gz +WORKDIR /tmp/cmake-$version.$build +RUN ./bootstrap +RUN make -j8 +RUN make install + +WORKDIR /home/$USERNAME/workspace/src + +# Mount the SSH keys and clone the vS-Graphs repositories +RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git +RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git +RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git +RUN --mount=type=ssh git clone -b r/4.57.2 --depth 1 git@github.com:IntelRealSense/realsense-ros.git +# RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git + +# Repositories for GNN-based room detection and reasoning +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_wrapper.git +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_datasets.git +# RUN --mount=type=ssh git clone -b develop git@github.com:snt-arg/situational_graphs_reasoning.git +# RUN --mount=type=ssh git clone -b main git@github.com:snt-arg/situational_graphs_reasoning_msgs.git + +# Install the vS-Graphs dependencies +WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker +RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt + +# Install object tracker dependencies when the package is present in the workspace +RUN if [ -f /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ]; then \ + pip3 install --break-system-packages --ignore-installed \ + -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt ; \ + fi + +# [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy +# (Read more: https://github.com/ros2/ros2/issues/1702) +# RUN pip3 install --break-system-packages setuptools==79.0.1 + +# Install reasoning dependencies +# RUN pip3 install --break-system-packages shapely==2.1.1 torch-geometric==2.6.1 transforms3d==0.4.2 +# RUN mkdir -p /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports \ +# && chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace/install/situational_graphs_reasoning/share/situational_graphs_reasoning/reports + +# Install the EOMT dependencies +WORKDIR /home/$USERNAME/workspace/src/scene_segment_ros/src/ +RUN git clone https://github.com/tue-mps/eomt.git +RUN pip3 install --ignore-installed -r /home/${USERNAME}/workspace/src/scene_segment_ros/src/eomt/requirements.txt +RUN pip3 install "numpy<2.0" --force-reinstall + +# WORKDIR /home/$USERNAME/workspace/src/ + +# Download the yoso checkpoint +RUN wget https://github.com/hujiecpp/YOSO/releases/download/v0.1/yoso_res50_coco.pth +RUN mv yoso_res50_coco.pth /home/$USERNAME/workspace/src/scene_segment_ros/include/ + +# USER root +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-rviz-visual-tools \ + ros-${ROS_DISTRO}-depth-image-proc \ + ros-${ROS_DISTRO}-backward-ros \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + ros-${ROS_DISTRO}-diagnostic-updater \ + ros-${ROS_DISTRO}-pcl-ros + +# Build the workspace +WORKDIR /home/$USERNAME/workspace/ +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" + +# --- Miscalleanous --- +RUN ldconfig + +# --- Clean up --- +# Remove the apt list files +RUN rm -rf /var/lib/apt/lists/* + +# Remove packages no longer needed +RUN apt-get clean && apt-get autoremove -y + +# Remove the ssh keys +RUN rm -rf /root/.ssh/ + +# --- Build entrypoint --- +RUN echo "#!/bin/bash" >> /entrypoint.sh \ + && echo "echo \"source /opt/ros/$ROS_DISTRO/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ + && echo "echo \"source /home/$USERNAME/workspace/install/setup.bash\" >> ~/.bashrc" >> /entrypoint.sh \ + && echo 'exec "$@"' >> /entrypoint.sh \ + && chmod a+x /entrypoint.sh + +# ------------------------------------ +# Download Vox2Ros Toolkit for Voxblox +# ------------------------------------ +WORKDIR /home/$USERNAME/workspace/vsgraphs_tools +RUN curl -L https://raw.githubusercontent.com/snt-arg/vsgraphs_tools/refs/heads/main/Voxblox/relay_jazzy.py -o /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py +RUN chmod +x /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py + +USER $USERNAME +RUN sudo chown -R $USERNAME:$USERNAME /home/$USERNAME/workspace +WORKDIR /home/$USERNAME/workspace/ + +# --------------------------- +# Download and Install mprocs +# --------------------------- +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | bash -s -- -y \ + && . "$HOME/.cargo/env" \ + && cargo install mprocs + + +# note: you might have to reinstall numpy and opencv python pkgs and rebuilding cause the system packages are pointed instead +# note: torch version might be wrong (cu130 wheel), reinstall torch and torchvision with the correct versions if you encounter issues related to torch. +RUN sudo rm -rf \ + /usr/local/lib/python3.12/dist-packages/torch \ + /usr/local/lib/python3.12/dist-packages/torch-* \ + /usr/local/lib/python3.12/dist-packages/torchvision \ + /usr/local/lib/python3.12/dist-packages/torchvision-* \ + /usr/local/lib/python3.12/dist-packages/torchaudio \ + /usr/local/lib/python3.12/dist-packages/torchaudio-* \ + /usr/local/lib/python3.12/dist-packages/functorch \ + /usr/local/lib/python3.12/dist-packages/torchgen + +RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall \ + torch torchvision torchaudio \ + --index-url https://download.pytorch.org/whl/cu126 + +RUN rm -rf ~/.local/lib/python3.12/site-packages/numpy \ + ~/.local/lib/python3.12/site-packages/numpy-*.dist-info \ + ~/.local/lib/python3.12/site-packages/numpy.libs + +RUN python3 -m pip install --user --break-system-packages --no-cache-dir "numpy==1.26.4" +RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall --no-deps \ + "scipy==1.13.1" +RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall --no-deps \ + "ultralytics==8.4.68" + +WORKDIR /home/$USERNAME/workspace/ +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge" +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge" + +# -------------------------- +# Aliases and Environment Setup +# -------------------------- +RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/config/mprocs.yml'" >> ~/.bashrc && \ + echo "alias rel_vox='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode voxblox_client'" >> ~/.bashrc && \ + echo "alias rel_pcl='python /home/$USERNAME/workspace/vsgraphs_tools/relay_jazzy.py --mode pc_server'" >> ~/.bashrc + +ENTRYPOINT ["/entrypoint.sh"] +USER $USERNAME +CMD ["/bin/bash"] +SHELL ["/bin/bash"] diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index bf25088..6dc4c36 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -48,7 +48,7 @@ services: # Working Directory - Situational Graphs Messages (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" # Working Directory - 3D object tracker (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + # - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" # Working Directory - Dynamic keypoint tracker (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" # Working Directory - Keyframe depth estimator (** MODIFY **) @@ -76,7 +76,7 @@ services: # Working Directory - Situational Graphs Messages (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/situational_graphs_msgs:/home/$USERNAME/workspace/src/situational_graphs_msgs:rw" # Working Directory - 3D object tracker (** MODIFY **) - - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" + # - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_tracker_3d_ros:/home/$USERNAME/workspace/src/object_tracker_3d_ros:rw" # Working Directory - Dynamic keypoint tracker (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_tracker:/home/$USERNAME/workspace/src/dynamic_keypoint_tracker:rw" # Working Directory - Keyframe depth estimator (** MODIFY **) From f1a9375e9204141e8ed25ceafa64426bd46afcf2 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 1 Jul 2026 16:44:56 +0200 Subject: [PATCH 44/59] add object motion estimator configuration and launch arguments --- docker/docker-compose.yml | 4 ++ launch/mono-imu.launch.py | 89 ++++++++++++++++++++++++++++++++++++++- 2 files changed, 92 insertions(+), 1 deletion(-) diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 6dc4c36..5773034 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -57,6 +57,8 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" # Working Directory - Dynamic keypoint 3D lifter (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" + # Working Directory - Object motion estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_motion_estimator:/home/$USERNAME/workspace/src/object_motion_estimator:rw" # Data Directory (** MODIFY **) - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" @@ -85,5 +87,7 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/keyframe_depth_validator:/home/$USERNAME/workspace/src/keyframe_depth_validator:rw" # Working Directory - Dynamic keypoint 3D lifter (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" + # Working Directory - Object motion estimator (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_motion_estimator:/home/$USERNAME/workspace/src/object_motion_estimator:rw" entrypoint: "" restart: "no" diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 7081f2b..32d28a8 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -32,6 +32,11 @@ def generate_launch_description(): "config", "config.yaml", ) + object_motion_estimator_config = os.path.join( + get_package_share_directory("object_motion_estimator"), + "config", + "config.yaml", + ) return LaunchDescription( [ @@ -54,7 +59,8 @@ def generate_launch_description(): DeclareLaunchArgument("launch_object_track_manager", default_value="true"), DeclareLaunchArgument("launch_track_outlier_rejector", default_value="true"), DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="true"), - DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="false"), + DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="true"), + DeclareLaunchArgument("launch_object_motion_estimator", default_value="true"), DeclareLaunchArgument( "keyframe_depth_model_path", default_value=( @@ -152,6 +158,10 @@ def generate_launch_description(): "dynamic_keypoint_3d_lifter_low_confidence_max_residual_m", default_value="0.25", ), + DeclareLaunchArgument( + "dynamic_keypoint_3d_lifter_max_lift_distance_m", + default_value="7.0", + ), DeclareLaunchArgument( "dynamic_keypoint_3d_lifter_publish_debug_image", default_value="true", @@ -184,6 +194,34 @@ def generate_launch_description(): "dynamic_keypoint_interpolator_publish_debug_image", default_value="true", ), + DeclareLaunchArgument( + "object_motion_input_mode", + default_value="keyframe_only", + description=( + "Phase 4 input mode: keyframe_only, keyframe_and_interpolated, " + "or keyframe_and_interpolated_weighted" + ), + ), + DeclareLaunchArgument( + "object_motion_output_topic", + default_value="/object_motion", + ), + DeclareLaunchArgument( + "object_motion_queue_depth", + default_value="300", + ), + DeclareLaunchArgument( + "object_motion_gap_timeout_ms", + default_value="2000", + ), + DeclareLaunchArgument( + "object_motion_publish_debug_image", + default_value="true", + ), + DeclareLaunchArgument( + "object_motion_arrow_scale", + default_value="1.0", + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -521,6 +559,12 @@ def generate_launch_description(): ), value_type=float, ), + "max_lift_distance_m": ParameterValue( + LaunchConfiguration( + "dynamic_keypoint_3d_lifter_max_lift_distance_m" + ), + value_type=float, + ), "publish_debug_image": ParameterValue( LaunchConfiguration( "dynamic_keypoint_3d_lifter_publish_debug_image" @@ -583,6 +627,49 @@ def generate_launch_description(): }, ], ), + # Object Motion Estimator + Node( + condition=IfCondition( + LaunchConfiguration("launch_object_motion_estimator") + ), + package="object_motion_estimator", + executable="object_motion_estimator_node", + name="object_motion_estimator", + output="screen", + parameters=[ + object_motion_estimator_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "input_mode": LaunchConfiguration("object_motion_input_mode"), + "dynamic_object_points_topic": LaunchConfiguration( + "dynamic_keypoint_3d_lifter_output_topic" + ), + "interpolated_points_topic": LaunchConfiguration( + "dynamic_keypoint_interpolator_output_topic" + ), + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "image_topic": LaunchConfiguration("rgb_image_topic"), + "output_topic": LaunchConfiguration("object_motion_output_topic"), + "queue_depth": ParameterValue( + LaunchConfiguration("object_motion_queue_depth"), + value_type=int, + ), + "gap_timeout_ms": ParameterValue( + LaunchConfiguration("object_motion_gap_timeout_ms"), + value_type=int, + ), + "publish_debug_image": ParameterValue( + LaunchConfiguration("object_motion_publish_debug_image"), + value_type=bool, + ), + "arrow_scale": ParameterValue( + LaunchConfiguration("object_motion_arrow_scale"), + value_type=float, + ), + }, + ], + ), # Static Transforms Node( package="tf2_ros", From a67c0dbbb8de6bb6b1e84b70c5c07ba25a88ac52 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 6 Jul 2026 15:04:30 +0200 Subject: [PATCH 45/59] add motion estimate 3d viz params --- launch/mono-imu.launch.py | 80 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 79 insertions(+), 1 deletion(-) diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 32d28a8..9b2470c 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -196,7 +196,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "object_motion_input_mode", - default_value="keyframe_only", + default_value="keyframe_and_interpolated_weighted", description=( "Phase 4 input mode: keyframe_only, keyframe_and_interpolated, " "or keyframe_and_interpolated_weighted" @@ -206,6 +206,10 @@ def generate_launch_description(): "object_motion_output_topic", default_value="/object_motion", ), + DeclareLaunchArgument( + "object_motion_world_frame_id", + default_value="world", + ), DeclareLaunchArgument( "object_motion_queue_depth", default_value="300", @@ -214,14 +218,50 @@ def generate_launch_description(): "object_motion_gap_timeout_ms", default_value="2000", ), + DeclareLaunchArgument( + "object_motion_trajectory_window_size", + default_value="2147483647", + ), + DeclareLaunchArgument( + "object_motion_mad_k", + default_value="2.5", + ), DeclareLaunchArgument( "object_motion_publish_debug_image", default_value="true", ), + DeclareLaunchArgument( + "object_motion_publish_markers", + default_value="true", + ), DeclareLaunchArgument( "object_motion_arrow_scale", default_value="1.0", ), + DeclareLaunchArgument( + "object_motion_arrow_time_scale_s", + default_value="1.0", + ), + DeclareLaunchArgument( + "object_motion_arrow_lifetime_s", + default_value="0.0", + ), + DeclareLaunchArgument( + "object_motion_clear_markers_on_track_lost", + default_value="false", + ), + DeclareLaunchArgument( + "object_motion_marker_line_width_m", + default_value="0.05", + ), + DeclareLaunchArgument( + "object_motion_marker_sphere_radius_m", + default_value="0.08", + ), + DeclareLaunchArgument( + "object_motion_marker_text_height_m", + default_value="0.15", + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -647,10 +687,12 @@ def generate_launch_description(): "interpolated_points_topic": LaunchConfiguration( "dynamic_keypoint_interpolator_output_topic" ), + "object_track_events_topic": "/object_tracks/events", "camera_pose_topic": "/vs_graphs/camera_pose", "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), "image_topic": LaunchConfiguration("rgb_image_topic"), "output_topic": LaunchConfiguration("object_motion_output_topic"), + "world_frame_id": LaunchConfiguration("object_motion_world_frame_id"), "queue_depth": ParameterValue( LaunchConfiguration("object_motion_queue_depth"), value_type=int, @@ -659,14 +701,50 @@ def generate_launch_description(): LaunchConfiguration("object_motion_gap_timeout_ms"), value_type=int, ), + "trajectory_window_size": ParameterValue( + LaunchConfiguration("object_motion_trajectory_window_size"), + value_type=int, + ), + "mad_k": ParameterValue( + LaunchConfiguration("object_motion_mad_k"), + value_type=float, + ), "publish_debug_image": ParameterValue( LaunchConfiguration("object_motion_publish_debug_image"), value_type=bool, ), + "publish_markers": ParameterValue( + LaunchConfiguration("object_motion_publish_markers"), + value_type=bool, + ), "arrow_scale": ParameterValue( LaunchConfiguration("object_motion_arrow_scale"), value_type=float, ), + "arrow_time_scale_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_time_scale_s"), + value_type=float, + ), + "arrow_lifetime_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_lifetime_s"), + value_type=float, + ), + "clear_markers_on_track_lost": ParameterValue( + LaunchConfiguration("object_motion_clear_markers_on_track_lost"), + value_type=bool, + ), + "marker_line_width_m": ParameterValue( + LaunchConfiguration("object_motion_marker_line_width_m"), + value_type=float, + ), + "marker_sphere_radius_m": ParameterValue( + LaunchConfiguration("object_motion_marker_sphere_radius_m"), + value_type=float, + ), + "marker_text_height_m": ParameterValue( + LaunchConfiguration("object_motion_marker_text_height_m"), + value_type=float, + ), }, ], ), From 61ff504aa0a2f326e73d66c6f4f77410c74eadc9 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 7 Jul 2026 15:30:54 +0200 Subject: [PATCH 46/59] update launch arguments for object motion BA and keyframe depth components --- config/Visualization/vsgraphs_rgbd.rviz | 102 +++++++++++-- launch/mono-imu.launch.py | 191 +++++++++++++++++++++++- 2 files changed, 271 insertions(+), 22 deletions(-) diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index ef0bd1a..3c9a303 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -8,8 +8,9 @@ Panels: - /Segmented PC1 - /Keyframe Depth Debug1 - /Keyframe Depth Validator Debug1 + - /Object Motion Debug Image1 Splitter Ratio: 0.679411768913269 - Tree Height: 128 + Tree Height: 160 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Tracked Points Visualization Manager: Class: "" Displays: @@ -94,7 +95,7 @@ Visualization Manager: Value: /vs_graphs/tracking_image Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -106,7 +107,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /camera/color/image_segment_vis - Value: true + Value: false - Alpha: 0.699999988079071 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -139,7 +140,7 @@ Visualization Manager: Enabled: true Name: KeyFrames Namespaces: - {} + kf_markers: true Topic: Depth: 5 Durability Policy: Volatile @@ -151,7 +152,7 @@ Visualization Manager: Enabled: true Name: Camera Pose Namespaces: - {} + camera_pose: true Topic: Depth: 5 Durability Policy: Volatile @@ -436,7 +437,7 @@ Visualization Manager: Enabled: true Name: Structural Elements Namespaces: - {} + "": true Topic: Depth: 5 Durability Policy: Volatile @@ -534,7 +535,7 @@ Visualization Manager: Value: /keyframe_depth_validator/debug_image Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -546,7 +547,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /keyframe_depth_validator/tuning_debug_image - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -576,7 +577,7 @@ Visualization Manager: Value: /dynamic_keypoint_tracker/debug_image Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -588,6 +589,71 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /object_track_manager/debug_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Dynamic Keypoint Interpolator Debug + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynamic_keypoint_interpolator/debug_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Object Motion Debug Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/debug_image + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: "Pipeline A: depth model" + Namespaces: + track_10: true + track_12: true + track_14: true + track_15: true + track_16: true + track_17: true + track_18: true + track_19: true + track_20: true + track_4: true + track_5: true + track_6: true + track_8: true + track_9: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/depth_model/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: "Pipeline B: BA+IMU scale" + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/ba/markers Value: true Enabled: true Global Options: @@ -642,24 +708,26 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 0.7986353635787964 - Y: 3.4789421558380127 - Z: 2.6508219242095947 + X: 2.479626417160034 + Y: 9.261330604553223 + Z: 4.061656951904297 Focal Shape Fixed Size: false Focal Shape Size: 0.0010000000474974513 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.065000295639038 + Pitch: 1.1750001907348633 Target Frame: camera Value: Orbit (rviz_default_plugins) - Yaw: 4.460034370422363 + Yaw: 5.405036926269531 Saved: ~ Window Geometry: Depth Estimate: collapsed: false Displays: collapsed: false + Dynamic Keypoint Interpolator Debug: + collapsed: false Dynamic Keypoint Lifter Debug: collapsed: false Dynamic Keypoint Tracker Debug: @@ -673,9 +741,11 @@ Window Geometry: collapsed: false Keyframe Depth Validator Debug: collapsed: false + Object Motion Debug Image: + collapsed: false Object Track Manager Debug: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index 9b2470c..e5c4691 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -53,14 +53,15 @@ def generate_launch_description(): DeclareLaunchArgument("colored_pointcloud", default_value="true"), DeclareLaunchArgument("visualize_segmented_scene", default_value="true"), DeclareLaunchArgument("use_aux_depth", default_value="false"), - DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="false"), - DeclareLaunchArgument("launch_keyframe_depth_validator", default_value="true"), + DeclareLaunchArgument("launch_keyframe_depth_estimator", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_keyframe_depth_validator", default_value="false"), # Pipeline DA3 DeclareLaunchArgument("launch_dynamic_keypoint_tracker", default_value="true"), DeclareLaunchArgument("launch_object_track_manager", default_value="true"), DeclareLaunchArgument("launch_track_outlier_rejector", default_value="true"), - DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="true"), - DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="true"), - DeclareLaunchArgument("launch_object_motion_estimator", default_value="true"), + DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_object_motion_estimator", default_value="false"), # Pipeline DA3 + DeclareLaunchArgument("launch_object_ba_tracker", default_value="true"), # Pipeline BA DeclareLaunchArgument( "keyframe_depth_model_path", default_value=( @@ -204,7 +205,11 @@ def generate_launch_description(): ), DeclareLaunchArgument( "object_motion_output_topic", - default_value="/object_motion", + default_value="/object_motion/depth_model", + ), + DeclareLaunchArgument( + "object_motion_markers_topic", + default_value="/object_motion/depth_model/markers", ), DeclareLaunchArgument( "object_motion_world_frame_id", @@ -226,6 +231,10 @@ def generate_launch_description(): "object_motion_mad_k", default_value="2.5", ), + DeclareLaunchArgument( + "object_motion_ema_alpha_h", + default_value="0.4", + ), DeclareLaunchArgument( "object_motion_publish_debug_image", default_value="true", @@ -262,6 +271,62 @@ def generate_launch_description(): "object_motion_marker_text_height_m", default_value="0.15", ), + DeclareLaunchArgument( + "object_ba_output_topic", + default_value="/object_motion/ba", + ), + DeclareLaunchArgument( + "object_ba_markers_topic", + default_value="/object_motion/ba/markers", + ), + DeclareLaunchArgument( + "object_ba_window_size", + default_value="20", + ), + DeclareLaunchArgument( + "object_ba_min_window_frames", + default_value="5", + ), + DeclareLaunchArgument( + "object_ba_min_point_observations", + default_value="3", + ), + DeclareLaunchArgument( + "object_ba_sigma_pixel", + default_value="1.5", + ), + DeclareLaunchArgument( + "object_ba_huber_delta_pixels", + default_value="3.0", + ), + DeclareLaunchArgument( + "object_ba_num_iterations", + default_value="10", + ), + DeclareLaunchArgument( + "object_ba_time_budget_ms", + default_value="15.0", + ), + DeclareLaunchArgument( + "object_ba_min_camera_displacement_m", + default_value="0.05", + ), + DeclareLaunchArgument( + "object_ba_alpha_scale", + default_value="0.3", + ), + DeclareLaunchArgument( + "object_ba_pose_topic_timeout_s", + default_value="5.0", + ), + DeclareLaunchArgument( + "object_ba_pose_sync_tolerance_ms", + default_value="10.0", + ), + DeclareLaunchArgument( + "object_ba_mad_k", + default_value="2.5", + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -692,6 +757,7 @@ def generate_launch_description(): "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), "image_topic": LaunchConfiguration("rgb_image_topic"), "output_topic": LaunchConfiguration("object_motion_output_topic"), + "markers_topic": LaunchConfiguration("object_motion_markers_topic"), "world_frame_id": LaunchConfiguration("object_motion_world_frame_id"), "queue_depth": ParameterValue( LaunchConfiguration("object_motion_queue_depth"), @@ -709,6 +775,10 @@ def generate_launch_description(): LaunchConfiguration("object_motion_mad_k"), value_type=float, ), + "ema_alpha_h": ParameterValue( + LaunchConfiguration("object_motion_ema_alpha_h"), + value_type=float, + ), "publish_debug_image": ParameterValue( LaunchConfiguration("object_motion_publish_debug_image"), value_type=bool, @@ -748,6 +818,115 @@ def generate_launch_description(): }, ], ), + # Object BA Tracker (Pipeline B) + Node( + condition=IfCondition( + LaunchConfiguration("launch_object_ba_tracker") + ), + package="object_motion_estimator", + executable="object_ba_tracker_node", + name="object_ba_tracker", + output="screen", + parameters=[ + object_motion_estimator_config, + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "object_track_events_topic": "/object_tracks/events", + "output_topic": LaunchConfiguration("object_ba_output_topic"), + "markers_topic": LaunchConfiguration("object_ba_markers_topic"), + "world_frame_id": LaunchConfiguration("object_motion_world_frame_id"), + "queue_depth": ParameterValue( + LaunchConfiguration("object_motion_queue_depth"), + value_type=int, + ), + "window_size": ParameterValue( + LaunchConfiguration("object_ba_window_size"), + value_type=int, + ), + "min_window_frames": ParameterValue( + LaunchConfiguration("object_ba_min_window_frames"), + value_type=int, + ), + "min_point_observations": ParameterValue( + LaunchConfiguration("object_ba_min_point_observations"), + value_type=int, + ), + "sigma_pixel": ParameterValue( + LaunchConfiguration("object_ba_sigma_pixel"), + value_type=float, + ), + "huber_delta_pixels": ParameterValue( + LaunchConfiguration("object_ba_huber_delta_pixels"), + value_type=float, + ), + "num_iterations": ParameterValue( + LaunchConfiguration("object_ba_num_iterations"), + value_type=int, + ), + "ba_time_budget_ms": ParameterValue( + LaunchConfiguration("object_ba_time_budget_ms"), + value_type=float, + ), + "min_camera_displacement_m": ParameterValue( + LaunchConfiguration("object_ba_min_camera_displacement_m"), + value_type=float, + ), + "alpha_scale": ParameterValue( + LaunchConfiguration("object_ba_alpha_scale"), + value_type=float, + ), + "pose_topic_timeout_s": ParameterValue( + LaunchConfiguration("object_ba_pose_topic_timeout_s"), + value_type=float, + ), + "pose_sync_tolerance_ms": ParameterValue( + LaunchConfiguration("object_ba_pose_sync_tolerance_ms"), + value_type=float, + ), + "mad_k": ParameterValue( + LaunchConfiguration("object_ba_mad_k"), + value_type=float, + ), + "trajectory_window_size": ParameterValue( + LaunchConfiguration("object_motion_trajectory_window_size"), + value_type=int, + ), + "publish_markers": ParameterValue( + LaunchConfiguration("object_motion_publish_markers"), + value_type=bool, + ), + "arrow_time_scale_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_time_scale_s"), + value_type=float, + ), + "arrow_lifetime_s": ParameterValue( + LaunchConfiguration("object_motion_arrow_lifetime_s"), + value_type=float, + ), + "clear_markers_on_track_lost": ParameterValue( + LaunchConfiguration("object_motion_clear_markers_on_track_lost"), + value_type=bool, + ), + "marker_line_width_m": ParameterValue( + LaunchConfiguration("object_motion_marker_line_width_m"), + value_type=float, + ), + "marker_sphere_radius_m": ParameterValue( + LaunchConfiguration("object_motion_marker_sphere_radius_m"), + value_type=float, + ), + "marker_text_height_m": ParameterValue( + LaunchConfiguration("object_motion_marker_text_height_m"), + value_type=float, + ), + }, + ], + ), # Static Transforms Node( package="tf2_ros", From 0093ad09303e37f19c5f7eb84f9f3345de122f31 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 8 Jul 2026 21:54:47 +0200 Subject: [PATCH 47/59] update launch parameters and add static map points publisher --- config/Visualization/vsgraphs_rgbd.rviz | 47 +++++++++-------------- core/src/Tracking.cc | 4 +- include/common.h | 3 ++ launch/mono-imu.launch.py | 26 ++++++++++++- src/common.cc | 51 +++++++++++++++++++++++++ 5 files changed, 98 insertions(+), 33 deletions(-) diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 3c9a303..7a7dacc 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -10,7 +10,7 @@ Panels: - /Keyframe Depth Validator Debug1 - /Object Motion Debug Image1 Splitter Ratio: 0.679411768913269 - Tree Height: 160 + Tree Height: 228 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -28,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Tracked Points + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -140,7 +140,7 @@ Visualization Manager: Enabled: true Name: KeyFrames Namespaces: - kf_markers: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -152,7 +152,7 @@ Visualization Manager: Enabled: true Name: Camera Pose Namespaces: - camera_pose: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -437,7 +437,7 @@ Visualization Manager: Enabled: true Name: Structural Elements Namespaces: - "": true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -549,7 +549,7 @@ Visualization Manager: Value: /keyframe_depth_validator/tuning_debug_image Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -561,7 +561,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dynamic_keypoint_3d_lifter/debug_image - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -577,7 +577,7 @@ Visualization Manager: Value: /dynamic_keypoint_tracker/debug_image Value: true - Class: rviz_default_plugins/Image - Enabled: false + Enabled: true Max Value: 1 Median window: 5 Min Value: 0 @@ -589,9 +589,9 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /object_track_manager/debug_image - Value: false + Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -603,7 +603,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dynamic_keypoint_interpolator/debug_image - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -622,20 +622,7 @@ Visualization Manager: Enabled: true Name: "Pipeline A: depth model" Namespaces: - track_10: true - track_12: true - track_14: true - track_15: true - track_16: true - track_17: true - track_18: true - track_19: true - track_20: true - track_4: true - track_5: true - track_6: true - track_8: true - track_9: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -701,16 +688,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 11.482182502746582 + Distance: 18.06743621826172 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 2.479626417160034 - Y: 9.261330604553223 - Z: 4.061656951904297 + X: 2.8777713775634766 + Y: 3.9726243019104004 + Z: 2.2547993659973145 Focal Shape Fixed Size: false Focal Shape Size: 0.0010000000474974513 Invert Z Axis: false @@ -745,7 +732,7 @@ Window Geometry: collapsed: false Object Track Manager Debug: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: diff --git a/core/src/Tracking.cc b/core/src/Tracking.cc index a08ad22..caad5a5 100644 --- a/core/src/Tracking.cc +++ b/core/src/Tracking.cc @@ -3100,7 +3100,7 @@ namespace ORB_SLAM3 // Decide if the tracking was succesful // More restrictive if there was a relocalization recently mpLocalMapper->mnMatchesInliers = mnMatchesInliers; - if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 50) + if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 25) return false; if ((mnMatchesInliers > 10) && (mState == RECENTLY_LOST)) @@ -3108,7 +3108,7 @@ namespace ORB_SLAM3 if (mSensor == System::IMU_MONOCULAR) { - if ((mnMatchesInliers < 15 && mpAtlas->isImuInitialized()) || (mnMatchesInliers < 50 && !mpAtlas->isImuInitialized())) + if ((mnMatchesInliers < 15 && mpAtlas->isImuInitialized()) || (mnMatchesInliers < 25 && !mpAtlas->isImuInitialized())) { return false; } diff --git a/include/common.h b/include/common.h index e374385..11ddfe1 100644 --- a/include/common.h +++ b/include/common.h @@ -80,6 +80,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ extern rclcpp::Publisher::SharedPtr pubOdometry; extern rclcpp::Publisher::SharedPtr pubKFImage; extern rclcpp::Publisher::SharedPtr pubKeyFrameCreated; extern rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; +extern rclcpp::Publisher::SharedPtr pubStaticMapPoints; extern rclcpp::Publisher::SharedPtr pubCameraPose; extern rclcpp::Publisher::SharedPtr pubAllMappoints; extern rclcpp::Publisher::SharedPtr pubTrackedMappoints; @@ -181,6 +183,7 @@ void publishPlanes(std::vector, rclcpp::Time); void publishTFTransform(Sophus::SE3f, string, string, rclcpp::Time); void publishAllPoints(std::vector, rclcpp::Time); void publishTrackedPoints(std::vector, rclcpp::Time); +void publishStaticMapPoints(std::vector, rclcpp::Time); void publishFiducialMarkers(std::vector, rclcpp::Time); void publishKeyFrameImages(std::vector, rclcpp::Time); void publishKeyFrameMarkers(std::vector, rclcpp::Time); diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index e5c4691..ca4e030 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -289,7 +289,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "object_ba_min_point_observations", - default_value="3", + default_value="6", ), DeclareLaunchArgument( "object_ba_sigma_pixel", @@ -327,6 +327,18 @@ def generate_launch_description(): "object_ba_mad_k", default_value="2.5", ), + DeclareLaunchArgument( + "object_ba_max_landmark_anchor_norm_m", + default_value="20.0", + ), + DeclareLaunchArgument( + "object_ba_max_landmark_reprojection_error_px", + default_value="8.0", + ), + DeclareLaunchArgument( + "object_ba_min_scale_object_displacement_m", + default_value="0.10", + ), DeclareLaunchArgument("keyframe_depth_sky_handling", default_value="true"), DeclareLaunchArgument( "aux_depth_topic", default_value="/camera/depth_da3/image_rect" @@ -892,6 +904,18 @@ def generate_launch_description(): LaunchConfiguration("object_ba_mad_k"), value_type=float, ), + "max_landmark_anchor_norm_m": ParameterValue( + LaunchConfiguration("object_ba_max_landmark_anchor_norm_m"), + value_type=float, + ), + "max_landmark_reprojection_error_px": ParameterValue( + LaunchConfiguration("object_ba_max_landmark_reprojection_error_px"), + value_type=float, + ), + "min_scale_object_displacement_m": ParameterValue( + LaunchConfiguration("object_ba_min_scale_object_displacement_m"), + value_type=float, + ), "trajectory_window_size": ParameterValue( LaunchConfiguration("object_motion_trajectory_window_size"), value_type=int, diff --git a/src/common.cc b/src/common.cc index bcf051c..cf5e592 100644 --- a/src/common.cc +++ b/src/common.cc @@ -53,6 +53,7 @@ rclcpp::Publisher::SharedPtr pubCameraPose; rclcpp::Publisher::SharedPtr pubKFImage; rclcpp::Publisher::SharedPtr pubKeyFrameCreated; rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; +rclcpp::Publisher::SharedPtr pubStaticMapPoints; rclcpp::Publisher::SharedPtr pubBuildingComponents; rclcpp::Publisher::SharedPtr pubTrackedMappoints; rclcpp::Publisher::SharedPtr pubFreespaceCluster; @@ -218,6 +219,9 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher( "/orbslam3/keyframe_static_map_points", 10); + pubStaticMapPoints = + node->create_publisher( + "/orbslam3/static_map_points", 10); pubTrackedMappoints = node->create_publisher(node_name + "/tracked_points", 1); pubWorldFramePointCloud = node->create_publisher(node_name + "/points_map", 1); pubKeyFrameMarker = node->create_publisher(node_name + "/kf_markers", 1); @@ -263,6 +267,7 @@ void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs: // Common topics publishCameraPose(Twc, msgTime); + publishStaticMapPoints(pSLAM->GetTrackedMapPoints(), msgTime); publishTFTransform(Twc, frameWorld, frameCamera, msgTime); publishFramePointCloud(Twc, msgPCL, msgTime); @@ -698,6 +703,52 @@ void publishTrackedPoints(std::vector trackedMapPoints, r pubTrackedMappoints->publish(cloud); } +void publishStaticMapPoints(std::vector trackedMapPoints, rclcpp::Time msgTime) +{ + if (!pubStaticMapPoints) + return; + + keyframe_depth_validator::msg::StaticMapPoints points_msg; + points_msg.header.stamp = msgTime; + points_msg.header.frame_id = frameWorld; + + const std::vector &trackedKeyPointsUn = pSLAM->GetTrackedKeyPointsUn(); + const Sophus::SE3f Tcw = pSLAM->GetCamTwc().inverse(); + + const std::size_t count = std::min(trackedMapPoints.size(), trackedKeyPointsUn.size()); + points_msg.point_ids.reserve(count); + points_msg.positions_world.reserve(count); + points_msg.pixels.reserve(count); + + for (std::size_t i = 0; i < count; ++i) + { + ORB_SLAM3::MapPoint *map_point = trackedMapPoints[i]; + if (map_point == nullptr || map_point->isBad()) + continue; + + const Eigen::Vector3f pos_world = map_point->GetWorldPos(); + const Eigen::Vector3f p_cam = Tcw * pos_world; + if (!std::isfinite(p_cam.z()) || p_cam.z() <= 0.0F) + continue; + + geometry_msgs::msg::Point p_world_msg; + p_world_msg.x = static_cast(pos_world.x()); + p_world_msg.y = static_cast(pos_world.y()); + p_world_msg.z = static_cast(pos_world.z()); + + geometry_msgs::msg::Point p_pixel_msg; + p_pixel_msg.x = static_cast(trackedKeyPointsUn[i].pt.x); + p_pixel_msg.y = static_cast(trackedKeyPointsUn[i].pt.y); + p_pixel_msg.z = 0.0; + + points_msg.point_ids.push_back(static_cast(map_point->mnId)); + points_msg.positions_world.push_back(p_world_msg); + points_msg.pixels.push_back(p_pixel_msg); + } + + pubStaticMapPoints->publish(points_msg); +} + void publishFramePointCloud(Sophus::SE3f Twc, const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPCL, rclcpp::Time msgTime) { if (!msgPCL) From fde6e4ba3927f102311ca0fd98fc951c1e76815b Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 10 Jul 2026 15:17:27 +0200 Subject: [PATCH 48/59] give dedicated thread to imuGrabber --- src/ros_mono_inertial.cc | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/ros_mono_inertial.cc b/src/ros_mono_inertial.cc index f52b26f..a446f40 100644 --- a/src/ros_mono_inertial.cc +++ b/src/ros_mono_inertial.cc @@ -879,14 +879,26 @@ int main(int argc, char **argv) auto igb = std::make_shared(state, auxDepthOptions, segmentOptions); // --- QoS (sensor data profile) --- + // rmw_qos_profile_sensor_data defaults to a keep-last depth of 5, which at ~400Hz is only a + // ~12.5ms buffer -- any delay servicing the IMU callback longer than that (e.g. the executor + // busy on a slower vision callback, see imuCallbackGroup below) causes DDS to silently drop + // the backlog before GrabImu ever runs, starving SyncWithImu's ready_imu_count even though + // the true publish rate is far higher than the ~30Hz camera rate would suggest is necessary. + // Widened to 0.5s worth of samples to absorb realistic scheduling jitter/bursts. rclcpp::QoS sensorQos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); sensorQos.reliability(rclcpp::ReliabilityPolicy::BestEffort); sensorQos.durability(rclcpp::DurabilityPolicy::Volatile); + sensorQos.keep_last(200); using sensor_msgs::msg::Image; using sensor_msgs::msg::Imu; - auto imuCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // automatically_add_to_executor_with_node = false: this group is serviced by its own + // dedicated executor/thread below instead of the shared 4-thread pool, so it must not also + // be picked up by executor.add_node(node) below (a callback group can only belong to one + // executor at a time). + auto imuCallbackGroup = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, /*automatically_add_to_executor_with_node=*/false); auto imageCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto auxDepthCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto instanceMaskCallbackGroup = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -952,6 +964,14 @@ int main(int argc, char **argv) // --- Sync thread --- std::thread syncThread(&ImageGrabber::SyncWithImu, igb); + // Dedicated executor + thread for imuCallbackGroup: isolates high-rate (~400Hz) IMU + // servicing from the heavier per-frame vision callbacks (image/instanceMask/semantic/ + // voxblox), which share only 4 threads on the executor below and can otherwise starve the + // IMU callback long enough to overrun sensorQos's buffer and silently drop samples. + rclcpp::executors::SingleThreadedExecutor imuExecutor; + imuExecutor.add_callback_group(imuCallbackGroup, node->get_node_base_interface()); + std::thread imuThread([&imuExecutor]() { imuExecutor.spin(); }); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); executor.add_node(node); executor.spin(); @@ -959,6 +979,8 @@ int main(int argc, char **argv) igb->mustStop = true; state->image_ready_cv.notify_all(); // unblock SyncWithImu if it's waiting syncThread.join(); + imuExecutor.cancel(); + imuThread.join(); pSLAM->Shutdown(); rclcpp::shutdown(); From 3226da7bdca95fe53a926c12aaa324513be41ef2 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Fri, 10 Jul 2026 15:17:54 +0200 Subject: [PATCH 49/59] fix docker dependencies --- docker/Jazzy_new.Dockerfile | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/docker/Jazzy_new.Dockerfile b/docker/Jazzy_new.Dockerfile index afef1fd..d7acf02 100644 --- a/docker/Jazzy_new.Dockerfile +++ b/docker/Jazzy_new.Dockerfile @@ -234,9 +234,26 @@ RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force torch torchvision torchaudio \ --index-url https://download.pytorch.org/whl/cu126 +# Purge numpy/scipy from BOTH the user site and the system dist-packages before +# reinstalling. Earlier layers install numpy/scipy system-wide with +# `--ignore-installed`, which does not clean up files from a differently-laid-out +# previous version. If those stale files are left in dist-packages, they can +# shadow the pinned --user install below (e.g. a stray flat +# scipy/sparse/linalg/_propack*.so shadowing the real _propack/ package), +# breaking `import scipy` at runtime even though `pip list` looks correct. RUN rm -rf ~/.local/lib/python3.12/site-packages/numpy \ ~/.local/lib/python3.12/site-packages/numpy-*.dist-info \ - ~/.local/lib/python3.12/site-packages/numpy.libs + ~/.local/lib/python3.12/site-packages/numpy.libs \ + ~/.local/lib/python3.12/site-packages/scipy \ + ~/.local/lib/python3.12/site-packages/scipy-*.dist-info \ + ~/.local/lib/python3.12/site-packages/scipy.libs +RUN sudo rm -rf \ + /usr/local/lib/python3.12/dist-packages/numpy \ + /usr/local/lib/python3.12/dist-packages/numpy-* \ + /usr/local/lib/python3.12/dist-packages/numpy.libs \ + /usr/local/lib/python3.12/dist-packages/scipy \ + /usr/local/lib/python3.12/dist-packages/scipy-* \ + /usr/local/lib/python3.12/dist-packages/scipy.libs RUN python3 -m pip install --user --break-system-packages --no-cache-dir "numpy==1.26.4" RUN python3 -m pip install --user --break-system-packages --no-cache-dir --force-reinstall --no-deps \ From 737680833357a7697725912e0fd1043a76e66666 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 13 Jul 2026 14:39:25 +0200 Subject: [PATCH 50/59] add config for uHumans2 dataset --- config/Monocular-Inertial/uHumans2_TESSE.yaml | 91 +++++++++++++++++++ 1 file changed, 91 insertions(+) create mode 100644 config/Monocular-Inertial/uHumans2_TESSE.yaml diff --git a/config/Monocular-Inertial/uHumans2_TESSE.yaml b/config/Monocular-Inertial/uHumans2_TESSE.yaml new file mode 100644 index 0000000..eb4bf44 --- /dev/null +++ b/config/Monocular-Inertial/uHumans2_TESSE.yaml @@ -0,0 +1,91 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. TESSE simulator (uHumans2 dataset) - left_cam +# Extracted directly from the recorded bag: +# /tesse/left_cam/camera_info -> intrinsics (720x480, fx=fy=415.692, cx=360, cy=240, zero distortion) +# /tf_static (base_link_gt -> left_cam) -> IMU.T_b_c1 (t=[0,0.05,0], q=[0.5,-0.5,0.5,-0.5]) +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Rectified Camera calibration (OpenCV) +Camera1.fx: 415.69219381653056 +Camera1.fy: 415.69219381653056 +Camera1.cx: 360.0 +Camera1.cy: 240.0 + +# Distortion parameters (TESSE renders undistorted images) +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +# Camera resolution +Camera.width: 720 +Camera.height: 480 + +# Camera frames per second (measured from consecutive /tesse/left_cam header stamps, median dt=0.1s) +Camera.fps: 10 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Transformation from body-frame (imu, "base_link_gt") to left camera ("left_cam") +# Derived from the bag's own /tf_static: t=[0, 0.05, 0], q=[x=0.5, y=-0.5, z=0.5, w=-0.5] +IMU.T_b_c1: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.0, 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0, 0.05, + 0.0, -1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0] + +# Do not insert KFs when recently lost +IMU.InsertKFsWhenLost: 0 + +# IMU noise - TESSE's actual simulated noise model is not recorded in the bag; +# these are generic starting values (same family as VINS-mono defaults used in the +# other configs here). Tune IMU.NoiseGyro/NoiseAcc/GyroWalk/AccWalk against the +# /tesse/imu/clean/imu vs /tesse/imu/noisy/imu topics if initialization remains unstable. +IMU.Threshold: 0.5 +IMU.NoiseGyro: 1e-3 +IMU.NoiseAcc: 1e-2 +IMU.GyroWalk: 1e-6 +IMU.AccWalk: 1e-4 +IMU.Frequency: 200.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 +Viewer.ViewpointF: 500.0 From 2e3b3bd52e8e2c3ff1586b80532c5c1f4c30f519 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 14 Jul 2026 19:09:20 +0200 Subject: [PATCH 51/59] add MapResetEvent and MapReadyEvent messages; update Atlas and common publishers --- CMakeLists.txt | 2 + core/src/Atlas.cc | 27 ++++ include/common.h | 2 + msg/MapReadyEvent.msg | 3 + msg/MapResetEvent.msg | 4 + src/common.cc | 281 +++++++++++++++++++++++++++++------------- 6 files changed, 230 insertions(+), 89 deletions(-) create mode 100644 msg/MapReadyEvent.msg create mode 100644 msg/MapResetEvent.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 339b994..1c50ebf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VSGraphsRoomData.msg" "msg/VSGraphsAllWallsData.msg" "msg/VSGraphsAllDetectdetRooms.msg" + "msg/MapResetEvent.msg" + "msg/MapReadyEvent.msg" DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) diff --git a/core/src/Atlas.cc b/core/src/Atlas.cc index 19b0245..59f2349 100644 --- a/core/src/Atlas.cc +++ b/core/src/Atlas.cc @@ -117,45 +117,72 @@ namespace ORB_SLAM3 pMapMP->AddMapPoint(pMP); } + namespace + { + // Non-inertial sensors have no separate scale-initialization phase: if a keyframe exists + // to hang an entity off at all, tracking was already OK and scale was always metric. + // Inertial sensors need scale/gravity to have actually converged (GetIniertialBA2 -- + // isImuInitialized() alone is only the earliest, rough bias-only stage, see Tracking.cc's + // own use of this same pairing) before any entity position can be trusted. + bool IsMapReadyForEntities(Map *pMap) + { + return !pMap->IsInertial() || (pMap->isImuInitialized() && pMap->GetIniertialBA2()); + } + } + void Atlas::AddMapMarker(Marker *marker) { Map *pMapMP = marker->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapMarker(marker); } void Atlas::AddMapPlane(ORB_SLAM3::Plane *plane) { ORB_SLAM3::Map *pMapMP = plane->GetMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapPlane(plane); } void Atlas::AddRoomWallPlane(ORB_SLAM3::Plane *pPlane) { ORB_SLAM3::Map *pMapMP = pPlane->GetMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddRoomWallPlane(pPlane); } void Atlas::AddMapDoor(Door *door) { Map *pMapMP = door->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapDoor(door); } void Atlas::AddDetectedMapRoom(Room *room) { Map *pMapMP = room->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddDetectedMapRoom(room); } void Atlas::AddCandidateMapRoom(Room *room) { Map *pMapMP = room->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddCandidateMapRoom(room); } void Atlas::AddMapFloor(Floor *floor) { Map *pMapMP = floor->getMap(); + if (!IsMapReadyForEntities(pMapMP)) + return; pMapMP->AddMapFloor(floor); } diff --git a/include/common.h b/include/common.h index 11ddfe1..dd37240 100644 --- a/include/common.h +++ b/include/common.h @@ -118,6 +118,8 @@ // vS-Graphs Custom Messages #include #include +#include +#include using json = nlohmann::json; diff --git a/msg/MapReadyEvent.msg b/msg/MapReadyEvent.msg new file mode 100644 index 0000000..b63dac7 --- /dev/null +++ b/msg/MapReadyEvent.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint64 map_id +bool is_ready diff --git a/msg/MapResetEvent.msg b/msg/MapResetEvent.msg new file mode 100644 index 0000000..96ff8e7 --- /dev/null +++ b/msg/MapResetEvent.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint64 previous_map_id +uint64 current_map_id +uint64 reset_count diff --git a/src/common.cc b/src/common.cc index cf5e592..ff327f7 100644 --- a/src/common.cc +++ b/src/common.cc @@ -22,6 +22,8 @@ #include "common.h" #include +#include +#include #include // Variables for ORB-SLAM3 @@ -66,6 +68,41 @@ rclcpp::Publisher::SharedPtr pubKeyFrameMa rclcpp::Publisher::SharedPtr pubFiducialMarker; rclcpp::Publisher::SharedPtr pubStructuralElements; rclcpp::Publisher::SharedPtr pubAllWalls_legacy; +rclcpp::Publisher::SharedPtr pubMapReset; +rclcpp::Publisher::SharedPtr pubMapReady; + +// Marker ids published in the previous cycle, keyed by RViz marker namespace. An entity stops +// appearing in the SLAM system's GetAllX() lists either because it was individually pruned, or +// because its owning map is no longer the active one (e.g. after a tracking-loss reset) -- in +// both cases the corresponding marker(s) must be explicitly DELETEd, since marker.lifetime is 0 +// (forever) and RViz otherwise keeps drawing them indefinitely. +std::unordered_map> lastPublishedMarkerIds; + +// Appends DELETE markers for any id previously published under `ns` that is absent from +// `currentIds` this cycle, then records `currentIds` as the new baseline for `ns`. +void appendDeletedMarkers( + visualization_msgs::msg::MarkerArray &markerArray, + const std::string &ns, + const std::set ¤tIds, + const std::string &frameId, + const rclcpp::Time &stamp) +{ + std::set &previousIds = lastPublishedMarkerIds[ns]; + for (int id : previousIds) + { + if (currentIds.count(id)) + continue; + + visualization_msgs::msg::Marker deleteMarker; + deleteMarker.ns = ns; + deleteMarker.id = id; + deleteMarker.header.frame_id = frameId; + deleteMarker.header.stamp = stamp; + deleteMarker.action = visualization_msgs::msg::Marker::DELETE; + markerArray.markers.push_back(deleteMarker); + } + previousIds = currentIds; +} void saveMapService( std::shared_ptr req, @@ -245,6 +282,16 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher(node_name + "/structural_elements", 1); + // Map reset notifications, for downstream nodes that keep their own per-map state + pubMapReset = node->create_publisher(node_name + "/map_reset", 1); + + // Map readiness (scale/IMU initialized), so downstream nodes know when it's safe to start + // creating entities/tracks. Unlike the other publishers here, this uses transient_local + // ("latched") durability: readiness is a level, not a pure edge, so a node that (re)connects + // later must learn the current state immediately instead of waiting for the next transition. + pubMapReady = node->create_publisher( + node_name + "/map_ready", rclcpp::QoS(1).transient_local()); + // Get body odometry if IMU data is also available if (sensorType == ORB_SLAM3::System::IMU_MONOCULAR || sensorType == ORB_SLAM3::System::IMU_STEREO || sensorType == ORB_SLAM3::System::IMU_RGBD) @@ -257,8 +304,78 @@ void setupPublishers(std::shared_ptr node, std::shared_ptr(*tfBuffer_); } +// Announces when the current active map changes (tracking-loss reset, new map created), so that +// downstream nodes keeping their own per-map/per-track state (e.g. object_motion_estimator) can +// clear it instead of silently mixing data across two unrelated coordinate frames. Detecting this +// needs no core changes: Map::GetId() is already a stable, globally unique, monotonically +// increasing id assigned once per Map object. +void publishMapResetIfChanged(rclcpp::Time msgTime) +{ + static uint64_t lastMapId = std::numeric_limits::max(); // sentinel: nothing seen yet + static uint64_t resetCount = 0; + + uint64_t currentMapId = pSLAM->GetCurrentMap()->GetId(); + if (currentMapId == lastMapId) + return; + + // Only the transition to a *different already-seen-once* map is a reset; the very first map + // observed at startup is not. + if (lastMapId != std::numeric_limits::max()) + { + vs_graphs::msg::MapResetEvent event; + event.header.stamp = msgTime; + event.header.frame_id = frameWorld; + event.previous_map_id = lastMapId; + event.current_map_id = currentMapId; + event.reset_count = ++resetCount; + pubMapReset->publish(event); + } + lastMapId = currentMapId; +} + +// Announces whether the current map is safe to create entities/tracks against -- non-inertial +// sensors are always ready once a map exists (no separate scale phase); inertial sensors aren't +// ready until GetIniertialBA2() (scale/gravity actually converged, not just the earliest +// bias-only isImuInitialized() stage -- mirrors the core-side guard in Atlas::AddMapDoor etc. and +// the existing precedent for this exact pairing in Tracking.cc). Publishes once per map (whatever +// the state is at first sight) and again only on each actual true/false flip thereafter. +void publishMapReadyIfChanged(rclcpp::Time msgTime) +{ + static uint64_t lastMapId = std::numeric_limits::max(); + static bool lastPublishedReady = false; + static bool havePublishedForThisMap = false; + + ORB_SLAM3::Map *pMap = pSLAM->GetCurrentMap(); + uint64_t currentMapId = pMap->GetId(); + bool currentlyReady = !pMap->IsInertial() || (pMap->isImuInitialized() && pMap->GetIniertialBA2()); + + if (currentMapId != lastMapId) + { + lastMapId = currentMapId; + havePublishedForThisMap = false; + } + if (havePublishedForThisMap && currentlyReady == lastPublishedReady) + return; + + vs_graphs::msg::MapReadyEvent event; + event.header.stamp = msgTime; + event.header.frame_id = frameWorld; + event.map_id = currentMapId; + event.is_ready = currentlyReady; + pubMapReady->publish(event); + lastPublishedReady = currentlyReady; + havePublishedForThisMap = true; +} + void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPCL) { + // Detect and announce a map reset/readiness change as early as possible in the cycle, even if + // the current pose is temporarily invalid (NaN, handled below) -- downstream consumers need to + // know their world-frame assumptions broke (or that it's now safe to resume) right away, not + // only once tracking recovers. + publishMapResetIfChanged(msgTime); + publishMapReadyIfChanged(msgTime); + Sophus::SE3f Twc = pSLAM->GetCamTwc(); // Avoid publishing NaN @@ -781,13 +898,16 @@ void publishAllPoints(std::vector allMapPoints, rclcpp::T void publishKeyFrameMarkers(std::vector keyframe_vec, rclcpp::Time msgTime) { sort(keyframe_vec.begin(), keyframe_vec.end(), ORB_SLAM3::KeyFrame::lId); - if (keyframe_vec.size() == 0) - return; + // kf_markers is a single SPHERE_LIST marker (fixed id) whose points are fully replaced + // below, so an empty keyframe_vec (e.g. right after a map reset) must still be published + // -- with an empty points list -- rather than skipped, otherwise the previous map's whole + // trajectory marker is left stuck on screen forever (marker lifetime is 0). visualization_msgs::msg::MarkerArray markerArray; visualization_msgs::msg::Marker kf_markers; kf_markers.header.frame_id = frameWorld; + kf_markers.header.stamp = msgTime; kf_markers.ns = "kf_markers"; kf_markers.type = visualization_msgs::msg::Marker::SPHERE_LIST; kf_markers.action = visualization_msgs::msg::Marker::ADD; @@ -851,17 +971,17 @@ void publishKeyFrameMarkers(std::vector keyframe_vec, rcl void publishFiducialMarkers(std::vector markers, rclcpp::Time msgTime) { - int numMarkers = markers.size(); - if (numMarkers == 0) - return; - visualization_msgs::msg::MarkerArray markerArray; - markerArray.markers.resize(numMarkers); + std::set currentIds; + rclcpp::Time now = rclcpp::Clock().now(); // rclcpp::Time().now(); - for (int idx = 0; idx < numMarkers; idx++) + for (auto *marker : markers) { + int id = marker->getId(); + currentIds.insert(id); + visualization_msgs::msg::Marker fiducial_marker; - Sophus::SE3f markerPose = markers[idx]->getGlobalPose(); + Sophus::SE3f markerPose = marker->getGlobalPose(); fiducial_marker.color.a = 0; fiducial_marker.scale.x = 0.2; @@ -870,8 +990,8 @@ void publishFiducialMarkers(std::vector markers, rclcpp::Ti fiducial_marker.ns = "fiducial_markers"; fiducial_marker.lifetime = rclcpp::Duration::from_seconds(0); fiducial_marker.action = fiducial_marker.ADD; - fiducial_marker.id = markerArray.markers.size(); - fiducial_marker.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + fiducial_marker.id = id; + fiducial_marker.header.stamp = now; fiducial_marker.mesh_use_embedded_materials = true; fiducial_marker.header.frame_id = frameBC; fiducial_marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; @@ -889,23 +1009,23 @@ void publishFiducialMarkers(std::vector markers, rclcpp::Ti markerArray.markers.push_back(fiducial_marker); } + appendDeletedMarkers(markerArray, "fiducial_markers", currentIds, frameBC, now); pubFiducialMarker->publish(markerArray); } void publishDoors(std::vector doors) { - // If there are no doors, return - int numDoors = doors.size(); - if (numDoors == 0) - return; - // Variables visualization_msgs::msg::MarkerArray doorArray; - doorArray.markers.resize(numDoors); + std::set currentIds; + rclcpp::Time now = rclcpp::Clock().now(); - for (int idx = 0; idx < numDoors; idx++) + for (auto *doorPtr : doors) { - Sophus::SE3f doorPose = doors[idx]->getGlobalPose(); + int id = doorPtr->getId(); + currentIds.insert(id); + + Sophus::SE3f doorPose = doorPtr->getGlobalPose(); visualization_msgs::msg::Marker door, doorLines, doorLabel; // Door values @@ -916,8 +1036,8 @@ void publishDoors(std::vector doors) door.scale.z = 0.5; door.action = door.ADD; door.lifetime = rclcpp::Duration::from_seconds(0); - door.id = doorArray.markers.size(); - door.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + door.id = id; + door.header.stamp = now; // rclcpp::Time().now(); door.mesh_use_embedded_materials = true; door.header.frame_id = frameBC; door.type = visualization_msgs::msg::Marker::MESH_RESOURCE; @@ -945,9 +1065,9 @@ void publishDoors(std::vector doors) doorLabel.ns = "doorLabel"; doorLabel.action = doorLabel.ADD; doorLabel.lifetime = rclcpp::Duration::from_seconds(0); - doorLabel.text = doors[idx]->getName(); - doorLabel.id = doorArray.markers.size(); - doorLabel.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + doorLabel.text = doorPtr->getName(); + doorLabel.id = id; + doorLabel.header.stamp = now; // rclcpp::Time().now(); doorLabel.header.frame_id = frameBC; doorLabel.pose.position.x = door.pose.position.x; doorLabel.pose.position.z = door.pose.position.z; @@ -966,15 +1086,15 @@ void publishDoors(std::vector doors) doorLines.ns = "doorLines"; doorLines.action = doorLines.ADD; doorLines.lifetime = rclcpp::Duration::from_seconds(0); - doorLines.id = doorArray.markers.size(); - doorLines.header.stamp = rclcpp::Clock().now(); // rclcpp::Time().now(); + doorLines.id = id; + doorLines.header.stamp = now; // rclcpp::Time().now(); doorLines.header.frame_id = frameBC; doorLines.type = visualization_msgs::msg::Marker::LINE_LIST; geometry_msgs::msg::Point point1; - point1.x = doors[idx]->getMarker()->getGlobalPose().translation().x(); - point1.y = doors[idx]->getMarker()->getGlobalPose().translation().y(); - point1.z = doors[idx]->getMarker()->getGlobalPose().translation().z(); + point1.x = doorPtr->getMarker()->getGlobalPose().translation().x(); + point1.y = doorPtr->getMarker()->getGlobalPose().translation().y(); + point1.z = doorPtr->getMarker()->getGlobalPose().translation().z(); doorLines.points.push_back(point1); geometry_msgs::msg::Point point2; @@ -986,16 +1106,14 @@ void publishDoors(std::vector doors) doorArray.markers.push_back(doorLines); } + appendDeletedMarkers(doorArray, "doors", currentIds, frameBC, now); + appendDeletedMarkers(doorArray, "doorLabel", currentIds, frameBC, now); + appendDeletedMarkers(doorArray, "doorLines", currentIds, frameBC, now); pubDoor->publish(doorArray); } void publishPlanes(std::vector planes, rclcpp::Time msgTime) { - // Publish the planes, if any - int numPlanes = planes.size(); - if (numPlanes == 0) - return; - // Check if sufficient time has passed since the last plane publication if (msgTime.seconds() - lastPlanePublishTime < 3.0) return; @@ -1004,7 +1122,7 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) // Variables visualization_msgs::msg::MarkerArray planeLabelArray; - planeLabelArray.markers.resize(numPlanes); + std::set currentPlaneIds; visualization_msgs::msg::Marker planeLabel, planeNormal; geometry_msgs::msg::Point normalStartPoint, normalEndPoint; @@ -1020,7 +1138,8 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) Eigen::Vector3d normal = plane->getGlobalEquation().normal(); const std::string planeLabelText = "Plane#" + std::to_string(plane->getId()); - // If the plane is undefined, skip it + // If the plane is undefined, skip it (it is left out of currentPlaneIds, so any + // previously-published label/normal for it is auto-deleted by appendDeletedMarkers) if (plane->getPlaneType() == ORB_SLAM3::Plane::planeVariant::UNDEFINED) continue; @@ -1029,6 +1148,8 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) if (planeClouds == nullptr || planeClouds->empty()) continue; + currentPlaneIds.insert(plane->getId()); + for (const auto &point : planeClouds->points) { pcl::PointXYZRGB newPoint; @@ -1080,7 +1201,7 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) planeNormal.color.r = color[0] / 255.0; planeNormal.color.g = color[1] / 255.0; planeNormal.color.b = color[2] / 255.0; - planeNormal.id = plane->getId() + numPlanes; + planeNormal.id = plane->getId(); planeNormal.lifetime = rclcpp::Duration::from_seconds(0); planeNormal.type = visualization_msgs::msg::Marker::ARROW; @@ -1103,6 +1224,10 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) planeLabelArray.markers.push_back(planeNormal); } + appendDeletedMarkers(planeLabelArray, "planeLabel", currentPlaneIds, frameBC, msgTime); + appendDeletedMarkers(planeLabelArray, "planeNormal", currentPlaneIds, frameBC, msgTime); + pubPlaneLabel->publish(planeLabelArray); + if (aggregatedCloud->empty()) return; @@ -1116,63 +1241,33 @@ void publishPlanes(std::vector planes, rclcpp::Time msgTime) // Publish the point cloud pubBuildingComponents->publish(cloudMsg); - pubPlaneLabel->publish(planeLabelArray); } void publishStructuralElements(std::vector rooms, std::vector floors, rclcpp::Time msgTime) { - // Publish rooms, if any int numRooms = rooms.size(); int numFloors = floors.size(); - // If there are no rooms or floors, return - if (numRooms == 0 && numFloors == 0) - return; - // Variables double textOffset = -0.5; double floorToRoomOffset = -2.0; // Visualization markers visualization_msgs::msg::MarkerArray roomArray, floorArray; - roomArray.markers.resize(numRooms); - floorArray.markers.resize(numFloors); + std::set currentRoomIds, currentFloorIds; // Publish rooms, if any - if (numRooms > 0) + for (int idx = 0; idx < numRooms; idx++) { - for (int idx = 0; idx < numRooms; idx++) - { - // Skip if the room is bad - if (rooms[idx]->isBad()) { - // Variables - visualization_msgs::msg::Marker delRoom, delRoomLabel, delRoomWallLine; - // Delete previous marker for this room - delRoom.id = idx; - delRoom.ns = "room"; - delRoom.header.stamp = msgTime; - delRoom.header.frame_id = frameSE; - delRoom.action = visualization_msgs::msg::Marker::DELETE; - // Delete previous marker for this room label - delRoomLabel.id = idx; - delRoomLabel.ns = "roomLabel"; - delRoomLabel.header.stamp = msgTime; - delRoomLabel.header.frame_id = frameSE; - delRoomLabel.action = visualization_msgs::msg::Marker::DELETE; - // Delete previous room-wall lines - delRoomWallLine.id = idx; - delRoomWallLine.ns = "roomWallLine"; - delRoomWallLine.header.stamp = msgTime; - delRoomWallLine.header.frame_id = frameWorld; - delRoomWallLine.action = visualization_msgs::msg::Marker::DELETE; - // Push the delete markers and skip them - roomArray.markers.push_back(delRoom); - roomArray.markers.push_back(delRoomLabel); - roomArray.markers.push_back(delRoomWallLine); + // A bad room is simply left out of currentRoomIds below, so it is + // auto-deleted by appendDeletedMarkers once it drops out of the room list + if (rooms[idx]->isBad()) continue; - } - + + int id = rooms[idx]->getId(); + currentRoomIds.insert(id); + // Variables std::string roomName = rooms[idx]->getName(); geometry_msgs::msg::PointStamped roomPoint, roomPointTr; @@ -1188,7 +1283,7 @@ void publishStructuralElements(std::vector rooms, visualization_msgs::msg::Marker room, roomWallLine, roomDoorLine, roomMarkerLine, roomLabel; // Room values - room.id = idx; + room.id = id; room.ns = "room"; room.scale.x = 0.3; room.scale.y = 0.3; @@ -1213,7 +1308,7 @@ void publishStructuralElements(std::vector rooms, roomArray.markers.push_back(room); // Room label (name) - roomLabel.id = idx; + roomLabel.id = id; roomLabel.color.a = 1; roomLabel.color.r = 0; roomLabel.color.g = 0; @@ -1232,7 +1327,7 @@ void publishStructuralElements(std::vector rooms, roomArray.markers.push_back(roomLabel); // Room to Wall connection line - roomWallLine.id = idx; + roomWallLine.id = id; roomWallLine.color.a = 0.9; roomWallLine.color.r = 0.0; roomWallLine.color.g = 0.0; @@ -1312,11 +1407,12 @@ void publishStructuralElements(std::vector rooms, roomArray.markers.push_back(roomWallLine); } - pubStructuralElements->publish(roomArray); - } + appendDeletedMarkers(roomArray, "room", currentRoomIds, frameSE, msgTime); + appendDeletedMarkers(roomArray, "roomLabel", currentRoomIds, frameSE, msgTime); + appendDeletedMarkers(roomArray, "roomWallLine", currentRoomIds, frameWorld, msgTime); + pubStructuralElements->publish(roomArray); // Publish floors, if any - if (numFloors > 0) { // Variables std::vector color = {0.3, 0.6, 0.7}; @@ -1324,10 +1420,14 @@ void publishStructuralElements(std::vector rooms, // Loop through all the floors for (int floorId = 0; floorId < numFloors; floorId++) { - // If the floor has no rooms, skip it + // If the floor has no rooms, skip it, so it is auto-deleted by appendDeletedMarkers + // below once it drops out of currentFloorIds if (floors[floorId]->getRooms().size() == 0) continue; - + + int id = floors[floorId]->getId(); + currentFloorIds.insert(id); + // Variables std::string floorName = floors[floorId]->getName(); geometry_msgs::msg::PointStamped floorPoint, floorPointTr; @@ -1335,7 +1435,7 @@ void publishStructuralElements(std::vector rooms, visualization_msgs::msg::Marker floorMarker, floorLabel, floorRoomLine; // Floor marker (cube) - floorMarker.id = floorId; + floorMarker.id = id; floorMarker.ns = "floors"; floorMarker.scale.x = 0.4; floorMarker.scale.y = 0.4; @@ -1363,7 +1463,7 @@ void publishStructuralElements(std::vector rooms, floorLabel.color.g = 0; floorLabel.color.b = 0; floorLabel.scale.z = 0.2; - floorLabel.id = floorId; + floorLabel.id = id; floorLabel.text = floorName; floorLabel.ns = "floorLabels"; floorLabel.header.stamp = msgTime; @@ -1388,7 +1488,7 @@ void publishStructuralElements(std::vector rooms, } // Floor to Room connection line - floorRoomLine.id = floorId; + floorRoomLine.id = id; floorRoomLine.color.a = 0.9; floorRoomLine.color.r = 0.0; floorRoomLine.color.g = 0.0; @@ -1465,9 +1565,12 @@ void publishStructuralElements(std::vector rooms, floorArray.markers.push_back(floorMarker); floorArray.markers.push_back(floorRoomLine); } - - pubStructuralElements->publish(floorArray); } + + appendDeletedMarkers(floorArray, "floors", currentFloorIds, frameSE, msgTime); + appendDeletedMarkers(floorArray, "floorLabels", currentFloorIds, frameSE, msgTime); + appendDeletedMarkers(floorArray, "floorRoomEdges", currentFloorIds, frameWorld, msgTime); + pubStructuralElements->publish(floorArray); } sensor_msgs::msg::PointCloud2 mapPointToPointcloud(std::vector mapPoints, rclcpp::Time msgTime) From f0adbf8ee77ebbbb91b9a4ae4c43c8b9670dafbb Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 14 Jul 2026 19:09:36 +0200 Subject: [PATCH 52/59] add evaluation node for uHumans2 datasets --- evaluation/evaluate_uhumans2.py | 546 ++++++++++++++++++++++++++++++++ 1 file changed, 546 insertions(+) create mode 100644 evaluation/evaluate_uhumans2.py diff --git a/evaluation/evaluate_uhumans2.py b/evaluation/evaluate_uhumans2.py new file mode 100644 index 0000000..40e03f1 --- /dev/null +++ b/evaluation/evaluate_uhumans2.py @@ -0,0 +1,546 @@ +#!/usr/bin/env python3 + +""" +Evaluation node for uHumans2-style datasets. + +Compares the live SLAM output of a mono-inertial vS-Graphs run against the +ground truth published on /tf by the dataset itself: + + - Camera platform: /tf world -> base_link_gt (ground truth) + /vs_graphs/body_odom (estimate, world -> imu) + - Humans: /tf world -> object_ (ground truth, 0..N humans) + /dynamic_object_points_3d (estimate, per-keyframe 3D points) + +Camera and human estimates share the same SLAM-internal "world" frame, so a +single rigid (rotation + translation, no scale) alignment is computed from the +camera trajectory and applied to both before computing errors. Scale is not +applied (mono-inertial SLAM should already be metric); the recovered scale is +reported only as a diagnostic to sanity-check IMU scale correctness. + +Human-to-ground-truth association is solved independently at every keyframe +via Hungarian assignment (minimum total distance) with a distance gate, since +estimated track ids are not guaranteed to match ground-truth human ids and the +number of humans visible at any instant may be 0, 1, or many. + +Usage (after sourcing the workspace and while a run is playing back): + python3 evaluate_uhumans2.py --ros-args \ + -p output_dir:=/path/to/results \ + -p body_odom_topic:=/vs_graphs/body_odom \ + -p human_points_topic:=/dynamic_object_points_3d + +The node buffers samples as they arrive and finalizes automatically once no +new messages have been seen for `idle_timeout_s` seconds (i.e. the bag/run has +ended), writing three CSV files to `output_dir`: + - camera_pose_errors.csv (per-sample camera position/orientation error) + - human_pose_errors.csv (per-keyframe, per-track human position error) + - human_track_summary.csv (per estimated track_id: dominant matched GT id, + match purity, mean/RMSE position error) +Ctrl+C also triggers a graceful finalize before exit. +""" + +import csv +import math +import os +import re +import threading +from collections import defaultdict + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.time import Time +from rclpy.duration import Duration + +from tf2_ros import Buffer, TransformListener +from tf2_ros import TransformException + +from nav_msgs.msg import Odometry +from dynamic_keypoint_3d_lifter.msg import DynamicObjectPoints3D + +OBJECT_FRAME_RE_TEMPLATE = r"^{prefix}(\d+)$" + + +def stamp_to_sec(stamp) -> float: + return float(stamp.sec) + float(stamp.nanosec) * 1e-9 + + +def rigid_align(src: np.ndarray, dst: np.ndarray): + """Closed-form rigid (rotation + translation, no scale) alignment mapping + `src` points onto `dst` points (Umeyama 1991, scale term computed only as + a diagnostic and not applied). src, dst: (N,3) arrays of corresponding + points, N >= 3. + + Returns (R (3,3), t (3,), scale_diagnostic (float)). + """ + n = src.shape[0] + src_mean = src.mean(axis=0) + dst_mean = dst.mean(axis=0) + src_c = src - src_mean + dst_c = dst - dst_mean + + h = src_c.T @ dst_c + u, s, vt = np.linalg.svd(h) + d = np.sign(np.linalg.det(vt.T @ u.T)) + if d == 0: + d = 1.0 + correction = np.diag([1.0, 1.0, d]) + r = vt.T @ correction @ u.T + t = dst_mean - r @ src_mean + + var_src = float((src_c ** 2).sum()) / n + scale_diag = float((s * np.array([1.0, 1.0, d])).sum() / (n * var_src)) if var_src > 0 else float("nan") + return r, t, scale_diag + + +# The environment this was developed against has a broken scipy install +# (scipy.sparse.linalg._propack is missing symbols, which scipy.optimize and +# scipy.spatial both transitively import), so quaternion composition and the +# min-cost assignment below are implemented directly in numpy instead of +# depending on scipy. Both are covered by self-tests during development +# (Hungarian algorithm checked against brute force, quaternion <-> matrix +# round-trip and composition checked against a QR-based random rotation +# generator) - see conversation history for the verification runs. + +def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Hamilton product q1 * q2, both in ROS [x, y, z, w] convention.""" + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + return np.array([ + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, + w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + ]) + + +def quat_from_matrix(m: np.ndarray) -> np.ndarray: + """Rotation matrix (3,3) -> unit quaternion [x, y, z, w] (Shepperd's method).""" + trace = m[0, 0] + m[1, 1] + m[2, 2] + if trace > 0: + s = 0.5 / math.sqrt(trace + 1.0) + w = 0.25 / s + x = (m[2, 1] - m[1, 2]) * s + y = (m[0, 2] - m[2, 0]) * s + z = (m[1, 0] - m[0, 1]) * s + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = 2.0 * math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) + w = (m[2, 1] - m[1, 2]) / s + x = 0.25 * s + y = (m[0, 1] + m[1, 0]) / s + z = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = 2.0 * math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) + w = (m[0, 2] - m[2, 0]) / s + x = (m[0, 1] + m[1, 0]) / s + y = 0.25 * s + z = (m[1, 2] + m[2, 1]) / s + else: + s = 2.0 * math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) + w = (m[1, 0] - m[0, 1]) / s + x = (m[0, 2] + m[2, 0]) / s + y = (m[1, 2] + m[2, 1]) / s + z = 0.25 * s + q = np.array([x, y, z, w]) + return q / np.linalg.norm(q) + + +def quat_angle_deg(q1: np.ndarray, q2: np.ndarray) -> float: + """Geodesic angle in degrees between two unit quaternions (double-cover safe).""" + dot = abs(float(np.dot(q1, q2))) + dot = min(1.0, max(-1.0, dot)) + return math.degrees(2.0 * math.acos(dot)) + + +def hungarian_min_cost_assignment(cost: np.ndarray): + """Minimum-cost bipartite assignment (Kuhn-Munkres with potentials, O(n^2 m)). + cost: (n_rows, n_cols). Returns (row_indices, col_indices) covering every row + if n_rows <= n_cols, or every column otherwise - i.e. min(n_rows, n_cols) pairs, + matching the semantics of scipy.optimize.linear_sum_assignment for a fully + finite cost matrix. Self-contained to avoid this environment's broken scipy + install (see module docstring above). + """ + cost = np.asarray(cost, dtype=float) + n_rows, n_cols = cost.shape + if n_rows == 0 or n_cols == 0: + return np.array([], dtype=int), np.array([], dtype=int) + + transposed = n_rows > n_cols + work = cost.T if transposed else cost + n, m = work.shape # n <= m guaranteed here + + inf = float("inf") + u = [0.0] * (n + 1) + v = [0.0] * (m + 1) + p = [0] * (m + 1) # p[j] = row (1-indexed) currently matched to column j + way = [0] * (m + 1) + a = np.zeros((n + 1, m + 1)) + a[1:, 1:] = work + + for i in range(1, n + 1): + p[0] = i + j0 = 0 + minv = [inf] * (m + 1) + used = [False] * (m + 1) + while True: + used[j0] = True + i0 = p[j0] + delta = inf + j1 = -1 + for j in range(1, m + 1): + if not used[j]: + cur = a[i0][j] - u[i0] - v[j] + if cur < minv[j]: + minv[j] = cur + way[j] = j0 + if minv[j] < delta: + delta = minv[j] + j1 = j + for j in range(m + 1): + if used[j]: + u[p[j]] += delta + v[j] -= delta + else: + minv[j] -= delta + j0 = j1 + if p[j0] == 0: + break + while j0: + j1 = way[j0] + p[j0] = p[j1] + j0 = j1 + + row_ind, col_ind = [], [] + for j in range(1, m + 1): + if p[j] != 0: + row_ind.append(p[j] - 1) + col_ind.append(j - 1) + row_ind = np.array(row_ind, dtype=int) + col_ind = np.array(col_ind, dtype=int) + order = np.argsort(row_ind) + row_ind, col_ind = row_ind[order], col_ind[order] + + if transposed: + row_ind, col_ind = col_ind, row_ind + return row_ind, col_ind + + +class UHumans2Evaluator(Node): + def __init__(self): + super().__init__("uhumans2_evaluator") + + self.declare_parameter("world_frame", "world") + self.declare_parameter("camera_gt_frame", "base_link_gt") + self.declare_parameter("object_frame_prefix", "object_") + self.declare_parameter("body_odom_topic", "/vs_graphs/body_odom") + self.declare_parameter("human_points_topic", "/dynamic_object_points_3d") + self.declare_parameter("output_dir", ".") + self.declare_parameter("idle_timeout_s", 5.0) + self.declare_parameter("tf_lookup_timeout_s", 0.05) + self.declare_parameter("max_match_distance_m", 1.5) + self.declare_parameter("tf_cache_time_s", 60.0) + + self.world_frame = self.get_parameter("world_frame").value + self.camera_gt_frame = self.get_parameter("camera_gt_frame").value + self.object_frame_prefix = self.get_parameter("object_frame_prefix").value + self.body_odom_topic = self.get_parameter("body_odom_topic").value + self.human_points_topic = self.get_parameter("human_points_topic").value + self.output_dir = self.get_parameter("output_dir").value + self.idle_timeout_s = float(self.get_parameter("idle_timeout_s").value) + self.tf_lookup_timeout_s = float(self.get_parameter("tf_lookup_timeout_s").value) + self.max_match_distance_m = float(self.get_parameter("max_match_distance_m").value) + tf_cache_time_s = float(self.get_parameter("tf_cache_time_s").value) + + self._object_frame_re = re.compile( + OBJECT_FRAME_RE_TEMPLATE.format(prefix=re.escape(self.object_frame_prefix))) + + self.tf_buffer = Buffer(cache_time=Duration(seconds=tf_cache_time_s)) + self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=False) + + self._lock = threading.Lock() + self.camera_samples = [] + self.human_samples = [] + self._finalized = False + self.last_msg_time = self.get_clock().now() + + self.create_subscription(Odometry, self.body_odom_topic, self._odom_cb, 50) + self.create_subscription( + DynamicObjectPoints3D, self.human_points_topic, self._human_cb, 50) + self.watchdog_timer = self.create_timer(1.0, self._watchdog_cb) + + self.get_logger().info( + f"uhumans2_evaluator: world_frame='{self.world_frame}' " + f"camera_gt_frame='{self.camera_gt_frame}' " + f"body_odom_topic='{self.body_odom_topic}' " + f"human_points_topic='{self.human_points_topic}' " + f"output_dir='{self.output_dir}' idle_timeout_s={self.idle_timeout_s}") + + # ------------------------------------------------------------------ + # Subscriptions + # ------------------------------------------------------------------ + def _odom_cb(self, msg: Odometry): + self.last_msg_time = self.get_clock().now() + stamp = msg.header.stamp + try: + tf = self.tf_buffer.lookup_transform( + self.world_frame, self.camera_gt_frame, Time.from_msg(stamp), + timeout=Duration(seconds=self.tf_lookup_timeout_s)) + except TransformException as ex: + self.get_logger().warn( + f"No GT transform {self.world_frame}->{self.camera_gt_frame} at " + f"t={stamp_to_sec(stamp):.3f}: {ex}", throttle_duration_sec=2.0) + return + + est_pos = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, + msg.pose.pose.position.z]) + est_quat = np.array([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) + gt_pos = np.array([tf.transform.translation.x, tf.transform.translation.y, + tf.transform.translation.z]) + gt_quat = np.array([tf.transform.rotation.x, tf.transform.rotation.y, + tf.transform.rotation.z, tf.transform.rotation.w]) + + with self._lock: + self.camera_samples.append({ + "t": stamp_to_sec(stamp), + "est_pos": est_pos, + "est_quat": est_quat, + "gt_pos": gt_pos, + "gt_quat": gt_quat, + }) + + def _discover_object_frames(self): + yaml_str = self.tf_buffer.all_frames_as_yaml() + ids = [] + for line in yaml_str.splitlines(): + line = line.strip() + if not line.endswith(":"): + continue + match = self._object_frame_re.match(line[:-1]) + if match: + ids.append(match.group(1)) + return ids + + def _human_cb(self, msg: DynamicObjectPoints3D): + self.last_msg_time = self.get_clock().now() + if not msg.objects: + return + stamp = msg.header.stamp + t = Time.from_msg(stamp) + + gt_snapshot = {} + for object_id in self._discover_object_frames(): + frame = f"{self.object_frame_prefix}{object_id}" + try: + tf = self.tf_buffer.lookup_transform( + self.world_frame, frame, t, + timeout=Duration(seconds=self.tf_lookup_timeout_s)) + except TransformException: + continue + gt_snapshot[object_id] = np.array([ + tf.transform.translation.x, tf.transform.translation.y, + tf.transform.translation.z]) + + new_samples = [] + for obj in msg.objects: + if not obj.points_world: + continue + pts = np.array([[p.x, p.y, p.z] for p in obj.points_world]) + new_samples.append({ + "t": stamp_to_sec(stamp), + "keyframe_id": msg.keyframe_id, + "track_id": obj.track_id, + "class_id": obj.class_id, + "centroid_raw": pts.mean(axis=0), + "n_points": pts.shape[0], + "gt_snapshot": gt_snapshot, + }) + + with self._lock: + self.human_samples.extend(new_samples) + + # ------------------------------------------------------------------ + # Finalization + # ------------------------------------------------------------------ + def _watchdog_cb(self): + if self._finalized: + return + now = self.get_clock().now() + idle_s = (now - self.last_msg_time).nanoseconds * 1e-9 + have_data = len(self.camera_samples) > 0 or len(self.human_samples) > 0 + if have_data and idle_s > self.idle_timeout_s: + self.get_logger().info( + f"No new data for {idle_s:.1f}s (>{self.idle_timeout_s}s) - " + "assuming the run has ended, finalizing evaluation.") + self.finalize() + + def finalize(self): + with self._lock: + if self._finalized: + return + self._finalized = True + camera_samples = list(self.camera_samples) + human_samples = list(self.human_samples) + + os.makedirs(self.output_dir, exist_ok=True) + + r, t, scale_diag = self._write_camera_csv(camera_samples) + self._write_human_csvs(human_samples, r, t) + + self.get_logger().info( + f"Evaluation complete. camera_samples={len(camera_samples)} " + f"human_samples={len(human_samples)} scale_diagnostic={scale_diag:.4f} " + f"(should be close to 1.0 for correctly-scaled mono-inertial output). " + f"CSVs written to '{self.output_dir}'.") + + def _write_camera_csv(self, camera_samples): + path = os.path.join(self.output_dir, "camera_pose_errors.csv") + if len(camera_samples) < 3: + self.get_logger().warn( + f"Only {len(camera_samples)} camera sample(s) with matching GT - " + "need >=3 to compute alignment. Writing raw (unaligned) values only.") + r, t, scale_diag = np.eye(3), np.zeros(3), float("nan") + else: + src = np.stack([s["est_pos"] for s in camera_samples]) + dst = np.stack([s["gt_pos"] for s in camera_samples]) + r, t, scale_diag = rigid_align(src, dst) + + q_r = quat_from_matrix(r) + pos_errors = [] + ang_errors = [] + with open(path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow([ + "timestamp", "gt_x", "gt_y", "gt_z", "gt_qx", "gt_qy", "gt_qz", "gt_qw", + "est_x_raw", "est_y_raw", "est_z_raw", + "est_x_aligned", "est_y_aligned", "est_z_aligned", + "est_qx_aligned", "est_qy_aligned", "est_qz_aligned", "est_qw_aligned", + "position_error_m", "orientation_error_deg", + ]) + for s in camera_samples: + aligned_pos = r @ s["est_pos"] + t + aligned_quat = quat_multiply(q_r, s["est_quat"]) + pos_err = float(np.linalg.norm(aligned_pos - s["gt_pos"])) + ang_err = quat_angle_deg(aligned_quat, s["gt_quat"]) + pos_errors.append(pos_err) + ang_errors.append(ang_err) + writer.writerow([ + f"{s['t']:.9f}", + *[f"{v:.6f}" for v in s["gt_pos"]], + *[f"{v:.6f}" for v in s["gt_quat"]], + *[f"{v:.6f}" for v in s["est_pos"]], + *[f"{v:.6f}" for v in aligned_pos], + *[f"{v:.6f}" for v in aligned_quat], + f"{pos_err:.6f}", f"{ang_err:.6f}", + ]) + + if pos_errors: + pos_errors_np = np.array(pos_errors) + ang_errors_np = np.array(ang_errors) + self.get_logger().info( + f"Camera ATE (rigid-aligned, {len(pos_errors)} samples): " + f"RMSE={np.sqrt((pos_errors_np ** 2).mean()):.4f} m " + f"mean={pos_errors_np.mean():.4f} m median={np.median(pos_errors_np):.4f} m " + f"max={pos_errors_np.max():.4f} m | " + f"orientation RMSE={np.sqrt((ang_errors_np ** 2).mean()):.3f} deg") + self.get_logger().info(f"Wrote {path}") + return r, t, scale_diag + + def _write_human_csvs(self, human_samples, r, t): + errors_path = os.path.join(self.output_dir, "human_pose_errors.csv") + summary_path = os.path.join(self.output_dir, "human_track_summary.csv") + + by_stamp = defaultdict(list) + for h in human_samples: + aligned = r @ h["centroid_raw"] + t + by_stamp[h["t"]].append({**h, "centroid_aligned": aligned}) + + track_match_counts = defaultdict(lambda: defaultdict(int)) + track_errors = defaultdict(list) + + with open(errors_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow([ + "timestamp", "keyframe_id", "track_id", "class_id", "n_points", + "est_x", "est_y", "est_z", "matched_gt_id", + "gt_x", "gt_y", "gt_z", "position_error_m", "n_gt_visible", + ]) + for stamp in sorted(by_stamp.keys()): + entries = by_stamp[stamp] + gt_snapshot = entries[0]["gt_snapshot"] + gt_ids = list(gt_snapshot.keys()) + n_gt = len(gt_ids) + + matched = {} + if n_gt > 0: + cost = np.zeros((len(entries), n_gt)) + for i, e in enumerate(entries): + for j, gid in enumerate(gt_ids): + cost[i, j] = np.linalg.norm( + e["centroid_aligned"] - gt_snapshot[gid]) + row_ind, col_ind = hungarian_min_cost_assignment(cost) + for row, col in zip(row_ind, col_ind): + if cost[row, col] <= self.max_match_distance_m: + matched[row] = (gt_ids[col], cost[row, col]) + + for i, e in enumerate(entries): + if i in matched: + gid, err = matched[i] + gt_pos = gt_snapshot[gid] + track_match_counts[e["track_id"]][gid] += 1 + track_errors[e["track_id"]].append(err) + writer.writerow([ + f"{stamp:.9f}", e["keyframe_id"], e["track_id"], e["class_id"], + e["n_points"], *[f"{v:.6f}" for v in e["centroid_aligned"]], + gid, *[f"{v:.6f}" for v in gt_pos], f"{err:.6f}", n_gt, + ]) + else: + writer.writerow([ + f"{stamp:.9f}", e["keyframe_id"], e["track_id"], e["class_id"], + e["n_points"], *[f"{v:.6f}" for v in e["centroid_aligned"]], + -1, "", "", "", "", n_gt, + ]) + + with open(summary_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow([ + "track_id", "dominant_gt_id", "n_frames_matched_dominant", + "n_frames_matched_any", "match_purity", "mean_position_error_m", + "rmse_position_error_m", + ]) + for track_id in sorted(track_match_counts.keys()): + gid_counts = track_match_counts[track_id] + dominant_gid, dominant_count = max(gid_counts.items(), key=lambda kv: kv[1]) + n_any = sum(gid_counts.values()) + errs = np.array(track_errors[track_id]) + purity = dominant_count / n_any if n_any else float("nan") + writer.writerow([ + track_id, dominant_gid, dominant_count, n_any, f"{purity:.4f}", + f"{errs.mean():.6f}", f"{math.sqrt((errs ** 2).mean()):.6f}", + ]) + + n_matched_rows = sum(len(v) for v in track_errors.values()) + self.get_logger().info( + f"Human evaluation: {len(human_samples)} raw estimate rows, " + f"{len(track_match_counts)} distinct track_ids, " + f"{n_matched_rows} matched to a GT human (max_match_distance_m=" + f"{self.max_match_distance_m}).") + self.get_logger().info(f"Wrote {errors_path}") + self.get_logger().info(f"Wrote {summary_path}") + + +def main(): + rclpy.init() + node = UHumans2Evaluator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.finalize() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() From a3ed0011161735930e83ecabf62cd2741b2e42a4 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 14 Jul 2026 19:21:31 +0200 Subject: [PATCH 53/59] rescale global poses and centroids for markers, doors, planes, rooms, and floors --- core/src/Map.cc | 75 +++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 72 insertions(+), 3 deletions(-) diff --git a/core/src/Map.cc b/core/src/Map.cc index d0fd6c5..5d51dd4 100644 --- a/core/src/Map.cc +++ b/core/src/Map.cc @@ -21,6 +21,8 @@ */ #include "Map.h" +#include "Semantic/Door.h" +#include "Geometric/Plane.h" #include @@ -510,10 +512,77 @@ namespace ORB_SLAM3 for (set::iterator sit = mspMarkers.begin(); sit != mspMarkers.end(); sit++) { - // MapPoint *pMP = *sit; [TODO] - // pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw); - // pMP->UpdateNormalAndDepth(); + Marker *pMarker = *sit; + Sophus::SE3f pose = pMarker->getGlobalPose(); + pose.translation() *= s; + pMarker->setGlobalPose(Tyw * pose); } + + for (set::iterator sit = mspDoors.begin(); sit != mspDoors.end(); sit++) + { + // Only the cached global pose needs correcting -- localPose is keyframe-relative + Door *pDoor = *sit; + Sophus::SE3f pose = pDoor->getGlobalPose(); + pose.translation() *= s; + pDoor->setGlobalPose(Tyw * pose); + } + + // g2o::Plane3D only has a rigid (rotation + translation) transform built in, no scale, so + // the similarity version is hand-rolled here: the normal is unaffected by a uniform scale + // (rotation only), while distance' = s*distance + normal'.dot(t) -- derived from requiring + // that a point x' = s*R*x + t satisfying the old plane equation also satisfies the new one. + Eigen::Matrix3d RywD = Ryw.cast(); + Eigen::Vector3d tywD = tyw.cast(); + auto rescalePlaneEquation = [&](const g2o::Plane3D &plane) -> g2o::Plane3D + { + Eigen::Vector3d normal = RywD * plane.normal(); + double distance = static_cast(s) * plane.distance() + normal.dot(tywD); + Eigen::Vector4d coeffs; + coeffs.head<3>() = normal; + coeffs(3) = -distance; + return g2o::Plane3D(coeffs); + }; + auto rescalePlane = [&](Plane *pPlane) + { + if (!pPlane) + return; + + pPlane->setCentroid(s * Ryw * pPlane->getCentroid() + tyw); + pPlane->setGlobalEquation(rescalePlaneEquation(pPlane->getGlobalEquation())); + pPlane->mPlaneGBA = rescalePlaneEquation(pPlane->mPlaneGBA); + + pcl::PointCloud::Ptr cloud = pPlane->getMapClouds(); + if (cloud && !cloud->empty()) + { + pcl::PointCloud::Ptr newCloud(new pcl::PointCloud()); + newCloud->reserve(cloud->size()); + for (const auto &point : cloud->points) + { + Eigen::Vector3f pNew = s * Ryw * Eigen::Vector3f(point.x, point.y, point.z) + tyw; + pcl::PointXYZRGBA newPoint = point; + newPoint.x = pNew.x(); + newPoint.y = pNew.y(); + newPoint.z = pNew.z(); + newCloud->push_back(newPoint); + } + pPlane->replaceMapClouds(newCloud); + } + }; + + // Two disjoint containers hold Plane* objects -- mspPlanes doesn't include room-wall planes + for (set::iterator sit = mspPlanes.begin(); sit != mspPlanes.end(); sit++) + rescalePlane(*sit); + for (auto &kv : mRoomWallPlaneIndex) + rescalePlane(kv.second); + + for (set::iterator sit = mspDetectedRooms.begin(); sit != mspDetectedRooms.end(); sit++) + (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + for (set::iterator sit = mspMarkerBasedRooms.begin(); sit != mspMarkerBasedRooms.end(); sit++) + (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + + for (set::iterator sit = mspFloors.begin(); sit != mspFloors.end(); sit++) + (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + mnMapChange++; } From a22eb175b475e12a92bb843f644b240d3544eecb Mon Sep 17 00:00:00 2001 From: mgiberna Date: Wed, 15 Jul 2026 11:40:59 +0200 Subject: [PATCH 54/59] add MapRescaleEvent message and related functionality for scale corrections --- CMakeLists.txt | 1 + core/include/Map.h | 12 ++++++++ core/src/Map.cc | 17 +++++++++++ include/common.h | 1 + msg/MapRescaleEvent.msg | 6 ++++ src/common.cc | 64 ++++++++++++++++++++++++++++++++++++++--- 6 files changed, 97 insertions(+), 4 deletions(-) create mode 100644 msg/MapRescaleEvent.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c50ebf..afaed5a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,6 +70,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VSGraphsAllDetectdetRooms.msg" "msg/MapResetEvent.msg" "msg/MapReadyEvent.msg" + "msg/MapRescaleEvent.msg" DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) diff --git a/core/include/Map.h b/core/include/Map.h index 95f84a1..f7d59e0 100644 --- a/core/include/Map.h +++ b/core/include/Map.h @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -167,6 +168,12 @@ namespace ORB_SLAM3 void ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel = false); + // Last similarity correction (T, s) applied by ApplyScaledRotation, and a version counter + // that increments each time -- lets external consumers that keep their own world-frame + // state (e.g. object_motion_estimator) detect a rescale and apply the same correction. + void GetLastScaleCorrection(Sophus::SE3f &T, float &s); + uint64_t GetScaleCorrectionVersion(); + bool IsInertial(); void SetIniertialBA1(); void SetIniertialBA2(); @@ -257,6 +264,11 @@ namespace ORB_SLAM3 bool mbIMU_BA1; bool mbIMU_BA2; + // Last similarity correction applied by ApplyScaledRotation -- see GetLastScaleCorrection + Sophus::SE3f mLastScaleCorrectionT; + float mLastScaleCorrectionS = 1.0f; + uint64_t mnScaleCorrectionVersion = 0; + // Mutex std::mutex mMutexMap; }; diff --git a/core/src/Map.cc b/core/src/Map.cc index 5d51dd4..a4caa5a 100644 --- a/core/src/Map.cc +++ b/core/src/Map.cc @@ -583,9 +583,26 @@ namespace ORB_SLAM3 for (set::iterator sit = mspFloors.begin(); sit != mspFloors.end(); sit++) (*sit)->setCentroid(static_cast(s) * RywD * (*sit)->getCentroid() + tywD); + mLastScaleCorrectionT = Tyw; + mLastScaleCorrectionS = s; + mnScaleCorrectionVersion++; + mnMapChange++; } + void Map::GetLastScaleCorrection(Sophus::SE3f &T, float &s) + { + unique_lock lock(mMutexMap); + T = mLastScaleCorrectionT; + s = mLastScaleCorrectionS; + } + + uint64_t Map::GetScaleCorrectionVersion() + { + unique_lock lock(mMutexMap); + return mnScaleCorrectionVersion; + } + void Map::SetInertialSensor() { unique_lock lock(mMutexMap); diff --git a/include/common.h b/include/common.h index dd37240..c5307c0 100644 --- a/include/common.h +++ b/include/common.h @@ -120,6 +120,7 @@ #include #include #include +#include using json = nlohmann::json; diff --git a/msg/MapRescaleEvent.msg b/msg/MapRescaleEvent.msg new file mode 100644 index 0000000..81224fb --- /dev/null +++ b/msg/MapRescaleEvent.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +uint64 map_id +float64 scale +geometry_msgs/Quaternion rotation +geometry_msgs/Vector3 translation +uint64 rescale_count diff --git a/src/common.cc b/src/common.cc index ff327f7..2768552 100644 --- a/src/common.cc +++ b/src/common.cc @@ -70,6 +70,7 @@ rclcpp::Publisher::SharedPtr pubStructural rclcpp::Publisher::SharedPtr pubAllWalls_legacy; rclcpp::Publisher::SharedPtr pubMapReset; rclcpp::Publisher::SharedPtr pubMapReady; +rclcpp::Publisher::SharedPtr pubMapRescale; // Marker ids published in the previous cycle, keyed by RViz marker namespace. An entity stops // appearing in the SLAM system's GetAllX() lists either because it was individually pruned, or @@ -292,6 +293,11 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher( node_name + "/map_ready", rclcpp::QoS(1).transient_local()); + // Map rescale notifications (scale/gravity refinement, or a map merge), so downstream nodes + // that buffer their own world-frame positions/poses can apply the same correction instead of + // going stale relative to the (now rescaled) map. + pubMapRescale = node->create_publisher(node_name + "/map_rescale", 1); + // Get body odometry if IMU data is also available if (sensorType == ORB_SLAM3::System::IMU_MONOCULAR || sensorType == ORB_SLAM3::System::IMU_STEREO || sensorType == ORB_SLAM3::System::IMU_RGBD) @@ -367,14 +373,64 @@ void publishMapReadyIfChanged(rclcpp::Time msgTime) havePublishedForThisMap = true; } +// Announces the similarity correction (scale, rotation, translation) whenever +// Map::ApplyScaledRotation runs (IMU scale/gravity refinement, or a map merge) -- downstream nodes +// that buffer their own world-frame positions/poses (e.g. object_motion_estimator) apply the same +// correction to stay consistent with the now-rescaled map. Uses Map::GetScaleCorrectionVersion(), +// a dedicated counter -- unlike GetMapChangeIndex()/mnMapChange, which is bumped by many unrelated +// events (BA, loop closure, merge welding) and would give false positives here. +void publishMapRescaleIfChanged(rclcpp::Time msgTime) +{ + static uint64_t lastMapId = std::numeric_limits::max(); + static uint64_t lastVersion = 0; + + ORB_SLAM3::Map *pMap = pSLAM->GetCurrentMap(); + uint64_t currentMapId = pMap->GetId(); + if (currentMapId != lastMapId) + { + // A new map's version counter starts back at 0 -- reset our bookkeeping too, so a new + // map's first rescale isn't skipped just because its version happens to match whatever + // the previous map's counter last was. + lastMapId = currentMapId; + lastVersion = 0; + } + + uint64_t currentVersion = pMap->GetScaleCorrectionVersion(); + if (currentVersion == lastVersion) + return; + lastVersion = currentVersion; + if (currentVersion == 0) + return; // no rescale has happened yet for this map + + Sophus::SE3f T; + float s; + pMap->GetLastScaleCorrection(T, s); + + vs_graphs::msg::MapRescaleEvent event; + event.header.stamp = msgTime; + event.header.frame_id = frameWorld; + event.map_id = pMap->GetId(); + event.scale = static_cast(s); + event.rotation.x = T.unit_quaternion().x(); + event.rotation.y = T.unit_quaternion().y(); + event.rotation.z = T.unit_quaternion().z(); + event.rotation.w = T.unit_quaternion().w(); + event.translation.x = T.translation().x(); + event.translation.y = T.translation().y(); + event.translation.z = T.translation().z(); + event.rescale_count = currentVersion; + pubMapRescale->publish(event); +} + void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msgPCL) { - // Detect and announce a map reset/readiness change as early as possible in the cycle, even if - // the current pose is temporarily invalid (NaN, handled below) -- downstream consumers need to - // know their world-frame assumptions broke (or that it's now safe to resume) right away, not - // only once tracking recovers. + // Detect and announce a map reset/readiness/rescale change as early as possible in the cycle, + // even if the current pose is temporarily invalid (NaN, handled below) -- downstream consumers + // need to know their world-frame assumptions broke (or that it's now safe to resume, or that a + // correction needs applying) right away, not only once tracking recovers. publishMapResetIfChanged(msgTime); publishMapReadyIfChanged(msgTime); + publishMapRescaleIfChanged(msgTime); Sophus::SE3f Twc = pSLAM->GetCamTwc(); From f1cb86657d9ce12a6bb5dca89be926ff263056a7 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Sat, 18 Jul 2026 19:58:43 +0200 Subject: [PATCH 55/59] Add IMU-aided relocalization and related features - Introduced a new setting for IMU-aided relocalization in Settings.h and Settings.cc. - Added methods in Tracking.h and Tracking.cc to handle relocalization using IMU predictions. - Implemented checks to verify relocalization candidates against IMU data to ensure pose accuracy. - Enhanced LocalMapping and System classes to manage keyframe queues and reset conditions more effectively. - Updated Frame class to include mutex for IMU data handling. - Improved error handling in Optimizer to discard non-finite estimates during pose optimization. - Adjusted logic in Tracking to utilize IMU data for better recovery from lost states. --- .../UniLu_RealSense_D435i_640.yaml | 2 +- ...niLu_RealSense_D435i_640_g2step4rerun.yaml | 90 +++++ config/Visualization/vsgraphs_rgbd.rviz | 44 +-- core/include/Settings.h | 2 + core/include/System.h | 6 + core/include/Tracking.h | 8 + core/src/Frame.cc | 18 +- core/src/LocalMapping.cc | 22 +- core/src/Optimizer.cc | 23 ++ core/src/Settings.cc | 9 + core/src/System.cc | 5 + core/src/Tracking.cc | 314 ++++++++++++++++-- 12 files changed, 484 insertions(+), 59 deletions(-) create mode 100644 config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml diff --git a/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml b/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml index 77b0f39..a65e493 100755 --- a/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml +++ b/config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml @@ -48,7 +48,7 @@ IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 IMU.GyroWalk: 1e-6 # rad/s^1.5 IMU.AccWalk: 1e-4 # m/s^2.5 -IMU.Frequency: 200.0 +IMU.Frequency: 400.0 #-------------------------------------------------------------------------------------------- # ORB Parameters diff --git a/config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml b/config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml new file mode 100644 index 0000000..79f29ee --- /dev/null +++ b/config/Monocular-Inertial/UniLu_RealSense_D435i_640_g2step4rerun.yaml @@ -0,0 +1,90 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! (RealSense D435 camera - Serial Number: 044322070965) +#-------------------------------------------------------------------------------------------- +File.version: "1.0" + +Camera.type: "PinHole" + +# Rectified Camera calibration (OpenCV) +Camera1.fx: 605.856689453125 +Camera1.fy: 606.26611328125 +Camera1.cx: 325.021026611328 +Camera1.cy: 244.760711669922 + +# Distortion parameters +Camera1.k1: 0.0 +Camera1.k2: 0.0 +Camera1.p1: 0.0 +Camera1.p2: 0.0 + +# Camera resolution +Camera.width: 640 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Transformation from body-frame (imu) to left camera +IMU.T_b_c1: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [1,0,0,-0.005, + 0,1,0,-0.005, + 0,0,1,0.0117, + 0.0, 0.0, 0.0, 1.0] + +# Do not insert KFs when recently lost +IMU.InsertKFsWhenLost: 0 + +# G2 Step 4 rerun (patched backend): enable IMU-aided relocalization during +# RECENTLY_LOST, ported from BinoSLAM. Off by default upstream in this fork; +# turned on here only, so this file is a rerun-only copy of +# UniLu_RealSense_D435i_640.yaml, not a shared-config edit. +IMU.ImuAidedReloc: 1 + +# IMU noise (Use those from VINS-mono) +IMU.Threshold: 0.5 # Default: 0.5 +IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 +IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 +IMU.GyroWalk: 1e-6 # rad/s^1.5 +IMU.AccWalk: 1e-4 # m/s^2.5 +IMU.Frequency: 400.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1.0 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2.0 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3.0 +Viewer.ViewpointX: 0.0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 +Viewer.ViewpointF: 500.0 diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 7a7dacc..829efb5 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -4,13 +4,15 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /IMU1/Shape1 + - /All Points1 - /Segmented PC1 - /Keyframe Depth Debug1 - /Keyframe Depth Validator Debug1 - /Object Motion Debug Image1 Splitter Ratio: 0.679411768913269 - Tree Height: 228 + Tree Height: 760 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -28,7 +30,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: All Points Visualization Manager: Class: "" Displays: @@ -140,7 +142,7 @@ Visualization Manager: Enabled: true Name: KeyFrames Namespaces: - {} + kf_markers: true Topic: Depth: 5 Durability Policy: Volatile @@ -152,7 +154,7 @@ Visualization Manager: Enabled: true Name: Camera Pose Namespaces: - {} + camera_pose: true Topic: Depth: 5 Durability Policy: Volatile @@ -199,7 +201,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /vs_graphs/body_odom Value: true - - Alpha: 0.3499999940395355 + - Alpha: 0.800000011920929 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 @@ -221,7 +223,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.019999999552965164 + Size (m): 0.029999999329447746 Style: Spheres Topic: Depth: 5 @@ -266,7 +268,7 @@ Visualization Manager: Use rainbow: true Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: true + Enabled: false Name: Fiducial Markers Namespaces: {} @@ -276,7 +278,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /vs_graphs/fiducial_markers - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -493,7 +495,7 @@ Visualization Manager: Value: /camera/depth_da3/image_rect Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -505,7 +507,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /camera/color/image_instance_masks - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: false Max Value: 1 @@ -563,7 +565,7 @@ Visualization Manager: Value: /dynamic_keypoint_3d_lifter/debug_image Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -575,7 +577,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dynamic_keypoint_tracker/debug_image - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 @@ -644,7 +646,7 @@ Visualization Manager: Value: true Enabled: true Global Options: - Background Color: 255; 255; 255 + Background Color: 122; 122; 122 Fixed Frame: map Frame Rate: 30 Name: root @@ -688,25 +690,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 18.06743621826172 + Distance: 94.14274597167969 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 2.8777713775634766 - Y: 3.9726243019104004 - Z: 2.2547993659973145 + X: -16.36867332458496 + Y: 18.900949478149414 + Z: 10.220645904541016 Focal Shape Fixed Size: false Focal Shape Size: 0.0010000000474974513 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.1750001907348633 + Pitch: 0.5100010633468628 Target Frame: camera Value: Orbit (rviz_default_plugins) - Yaw: 5.405036926269531 + Yaw: 2.04367995262146 Saved: ~ Window Geometry: Depth Estimate: @@ -732,7 +734,7 @@ Window Geometry: collapsed: false Object Track Manager Debug: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: @@ -750,5 +752,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 3840 + X: 1920 Y: 0 diff --git a/core/include/Settings.h b/core/include/Settings.h index 754766a..ed84a5c 100644 --- a/core/include/Settings.h +++ b/core/include/Settings.h @@ -99,6 +99,7 @@ namespace ORB_SLAM3 float noiseGyro() { return noiseGyro_; } Sophus::SE3f Tbc() { return Tbc_; } bool insertKFsWhenLost() { return insertKFsWhenLost_; } + bool imuAidedReloc() { return imuAidedReloc_; } float depthMapFactor() { return depthMapFactor_; } @@ -205,6 +206,7 @@ namespace ORB_SLAM3 float imuThreshold_; Sophus::SE3f Tbc_; bool insertKFsWhenLost_; + bool imuAidedReloc_; /* * RGBD stuff diff --git a/core/include/System.h b/core/include/System.h index f0ed531..35d0b55 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -259,6 +259,12 @@ namespace ORB_SLAM3 bool isLost(); bool isFinished(); + // Number of keyframes pending in LocalMapping's queue. Lets a caller feeding + // frames faster than real time (e.g. offline bag replay) apply backpressure so + // LocalMapping's IMU init/BA gets real wall-clock time to keep up, instead of + // racing far ahead of what it has actually processed. + int GetLocalMappingQueueSize(); + void ChangeDataset(); float GetImageScale(); diff --git a/core/include/Tracking.h b/core/include/Tracking.h index 1ae28ac..98ebbe8 100644 --- a/core/include/Tracking.h +++ b/core/include/Tracking.h @@ -270,6 +270,14 @@ namespace ORB_SLAM3 bool Relocalization(); + // IMU-aided relocalization: attempted while RECENTLY_LOST (inertial sensors) to + // recover visual tracking via the IMU-predicted pose instead of only coasting on + // PredictStateIMU() until time_recently_lost expires. + bool mbImuAidedReloc = false; + unsigned int mnLastImuRelocFrameId = 0; + bool RelocalizationWithIMU(); + bool VerifyRelocAgainstIMU(const Sophus::SE3f &Tcw_reloc); + void UpdateLocalMap(); void UpdateLocalPoints(); void UpdateLocalKeyFrames(); diff --git a/core/src/Frame.cc b/core/src/Frame.cc index 549d99c..86548dd 100644 --- a/core/src/Frame.cc +++ b/core/src/Frame.cc @@ -46,7 +46,7 @@ namespace ORB_SLAM3 // For stereo fisheye matching cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING); - Frame::Frame() : mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false) + Frame::Frame() : mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { #ifdef REGISTER_TIMES mTimeStereoMatch = 0; @@ -109,7 +109,7 @@ namespace ORB_SLAM3 const std::vector markers) : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), - mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) + mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { // Setting the color image for Semantic Segmentation colorImg = imColor.clone(); @@ -197,8 +197,6 @@ namespace ORB_SLAM3 mVw.setZero(); } - mpMutexImu = new std::mutex(); - // Set no stereo fisheye information Nleft = -1; Nright = -1; @@ -218,7 +216,7 @@ namespace ORB_SLAM3 Frame *pPrevF, const IMU::Calib &ImuCalib, const std::vector markers) : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(pCamera2), - mbHasPose(false), mbHasVelocity(false) + mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { imgLeft = imLeft.clone(); @@ -309,8 +307,6 @@ namespace ORB_SLAM3 AssignFeaturesToGrid(); - mpMutexImu = new std::mutex(); - UndistortKeyPoints(); } @@ -324,7 +320,7 @@ namespace ORB_SLAM3 mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), - mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) + mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { // Setting the color image for Semantic Segmentation colorImg = imColor.clone(); @@ -400,8 +396,6 @@ namespace ORB_SLAM3 else mVw.setZero(); - mpMutexImu = new std::mutex(); - // Set no stereo fisheye information Nleft = -1; Nright = -1; @@ -421,7 +415,7 @@ namespace ORB_SLAM3 : mpcpi(NULL), mpORBvocabulary(voc), mpORBextractorLeft(extractor), mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(static_cast(pCamera)->toK()), mK_(static_cast(pCamera)->toK_()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbIsSet(false), mbImuPreintegrated(false), mpCamera(pCamera), - mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) + mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false), mpMutexImu(new std::mutex()) { // Setting the color image for Semantic Segmentation colorImg = imColor.clone(); @@ -514,8 +508,6 @@ namespace ORB_SLAM3 { mVw.setZero(); } - - mpMutexImu = new std::mutex(); } void Frame::AssignFeaturesToGrid() diff --git a/core/src/LocalMapping.cc b/core/src/LocalMapping.cc index ccf7313..10a8f6a 100644 --- a/core/src/LocalMapping.cc +++ b/core/src/LocalMapping.cc @@ -35,7 +35,7 @@ namespace ORB_SLAM3 LocalMapping::LocalMapping(System *pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName) : mpSystem(pSys), mbMonocular(bMonocular), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true), - mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9, 9)) + mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9, 9)), mpCurrentKeyFrame(nullptr) { mNumLM = 0; mTinit = 0.f; @@ -137,7 +137,17 @@ namespace ORB_SLAM3 mTinit += mpCurrentKeyFrame->mTimeStamp - mpCurrentKeyFrame->mPrevKF->mTimeStamp; if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()) { - if ((mTinit < 10.f) && (dist < 0.02)) + // Guard: don't reset within the first 3s of map creation. A map that + // starts with the sensor stationary (e.g. before the platform begins + // moving) has near-zero motion by construction, but the map itself is + // still valid — resetting here just discards a good map and restarts + // the whole init process for no benefit. + float elapsedSinceMapCreation = 0.f; + const std::vector &vpOrigins = mpCurrentKeyFrame->GetMap()->mvpKeyFrameOrigins; + if (!vpOrigins.empty()) + elapsedSinceMapCreation = mpCurrentKeyFrame->mTimeStamp - vpOrigins.front()->mTimeStamp; + + if ((mTinit < 10.f) && (dist < 0.02) && (elapsedSinceMapCreation > 3.0f)) { std::cout << "[Mapping] Not enough motion for map initializing. Reseting..." << std::endl; unique_lock lock(mMutexReset); @@ -1123,6 +1133,10 @@ namespace ORB_SLAM3 mbResetRequested = false; mbResetRequestedActiveMap = false; + // mpAtlas->clearAtlas()/clearMap() deletes the KeyFrame mpCurrentKeyFrame + // points to; leaving it set would dangle until the next ProcessNewKeyFrame(). + mpCurrentKeyFrame = nullptr; + // Inertial parameters mTinit = 0.f; mIdxInit = 0; @@ -1146,6 +1160,10 @@ namespace ORB_SLAM3 mbBadImu = false; mbResetRequested = false; mbResetRequestedActiveMap = false; + + // Same dangling-pointer hazard as the full-Atlas reset above, for the + // active-map-only reset path (mpAtlas->clearMap()). + mpCurrentKeyFrame = nullptr; } } } diff --git a/core/src/Optimizer.cc b/core/src/Optimizer.cc index 604849b..441bb5a 100644 --- a/core/src/Optimizer.cc +++ b/core/src/Optimizer.cc @@ -6263,6 +6263,18 @@ namespace ORB_SLAM3 } } + // Gauss-Newton here runs undamped: when the outlier rounds reject (nearly) all + // visual edges, the remaining system can be singular and the dense solve diverges + // to NaN. Writing that state back would abort in Sophus (SO3 from a NaN rotation) + // and stamping mpcpi with it would poison the next frame's optimization — discard + // the result instead and report zero inliers so tracking treats this as a failure. + if (!VP->estimate().Rwb.allFinite() || !VP->estimate().twb.allFinite() || + !VV->estimate().allFinite() || !VG->estimate().allFinite() || !VA->estimate().allFinite()) + { + std::cout << "[Optimizer] PoseInertialOptimizationLastKeyFrame diverged (non-finite estimate), discarding result" << std::endl; + return 0; + } + // Recover optimized pose, velocity and biases pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); Vector6d b; @@ -6660,6 +6672,17 @@ namespace ORB_SLAM3 nInliers = nInliersMono + nInliersStereo; + // Same undamped-GN divergence guard as in PoseInertialOptimizationLastKeyFrame: + // discard a non-finite solution instead of aborting in Sophus / poisoning mpcpi. + // Skipping the mpcpi stamp/delete below is safe: the TrackLocalMap dispatch only + // selects this variant when the previous frame's mpcpi exists. + if (!VP->estimate().Rwb.allFinite() || !VP->estimate().twb.allFinite() || + !VV->estimate().allFinite() || !VG->estimate().allFinite() || !VA->estimate().allFinite()) + { + std::cout << "[Optimizer] PoseInertialOptimizationLastFrame diverged (non-finite estimate), discarding result" << std::endl; + return 0; + } + // Recover optimized pose, velocity and biases pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); Vector6d b; diff --git a/core/src/Settings.cc b/core/src/Settings.cc index 8efa18b..0b1992d 100644 --- a/core/src/Settings.cc +++ b/core/src/Settings.cc @@ -479,6 +479,15 @@ namespace ORB_SLAM3 insertKFsWhenLost_ = (bool)readParameter(fSettings, "IMU.InsertKFsWhenLost", found, false); else insertKFsWhenLost_ = true; + + // When RECENTLY_LOST, attempt an IMU-guided visual relocalization instead of only + // coasting on IMU pose prediction until the timeout expires. Opt-in: off by default + // since it hasn't been tuned/validated for every sensor configuration. + readParameter(fSettings, "IMU.ImuAidedReloc", found, false); + if (found) + imuAidedReloc_ = (bool)readParameter(fSettings, "IMU.ImuAidedReloc", found, false); + else + imuAidedReloc_ = false; } void Settings::readRGBD(cv::FileStorage &fSettings) diff --git a/core/src/System.cc b/core/src/System.cc index d378450..dc5d573 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -1152,6 +1152,11 @@ namespace ORB_SLAM3 return (GetTimeFromIMUInit() > 0.1); } + int System::GetLocalMappingQueueSize() + { + return mpLocalMapper->KeyframesInQueue(); + } + void System::ChangeDataset() { if (mpAtlas->GetCurrentMap()->KeyFramesInMap() < 12) diff --git a/core/src/Tracking.cc b/core/src/Tracking.cc index caad5a5..65f7829 100644 --- a/core/src/Tracking.cc +++ b/core/src/Tracking.cc @@ -665,6 +665,7 @@ namespace ORB_SLAM3 mImuFreq = settings->imuFrequency(); imuThresh = settings->imuThreshold(); mInsertKFsLost = settings->insertKFsWhenLost(); + mbImuAidedReloc = settings->imuAidedReloc(); mImuPer = 1.0 / static_cast(mImuFreq); float Ng = settings->noiseGyro(); float Na = settings->noiseAcc(); @@ -2055,6 +2056,26 @@ namespace ORB_SLAM3 else bOK = false; + // Attempt visual relocalization guided by the IMU-predicted pose, + // instead of only coasting on PredictStateIMU() until the timeout + // below expires and the map gets reset/recreated. Only after 0.5s + // lost: shorter blips are recovered by TrackLocalMap on the + // IMU-predicted pose alone (vanilla behavior), and triggering a + // relocalization there needlessly activates the 30-frame + // post-reloc vision-only grace window for a self-healing hiccup. + if (mbImuAidedReloc && pCurrentMap->isImuInitialized() && + (mCurrentFrame.mTimeStamp - mTimeStampLost) >= 0.5 && + (mCurrentFrame.mnId - mnLastImuRelocFrameId) >= 5) + { + mnLastImuRelocFrameId = mCurrentFrame.mnId; + if (RelocalizationWithIMU()) + { + mState = OK; + bOK = true; + std::cout << "[Tracking][IMU-Reloc] Relocalized at frame " << mCurrentFrame.mnId << std::endl; + } + } + if (mCurrentFrame.mTimeStamp - mTimeStampLost > time_recently_lost) { mState = LOST; @@ -2241,6 +2262,29 @@ namespace ORB_SLAM3 { if (bOK) { + // Post-relocalization grace window: TrackLocalMap optimizes these frames + // vision-only, so their world velocity (mVw) is never re-estimated — it + // stays whatever dead-reckoning predicted at the moment of the loss, and + // propagates unchanged frame-to-frame via the Frame constructor. Handing + // that ~1s-stale velocity to the first inertial optimization after the + // window made its stiff inertial edge reject every visual match and + // diverge to NaN (confirmed via gdb on Marco_Dynamics). Re-seed each + // grace-window frame's velocity from the vision-tracked pose difference — + // the lightweight version of what the ResetFrameIMU() stub below was + // meant to do. + if (mnLastRelocFrameId > 0 && mCurrentFrame.mnId > mnLastRelocFrameId && + mCurrentFrame.mnId <= (mnLastRelocFrameId + mnFramesToResetIMU) && + mCurrentFrame.HasPose() && mLastFrame.HasPose()) + { + const double dt = mCurrentFrame.mTimeStamp - mLastFrame.mTimeStamp; + if (dt > 1e-4) + { + const Eigen::Vector3f vwb = + (mCurrentFrame.GetImuPosition() - mLastFrame.GetImuPosition()) / static_cast(dt); + mCurrentFrame.SetVelocity(vwb); + } + } + if (mCurrentFrame.mnId == (mnLastRelocFrameId + mnFramesToResetIMU)) { cout << "RESETING FRAME!!!" << endl; @@ -3052,7 +3096,14 @@ namespace ORB_SLAM3 else { // if(!mbMapUpdated && mState == OK) // && (mnMatchesInliers>30)) - if (!mbMapUpdated) // && (mnMatchesInliers>30)) + // The LastFrame variant builds an EdgePriorPoseImu from mpPrevFrame->mpcpi and + // dereferences it unconditionally. mpcpi is only stamped by these two inertial + // optimizers, so any frame that last went through vision-only PoseOptimization + // (each frame in the post-relocalization grace window above) has mpcpi == null, + // and using it as the previous frame crashes. Unreachable in vanilla inertial + // mapping (relocalization never fires there), reachable with IMU-aided reloc. + // The LastKeyFrame variant anchors on mpLastKeyFrame instead and needs no prior. + if (!mbMapUpdated && mCurrentFrame.mpPrevFrame && mCurrentFrame.mpPrevFrame->mpcpi) // && (mnMatchesInliers>30)) { Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); @@ -3687,6 +3738,236 @@ namespace ORB_SLAM3 } } + // Sanity-check a relocalization pose candidate against the IMU-predicted body state: + // rejects matches that are geometrically plausible (enough inliers) but land far from + // where the IMU says the sensor actually is, which would otherwise silently teleport + // the trajectory to an unrelated part of the map. + bool Tracking::VerifyRelocAgainstIMU(const Sophus::SE3f &Tcw_reloc) + { + Sophus::SE3f Tbc = mCurrentFrame.mImuCalib.mTbc; + Sophus::SE3f Twb_reloc = (Tcw_reloc * Tbc).inverse(); + + Eigen::Vector3f pos_reloc = Twb_reloc.translation(); + Eigen::Vector3f pos_imu = mCurrentFrame.GetImuPosition(); + + float timeSinceLost = (float)(mCurrentFrame.mTimeStamp - mTimeStampLost); + float maxPosDist = max(3.0f, 2.0f + 1.0f * timeSinceLost); + + float posDist = (pos_reloc - pos_imu).norm(); + if (posDist > maxPosDist) + { + cout << "[Tracking][IMU-Reloc] Rejected: pos dist " << posDist << "m > " << maxPosDist << "m" << endl; + return false; + } + + Eigen::Matrix3f R_reloc = Twb_reloc.rotationMatrix(); + Eigen::Matrix3f R_imu = mCurrentFrame.GetImuRotation(); + float traceVal = ((R_reloc.transpose() * R_imu).trace() - 1.0f) / 2.0f; + traceVal = min(1.0f, max(-1.0f, traceVal)); + float angleDiff = acos(traceVal); + if (angleDiff > 0.35f) + { + cout << "[Tracking][IMU-Reloc] Rejected: angle diff " << angleDiff * 180.0f / M_PI << " deg > 20 deg" << endl; + return false; + } + + return true; + } + + // Relocalization attempt used while RECENTLY_LOST on inertial sensors: instead of only + // coasting on IMU dead-reckoning (PredictStateIMU) until the timeout expires, actively + // search for a visual match, filtered and verified against the IMU-predicted pose so + // the platform can recover tracking without a map reset. + bool Tracking::RelocalizationWithIMU() + { + mCurrentFrame.ComputeBoW(); + + vector vpCandidateKFs = + mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame, mpAtlas->GetCurrentMap()); + + if (vpCandidateKFs.empty()) + return false; + + // Spatial filter: reject candidates far from the IMU-predicted position + Eigen::Vector3f imuPos = mCurrentFrame.GetCameraCenter(); + float timeSinceLost = (float)(mCurrentFrame.mTimeStamp - mTimeStampLost); + float maxRadius = 3.0f + 1.0f * timeSinceLost; + + vector> vDistKF; + for (KeyFrame *pKF : vpCandidateKFs) + { + if (!pKF || pKF->isBad()) + continue; + float dist = (pKF->GetCameraCenter() - imuPos).norm(); + vDistKF.push_back({dist, pKF}); + } + sort(vDistKF.begin(), vDistKF.end()); + + vector vpFilteredKFs; + for (auto &p : vDistKF) + { + if (p.first <= maxRadius) + vpFilteredKFs.push_back(p.second); + } + + // Fallback: keep top 3 closest if the spatial filter rejected everything + if (vpFilteredKFs.empty()) + { + for (int i = 0; i < min(3, (int)vDistKF.size()); i++) + vpFilteredKFs.push_back(vDistKF[i].second); + } + + cout << "[Tracking][IMU-Reloc] " << vpFilteredKFs.size() << "/" << vpCandidateKFs.size() + << " candidates within " << maxRadius << "m (t_lost=" << timeSinceLost << "s)" << endl; + + ORBmatcher matcher(0.9, true); + + // Step A: projection-based matching using the IMU-predicted pose + for (KeyFrame *pKF : vpFilteredKFs) + { + set sFound; + int nProj = matcher.SearchByProjection(mCurrentFrame, pKF, sFound, 15, 100); + + if (nProj < 20) + continue; + + int nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + if (nGood < 10) + continue; + + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + + // Progressive refinement + if (nGood < 50) + { + sFound.clear(); + for (int ip = 0; ip < mCurrentFrame.N; ip++) + if (mCurrentFrame.mvpMapPoints[ip]) + sFound.insert(mCurrentFrame.mvpMapPoints[ip]); + int nExtra = matcher.SearchByProjection(mCurrentFrame, pKF, sFound, 5, 64); + + if (nGood + nExtra >= 30) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + } + } + + if (nGood >= 30 && VerifyRelocAgainstIMU(mCurrentFrame.GetPose())) + { + mnLastRelocFrameId = mCurrentFrame.mnId; + cout << "[Tracking][IMU-Reloc] Projection success: " << nGood << " inliers with KF" << pKF->mnId << endl; + return true; + } + } + + // Step B: BoW fallback with MLPnP RANSAC + ORBmatcher matcherBoW(0.75, true); + const int nKFs = vpFilteredKFs.size(); + vector vpSolvers(nKFs, nullptr); + vector> vvpMatches(nKFs); + vector vbDiscarded(nKFs, false); + int nCandidates = 0; + + for (int i = 0; i < nKFs; i++) + { + KeyFrame *pKF = vpFilteredKFs[i]; + if (pKF->isBad()) + { + vbDiscarded[i] = true; + continue; + } + + int nmatches = matcherBoW.SearchByBoW(pKF, mCurrentFrame, vvpMatches[i]); + if (nmatches < 15) + { + vbDiscarded[i] = true; + continue; + } + + vpSolvers[i] = new MLPnPsolver(mCurrentFrame, vvpMatches[i]); + vpSolvers[i]->SetRansacParameters(0.99, 10, 300, 6, 0.5, 5.991); + nCandidates++; + } + + bool bMatch = false; + while (nCandidates > 0 && !bMatch) + { + for (int i = 0; i < nKFs; i++) + { + if (vbDiscarded[i]) + continue; + + vector vbInliers; + int nInliers; + bool bNoMore; + Eigen::Matrix4f eigTcw; + bool bTcw = vpSolvers[i]->iterate(5, bNoMore, vbInliers, nInliers, eigTcw); + + if (bNoMore) + { + vbDiscarded[i] = true; + nCandidates--; + } + + if (bTcw) + { + mCurrentFrame.SetPose(Sophus::SE3f(eigTcw)); + + set sFound; + const int np = vbInliers.size(); + for (int j = 0; j < np; j++) + { + if (vbInliers[j]) + { + mCurrentFrame.mvpMapPoints[j] = vvpMatches[i][j]; + sFound.insert(vvpMatches[i][j]); + } + else + mCurrentFrame.mvpMapPoints[j] = NULL; + } + + int nGood = Optimizer::PoseOptimization(&mCurrentFrame); + if (nGood < 10) + continue; + + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + + if (nGood < 50) + { + int nadditional = matcher.SearchByProjection(mCurrentFrame, vpFilteredKFs[i], sFound, 10, 100); + if (nadditional + nGood >= 30) + { + nGood = Optimizer::PoseOptimization(&mCurrentFrame); + for (int io = 0; io < mCurrentFrame.N; io++) + if (mCurrentFrame.mvbOutlier[io]) + mCurrentFrame.mvpMapPoints[io] = NULL; + } + } + + if (nGood >= 30 && VerifyRelocAgainstIMU(mCurrentFrame.GetPose())) + { + bMatch = true; + mnLastRelocFrameId = mCurrentFrame.mnId; + cout << "[Tracking][IMU-Reloc] BoW success: " << nGood << " inliers with KF" + << vpFilteredKFs[i]->mnId << endl; + break; + } + } + } + } + + return bMatch; + } + bool Tracking::Relocalization() { Verbose::PrintMess("Starting relocalization", Verbose::VERBOSITY_NORMAL); @@ -3953,29 +4234,18 @@ namespace ORB_SLAM3 mbReadyToInitializate = false; - unsigned int index = mnFirstFrameId; - for (Map *pMap : mpAtlas->GetAllMaps()) - if (pMap->GetAllKeyFrames().size() > 0) - if (index > pMap->GetLowerKFID()) - index = pMap->GetLowerKFID(); - - // Count lost frames - std::list lbLost; - int lostFrameCount = 0; - for (list::iterator ilbL = mlbLost.begin(); ilbL != mlbLost.end(); ilbL++) - { - if (index < mnInitialFrameId) - lbLost.push_back(*ilbL); - else - { - lbLost.push_back(true); - lostFrameCount += 1; - } - index++; - } + // mpAtlas->clearMap() above deletes the KeyFrames of the map being reset, so any + // pointer to one of them in mlRelativeFramePoses/mlpReferences/mlFrameTimes/mlbLost + // is now dangling; walking those lists later (e.g. SaveTrajectoryTUM at shutdown) + // would be undefined behavior otherwise. Mirrors Tracking::Reset()'s unconditional + // clear of the same four lists. + const int lostFrameCount = static_cast(mlbLost.size()); std::cout << "[Tracking] " << lostFrameCount << " frames were set to lost!" << endl; - mlbLost = lbLost; + mlRelativeFramePoses.clear(); + mlpReferences.clear(); + mlFrameTimes.clear(); + mlbLost.clear(); mnInitialFrameId = mCurrentFrame.mnId; mnLastRelocFrameId = mCurrentFrame.mnId; From 02a75db68bc0cbdbdfe02482a6baa740d8268323 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 20 Jul 2026 12:06:21 +0200 Subject: [PATCH 56/59] Add IMU bias estimation functionality and related message --- CMakeLists.txt | 1 + core/include/System.h | 1 + core/include/Tracking.h | 1 + core/src/System.cc | 5 +++++ core/src/Tracking.cc | 5 +++++ include/common.h | 3 +++ msg/ImuBiasEstimate.msg | 7 +++++++ src/common.cc | 24 ++++++++++++++++++++++++ 8 files changed, 47 insertions(+) create mode 100644 msg/ImuBiasEstimate.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index afaed5a..580a25d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +71,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/MapResetEvent.msg" "msg/MapReadyEvent.msg" "msg/MapRescaleEvent.msg" + "msg/ImuBiasEstimate.msg" DEPENDENCIES std_msgs geometry_msgs sensor_msgs ) diff --git a/core/include/System.h b/core/include/System.h index 35d0b55..9884abe 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -252,6 +252,7 @@ namespace ORB_SLAM3 Sophus::SE3f GetCamTwc(); Sophus::SE3f GetImuTwb(); Eigen::Vector3f GetImuVwb(); + IMU::Bias GetImuBias(); bool isImuPreintegrated(); // For debugging diff --git a/core/include/Tracking.h b/core/include/Tracking.h index 98ebbe8..fc4bc17 100644 --- a/core/include/Tracking.h +++ b/core/include/Tracking.h @@ -124,6 +124,7 @@ namespace ORB_SLAM3 Sophus::SE3f GetCamTwc(); Sophus::SE3f GetImuTwb(); Eigen::Vector3f GetImuVwb(); + IMU::Bias GetImuBias(); bool isImuPreintegrated(); void CreateMapInAtlas(); diff --git a/core/src/System.cc b/core/src/System.cc index dc5d573..a086d78 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -1120,6 +1120,11 @@ namespace ORB_SLAM3 return mpTracker->GetImuVwb(); } + IMU::Bias System::GetImuBias() + { + return mpTracker->GetImuBias(); + } + bool System::isImuPreintegrated() { return mpTracker->isImuPreintegrated(); diff --git a/core/src/Tracking.cc b/core/src/Tracking.cc index 65f7829..fd305b8 100644 --- a/core/src/Tracking.cc +++ b/core/src/Tracking.cc @@ -4431,6 +4431,11 @@ namespace ORB_SLAM3 return mCurrentFrame.GetVelocity(); } + IMU::Bias Tracking::GetImuBias() + { + return mCurrentFrame.mImuBias; + } + bool Tracking::isImuPreintegrated() { return mCurrentFrame.mpImuPreintegrated; diff --git a/include/common.h b/include/common.h index c5307c0..a222e24 100644 --- a/include/common.h +++ b/include/common.h @@ -121,6 +121,7 @@ #include #include #include +#include using json = nlohmann::json; @@ -153,6 +154,7 @@ extern rclcpp::Publisher::SharedPtr pu extern double lastPlanePublishTime; extern std::shared_ptr pubTrackingImage; extern rclcpp::Publisher::SharedPtr pubOdometry; +extern rclcpp::Publisher::SharedPtr pubImuBias; extern rclcpp::Publisher::SharedPtr pubKFImage; extern rclcpp::Publisher::SharedPtr pubKeyFrameCreated; extern rclcpp::Publisher::SharedPtr pubKeyFrameStaticMapPoints; @@ -191,6 +193,7 @@ void publishFiducialMarkers(std::vector, rclcpp::Time); void publishKeyFrameImages(std::vector, rclcpp::Time); void publishKeyFrameMarkers(std::vector, rclcpp::Time); void publishBodyOdometry(Sophus::SE3f, Eigen::Vector3f, Eigen::Vector3f, rclcpp::Time); +void publishImuBias(const ORB_SLAM3::IMU::Bias &, rclcpp::Time); void publishStructuralElements(std::vector, std::vector, rclcpp::Time); /** diff --git a/msg/ImuBiasEstimate.msg b/msg/ImuBiasEstimate.msg new file mode 100644 index 0000000..4ca64bd --- /dev/null +++ b/msg/ImuBiasEstimate.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +float64 bax +float64 bay +float64 baz +float64 bwx +float64 bwy +float64 bwz diff --git a/src/common.cc b/src/common.cc index 2768552..a3644aa 100644 --- a/src/common.cc +++ b/src/common.cc @@ -49,6 +49,7 @@ std::string frameWorld, frameCamera, frameImu, frameMap, frameBC, frameSE; double lastPlanePublishTime = -std::numeric_limits::infinity(); rclcpp::Publisher::SharedPtr pubKeyFrameList; rclcpp::Publisher::SharedPtr pubOdometry; +rclcpp::Publisher::SharedPtr pubImuBias; rclcpp::Publisher::SharedPtr pubDoor; rclcpp::Publisher::SharedPtr pubAllMappoints; rclcpp::Publisher::SharedPtr pubCameraPose; @@ -301,7 +302,13 @@ void setupPublishers(std::shared_ptr node, std::shared_ptrcreate_publisher(node_name + "/body_odom", 1); + // Latest accel/gyro bias estimate, published alongside body_odom so downstream nodes + // that build their own IMU::Preintegrated factors (rather than reimplementing Forster + // preintegration) have the bias value ORB-SLAM3's own inertial edges are using. + pubImuBias = node->create_publisher(node_name + "/imu_bias", 1); + } if (pSLAM) pSLAM->SetKeyFrameCreatedCallback(publishKeyFrameCreatedEvent); @@ -488,6 +495,7 @@ void publishTopics(rclcpp::Time msgTime, Eigen::Vector3f Wbb, const sensor_msgs: publishTFTransform(Twb, frameWorld, frameImu, msgTime); publishBodyOdometry(Twb, Vwb, Wwb, msgTime); + publishImuBias(pSLAM->GetImuBias(), msgTime); } } @@ -518,6 +526,22 @@ void publishBodyOdometry(Sophus::SE3f Twb_SE3f, Eigen::Vector3f Vwb_E3f, Eigen:: pubOdometry->publish(odom_msg); } +void publishImuBias(const ORB_SLAM3::IMU::Bias &bias, rclcpp::Time msgTime) +{ + vs_graphs::msg::ImuBiasEstimate bias_msg; + bias_msg.header.frame_id = frameImu; + bias_msg.header.stamp = msgTime; + + bias_msg.bax = bias.bax; + bias_msg.bay = bias.bay; + bias_msg.baz = bias.baz; + bias_msg.bwx = bias.bwx; + bias_msg.bwy = bias.bwy; + bias_msg.bwz = bias.bwz; + + pubImuBias->publish(bias_msg); +} + void publishCameraPose(Sophus::SE3f Tcw_SE3f, rclcpp::Time msgTime) { geometry_msgs::msg::PoseStamped poseMsg; From a7c893a16279e33207a81309c7a6dc8c15c8dd62 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 21 Jul 2026 13:41:59 +0200 Subject: [PATCH 57/59] Implement UpdateTrackedPointsFromCurrentFrame method and update tracked points in System class; enhance publishStaticMapPoints to include number of observations. --- core/include/System.h | 6 ++++++ core/src/System.cc | 33 +++++++++++++++++++++++++++------ src/common.cc | 2 ++ 3 files changed, 35 insertions(+), 6 deletions(-) diff --git a/core/include/System.h b/core/include/System.h index 9884abe..d87f347 100644 --- a/core/include/System.h +++ b/core/include/System.h @@ -312,6 +312,12 @@ namespace ORB_SLAM3 bool SaveAtlas(int type); bool LoadAtlas(int type); + // Snapshot mCurrentFrame's tracked map points/keypoints into mTrackedMapPoints/ + // mTrackedKeyPointsUn, excluding entries the tracker's own pose optimization has already + // flagged as outliers (mvbOutlier) this frame -- consumers (e.g. publishStaticMapPoints) + // must not receive point/pixel associations ORB-SLAM3 itself has already rejected. + void UpdateTrackedPointsFromCurrentFrame(); + string CalculateCheckSum(string filename, int type); // Input sensor diff --git a/core/src/System.cc b/core/src/System.cc index a086d78..9c547c0 100644 --- a/core/src/System.cc +++ b/core/src/System.cc @@ -371,8 +371,7 @@ namespace ORB_SLAM3 unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + UpdateTrackedPointsFromCurrentFrame(); return Tcw; } @@ -447,8 +446,7 @@ namespace ORB_SLAM3 unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + UpdateTrackedPointsFromCurrentFrame(); return Tcw; } @@ -529,11 +527,34 @@ namespace ORB_SLAM3 unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + UpdateTrackedPointsFromCurrentFrame(); return Tcw; } + void System::UpdateTrackedPointsFromCurrentFrame() + { + const Frame &f = mpTracker->mCurrentFrame; + const size_t n = std::min(f.mvpMapPoints.size(), f.mvKeysUn.size()); + + mTrackedMapPoints.clear(); + mTrackedKeyPointsUn.clear(); + mTrackedMapPoints.reserve(n); + mTrackedKeyPointsUn.reserve(n); + + for (size_t i = 0; i < n; i++) + { + // mvbOutlier[i] is set by the tracker's own pose optimization (PoseOptimization / + // inertial variants) when this keypoint<->map-point association is rejected this + // frame; the map point pointer itself is left non-null (see Frame::mvbOutlier usage + // elsewhere, e.g. GetMapPointMatches), so it must be checked separately here. + if (i < f.mvbOutlier.size() && f.mvbOutlier[i]) + continue; + + mTrackedMapPoints.push_back(f.mvpMapPoints[i]); + mTrackedKeyPointsUn.push_back(f.mvKeysUn[i]); + } + } + void System::ActivateLocalizationMode() { unique_lock lock(mMutexMode); diff --git a/src/common.cc b/src/common.cc index a3644aa..528cba5 100644 --- a/src/common.cc +++ b/src/common.cc @@ -916,6 +916,7 @@ void publishStaticMapPoints(std::vector trackedMapPoints, points_msg.point_ids.reserve(count); points_msg.positions_world.reserve(count); points_msg.pixels.reserve(count); + points_msg.num_observations.reserve(count); for (std::size_t i = 0; i < count; ++i) { @@ -941,6 +942,7 @@ void publishStaticMapPoints(std::vector trackedMapPoints, points_msg.point_ids.push_back(static_cast(map_point->mnId)); points_msg.positions_world.push_back(p_world_msg); points_msg.pixels.push_back(p_pixel_msg); + points_msg.num_observations.push_back(static_cast(map_point->Observations())); } pubStaticMapPoints->publish(points_msg); From 76ab2e363dac669e188d3913676f798f6d93b00f Mon Sep 17 00:00:00 2001 From: mgiberna Date: Mon, 27 Jul 2026 13:35:54 +0200 Subject: [PATCH 58/59] add install script --- install_workspace_repos.sh | 71 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100755 install_workspace_repos.sh diff --git a/install_workspace_repos.sh b/install_workspace_repos.sh new file mode 100755 index 0000000..2b5320f --- /dev/null +++ b/install_workspace_repos.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Clone/update the companion repositories used by this workspace. +# Run this script from anywhere inside a clone of visual_sgraphs. + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WORKSPACE_SRC="$(cd "${SCRIPT_DIR}/.." && pwd)" + +GIT_PROTOCOL="${GIT_PROTOCOL:-ssh}" + +if [[ "${GIT_PROTOCOL}" != "ssh" && "${GIT_PROTOCOL}" != "https" ]]; then + echo "GIT_PROTOCOL must be either 'ssh' or 'https'." >&2 + exit 1 +fi + +repo_url() { + local owner="$1" + local repo="$2" + + if [[ "${GIT_PROTOCOL}" == "https" ]]; then + printf 'https://github.com/%s/%s.git' "${owner}" "${repo}" + else + printf 'git@github.com:%s/%s.git' "${owner}" "${repo}" + fi +} + +clone_or_update() { + local path="$1" + local owner="$2" + local repo="$3" + local branch="$4" + local target="${WORKSPACE_SRC}/${path}" + local url + url="$(repo_url "${owner}" "${repo}")" + + if [[ -d "${target}/.git" ]]; then + echo "Updating ${path} (${branch})" + git -C "${target}" fetch origin "${branch}" + git -C "${target}" checkout "${branch}" + git -C "${target}" pull --ff-only origin "${branch}" + return + fi + + if [[ -e "${target}" ]]; then + echo "Skipping ${path}: path exists but is not a git repository." >&2 + return + fi + + echo "Cloning ${path} (${branch})" + mkdir -p "$(dirname "${target}")" + git clone --branch "${branch}" "${url}" "${target}" +} + +echo "Workspace source directory: ${WORKSPACE_SRC}" +echo "Git protocol: ${GIT_PROTOCOL}" + +clone_or_update "dynamic_keypoint_3d_lifter" "snt-arg" "dynamic_keypoint_3d_lifter" "master" +clone_or_update "dynamic_keypoint_tracker" "snt-arg" "dynamic_keypoint_tracker" "master" +clone_or_update "keyframe_depth_estimator" "snt-arg" "keyframe_depth_estimator" "master" +clone_or_update "keyframe_depth_validator" "snt-arg" "keyframe_depth_validator" "master" +clone_or_update "object_motion_estimator" "snt-arg" "object_motion_estimator" "master" +clone_or_update "realsense-ros" "IntelRealSense" "realsense-ros" "ros2-master" +clone_or_update "scene_segment_ros" "snt-arg" "scene_segment_ros" "dynamic" +clone_or_update "situational_graphs_msgs" "snt-arg" "situational_graphs_msgs" "main" +clone_or_update "voxblox_ros2_minimal" "snt-arg" "voxblox_ros2_minimal" "master" +clone_or_update "bin/dynamic_s_graphs" "snt-arg" "dynamic_s_graphs" "master" +clone_or_update "bin/object_tracker_3d_ros" "snt-arg" "object_tracker_3d_ros" "master" +clone_or_update "scene_segment_ros/src/eomt" "tue-mps" "eomt" "master" + +echo "Done." From fff23d8898b12b0d8ca21b96c024a1ca96cef4e2 Mon Sep 17 00:00:00 2001 From: mgiberna Date: Tue, 28 Jul 2026 17:03:02 +0200 Subject: [PATCH 59/59] Update launch configuration and docker-compose for new directories and parameters --- config/Visualization/vsgraphs_rgbd.rviz | 190 +++++++++++++++++++++-- docker/docker-compose.yml | 4 + launch/mono-imu.launch.py | 197 +++++++++++++++++------- 3 files changed, 328 insertions(+), 63 deletions(-) diff --git a/config/Visualization/vsgraphs_rgbd.rviz b/config/Visualization/vsgraphs_rgbd.rviz index 829efb5..c440f02 100644 --- a/config/Visualization/vsgraphs_rgbd.rviz +++ b/config/Visualization/vsgraphs_rgbd.rviz @@ -607,7 +607,7 @@ Visualization Manager: Value: /dynamic_keypoint_interpolator/debug_image Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -619,12 +619,124 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /object_motion/debug_image - Value: true + Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true Name: "Pipeline A: depth model" Namespaces: - {} + track_121: true + track_127: true + track_129: true + track_131: true + track_132: true + track_135: true + track_136: true + track_137: true + track_139: true + track_140: true + track_142: true + track_143: true + track_145: true + track_146: true + track_147: true + track_148: true + track_149: true + track_150: true + track_151: true + track_155: true + track_158: true + track_159: true + track_165: true + track_166: true + track_167: true + track_168: true + track_174: true + track_175: true + track_177: true + track_183: true + track_184: true + track_185: true + track_189: true + track_193: true + track_195: true + track_198: true + track_199: true + track_201: true + track_202: true + track_203: true + track_205: true + track_206: true + track_209: true + track_213: true + track_214: true + track_215: true + track_219: true + track_223: true + track_224: true + track_225: true + track_227: true + track_228: true + track_229: true + track_231: true + track_233: true + track_234: true + track_237: true + track_238: true + track_241: true + track_245: true + track_246: true + track_248: true + track_249: true + track_251: true + track_255: true + track_256: true + track_257: true + track_261: true + track_262: true + track_273: true + track_288: true + track_289: true + track_290: true + track_292: true + track_293: true + track_294: true + track_295: true + track_298: true + track_302: true + track_303: true + track_304: true + track_305: true + track_307: true + track_309: true + track_310: true + track_316: true + track_318: true + track_323: true + track_326: true + track_337: true + track_338: true + track_340: true + track_341: true + track_345: true + track_349: true + track_351: true + track_353: true + track_354: true + track_358: true + track_361: true + track_362: true + track_364: true + track_365: true + track_367: true + track_368: true + track_369: true + track_370: true + track_373: true + track_374: true + track_376: true + track_377: true + track_378: true + track_381: true Topic: Depth: 5 Durability Policy: Volatile @@ -634,7 +746,20 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: "Pipeline B: BA+IMU scale" + Name: "Joint window: object pose+landmarks (D12)" + Namespaces: + track_132: true + track_135: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object_motion/joint/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: "Pipeline B: BA+triangulation (retired)" Namespaces: {} Topic: @@ -644,6 +769,53 @@ Visualization Manager: Reliability Policy: Reliable Value: /object_motion/ba/markers Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Ground Truth (live, bright green) + Namespaces: + gt_object_0: true + gt_object_1: true + gt_object_10: true + gt_object_11: true + gt_object_12: true + gt_object_13: true + gt_object_14: true + gt_object_15: true + gt_object_16: true + gt_object_17: true + gt_object_18: true + gt_object_19: true + gt_object_2: true + gt_object_20: true + gt_object_21: true + gt_object_22: true + gt_object_23: true + gt_object_24: true + gt_object_25: true + gt_object_26: true + gt_object_27: true + gt_object_28: true + gt_object_29: true + gt_object_3: true + gt_object_30: true + gt_object_31: true + gt_object_32: true + gt_object_33: true + gt_object_34: true + gt_object_35: true + gt_object_4: true + gt_object_5: true + gt_object_6: true + gt_object_7: true + gt_object_8: true + gt_object_9: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ground_truth/markers + Value: true Enabled: true Global Options: Background Color: 122; 122; 122 @@ -690,16 +862,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 94.14274597167969 + Distance: 8.140125274658203 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -16.36867332458496 - Y: 18.900949478149414 - Z: 10.220645904541016 + X: 1.705122709274292 + Y: 30.160234451293945 + Z: 7.017193794250488 Focal Shape Fixed Size: false Focal Shape Size: 0.0010000000474974513 Invert Z Axis: false @@ -734,7 +906,7 @@ Window Geometry: collapsed: false Object Track Manager Debug: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Raw Frames: collapsed: false Segmented KeyFrames: diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 5773034..f4719e1 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -59,6 +59,8 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" # Working Directory - Object motion estimator (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_motion_estimator:/home/$USERNAME/workspace/src/object_motion_estimator:rw" + # Working Directory - ORB-SLAM3 bag runner (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/orbslam3_bag_runner:/home/$USERNAME/workspace/src/orbslam3_bag_runner:rw" # Data Directory (** MODIFY **) - "/media/$USERNAME/muffin/datasets:/home/$USERNAME/datasets:rw" network_mode: "host" @@ -89,5 +91,7 @@ services: - "/home/$USERNAME/workspaces/dv_sgraphs/src/dynamic_keypoint_3d_lifter:/home/$USERNAME/workspace/src/dynamic_keypoint_3d_lifter:rw" # Working Directory - Object motion estimator (** MODIFY **) - "/home/$USERNAME/workspaces/dv_sgraphs/src/object_motion_estimator:/home/$USERNAME/workspace/src/object_motion_estimator:rw" + # Working Directory - ORB-SLAM3 bag runner (** MODIFY **) + - "/home/$USERNAME/workspaces/dv_sgraphs/src/orbslam3_bag_runner:/home/$USERNAME/workspace/src/orbslam3_bag_runner:rw" entrypoint: "" restart: "no" diff --git a/launch/mono-imu.launch.py b/launch/mono-imu.launch.py index ca4e030..2efa94d 100644 --- a/launch/mono-imu.launch.py +++ b/launch/mono-imu.launch.py @@ -61,7 +61,20 @@ def generate_launch_description(): DeclareLaunchArgument("launch_dynamic_keypoint_3d_lifter", default_value="false"), # Pipeline DA3 DeclareLaunchArgument("launch_dynamic_keypoint_interpolator", default_value="false"), # Pipeline DA3 DeclareLaunchArgument("launch_object_motion_estimator", default_value="false"), # Pipeline DA3 - DeclareLaunchArgument("launch_object_ba_tracker", default_value="true"), # Pipeline BA + # D12: Pipeline B (object_ba_tracker_node) retired from the default launch graph -- + # its fixed-landmark BA output is no longer consumed by joint_window_node, which now + # gets its landmark seeds from landmark_initializer_node and optimizes landmarks + # in-graph. Flag kept (not removed) so the node can still be launched standalone for + # debugging/comparison; object_ba_tracker_node.cpp/.hpp are kept in the source tree + # unbuilt-from-launch (still compiled as an executable) since + # landmark_initializer_node's triangulation was ported from it. + DeclareLaunchArgument("launch_object_ba_tracker", default_value="false"), # Pipeline BA (D12: retired by default) + DeclareLaunchArgument("launch_landmark_initializer", default_value="true"), # D12: landmark seed source + DeclareLaunchArgument( + "landmark_seed_topic", default_value="/object_motion/landmark_seed" + ), + DeclareLaunchArgument("landmark_min_frames_for_triangulation", default_value="5"), + DeclareLaunchArgument("landmark_window_max_frames", default_value="15"), DeclareLaunchArgument( "keyframe_depth_model_path", default_value=( @@ -173,7 +186,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "dynamic_keypoint_3d_lifter_debug_match_logging", - default_value="true", + default_value="false", ), DeclareLaunchArgument( "dynamic_keypoint_interpolator_output_topic", @@ -350,18 +363,18 @@ def generate_launch_description(): choices=["yoso", "pfcn", "yolo26", "off"], ), # Topics - DeclareLaunchArgument("camera_frame", default_value="camera"), - DeclareLaunchArgument("sensor_config", default_value="UniLu_RealSense_D435i_640"), + DeclareLaunchArgument("camera_frame", default_value="camera"), #left_cam + DeclareLaunchArgument("sensor_config", default_value="uHumans2_TESSE"), #UniLu_RealSense_D435i_640 DeclareLaunchArgument( - "rgb_image_topic", default_value="/camera/realsense/color/image_raw" + "rgb_image_topic", default_value="/tesse/left_cam/rgb/image_raw" #"/camera/realsense/color/image_raw" ), DeclareLaunchArgument( "rgb_camera_info_topic", - default_value="/camera/realsense/color/camera_info", + default_value="/tesse/left_cam/camera_info", # "/camera/realsense/color/camera_info", ), DeclareLaunchArgument( "imu_topic", - default_value="/camera/realsense/imu", + default_value="/tesse/imu/noisy/imu", #"/camera/realsense/imu" ), # VS-Graphs Node Node( @@ -436,6 +449,12 @@ def generate_launch_description(): "camera_info_topic": LaunchConfiguration( "rgb_camera_info_topic" ), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], "metric_depth_topic": LaunchConfiguration( "keyframe_depth_metric_topic" ), @@ -660,6 +679,12 @@ def generate_launch_description(): ), "keyframe_pose_topic": "/vs_graphs/camera_pose", "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], "output_topic": LaunchConfiguration( "dynamic_keypoint_3d_lifter_output_topic" ), @@ -718,6 +743,12 @@ def generate_launch_description(): ), "camera_pose_topic": "/vs_graphs/camera_pose", "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], "output_topic": LaunchConfiguration( "dynamic_keypoint_interpolator_output_topic" ), @@ -767,6 +798,12 @@ def generate_launch_description(): "object_track_events_topic": "/object_tracks/events", "camera_pose_topic": "/vs_graphs/camera_pose", "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], "image_topic": LaunchConfiguration("rgb_image_topic"), "output_topic": LaunchConfiguration("object_motion_output_topic"), "markers_topic": LaunchConfiguration("object_motion_markers_topic"), @@ -848,6 +885,12 @@ def generate_launch_description(): ), "camera_pose_topic": "/vs_graphs/camera_pose", "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], "object_track_events_topic": "/object_tracks/events", "output_topic": LaunchConfiguration("object_ba_output_topic"), "markers_topic": LaunchConfiguration("object_ba_markers_topic"), @@ -951,56 +994,102 @@ def generate_launch_description(): }, ], ), - # Static Transforms - Node( - package="tf2_ros", - name="map_to_map_elevated", # For Voxblox Skeleton - executable="static_transform_publisher", - arguments=["0", "0", "0", "0", "0", "0", "map", "map_elevated"], - ), - Node( - name="bc_to_se", - package="tf2_ros", - executable="static_transform_publisher", - arguments=["0", "0", "3", "0", "0", "0", "build_comp", "struc_elem"], - ), - Node( - package="tf2_ros", - name="world_to_bc", - executable="static_transform_publisher", - arguments=["0", "0", "3", "0", "0", "0", "world", "build_comp"], - ), - Node( - package="tf2_ros", - name="camera_to_imu", - executable="static_transform_publisher", - arguments=[ - "-0.011739999987185001", - "-0.005520000122487545", - "0.005100000184029341", - "0", - "0", - "0", - "camera", - "imu", - ], - ), + # D12 Step 2: Landmark Initializer -- replaces Pipeline BA as joint_window_node's + # landmark-seed source. Triangulation-only, no optimizer, publishes a one-shot seed + # per track_id (see landmark_initializer_node.hpp doc comment). Node( - package="tf2_ros", - name="camera_to_camera_optical", - executable="static_transform_publisher", - arguments=[ - "0.00011983246804447845", - "0.014999180100858212", - "0.00015637179603800178", - "-1.5730284322450263", - "0.019080586334886032", - "-1.574238659946616", - "camera", - "camera_color_optical_frame", - # RealSense: camera_color_optical_frame, OpenLoris: d400_color + condition=IfCondition( + LaunchConfiguration("launch_landmark_initializer") + ), + package="object_motion_estimator", + executable="landmark_initializer_node", + name="landmark_initializer", + output="screen", + parameters=[ + { + "use_sim_time": LaunchConfiguration("offline"), + "object_tracks_topic": LaunchConfiguration( + "dynamic_keypoint_filtered_object_tracks_topic" + ), + "object_track_events_topic": "/object_tracks/events", + "camera_pose_topic": "/vs_graphs/camera_pose", + "camera_info_topic": LaunchConfiguration("rgb_camera_info_topic"), + "settings_file": [ + get_package_share_directory("vs_graphs"), + "/config/Monocular-Inertial/", + LaunchConfiguration("sensor_config"), + ".yaml", + ], + "output_topic": LaunchConfiguration("landmark_seed_topic"), + "pose_sync_tolerance_ms": ParameterValue( + LaunchConfiguration("object_ba_pose_sync_tolerance_ms"), + value_type=float, + ), + "min_frames_for_triangulation": ParameterValue( + LaunchConfiguration("landmark_min_frames_for_triangulation"), + value_type=int, + ), + "window_max_frames": ParameterValue( + LaunchConfiguration("landmark_window_max_frames"), + value_type=int, + ), + "queue_depth": ParameterValue( + LaunchConfiguration("object_motion_queue_depth"), + value_type=int, + ), + }, ], ), + # # Static Transforms + # Node( + # package="tf2_ros", + # name="map_to_map_elevated", # For Voxblox Skeleton + # executable="static_transform_publisher", + # arguments=["0", "0", "0", "0", "0", "0", "map", "map_elevated"], + # ), + # Node( + # name="bc_to_se", + # package="tf2_ros", + # executable="static_transform_publisher", + # arguments=["0", "0", "3", "0", "0", "0", "build_comp", "struc_elem"], + # ), + # Node( + # package="tf2_ros", + # name="world_to_bc", + # executable="static_transform_publisher", + # arguments=["0", "0", "3", "0", "0", "0", "world", "build_comp"], + # ), + # Node( + # package="tf2_ros", + # name="camera_to_imu", + # executable="static_transform_publisher", + # arguments=[ + # "-0.011739999987185001", + # "-0.005520000122487545", + # "0.005100000184029341", + # "0", + # "0", + # "0", + # "camera", + # "imu", + # ], + # ), + # Node( + # package="tf2_ros", + # name="camera_to_camera_optical", + # executable="static_transform_publisher", + # arguments=[ + # "0.00011983246804447845", + # "0.014999180100858212", + # "0.00015637179603800178", + # "-1.5730284322450263", + # "0.019080586334886032", + # "-1.574238659946616", + # "camera", + # "camera_color_optical_frame", + # # RealSense: camera_color_optical_frame, OpenLoris: d400_color + # ], + # ), # RViz Node( condition=IfCondition(LaunchConfiguration("launch_rviz")),