├── config └── vio_params.yaml ├── package.xml ├── LICENSE ├── launch ├── vio.launch.py └── rgbd_vo.launch.py ├── include └── ThreadSafeQueue.h ├── README.md ├── CMakeLists.txt ├── rviz └── vio.rviz └── src └── vio_node.cpp /config/vio_params.yaml: -------------------------------------------------------------------------------- 1 | # VIO ROS2 Configuration 2 | 3 | # Topic names 4 | left_image_topic: "/camera/left/image_raw" 5 | right_image_topic: "/camera/right/image_raw" 6 | imu_topic: "/imu/data" 7 | 8 | # Subscriber settings 9 | queue_size: 10 10 | 11 | # VIO parameters (TODO: add more as needed) 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vio_ros_wrapper 5 | 0.1.0 6 | ROS2 wrapper for lightweight VIO 7 | Eugene 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | sensor_msgs 14 | geometry_msgs 15 | nav_msgs 16 | cv_bridge 17 | image_transport 18 | message_filters 19 | tf2 20 | tf2_ros 21 | tf2_geometry_msgs 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Seungwon Choi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /launch/vio.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | from launch.conditions import IfCondition 6 | from ament_index_python.packages import get_package_share_directory 7 | import os 8 | 9 | 10 | def generate_launch_description(): 11 | # Get package share directory 12 | pkg_share = get_package_share_directory('vio_ros_wrapper') 13 | rviz_config_file = os.path.join(pkg_share, 'rviz', 'vio.rviz') 14 | 15 | return LaunchDescription([ 16 | # Declare launch arguments 17 | DeclareLaunchArgument( 18 | 'left_image_topic', 19 | default_value='/camera/left/image_raw', 20 | description='Left camera image topic' 21 | ), 22 | DeclareLaunchArgument( 23 | 'right_image_topic', 24 | default_value='/camera/right/image_raw', 25 | description='Right camera image topic' 26 | ), 27 | DeclareLaunchArgument( 28 | 'imu_topic', 29 | default_value='/imu/data', 30 | description='IMU data topic' 31 | ), 32 | DeclareLaunchArgument( 33 | 'queue_size', 34 | default_value='10000', 35 | description='Subscriber queue size' 36 | ), 37 | DeclareLaunchArgument( 38 | 'config_file', 39 | description='VIO configuration file path (REQUIRED!)' 40 | ), 41 | DeclareLaunchArgument( 42 | 'use_rviz', 43 | default_value='true', 44 | description='Launch RViz2' 45 | ), 46 | 47 | # VIO Node 48 | Node( 49 | package='vio_ros_wrapper', 50 | executable='vio_node', 51 | name='vio_node', 52 | output='screen', 53 | parameters=[{ 54 | 'left_image_topic': LaunchConfiguration('left_image_topic'), 55 | 'right_image_topic': LaunchConfiguration('right_image_topic'), 56 | 'imu_topic': LaunchConfiguration('imu_topic'), 57 | 'queue_size': LaunchConfiguration('queue_size'), 58 | 'config_file': LaunchConfiguration('config_file'), 59 | }] 60 | ), 61 | 62 | # RViz2 63 | Node( 64 | package='rviz2', 65 | executable='rviz2', 66 | name='rviz2', 67 | output='screen', 68 | arguments=['-d', rviz_config_file], 69 | condition=IfCondition(LaunchConfiguration('use_rviz')) 70 | ), 71 | ]) 72 | -------------------------------------------------------------------------------- /launch/rgbd_vo.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | from launch.conditions import IfCondition 6 | from ament_index_python.packages import get_package_share_directory 7 | import os 8 | 9 | 10 | def generate_launch_description(): 11 | # Get package share directory 12 | pkg_share = get_package_share_directory('vio_ros_wrapper') 13 | rviz_config_file = os.path.join(pkg_share, 'rviz', 'rgbd.rviz') 14 | default_config_file = os.path.join(pkg_share, 'config', 'd435i_cafe.yaml') 15 | 16 | return LaunchDescription([ 17 | # Declare launch arguments for RGBD Visual Odometry (VO) 18 | # Note: RGBD-VO mode does not use IMU data 19 | DeclareLaunchArgument( 20 | 'rgb_image_topic', 21 | default_value='/camera/rgb/image_raw', 22 | description='RGB camera image topic' 23 | ), 24 | DeclareLaunchArgument( 25 | 'depth_image_topic', 26 | default_value='/camera/depth/image_raw', 27 | description='Depth camera image topic' 28 | ), 29 | DeclareLaunchArgument( 30 | 'queue_size', 31 | default_value='10000', 32 | description='Subscriber queue size' 33 | ), 34 | DeclareLaunchArgument( 35 | 'config_file', 36 | default_value=default_config_file, 37 | description='VIO configuration file path (defaults to d435i_cafe.yaml)' 38 | ), 39 | DeclareLaunchArgument( 40 | 'use_rviz', 41 | default_value='true', 42 | description='Launch RViz2' 43 | ), 44 | 45 | # RGBD-VO Node (Visual Odometry using RGB+Depth, no IMU) 46 | Node( 47 | package='vio_ros_wrapper', 48 | executable='vio_node', 49 | name='vio_node', 50 | output='screen', 51 | parameters=[{ 52 | 'rgb_image_topic': LaunchConfiguration('rgb_image_topic'), 53 | 'depth_image_topic': LaunchConfiguration('depth_image_topic'), 54 | 'queue_size': LaunchConfiguration('queue_size'), 55 | 'config_file': LaunchConfiguration('config_file'), 56 | }] 57 | ), 58 | 59 | # RViz2 60 | Node( 61 | package='rviz2', 62 | executable='rviz2', 63 | name='rviz2', 64 | output='screen', 65 | arguments=['-d', rviz_config_file], 66 | condition=IfCondition(LaunchConfiguration('use_rviz')) 67 | ), 68 | ]) 69 | -------------------------------------------------------------------------------- /include/ThreadSafeQueue.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /** 10 | * @brief Thread-safe queue implementation using mutex and condition variable 11 | * @tparam T Type of elements stored in the queue 12 | */ 13 | template 14 | class ThreadSafeQueue { 15 | public: 16 | ThreadSafeQueue() : shutdown_(false) {} 17 | 18 | ~ThreadSafeQueue() { 19 | shutdown(); 20 | } 21 | 22 | /** 23 | * @brief Push an item to the queue (non-blocking) 24 | */ 25 | void push(T item) { 26 | { 27 | std::lock_guard lock(mutex_); 28 | if (shutdown_) return; // Don't accept new items after shutdown 29 | queue_.push(std::move(item)); 30 | } 31 | cv_.notify_one(); // Wake up one waiting thread 32 | } 33 | 34 | /** 35 | * @brief Pop an item from the queue (blocking until item available or shutdown) 36 | * @return std::optional The item, or std::nullopt if queue is shutdown 37 | */ 38 | std::optional pop() { 39 | std::unique_lock lock(mutex_); 40 | 41 | // Wait until queue has items or shutdown is called 42 | cv_.wait(lock, [this]() { 43 | return !queue_.empty() || shutdown_; 44 | }); 45 | 46 | // If shutdown and queue is empty, return nullopt 47 | if (shutdown_ && queue_.empty()) { 48 | return std::nullopt; 49 | } 50 | 51 | // Get item from queue 52 | T item = std::move(queue_.front()); 53 | queue_.pop(); 54 | return item; 55 | } 56 | 57 | /** 58 | * @brief Try to pop an item with timeout 59 | * @param timeout_ms Timeout in milliseconds 60 | * @return std::optional The item, or std::nullopt if timeout or shutdown 61 | */ 62 | std::optional pop_timeout(int timeout_ms) { 63 | std::unique_lock lock(mutex_); 64 | 65 | // Wait with timeout 66 | bool has_item = cv_.wait_for(lock, std::chrono::milliseconds(timeout_ms), 67 | [this]() { return !queue_.empty() || shutdown_; }); 68 | 69 | if (!has_item || (shutdown_ && queue_.empty())) { 70 | return std::nullopt; 71 | } 72 | 73 | T item = std::move(queue_.front()); 74 | queue_.pop(); 75 | return item; 76 | } 77 | 78 | /** 79 | * @brief Get current queue size 80 | */ 81 | size_t size() const { 82 | std::lock_guard lock(mutex_); 83 | return queue_.size(); 84 | } 85 | 86 | /** 87 | * @brief Check if queue is empty 88 | */ 89 | bool empty() const { 90 | std::lock_guard lock(mutex_); 91 | return queue_.empty(); 92 | } 93 | 94 | /** 95 | * @brief Shutdown the queue (unblock all waiting threads) 96 | */ 97 | void shutdown() { 98 | { 99 | std::lock_guard lock(mutex_); 100 | shutdown_ = true; 101 | } 102 | cv_.notify_all(); // Wake up all waiting threads 103 | } 104 | 105 | /** 106 | * @brief Clear all items in the queue 107 | */ 108 | void clear() { 109 | std::lock_guard lock(mutex_); 110 | while (!queue_.empty()) { 111 | queue_.pop(); 112 | } 113 | } 114 | 115 | private: 116 | std::queue queue_; 117 | mutable std::mutex mutex_; 118 | std::condition_variable cv_; 119 | bool shutdown_; 120 | }; 121 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Lightweight VIO ROS2 Wrapper 2 | 3 | ROS2 wrapper for lightweight Visual-Inertial Odometry system. 4 | 5 | Supports **Stereo VIO** and **RGBD VO** modes. 6 | 7 | ## Demo 8 | 9 | [![Demo Video](https://img.youtube.com/vi/aat8Nvcq-c8/0.jpg)](https://youtu.be/aat8Nvcq-c8) 10 | 11 | --- 12 | 13 | ## About 14 | 15 | This project implements the statistical uncertainty learning method proposed in: 16 | 17 | **Statistical Uncertainty Learning for Robust Visual-Inertial State Estimation** 18 | Seungwon Choi, Donggyu Park, Seo-Yeon Hwang, Tae-Wan Kim 19 | arXiv:2510.01648 20 | 21 | **Original Source**: [https://github.com/93won/lightweight_vio](https://github.com/93won/lightweight_vio) 22 | 23 | ### Citation 24 | 25 | If you use this work in your research, please cite: 26 | 27 | ```bibtex 28 | @misc{choi2025statistical, 29 | title={Statistical Uncertainty Learning for Robust Visual-Inertial State Estimation}, 30 | author={Seungwon Choi and Donggyu Park and Seo-Yeon Hwang and Tae-Wan Kim}, 31 | year={2025}, 32 | eprint={2510.01648}, 33 | archivePrefix={arXiv}, 34 | primaryClass={cs.RO}, 35 | url={https://arxiv.org/abs/2510.01648} 36 | } 37 | ``` 38 | 39 | --- 40 | 41 | ## Requirements 42 | 43 | - Ubuntu 22.04 44 | - ROS2 Humble 45 | 46 | --- 47 | 48 | ## Installation 49 | 50 | ### 1. Create Workspace 51 | 52 | ```bash 53 | mkdir -p /vio_ws/src 54 | cd /vio_ws/src 55 | git clone https://github.com/93won/vio_ros_wrapper.git 56 | cd vio_ros_wrapper 57 | git submodule update --init --recursive 58 | cd /vio_ws 59 | ``` 60 | 61 | ### 2. Build 62 | 63 | ```bash 64 | colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release 65 | source install/setup.bash 66 | ``` 67 | 68 | --- 69 | 70 | ## Sample Datasets 71 | 72 | Pre-recorded ROS2 bags for testing: 73 | 74 | ### 1. OpenLORIS RGBD Dataset (Cafe) 75 | - **File**: `cafe.bag.db3` 76 | - **Description**: Indoor cafe scene with RealSense D435i 77 | - **Topics**: RGB + Depth images 78 | - **Download**: [Google Drive](https://drive.google.com/file/d/1hH-n9-Movdq4sZkGATvNCcl27lDOYep5/view?usp=sharing) 79 | 80 | ### 2. TUM-VI Dataset (Room1) 81 | - **File**: `room1_stereo_imu.bag_0.db3` 82 | - **Description**: Indoor room with fisheye stereo camera + IMU 83 | - **Topics**: Stereo images + IMU data 84 | - **Download**: [Google Drive](https://drive.google.com/file/d/1-_y33_YkB8pTDkro3VZaTcIBFieqORVa/view?usp=sharing) 85 | 86 | ### 3. EuRoC Dataset (V101 Easy) 87 | - **File**: `v101_stereo_imu.bag.db3` 88 | - **Description**: Vicon room, easy trajectory 89 | - **Topics**: Rectilinear Stereo images + IMU data 90 | - **Download**: [Google Drive](https://drive.google.com/file/d/1gthuWv-eLEPY3N6G4SFUWppHvh0sTWZo/view?usp=sharing) 91 | 92 | --- 93 | 94 | ## Quick Start 95 | 96 | ### RGBD VO (OpenLORIS Cafe Dataset) 97 | 98 | ```bash 99 | # Terminal 1 100 | ros2 launch vio_ros_wrapper rgbd_vo.launch.py 101 | 102 | # Terminal 2 103 | ros2 bag play cafe.bag.db3 --clock 104 | ``` 105 | 106 | ### Stereo VIO (TUM-VI Room1) 107 | 108 | ```bash 109 | # Terminal 1 (in /vio_ws) 110 | ros2 launch vio_ros_wrapper vio.launch.py \ 111 | config_file:=/vio_ws/src/vio_ros_wrapper/lightweight_vio/config/tum_vio.yaml 112 | 113 | # Terminal 2 114 | ros2 bag play room1_stereo_imu.bag_0.db3 --clock 115 | ``` 116 | 117 | ### Stereo VIO (EuRoC V101) 118 | 119 | ```bash 120 | # Terminal 1 (in /vio_ws) 121 | ros2 launch vio_ros_wrapper vio.launch.py \ 122 | config_file:=/vio_ws/src/vio_ros_wrapper/lightweight_vio/config/euroc_vio.yaml 123 | 124 | # Terminal 2 125 | ros2 bag play v101_stereo_imu.bag.db3 --clock 126 | ``` 127 | 128 | --- 129 | 130 | ## Configuration 131 | 132 | Config files: `lightweight_vio/config/` 133 | 134 | - `euroc_vio.yaml` - EuRoC stereo + IMU 135 | - `d435i_cafe.yaml` - RealSense RGBD 136 | - `tum_vio.yaml` - TUM-VI stereo + IMU 137 | 138 | --- 139 | 140 | ## Topics 141 | 142 | ### Published 143 | - `/vio/odometry` - 6-DOF pose 144 | - `/vio/trajectory` - Path history 145 | - `/vio/tracking_image` - Feature visualization 146 | - `/vio/depth_image` - Depth heatmap (RGBD only) 147 | 148 | ### Subscribed (Stereo) 149 | - `/cam0/image_raw` - Left camera 150 | - `/cam1/image_raw` - Right camera 151 | - `/imu0` - IMU data 152 | 153 | ### Subscribed (RGBD) 154 | - `/camera/rgb/image_raw` - RGB image 155 | - `/camera/depth/image_raw` - Depth image 156 | 157 | --- 158 | 159 | ## License 160 | 161 | MIT License 162 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vio_ros_wrapper) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Set C++17 standard 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | 12 | # Find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(nav_msgs REQUIRED) 18 | find_package(visualization_msgs REQUIRED) # ⭐ Added for camera frustum 19 | find_package(cv_bridge REQUIRED) 20 | find_package(image_transport REQUIRED) 21 | find_package(message_filters REQUIRED) 22 | find_package(tf2 REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(tf2_geometry_msgs REQUIRED) 25 | 26 | find_package(OpenCV REQUIRED) 27 | find_package(Eigen3 REQUIRED) 28 | 29 | # Force system spdlog to avoid conflicts (same as lidar_odometry_ros_wrapper) 30 | find_package(PkgConfig REQUIRED) 31 | pkg_check_modules(SPDLOG REQUIRED spdlog) 32 | 33 | # Build Ceres from thirdparty (needed for lightweight_vio) 34 | set(LIGHTWEIGHT_VIO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lightweight_vio) 35 | set(THIRDPARTY_DIR ${LIGHTWEIGHT_VIO_DIR}/thirdparty) 36 | set(CERES_DIR ${THIRDPARTY_DIR}/ceres-solver) 37 | set(BUILD_EXAMPLES OFF CACHE BOOL "Build Ceres examples" FORCE) 38 | set(BUILD_BENCHMARKS OFF CACHE BOOL "Build Ceres benchmarks" FORCE) 39 | set(BUILD_TESTING OFF CACHE BOOL "Build Ceres tests" FORCE) 40 | set(PROVIDE_UNINSTALL_TARGET OFF CACHE BOOL "Disable Ceres uninstall target" FORCE) 41 | set(SUITESPARSE OFF CACHE BOOL "Disable SuiteSparse for Ceres" FORCE) 42 | add_subdirectory(${CERES_DIR} ${CMAKE_BINARY_DIR}/ceres-solver EXCLUDE_FROM_ALL) 43 | 44 | # Build Sophus from thirdparty 45 | set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Build Sophus tests" FORCE) 46 | set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Build Sophus examples" FORCE) 47 | add_subdirectory(${THIRDPARTY_DIR}/Sophus ${CMAKE_BINARY_DIR}/Sophus) 48 | 49 | # Build Pangolin from thirdparty 50 | set(BUILD_PANGOLIN_EXAMPLES OFF CACHE BOOL "Build Pangolin examples" FORCE) 51 | set(BUILD_PANGOLIN_TESTS OFF CACHE BOOL "Build Pangolin tests" FORCE) 52 | add_subdirectory(${THIRDPARTY_DIR}/pangolin ${CMAKE_BINARY_DIR}/pangolin EXCLUDE_FROM_ALL) 53 | 54 | # Include directories 55 | include_directories( 56 | include 57 | ${OpenCV_INCLUDE_DIRS} 58 | ${EIGEN3_INCLUDE_DIR} 59 | ${LIGHTWEIGHT_VIO_DIR}/src 60 | ${THIRDPARTY_DIR}/Sophus 61 | ) 62 | 63 | # Collect lightweight_vio source files 64 | set(LIGHTWEIGHT_VIO_SOURCES 65 | ${LIGHTWEIGHT_VIO_DIR}/src/database/Frame.cpp 66 | ${LIGHTWEIGHT_VIO_DIR}/src/database/Feature.cpp 67 | ${LIGHTWEIGHT_VIO_DIR}/src/database/MapPoint.cpp 68 | ${LIGHTWEIGHT_VIO_DIR}/src/processing/FeatureTracker.cpp 69 | ${LIGHTWEIGHT_VIO_DIR}/src/processing/Optimizer.cpp 70 | ${LIGHTWEIGHT_VIO_DIR}/src/processing/Estimator.cpp 71 | ${LIGHTWEIGHT_VIO_DIR}/src/processing/IMUHandler.cpp 72 | ${LIGHTWEIGHT_VIO_DIR}/src/optimization/Factors.cpp 73 | ${LIGHTWEIGHT_VIO_DIR}/src/optimization/Parameters.cpp 74 | ${LIGHTWEIGHT_VIO_DIR}/src/util/Config.cpp 75 | ${LIGHTWEIGHT_VIO_DIR}/src/util/EurocUtils.cpp 76 | ${LIGHTWEIGHT_VIO_DIR}/src/util/TUMUtils.cpp 77 | ${LIGHTWEIGHT_VIO_DIR}/src/util/StringUtils.cpp 78 | ${LIGHTWEIGHT_VIO_DIR}/src/viewer/PangolinViewer.cpp 79 | ) 80 | 81 | # VIO ROS2 Node - Direct linking approach 82 | add_executable(vio_node 83 | src/vio_node.cpp 84 | ${LIGHTWEIGHT_VIO_SOURCES} 85 | ) 86 | 87 | ament_target_dependencies(vio_node 88 | rclcpp 89 | sensor_msgs 90 | geometry_msgs 91 | nav_msgs 92 | visualization_msgs # ⭐ Added for camera frustum 93 | cv_bridge 94 | image_transport 95 | message_filters 96 | tf2 97 | tf2_ros 98 | tf2_geometry_msgs 99 | ) 100 | 101 | target_link_libraries(vio_node 102 | ${OpenCV_LIBRARIES} 103 | ceres 104 | yaml-cpp 105 | ${SPDLOG_LIBRARIES} 106 | Sophus::Sophus 107 | pango_display 108 | pango_windowing 109 | pango_opengl 110 | pango_core 111 | pango_image 112 | ) 113 | 114 | target_include_directories(vio_node PRIVATE 115 | ${SPDLOG_INCLUDE_DIRS} 116 | ) 117 | 118 | if(TARGET Eigen3::Eigen) 119 | target_link_libraries(vio_node Eigen3::Eigen) 120 | else() 121 | target_include_directories(vio_node PRIVATE ${EIGEN3_INCLUDE_DIR}) 122 | endif() 123 | 124 | # Install targets 125 | install(TARGETS 126 | vio_node 127 | DESTINATION lib/${PROJECT_NAME} 128 | ) 129 | 130 | # Install launch files 131 | install(DIRECTORY 132 | launch 133 | DESTINATION share/${PROJECT_NAME} 134 | ) 135 | 136 | # Install rviz config files 137 | install(DIRECTORY 138 | rviz 139 | DESTINATION share/${PROJECT_NAME} 140 | ) 141 | 142 | # Install config files from lightweight_vio 143 | install(DIRECTORY 144 | lightweight_vio/config/ 145 | DESTINATION share/${PROJECT_NAME}/config 146 | FILES_MATCHING PATTERN "*.yaml" 147 | ) 148 | 149 | if(BUILD_TESTING) 150 | find_package(ament_lint_auto REQUIRED) 151 | ament_lint_auto_find_test_dependencies() 152 | endif() 153 | 154 | ament_package() 155 | -------------------------------------------------------------------------------- /rviz/vio.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Odometry1/Topic1 9 | - /Odometry1/Shape1 10 | Splitter Ratio: 0.5 11 | Tree Height: 763 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: PointCloud2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Buffer Length: 10 53 | Class: rviz_default_plugins/Path 54 | Color: 25; 255; 0 55 | Enabled: true 56 | Head Diameter: 0.30000001192092896 57 | Head Length: 0.20000000298023224 58 | Length: 0.30000001192092896 59 | Line Style: Lines 60 | Line Width: 0.029999999329447746 61 | Name: Path 62 | Offset: 63 | X: 0 64 | Y: 0 65 | Z: 0 66 | Pose Color: 255; 85; 255 67 | Pose Style: None 68 | Radius: 0.029999999329447746 69 | Shaft Diameter: 0.10000000149011612 70 | Shaft Length: 0.10000000149011612 71 | Topic: 72 | Depth: 5 73 | Durability Policy: Volatile 74 | Filter size: 10 75 | History Policy: Keep Last 76 | Reliability Policy: Reliable 77 | Value: /vio/trajectory 78 | Value: true 79 | - Angle Tolerance: 0.05000000074505806 80 | Class: rviz_default_plugins/Odometry 81 | Covariance: 82 | Orientation: 83 | Alpha: 0.5 84 | Color: 255; 255; 127 85 | Color Style: Unique 86 | Frame: Local 87 | Offset: 1 88 | Scale: 1 89 | Value: true 90 | Position: 91 | Alpha: 0.30000001192092896 92 | Color: 204; 51; 204 93 | Scale: 1 94 | Value: true 95 | Value: true 96 | Enabled: true 97 | Keep: 1 98 | Name: Odometry 99 | Position Tolerance: 0.05000000074505806 100 | Shape: 101 | Alpha: 1 102 | Axes Length: 0.20000000298023224 103 | Axes Radius: 0.019999999552965164 104 | Color: 255; 25; 0 105 | Head Length: 0.30000001192092896 106 | Head Radius: 0.10000000149011612 107 | Shaft Length: 1 108 | Shaft Radius: 0.05000000074505806 109 | Value: Axes 110 | Topic: 111 | Depth: 5 112 | Durability Policy: Volatile 113 | Filter size: 10 114 | History Policy: Keep Last 115 | Reliability Policy: Reliable 116 | Value: /vio/odometry 117 | Value: true 118 | - Alpha: 1 119 | Autocompute Intensity Bounds: true 120 | Autocompute Value Bounds: 121 | Max Value: 10 122 | Min Value: -10 123 | Value: true 124 | Axis: Z 125 | Channel Name: intensity 126 | Class: rviz_default_plugins/PointCloud2 127 | Color: 239; 41; 41 128 | Color Transformer: FlatColor 129 | Decay Time: 0 130 | Enabled: true 131 | Invert Rainbow: false 132 | Max Color: 255; 255; 255 133 | Max Intensity: 4096 134 | Min Color: 0; 0; 0 135 | Min Intensity: 0 136 | Name: PointCloud2 137 | Position Transformer: XYZ 138 | Selectable: true 139 | Size (Pixels): 10 140 | Size (m): 0.009999999776482582 141 | Style: Points 142 | Topic: 143 | Depth: 5 144 | Durability Policy: Volatile 145 | Filter size: 10 146 | History Policy: Keep Last 147 | Reliability Policy: Reliable 148 | Value: /vio/current_frame_points 149 | Use Fixed Frame: true 150 | Use rainbow: true 151 | Value: true 152 | - Alpha: 1 153 | Autocompute Intensity Bounds: true 154 | Autocompute Value Bounds: 155 | Max Value: 10 156 | Min Value: -10 157 | Value: true 158 | Axis: Z 159 | Channel Name: intensity 160 | Class: rviz_default_plugins/PointCloud2 161 | Color: 255; 255; 255 162 | Color Transformer: FlatColor 163 | Decay Time: 0 164 | Enabled: true 165 | Invert Rainbow: false 166 | Max Color: 255; 255; 255 167 | Max Intensity: 4096 168 | Min Color: 0; 0; 0 169 | Min Intensity: 0 170 | Name: PointCloud2 171 | Position Transformer: XYZ 172 | Selectable: true 173 | Size (Pixels): 5 174 | Size (m): 0.009999999776482582 175 | Style: Points 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | Filter size: 10 180 | History Policy: Keep Last 181 | Reliability Policy: Reliable 182 | Value: /vio/keyframe_points 183 | Use Fixed Frame: true 184 | Use rainbow: true 185 | Value: true 186 | - Class: rviz_default_plugins/Marker 187 | Enabled: true 188 | Name: Marker 189 | Namespaces: 190 | camera_frustum: true 191 | Topic: 192 | Depth: 5 193 | Durability Policy: Volatile 194 | Filter size: 10 195 | History Policy: Keep Last 196 | Reliability Policy: Reliable 197 | Value: /vio/camera_frustum 198 | Value: true 199 | - Class: rviz_default_plugins/Image 200 | Enabled: false 201 | Max Value: 1 202 | Median window: 5 203 | Min Value: 0 204 | Name: Image 205 | Normalize Range: true 206 | Topic: 207 | Depth: 5 208 | Durability Policy: Volatile 209 | History Policy: Keep Last 210 | Reliability Policy: Reliable 211 | Value: /cam0/image_raw 212 | Value: false 213 | - Class: rviz_default_plugins/Image 214 | Enabled: false 215 | Max Value: 1 216 | Median window: 5 217 | Min Value: 0 218 | Name: Image 219 | Normalize Range: true 220 | Topic: 221 | Depth: 5 222 | Durability Policy: Volatile 223 | History Policy: Keep Last 224 | Reliability Policy: Reliable 225 | Value: /cam0/image_raw 226 | Value: false 227 | - Class: rviz_default_plugins/Camera 228 | Enabled: false 229 | Image Rendering: background and overlay 230 | Name: Camera 231 | Overlay Alpha: 0.5 232 | Topic: 233 | Depth: 5 234 | Durability Policy: Volatile 235 | Filter size: 10 236 | History Policy: Keep Last 237 | Reliability Policy: Reliable 238 | Value: /cam1/image_raw/compressed 239 | Value: false 240 | Visibility: 241 | Grid: true 242 | Image: true 243 | Marker: true 244 | Odometry: true 245 | Path: true 246 | PointCloud2: true 247 | Value: true 248 | Zoom Factor: 1 249 | - Class: rviz_default_plugins/Image 250 | Enabled: true 251 | Max Value: 1 252 | Median window: 5 253 | Min Value: 0 254 | Name: Image 255 | Normalize Range: true 256 | Topic: 257 | Depth: 5 258 | Durability Policy: Volatile 259 | History Policy: Keep Last 260 | Reliability Policy: Reliable 261 | Value: /vio/tracking_image 262 | Value: true 263 | Enabled: true 264 | Global Options: 265 | Background Color: 48; 48; 48 266 | Fixed Frame: map 267 | Frame Rate: 30 268 | Name: root 269 | Tools: 270 | - Class: rviz_default_plugins/Interact 271 | Hide Inactive Objects: true 272 | - Class: rviz_default_plugins/MoveCamera 273 | - Class: rviz_default_plugins/Select 274 | - Class: rviz_default_plugins/FocusCamera 275 | - Class: rviz_default_plugins/Measure 276 | Line color: 128; 128; 0 277 | - Class: rviz_default_plugins/SetInitialPose 278 | Covariance x: 0.25 279 | Covariance y: 0.25 280 | Covariance yaw: 0.06853891909122467 281 | Topic: 282 | Depth: 5 283 | Durability Policy: Volatile 284 | History Policy: Keep Last 285 | Reliability Policy: Reliable 286 | Value: /initialpose 287 | - Class: rviz_default_plugins/SetGoal 288 | Topic: 289 | Depth: 5 290 | Durability Policy: Volatile 291 | History Policy: Keep Last 292 | Reliability Policy: Reliable 293 | Value: /goal_pose 294 | - Class: rviz_default_plugins/PublishPoint 295 | Single click: true 296 | Topic: 297 | Depth: 5 298 | Durability Policy: Volatile 299 | History Policy: Keep Last 300 | Reliability Policy: Reliable 301 | Value: /clicked_point 302 | Transformation: 303 | Current: 304 | Class: rviz_default_plugins/TF 305 | Value: true 306 | Views: 307 | Current: 308 | Class: rviz_default_plugins/Orbit 309 | Distance: 6.317116737365723 310 | Enable Stereo Rendering: 311 | Stereo Eye Separation: 0.05999999865889549 312 | Stereo Focal Distance: 1 313 | Swap Stereo Eyes: false 314 | Value: false 315 | Focal Point: 316 | X: 0.4879162609577179 317 | Y: -1.3521316051483154 318 | Z: 0.9803502559661865 319 | Focal Shape Fixed Size: false 320 | Focal Shape Size: 0.05000000074505806 321 | Invert Z Axis: false 322 | Name: Current View 323 | Near Clip Distance: 0.009999999776482582 324 | Pitch: 0.7297970056533813 325 | Target Frame: base_link 326 | Value: Orbit (rviz) 327 | Yaw: 1.612209439277649 328 | Saved: ~ 329 | Window Geometry: 330 | Camera: 331 | collapsed: false 332 | Displays: 333 | collapsed: false 334 | Height: 1802 335 | Hide Left Dock: false 336 | Hide Right Dock: false 337 | Image: 338 | collapsed: false 339 | QMainWindow State: 000000ff00000000fd0000000400000000000001f700000608fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000003f30000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000003c1000001c00000004a00fffffffb0000000a0049006d006100670065000000058d000000e90000004a00fffffffb0000000c00430061006d006500720061000000058e000000e80000004a00fffffffb0000000a0049006d006100670065010000046d000002090000004a00ffffff000000010000038e00000608fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000006080000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000010290000005afc0100000002fb0000000800540069006d00650100000000000010290000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000a8c0000060800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 340 | Selection: 341 | collapsed: false 342 | Time: 343 | collapsed: false 344 | Tool Properties: 345 | collapsed: false 346 | Views: 347 | collapsed: false 348 | Width: 4137 349 | X: 1025 350 | Y: 512 351 | -------------------------------------------------------------------------------- /src/vio_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // lightweight_vio includes 25 | #include "processing/Estimator.h" 26 | #include "ThreadSafeQueue.h" 27 | #include "database/Frame.h" 28 | #include "database/Feature.h" 29 | #include "database/MapPoint.h" 30 | #include "util/Config.h" 31 | 32 | using namespace std::placeholders; 33 | using namespace lightweight_vio; 34 | 35 | // IMU measurement data 36 | struct ImuData { 37 | double timestamp; 38 | double acc_x, acc_y, acc_z; // Linear acceleration (m/s^2) 39 | double gyro_x, gyro_y, gyro_z; // Angular velocity (rad/s) 40 | 41 | ImuData(double t, double ax, double ay, double az, 42 | double gx, double gy, double gz) 43 | : timestamp(t), acc_x(ax), acc_y(ay), acc_z(az), 44 | gyro_x(gx), gyro_y(gy), gyro_z(gz) {} 45 | 46 | // Convert to lightweight_vio::IMUData 47 | IMUData to_vio_imu_data() const { 48 | return IMUData(timestamp, 49 | Eigen::Vector3f(acc_x, acc_y, acc_z), 50 | Eigen::Vector3f(gyro_x, gyro_y, gyro_z)); 51 | } 52 | }; 53 | 54 | // VIO measurement dataset: stereo images OR rgbd + IMU bundle 55 | struct VioDataset { 56 | // Stereo images (for stereo mode) 57 | cv::Mat left_image; 58 | cv::Mat right_image; 59 | 60 | // RGBD images (for rgbd mode) 61 | cv::Mat rgb_image; 62 | cv::Mat depth_image; 63 | 64 | double image_timestamp; 65 | bool is_rgbd; // true for RGBD, false for stereo 66 | 67 | // IMU measurements between previous and current image 68 | std::vector imu_bundle; 69 | 70 | // Metadata 71 | size_t dataset_id; // Sequential ID 72 | size_t imu_count; // Number of IMU measurements 73 | double time_span; // Time duration of IMU bundle 74 | 75 | VioDataset() 76 | : image_timestamp(0.0), is_rgbd(false), dataset_id(0), imu_count(0), time_span(0.0) {} 77 | 78 | // Get statistics 79 | double getAverageImuRate() const { 80 | return (time_span > 0 && imu_count > 0) ? imu_count / time_span : 0.0; 81 | } 82 | 83 | bool isValid() const { 84 | if (is_rgbd) { 85 | return !rgb_image.empty() && !depth_image.empty(); 86 | } else { 87 | return !left_image.empty() && !right_image.empty(); 88 | } 89 | } 90 | 91 | std::string toString() const { 92 | char buffer[512]; 93 | const char* mode_str = is_rgbd ? "RGBD" : "Stereo"; 94 | int img_width = is_rgbd ? rgb_image.cols : left_image.cols; 95 | int img_height = is_rgbd ? rgb_image.rows : left_image.rows; 96 | 97 | if (imu_count > 0) { 98 | double imu_start_time = imu_bundle.front().timestamp; 99 | double imu_end_time = imu_bundle.back().timestamp; 100 | snprintf(buffer, sizeof(buffer), 101 | "Dataset #%zu [%s] | Image: t=%.6f | IMU: count=%zu, start=%.6f, end=%.6f, span=%.4fs (%.1f Hz) | Size=[%dx%d]", 102 | dataset_id, mode_str, image_timestamp, imu_count, 103 | imu_start_time, imu_end_time, time_span, getAverageImuRate(), 104 | img_width, img_height); 105 | } else { 106 | snprintf(buffer, sizeof(buffer), 107 | "Dataset #%zu [%s] | Image: t=%.6f | IMU: count=0 (NO IMU DATA) | Size=[%dx%d]", 108 | dataset_id, mode_str, image_timestamp, 109 | img_width, img_height); 110 | } 111 | return std::string(buffer); 112 | } 113 | }; 114 | 115 | // ⭐ VIO processing result for publisher thread 116 | struct VIOProcessingResult { 117 | bool success; 118 | Eigen::Matrix4f Twb; // Body to world transform 119 | double timestamp; 120 | int num_features; 121 | int num_inliers; 122 | std::shared_ptr current_frame; 123 | 124 | VIOProcessingResult() : success(false), num_features(0), num_inliers(0) { 125 | Twb = Eigen::Matrix4f::Identity(); 126 | } 127 | }; 128 | 129 | class VIONode : public rclcpp::Node 130 | { 131 | public: 132 | VIONode() : Node("vio_node"), 133 | stereo_count_(0), 134 | rgbd_count_(0), 135 | imu_count_(0), 136 | left_count_(0), 137 | right_count_(0), 138 | imu_pivot_index_(0), 139 | last_image_time_(-1.0), 140 | dataset_id_(0), 141 | is_rgbd_mode_(false), 142 | running_(true) // ⭐ Thread running flag 143 | { 144 | // Declare parameters 145 | this->declare_parameter("left_image_topic", "/cam0/image_raw"); 146 | this->declare_parameter("right_image_topic", "/cam1/image_raw"); 147 | this->declare_parameter("rgb_image_topic", "/camera/color/image_raw"); 148 | this->declare_parameter("depth_image_topic", "/camera/depth/image_rect_raw"); 149 | this->declare_parameter("imu_topic", "/imu0"); 150 | this->declare_parameter("queue_size", 10); 151 | this->declare_parameter("imu_time_tolerance", 0.005); // 5ms 152 | this->declare_parameter("config_file", ""); 153 | 154 | // Get parameters 155 | std::string left_topic = this->get_parameter("left_image_topic").as_string(); 156 | std::string right_topic = this->get_parameter("right_image_topic").as_string(); 157 | std::string rgb_topic = this->get_parameter("rgb_image_topic").as_string(); 158 | std::string depth_topic = this->get_parameter("depth_image_topic").as_string(); 159 | std::string imu_topic = this->get_parameter("imu_topic").as_string(); 160 | int queue_size = this->get_parameter("queue_size").as_int(); 161 | imu_time_tolerance_ = this->get_parameter("imu_time_tolerance").as_double(); 162 | std::string config_path = this->get_parameter("config_file").as_string(); 163 | 164 | RCLCPP_INFO(this->get_logger(), "Starting VIO Node"); 165 | RCLCPP_INFO(this->get_logger(), "IMU topic: %s", imu_topic.c_str()); 166 | RCLCPP_INFO(this->get_logger(), "IMU time tolerance: %.1f ms", imu_time_tolerance_ * 1000); 167 | RCLCPP_INFO(this->get_logger(), "Config file: %s", config_path.c_str()); 168 | 169 | // Initialize lightweight_vio estimator 170 | try { 171 | if (config_path.empty()) { 172 | throw std::runtime_error("Config file path not specified! Please set 'config_file' parameter."); 173 | } 174 | 175 | if (!Config::getInstance().load(config_path)) { 176 | throw std::runtime_error("Failed to load config file: " + config_path); 177 | } 178 | 179 | // ⭐ Detect camera type from config 180 | is_rgbd_mode_ = Config::getInstance().is_rgbd(); 181 | 182 | if (is_rgbd_mode_) { 183 | RCLCPP_INFO(this->get_logger(), "🎥 RGBD Mode Enabled"); 184 | RCLCPP_INFO(this->get_logger(), "RGB image topic: %s", rgb_topic.c_str()); 185 | RCLCPP_INFO(this->get_logger(), "Depth image topic: %s", depth_topic.c_str()); 186 | } else { 187 | RCLCPP_INFO(this->get_logger(), "🎥 Stereo Mode Enabled"); 188 | RCLCPP_INFO(this->get_logger(), "Left image topic: %s", left_topic.c_str()); 189 | RCLCPP_INFO(this->get_logger(), "Right image topic: %s", right_topic.c_str()); 190 | } 191 | 192 | estimator_ = std::make_shared(); 193 | RCLCPP_INFO(this->get_logger(), "VIO Estimator initialized"); 194 | } catch (const std::exception& e) { 195 | RCLCPP_ERROR(this->get_logger(), "Failed to initialize VIO estimator: %s", e.what()); 196 | throw; 197 | } 198 | 199 | // Create subscribers based on camera type 200 | if (is_rgbd_mode_) { 201 | // RGBD mode: subscribe to RGB + Depth 202 | rgb_image_sub_.subscribe(this, rgb_topic); 203 | depth_image_sub_.subscribe(this, depth_topic); 204 | 205 | // Synchronize RGB + Depth images 206 | rgbd_sync_ = std::make_shared>>( 210 | message_filters::sync_policies::ApproximateTime< 211 | sensor_msgs::msg::Image, 212 | sensor_msgs::msg::Image>(queue_size), 213 | rgb_image_sub_, depth_image_sub_); 214 | 215 | rgbd_sync_->registerCallback(&VIONode::rgbdImageCallback, this); 216 | } else { 217 | // Stereo mode: subscribe to Left + Right 218 | left_image_sub_.subscribe(this, left_topic); 219 | right_image_sub_.subscribe(this, right_topic); 220 | 221 | // Synchronize stereo images 222 | sync_ = std::make_shared>>( 226 | message_filters::sync_policies::ApproximateTime< 227 | sensor_msgs::msg::Image, 228 | sensor_msgs::msg::Image>(queue_size), 229 | left_image_sub_, right_image_sub_); 230 | 231 | sync_->registerCallback(&VIONode::stereoImageCallback, this); 232 | } 233 | 234 | // IMU subscriber 235 | imu_sub_ = this->create_subscription( 236 | imu_topic, queue_size, 237 | [this](const sensor_msgs::msg::Imu::ConstSharedPtr msg) { 238 | this->imuCallback(msg); 239 | }); 240 | 241 | // Odometry publisher 242 | odom_pub_ = this->create_publisher( 243 | "/vio/odometry", 10); 244 | 245 | // Pose publisher 246 | pose_pub_ = this->create_publisher( 247 | "/vio/pose", 10); 248 | 249 | // Map point cloud publishers 250 | current_frame_points_pub_ = this->create_publisher( 251 | "/vio/current_frame_points", 10); 252 | 253 | keyframe_points_pub_ = this->create_publisher( 254 | "/vio/keyframe_points", 10); 255 | 256 | // Trajectory publisher 257 | trajectory_pub_ = this->create_publisher( 258 | "/vio/trajectory", 10); 259 | 260 | // Camera frustum publisher 261 | camera_frustum_pub_ = this->create_publisher( 262 | "/vio/camera_frustum", 10); 263 | 264 | // Tracking image publisher 265 | tracking_image_pub_ = this->create_publisher( 266 | "/vio/tracking_image", 10); 267 | 268 | // Dense point cloud publisher (RGBD only) 269 | dense_cloud_pub_ = this->create_publisher( 270 | "/vio/dense_cloud", 10); 271 | 272 | // Depth image publisher (RGBD only) 273 | depth_image_pub_ = this->create_publisher( 274 | "/vio/depth_image", 10); 275 | 276 | // TF broadcaster 277 | tf_broadcaster_ = std::make_shared(this); 278 | 279 | // ⭐ Start worker threads (Producer-Consumer pattern) 280 | processing_thread_ = std::thread(&VIONode::processingThreadLoop, this); 281 | publisher_thread_ = std::thread(&VIONode::publisherThreadLoop, this); 282 | 283 | RCLCPP_INFO(this->get_logger(), "VIO Node initialized successfully"); 284 | RCLCPP_INFO(this->get_logger(), "✅ Processing & Publisher threads started"); 285 | } 286 | 287 | // ⭐ Destructor: shutdown threads gracefully 288 | ~VIONode() { 289 | RCLCPP_INFO(this->get_logger(), "Shutting down VIO Node..."); 290 | 291 | // Stop threads 292 | running_ = false; 293 | image_queue_.shutdown(); 294 | result_queue_.shutdown(); 295 | 296 | // Wait for threads to finish 297 | if (processing_thread_.joinable()) { 298 | processing_thread_.join(); 299 | RCLCPP_INFO(this->get_logger(), "Processing thread stopped"); 300 | } 301 | 302 | if (publisher_thread_.joinable()) { 303 | publisher_thread_.join(); 304 | RCLCPP_INFO(this->get_logger(), "Publisher thread stopped"); 305 | } 306 | 307 | RCLCPP_INFO(this->get_logger(), "VIO Node shutdown complete"); 308 | } 309 | 310 | private: 311 | // ⭐ LIGHTWEIGHT Callback: 데이터만 queue에 넣고 즉시 return (Stereo) 312 | void stereoImageCallback( 313 | const sensor_msgs::msg::Image::ConstSharedPtr& left_msg, 314 | const sensor_msgs::msg::Image::ConstSharedPtr& right_msg) 315 | { 316 | try { 317 | stereo_count_++; 318 | 319 | // Convert ROS images to OpenCV format (빠름) 320 | cv_bridge::CvImageConstPtr left_ptr = cv_bridge::toCvShare(left_msg, "mono8"); 321 | cv_bridge::CvImageConstPtr right_ptr = cv_bridge::toCvShare(right_msg, "mono8"); 322 | 323 | cv::Mat left_image = left_ptr->image.clone(); // Clone for thread safety 324 | cv::Mat right_image = right_ptr->image.clone(); 325 | 326 | double current_image_time = left_msg->header.stamp.sec + 327 | left_msg->header.stamp.nanosec * 1e-9; 328 | 329 | // Store first and last timestamps 330 | if (stereo_count_ == 1) { 331 | first_stereo_time_ = current_image_time; 332 | last_image_time_ = current_image_time; 333 | RCLCPP_INFO(this->get_logger(), 334 | "First stereo image received at %.6f - initializing VIO", 335 | current_image_time); 336 | } 337 | last_stereo_time_ = current_image_time; 338 | 339 | // Build VIO dataset (빠름 - IMU data copy만) 340 | VioDataset dataset = buildVioDataset(left_image, right_image, 341 | current_image_time); 342 | 343 | // ⭐ Queue에 push만 하고 바로 return (NO BLOCKING!) 344 | image_queue_.push(dataset); 345 | 346 | size_t queue_size = image_queue_.size(); 347 | if (queue_size > 10) { // Warn if queue is growing 348 | RCLCPP_WARN(this->get_logger(), 349 | "⚠️ Image queue size: %zu (processing may be slower than incoming data)", 350 | queue_size); 351 | } 352 | 353 | last_image_time_ = current_image_time; 354 | 355 | } catch (cv_bridge::Exception& e) { 356 | RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); 357 | } 358 | } 359 | 360 | // ⭐ RGBD Callback: RGB + Depth 동기화 361 | void rgbdImageCallback( 362 | const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg, 363 | const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) 364 | { 365 | try { 366 | rgbd_count_++; 367 | 368 | // Convert ROS images to OpenCV format 369 | cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg, "bgr8"); 370 | cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1); 371 | 372 | cv::Mat rgb_image = rgb_ptr->image.clone(); // Clone for thread safety 373 | cv::Mat depth_image = depth_ptr->image.clone(); 374 | 375 | // Convert depth from uint16 (mm) to float32 (m) 376 | cv::Mat depth_float; 377 | depth_image.convertTo(depth_float, CV_32FC1, 0.001); // mm to m 378 | 379 | double current_image_time = rgb_msg->header.stamp.sec + 380 | rgb_msg->header.stamp.nanosec * 1e-9; 381 | 382 | // Store first and last timestamps 383 | if (rgbd_count_ == 1) { 384 | first_rgbd_time_ = current_image_time; 385 | last_image_time_ = current_image_time; 386 | RCLCPP_INFO(this->get_logger(), 387 | "First RGBD image received at %.6f - initializing VIO", 388 | current_image_time); 389 | } 390 | last_rgbd_time_ = current_image_time; 391 | 392 | // Build RGBD dataset 393 | VioDataset dataset = buildRGBDDataset(rgb_image, depth_float, current_image_time); 394 | 395 | // ⭐ Queue에 push만 하고 바로 return (NO BLOCKING!) 396 | image_queue_.push(dataset); 397 | 398 | size_t queue_size = image_queue_.size(); 399 | if (queue_size > 10) { 400 | RCLCPP_WARN(this->get_logger(), 401 | "⚠️ Image queue size: %zu (processing may be slower than incoming data)", 402 | queue_size); 403 | } 404 | 405 | last_image_time_ = current_image_time; 406 | 407 | } catch (cv_bridge::Exception& e) { 408 | RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); 409 | } 410 | } 411 | 412 | void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr& msg) 413 | { 414 | imu_count_++; 415 | 416 | double timestamp = msg->header.stamp.sec + 417 | msg->header.stamp.nanosec * 1e-9; 418 | 419 | // Store first and last timestamps 420 | if (imu_count_ == 1) { 421 | first_imu_time_ = timestamp; 422 | } 423 | last_imu_time_ = timestamp; 424 | 425 | // Extract IMU data and add to buffer 426 | ImuData imu_data( 427 | timestamp, 428 | msg->linear_acceleration.x, 429 | msg->linear_acceleration.y, 430 | msg->linear_acceleration.z, 431 | msg->angular_velocity.x, 432 | msg->angular_velocity.y, 433 | msg->angular_velocity.z 434 | ); 435 | 436 | imu_buffer_.push_back(imu_data); 437 | 438 | RCLCPP_DEBUG(this->get_logger(), 439 | "IMU #%ld at %.6f - Buffer size: %zu", 440 | imu_count_, timestamp, imu_buffer_.size()); 441 | } 442 | 443 | VioDataset buildVioDataset(const cv::Mat& left_image, 444 | const cv::Mat& right_image, 445 | double current_image_time) 446 | { 447 | VioDataset dataset; 448 | dataset.dataset_id = ++dataset_id_; 449 | dataset.is_rgbd = false; 450 | dataset.left_image = left_image.clone(); 451 | dataset.right_image = right_image.clone(); 452 | dataset.image_timestamp = current_image_time; 453 | 454 | // Extract IMU measurements 455 | extractIMUData(dataset, current_image_time); 456 | 457 | return dataset; 458 | } 459 | 460 | VioDataset buildRGBDDataset(const cv::Mat& rgb_image, 461 | const cv::Mat& depth_image, 462 | double current_image_time) 463 | { 464 | VioDataset dataset; 465 | dataset.dataset_id = ++dataset_id_; 466 | dataset.is_rgbd = true; 467 | dataset.rgb_image = rgb_image.clone(); 468 | dataset.depth_image = depth_image.clone(); 469 | dataset.image_timestamp = current_image_time; 470 | 471 | // Extract IMU measurements 472 | extractIMUData(dataset, current_image_time); 473 | 474 | return dataset; 475 | } 476 | 477 | void extractIMUData(VioDataset& dataset, double current_image_time) 478 | { 479 | // Extract IMU measurements between last_image_time and current_image_time 480 | size_t start_idx = imu_pivot_index_; 481 | 482 | for (size_t i = start_idx; i < imu_buffer_.size(); i++) { 483 | double imu_time = imu_buffer_[i].timestamp; 484 | 485 | // Skip IMU data before last image time (with tolerance) 486 | if (imu_time < last_image_time_ - imu_time_tolerance_) { 487 | continue; 488 | } 489 | 490 | // Include IMU data up to current image time + tolerance 491 | if (imu_time <= current_image_time + imu_time_tolerance_) { 492 | dataset.imu_bundle.push_back(imu_buffer_[i]); 493 | imu_pivot_index_ = i + 1; // Move pivot forward 494 | } else { 495 | // IMU data is beyond current image time 496 | break; 497 | } 498 | } 499 | 500 | // Calculate metadata 501 | dataset.imu_count = dataset.imu_bundle.size(); 502 | if (dataset.imu_count > 0) { 503 | dataset.time_span = dataset.imu_bundle.back().timestamp - 504 | dataset.imu_bundle.front().timestamp; 505 | } 506 | } 507 | 508 | // ⭐ Processing Thread: VIO 처리 (무거운 작업, 여기서 blocking 해도 callback 안 막힘!) 509 | void processingThreadLoop() { 510 | RCLCPP_INFO(this->get_logger(), "🔧 Processing thread started"); 511 | 512 | size_t processed_count = 0; 513 | 514 | while (running_) { 515 | // Queue에서 dataset pop (blocking) 516 | auto dataset_opt = image_queue_.pop(); 517 | 518 | if (!dataset_opt.has_value()) { 519 | // Queue shutdown signal 520 | break; 521 | } 522 | 523 | VioDataset dataset = dataset_opt.value(); 524 | processed_count++; 525 | 526 | // Process VIO dataset 527 | VIOProcessingResult result; 528 | result.timestamp = dataset.image_timestamp; 529 | 530 | if (!estimator_) { 531 | RCLCPP_ERROR(this->get_logger(), "Estimator not initialized!"); 532 | continue; 533 | } 534 | 535 | try { 536 | // Convert timestamp to nanoseconds 537 | long long timestamp_ns = static_cast(dataset.image_timestamp * 1e9); 538 | 539 | // Convert IMU data 540 | std::vector vio_imu_data; 541 | vio_imu_data.reserve(dataset.imu_bundle.size()); 542 | for (const auto& imu : dataset.imu_bundle) { 543 | vio_imu_data.push_back(imu.to_vio_imu_data()); 544 | } 545 | 546 | // Process frame based on mode 547 | Estimator::EstimationResult est_result; 548 | 549 | if (dataset.is_rgbd) { 550 | // ⭐ RGBD mode 551 | // Preprocess RGB image (histogram equalization) 552 | cv::Mat processed_rgb; 553 | if (dataset.rgb_image.channels() == 3) { 554 | cv::Mat gray; 555 | cv::cvtColor(dataset.rgb_image, gray, cv::COLOR_BGR2GRAY); 556 | cv::equalizeHist(gray, processed_rgb); 557 | } else { 558 | cv::equalizeHist(dataset.rgb_image, processed_rgb); 559 | } 560 | 561 | // Process RGBD frame (VO mode - no IMU support yet for RGBD) 562 | est_result = estimator_->process_rgbd_frame( 563 | processed_rgb, 564 | dataset.depth_image, 565 | timestamp_ns 566 | ); 567 | } else { 568 | // ⭐ Stereo mode 569 | // Preprocess images (histogram equalization) 570 | cv::Mat processed_left, processed_right; 571 | cv::equalizeHist(dataset.left_image, processed_left); 572 | cv::equalizeHist(dataset.right_image, processed_right); 573 | 574 | if (processed_count == 1) { 575 | // First frame: VO mode (no IMU) 576 | est_result = estimator_->process_frame( 577 | processed_left, 578 | processed_right, 579 | timestamp_ns 580 | ); 581 | } else { 582 | // Subsequent frames: VIO mode with IMU data 583 | est_result = estimator_->process_frame( 584 | processed_left, 585 | processed_right, 586 | timestamp_ns, 587 | vio_imu_data 588 | ); 589 | } 590 | } 591 | 592 | if (est_result.success) { 593 | result.success = true; 594 | result.Twb = estimator_->get_current_pose(); 595 | result.num_features = est_result.num_features; 596 | result.num_inliers = est_result.num_inliers; 597 | result.current_frame = estimator_->get_current_frame(); 598 | 599 | // ⭐ Result queue에 push (publisher thread가 처리) 600 | result_queue_.push(result); 601 | 602 | // RCLCPP_INFO(this->get_logger(), 603 | // "✅ VIO #%zu: features=%d, inliers=%d, opt_time=%.2fms | Queues: img=%zu, result=%zu", 604 | // processed_count, est_result.num_features, est_result.num_inliers, 605 | // est_result.optimization_time_ms, image_queue_.size(), result_queue_.size()); 606 | } else { 607 | RCLCPP_WARN(this->get_logger(), 608 | "❌ VIO estimation failed: features=%d, inliers=%d", 609 | est_result.num_features, est_result.num_inliers); 610 | } 611 | 612 | } catch (const std::exception& e) { 613 | RCLCPP_ERROR(this->get_logger(), 614 | "Exception in VIO processing: %s", e.what()); 615 | } 616 | } 617 | 618 | RCLCPP_INFO(this->get_logger(), 619 | "🛑 Processing thread stopped (processed %zu frames)", 620 | processed_count); 621 | } 622 | 623 | // ⭐ Publisher Thread: ROS topic publish (분리된 thread에서 독립 실행) 624 | void publisherThreadLoop() { 625 | RCLCPP_INFO(this->get_logger(), "📡 Publisher thread started"); 626 | 627 | size_t published_count = 0; 628 | 629 | while (running_) { 630 | // Result queue에서 pop (blocking) 631 | auto result_opt = result_queue_.pop(); 632 | 633 | if (!result_opt.has_value()) { 634 | // Queue shutdown signal 635 | break; 636 | } 637 | 638 | VIOProcessingResult result = result_opt.value(); 639 | 640 | if (result.success) { 641 | // Publish odometry & TF 642 | publishOdometry(result.Twb, result.timestamp); 643 | 644 | // Publish visualization 645 | publishVisualization(result.timestamp); 646 | 647 | published_count++; 648 | 649 | RCLCPP_DEBUG(this->get_logger(), 650 | "📤 Published frame #%zu", published_count); 651 | } 652 | } 653 | 654 | RCLCPP_INFO(this->get_logger(), 655 | "🛑 Publisher thread stopped (published %zu frames)", 656 | published_count); 657 | } 658 | 659 | void publishOdometry(const Eigen::Matrix4f& Twb, double timestamp) 660 | { 661 | // Twb: body to world transform 662 | // Extract rotation and translation 663 | Eigen::Matrix3f R = Twb.block<3, 3>(0, 0); 664 | Eigen::Vector3f t = Twb.block<3, 1>(0, 3); 665 | 666 | // Convert rotation matrix to quaternion 667 | Eigen::Quaternionf q(R); 668 | q.normalize(); 669 | 670 | // Create ROS timestamp 671 | rclcpp::Time ros_time(static_cast(timestamp * 1e9)); 672 | 673 | // Publish Odometry 674 | auto odom_msg = nav_msgs::msg::Odometry(); 675 | odom_msg.header.stamp = ros_time; 676 | odom_msg.header.frame_id = "map"; // Fixed frame for rviz 677 | odom_msg.child_frame_id = "base_link"; 678 | 679 | odom_msg.pose.pose.position.x = t.x(); 680 | odom_msg.pose.pose.position.y = t.y(); 681 | odom_msg.pose.pose.position.z = t.z(); 682 | 683 | odom_msg.pose.pose.orientation.x = q.x(); 684 | odom_msg.pose.pose.orientation.y = q.y(); 685 | odom_msg.pose.pose.orientation.z = q.z(); 686 | odom_msg.pose.pose.orientation.w = q.w(); 687 | 688 | odom_pub_->publish(odom_msg); 689 | 690 | // Publish PoseStamped 691 | auto pose_msg = geometry_msgs::msg::PoseStamped(); 692 | pose_msg.header.stamp = ros_time; 693 | pose_msg.header.frame_id = "map"; // Fixed frame for rviz 694 | 695 | pose_msg.pose.position.x = t.x(); 696 | pose_msg.pose.position.y = t.y(); 697 | pose_msg.pose.position.z = t.z(); 698 | 699 | pose_msg.pose.orientation.x = q.x(); 700 | pose_msg.pose.orientation.y = q.y(); 701 | pose_msg.pose.orientation.z = q.z(); 702 | pose_msg.pose.orientation.w = q.w(); 703 | 704 | pose_pub_->publish(pose_msg); 705 | 706 | // Broadcast TF: map -> base_link 707 | geometry_msgs::msg::TransformStamped transform; 708 | transform.header.stamp = ros_time; 709 | transform.header.frame_id = "map"; 710 | transform.child_frame_id = "base_link"; 711 | 712 | transform.transform.translation.x = t.x(); 713 | transform.transform.translation.y = t.y(); 714 | transform.transform.translation.z = t.z(); 715 | 716 | transform.transform.rotation.x = q.x(); 717 | transform.transform.rotation.y = q.y(); 718 | transform.transform.rotation.z = q.z(); 719 | transform.transform.rotation.w = q.w(); 720 | 721 | tf_broadcaster_->sendTransform(transform); 722 | 723 | // Add to trajectory (최근 5개 키프레임만 유지) 724 | trajectory_.poses.push_back(pose_msg); 725 | 726 | // ⭐ 최근 5개만 유지 (오래된 pose 제거) 727 | const size_t max_trajectory_size = 5; 728 | if (trajectory_.poses.size() > max_trajectory_size) { 729 | trajectory_.poses.erase( 730 | trajectory_.poses.begin(), 731 | trajectory_.poses.begin() + (trajectory_.poses.size() - max_trajectory_size) 732 | ); 733 | } 734 | 735 | trajectory_.header.stamp = ros_time; 736 | trajectory_.header.frame_id = "map"; 737 | } 738 | 739 | void publishVisualization(double timestamp) 740 | { 741 | rclcpp::Time ros_time(static_cast(timestamp * 1e9)); 742 | 743 | // Get current frame 744 | auto current_frame = estimator_->get_current_frame(); 745 | if (!current_frame) return; 746 | 747 | // Publish current frame map points (GREEN) 748 | publishCurrentFramePoints(current_frame, ros_time); 749 | 750 | // Publish keyframe map points (RED) 751 | publishKeyframePoints(ros_time); 752 | 753 | // Publish camera frustum visualization 754 | publishCameraFrustum(current_frame, ros_time); 755 | 756 | // Publish tracking image with features 757 | publishTrackingImage(current_frame, ros_time); 758 | 759 | // ⭐ RGBD-specific publications 760 | if (is_rgbd_mode_ && current_frame && current_frame->is_rgbd()) { 761 | // Dense cloud is too slow - disabled 762 | // publishDenseCloud(current_frame, ros_time); 763 | 764 | // Only publish depth image (fast) 765 | publishDepthImage(current_frame, ros_time); 766 | } 767 | 768 | // Publish trajectory 769 | trajectory_pub_->publish(trajectory_); 770 | } 771 | 772 | void publishCurrentFramePoints(std::shared_ptr frame, const rclcpp::Time& timestamp) 773 | { 774 | if (!frame) return; 775 | 776 | const auto& map_points = frame->get_map_points(); 777 | 778 | // Count valid map points 779 | size_t valid_count = 0; 780 | for (const auto& mp : map_points) { 781 | if (mp && !mp->is_bad()) { 782 | valid_count++; 783 | } 784 | } 785 | 786 | if (valid_count == 0) return; 787 | 788 | // Create PointCloud2 message 789 | sensor_msgs::msg::PointCloud2 cloud_msg; 790 | cloud_msg.header.stamp = timestamp; 791 | cloud_msg.header.frame_id = "map"; 792 | cloud_msg.height = 1; 793 | cloud_msg.width = valid_count; 794 | cloud_msg.is_dense = false; 795 | cloud_msg.is_bigendian = false; 796 | 797 | // Setup fields: x, y, z, rgb 798 | sensor_msgs::PointCloud2Modifier modifier(cloud_msg); 799 | modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); 800 | modifier.resize(valid_count); 801 | 802 | // Create iterators 803 | sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); 804 | sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); 805 | sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); 806 | sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); 807 | sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); 808 | sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); 809 | 810 | // Fill point cloud (GREEN for current frame) 811 | for (const auto& mp : map_points) { 812 | if (!mp || mp->is_bad()) continue; 813 | 814 | Eigen::Vector3f pos = mp->get_position(); 815 | *iter_x = pos.x(); 816 | *iter_y = pos.y(); 817 | *iter_z = pos.z(); 818 | *iter_r = 0; 819 | *iter_g = 255; // Green 820 | *iter_b = 0; 821 | 822 | ++iter_x; ++iter_y; ++iter_z; 823 | ++iter_r; ++iter_g; ++iter_b; 824 | } 825 | 826 | current_frame_points_pub_->publish(cloud_msg); 827 | } 828 | 829 | void publishKeyframePoints(const rclcpp::Time& timestamp) 830 | { 831 | // Get all keyframes (use get_keyframes_safe instead of get_all_keyframes) 832 | auto keyframes = estimator_->get_keyframes_safe(); 833 | 834 | // Collect all unique map points from keyframes 835 | std::set> unique_map_points; 836 | for (const auto& kf : keyframes) { 837 | if (!kf) continue; 838 | const auto& map_points = kf->get_map_points(); 839 | for (const auto& mp : map_points) { 840 | if (mp && !mp->is_bad()) { 841 | unique_map_points.insert(mp); 842 | } 843 | } 844 | } 845 | 846 | if (unique_map_points.empty()) return; 847 | 848 | // Create PointCloud2 message 849 | sensor_msgs::msg::PointCloud2 cloud_msg; 850 | cloud_msg.header.stamp = timestamp; 851 | cloud_msg.header.frame_id = "map"; 852 | cloud_msg.height = 1; 853 | cloud_msg.width = unique_map_points.size(); 854 | cloud_msg.is_dense = false; 855 | cloud_msg.is_bigendian = false; 856 | 857 | // Setup fields: x, y, z, rgb 858 | sensor_msgs::PointCloud2Modifier modifier(cloud_msg); 859 | modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); 860 | modifier.resize(unique_map_points.size()); 861 | 862 | // Create iterators 863 | sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); 864 | sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); 865 | sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); 866 | sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); 867 | sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); 868 | sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); 869 | 870 | // Fill point cloud (RED for keyframes) 871 | for (const auto& mp : unique_map_points) { 872 | Eigen::Vector3f pos = mp->get_position(); 873 | *iter_x = pos.x(); 874 | *iter_y = pos.y(); 875 | *iter_z = pos.z(); 876 | *iter_r = 255; // Red 877 | *iter_g = 0; 878 | *iter_b = 0; 879 | 880 | ++iter_x; ++iter_y; ++iter_z; 881 | ++iter_r; ++iter_g; ++iter_b; 882 | } 883 | 884 | keyframe_points_pub_->publish(cloud_msg); 885 | } 886 | 887 | // ⭐ Tracking image visualization - draw features on camera image 888 | void publishTrackingImage(std::shared_ptr frame, const rclcpp::Time& timestamp) 889 | { 890 | if (!frame) return; 891 | 892 | // Get left image 893 | const cv::Mat& raw_image = frame->get_left_image(); 894 | if (raw_image.empty()) return; 895 | 896 | // Convert to BGR for color drawing 897 | cv::Mat display_image; 898 | if (raw_image.channels() == 1) { 899 | cv::cvtColor(raw_image, display_image, cv::COLOR_GRAY2BGR); 900 | } else { 901 | display_image = raw_image.clone(); 902 | } 903 | 904 | // Get features and map points 905 | const auto& features = frame->get_features(); 906 | const auto& map_points = frame->get_map_points(); 907 | 908 | // Draw features with different colors 909 | for (size_t i = 0; i < features.size(); ++i) { 910 | const auto& feature = features[i]; 911 | if (!feature || !feature->is_valid()) continue; 912 | 913 | const cv::Point2f& pt = feature->get_pixel_coord(); 914 | 915 | // Determine color based on tracking status 916 | cv::Scalar color; 917 | if (frame->get_outlier_flag(i)) { 918 | // Gray for outliers 919 | color = cv::Scalar(128, 128, 128); 920 | } else if (i < map_points.size() && map_points[i] && !map_points[i]->is_bad()) { 921 | // Green for successfully tracked features (have valid map point) 922 | color = cv::Scalar(0, 255, 0); 923 | } else { 924 | // Red for new features (no map point yet) 925 | color = cv::Scalar(0, 0, 255); 926 | } 927 | 928 | // Draw circle at feature location 929 | cv::circle(display_image, pt, 3, color, -1); 930 | } 931 | 932 | // Convert to ROS message using cv_bridge 933 | std_msgs::msg::Header header; 934 | header.stamp = timestamp; 935 | header.frame_id = "camera"; 936 | 937 | auto msg = cv_bridge::CvImage(header, "bgr8", display_image).toImageMsg(); 938 | tracking_image_pub_->publish(*msg); 939 | } 940 | 941 | // ⭐ RGBD Dense Point Cloud Publisher 942 | void publishDenseCloud(std::shared_ptr frame, const rclcpp::Time& timestamp) 943 | { 944 | if (!frame || !frame->is_rgbd()) { 945 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, 946 | "Dense cloud: frame is null or not RGBD"); 947 | return; 948 | } 949 | 950 | // ⭐ Safety check: ensure depth map and RGB image are valid 951 | const cv::Mat& depth_map = frame->get_depth_map(); 952 | const cv::Mat& rgb_image = frame->get_rgb_image(); 953 | 954 | if (depth_map.empty() || rgb_image.empty()) { 955 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, 956 | "Dense cloud: depth_map empty=%d, rgb_image empty=%d", 957 | depth_map.empty(), rgb_image.empty()); 958 | return; 959 | } 960 | 961 | // Get parameters from config 962 | const Config& config = Config::getInstance(); 963 | if (!config.m_rgbd_enable_dense_cloud) { 964 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, 965 | "Dense cloud: disabled in config"); 966 | return; 967 | } 968 | 969 | int stride = config.m_rgbd_dense_cloud_stride; 970 | float min_depth = config.m_min_depth; 971 | float max_depth = config.m_max_depth; 972 | 973 | // Generate colored point cloud (RGB mode) with try-catch for safety 974 | std::vector colored_points; 975 | try { 976 | colored_points = frame->generate_colored_point_cloud(stride, min_depth, max_depth, 1); 977 | } catch (const std::exception& e) { 978 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, 979 | "Failed to generate dense cloud: %s", e.what()); 980 | return; 981 | } 982 | 983 | if (colored_points.empty()) { 984 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, 985 | "Dense cloud is empty - no valid depth points"); 986 | return; 987 | } 988 | 989 | RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, 990 | "Publishing dense cloud with %zu points", colored_points.size()); 991 | 992 | // Create PointCloud2 message manually 993 | sensor_msgs::msg::PointCloud2 cloud_msg; 994 | cloud_msg.header.stamp = timestamp; 995 | cloud_msg.header.frame_id = "map"; 996 | cloud_msg.height = 1; 997 | cloud_msg.width = colored_points.size(); 998 | cloud_msg.is_bigendian = false; 999 | cloud_msg.is_dense = false; 1000 | 1001 | // Define fields manually: x, y, z, rgb 1002 | sensor_msgs::msg::PointField field_x, field_y, field_z, field_rgb; 1003 | 1004 | field_x.name = "x"; 1005 | field_x.offset = 0; 1006 | field_x.datatype = sensor_msgs::msg::PointField::FLOAT32; 1007 | field_x.count = 1; 1008 | 1009 | field_y.name = "y"; 1010 | field_y.offset = 4; 1011 | field_y.datatype = sensor_msgs::msg::PointField::FLOAT32; 1012 | field_y.count = 1; 1013 | 1014 | field_z.name = "z"; 1015 | field_z.offset = 8; 1016 | field_z.datatype = sensor_msgs::msg::PointField::FLOAT32; 1017 | field_z.count = 1; 1018 | 1019 | field_rgb.name = "rgb"; 1020 | field_rgb.offset = 12; 1021 | field_rgb.datatype = sensor_msgs::msg::PointField::FLOAT32; 1022 | field_rgb.count = 1; 1023 | 1024 | cloud_msg.fields = {field_x, field_y, field_z, field_rgb}; 1025 | cloud_msg.point_step = 16; // 4 floats * 4 bytes 1026 | cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width; 1027 | cloud_msg.data.resize(cloud_msg.row_step * cloud_msg.height); 1028 | 1029 | // Fill point cloud data 1030 | uint8_t* data_ptr = cloud_msg.data.data(); 1031 | for (const auto& cp : colored_points) { 1032 | // x, y, z 1033 | memcpy(data_ptr + 0, &cp.position.x(), sizeof(float)); 1034 | memcpy(data_ptr + 4, &cp.position.y(), sizeof(float)); 1035 | memcpy(data_ptr + 8, &cp.position.z(), sizeof(float)); 1036 | 1037 | // Pack RGB into float (as uint32) 1038 | uint8_t r = static_cast(cp.color.x() * 255.0f); 1039 | uint8_t g = static_cast(cp.color.y() * 255.0f); 1040 | uint8_t b = static_cast(cp.color.z() * 255.0f); 1041 | uint32_t rgb = (static_cast(r) << 16) | 1042 | (static_cast(g) << 8) | 1043 | static_cast(b); 1044 | memcpy(data_ptr + 12, &rgb, sizeof(uint32_t)); 1045 | 1046 | data_ptr += cloud_msg.point_step; 1047 | } 1048 | 1049 | dense_cloud_pub_->publish(cloud_msg); 1050 | } 1051 | 1052 | // ⭐ Normalized Depth Image Publisher (heatmap visualization) 1053 | void publishDepthImage(std::shared_ptr frame, const rclcpp::Time& timestamp) 1054 | { 1055 | if (!frame || !frame->is_rgbd()) return; 1056 | 1057 | const cv::Mat& depth_map = frame->get_depth_map(); 1058 | if (depth_map.empty()) { 1059 | return; 1060 | } 1061 | 1062 | // ⭐ Safety check: validate depth map type 1063 | if (depth_map.type() != CV_32FC1) { 1064 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, 1065 | "Depth map is not CV_32FC1 type"); 1066 | return; 1067 | } 1068 | 1069 | // Get visualization range from config 1070 | const Config& config = Config::getInstance(); 1071 | float min_depth = config.m_rgbd_vis_min_depth; 1072 | float max_depth = config.m_rgbd_vis_max_depth; 1073 | 1074 | // Create heatmap (same as Pangolin viewer) 1075 | cv::Mat heatmap(depth_map.rows, depth_map.cols, CV_8UC3); 1076 | 1077 | try { 1078 | for (int v = 0; v < depth_map.rows; ++v) { 1079 | for (int u = 0; u < depth_map.cols; ++u) { 1080 | float depth = depth_map.at(v, u); 1081 | 1082 | cv::Vec3b color; 1083 | 1084 | // Depth = 0 → Black (no depth data) 1085 | if (depth <= 0.0f) { 1086 | color = cv::Vec3b(0, 0, 0); 1087 | heatmap.at(v, u) = color; 1088 | continue; 1089 | } 1090 | 1091 | // Clamp depth to [min_depth, max_depth] 1092 | float clamped_depth = std::max(min_depth, std::min(max_depth, depth)); 1093 | float normalized_depth = (clamped_depth - min_depth) / (max_depth - min_depth); 1094 | normalized_depth = std::max(0.0f, std::min(1.0f, normalized_depth)); 1095 | 1096 | // Heatmap: Red (near) -> Yellow -> Green -> Cyan -> Blue (far) 1097 | float r, g, b; 1098 | if (normalized_depth < 0.25f) { 1099 | float t = normalized_depth / 0.25f; 1100 | r = 1.0f; g = t; b = 0.0f; 1101 | } else if (normalized_depth < 0.5f) { 1102 | float t = (normalized_depth - 0.25f) / 0.25f; 1103 | r = 1.0f - t; g = 1.0f; b = 0.0f; 1104 | } else if (normalized_depth < 0.75f) { 1105 | float t = (normalized_depth - 0.5f) / 0.25f; 1106 | r = 0.0f; g = 1.0f; b = t; 1107 | } else { 1108 | float t = (normalized_depth - 0.75f) / 0.25f; 1109 | r = 0.0f; g = 1.0f - t; b = 1.0f; 1110 | } 1111 | 1112 | // Convert to BGR for OpenCV 1113 | color = cv::Vec3b( 1114 | static_cast(b * 255), 1115 | static_cast(g * 255), 1116 | static_cast(r * 255) 1117 | ); 1118 | 1119 | heatmap.at(v, u) = color; 1120 | } 1121 | } 1122 | } catch (const std::exception& e) { 1123 | RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, 1124 | "Failed to create depth heatmap: %s", e.what()); 1125 | return; 1126 | } 1127 | 1128 | // Convert to ROS message 1129 | std_msgs::msg::Header header; 1130 | header.stamp = timestamp; 1131 | header.frame_id = "camera"; 1132 | 1133 | auto msg = cv_bridge::CvImage(header, "bgr8", heatmap).toImageMsg(); 1134 | depth_image_pub_->publish(*msg); 1135 | } 1136 | 1137 | // ⭐ Camera frustum visualization - LINE_LIST로 pyramid 모양 그리기 1138 | void publishCameraFrustum(std::shared_ptr frame, const rclcpp::Time& timestamp) 1139 | { 1140 | if (!frame) return; 1141 | 1142 | // Get camera pose: Twc (camera to world) 1143 | Eigen::Matrix4f Twc = frame->get_Twc(); 1144 | Eigen::Vector3f camera_pos = Twc.block<3, 1>(0, 3); 1145 | Eigen::Matrix3f R_wc = Twc.block<3, 3>(0, 0); 1146 | 1147 | // ⭐ Get camera intrinsics from frame 1148 | float fx = frame->get_fx(); 1149 | float fy = frame->get_fy(); 1150 | float cx = frame->get_cx(); 1151 | float cy = frame->get_cy(); 1152 | 1153 | // ⭐ Get actual image size from frame 1154 | const cv::Mat& img = frame->get_left_image(); 1155 | float img_width = static_cast(img.cols); 1156 | float img_height = static_cast(img.rows); 1157 | 1158 | // Frustum size (depth in meters from camera center) 1159 | float frustum_depth = 0.2f; // 20cm pyramid depth 1160 | 1161 | // ⭐ Calculate 4 corners in normalized image coordinates, then scale by depth 1162 | // Corners: Top-left, Top-right, Bottom-right, Bottom-left 1163 | Eigen::Vector3f corners_cam[4]; 1164 | 1165 | // Top-left (0, 0) 1166 | corners_cam[0] = Eigen::Vector3f((0.0f - cx) / fx, (0.0f - cy) / fy, 1.0f); 1167 | corners_cam[0] = corners_cam[0].normalized() * frustum_depth; 1168 | 1169 | // Top-right (width, 0) 1170 | corners_cam[1] = Eigen::Vector3f((img_width - cx) / fx, (0.0f - cy) / fy, 1.0f); 1171 | corners_cam[1] = corners_cam[1].normalized() * frustum_depth; 1172 | 1173 | // Bottom-right (width, height) 1174 | corners_cam[2] = Eigen::Vector3f((img_width - cx) / fx, (img_height - cy) / fy, 1.0f); 1175 | corners_cam[2] = corners_cam[2].normalized() * frustum_depth; 1176 | 1177 | // Bottom-left (0, height) 1178 | corners_cam[3] = Eigen::Vector3f((0.0f - cx) / fx, (img_height - cy) / fy, 1.0f); 1179 | corners_cam[3] = corners_cam[3].normalized() * frustum_depth; 1180 | 1181 | // Transform corners to world frame 1182 | Eigen::Vector3f corners_world[4]; 1183 | for (int i = 0; i < 4; ++i) { 1184 | corners_world[i] = R_wc * corners_cam[i] + camera_pos; 1185 | } 1186 | 1187 | // Create LINE_LIST marker 1188 | visualization_msgs::msg::Marker frustum_marker; 1189 | frustum_marker.header.stamp = timestamp; 1190 | frustum_marker.header.frame_id = "map"; 1191 | frustum_marker.ns = "camera_frustum"; 1192 | frustum_marker.id = 0; 1193 | frustum_marker.type = visualization_msgs::msg::Marker::LINE_LIST; 1194 | frustum_marker.action = visualization_msgs::msg::Marker::ADD; 1195 | 1196 | // Frustum color (cyan) 1197 | frustum_marker.color.r = 0.0f; 1198 | frustum_marker.color.g = 1.0f; 1199 | frustum_marker.color.b = 1.0f; 1200 | frustum_marker.color.a = 1.0f; 1201 | 1202 | // Line width 1203 | frustum_marker.scale.x = 0.01; // 1cm thick lines 1204 | 1205 | // Add lines: camera center to 4 corners 1206 | for (int i = 0; i < 4; ++i) { 1207 | geometry_msgs::msg::Point p_center, p_corner; 1208 | p_center.x = camera_pos.x(); 1209 | p_center.y = camera_pos.y(); 1210 | p_center.z = camera_pos.z(); 1211 | 1212 | p_corner.x = corners_world[i].x(); 1213 | p_corner.y = corners_world[i].y(); 1214 | p_corner.z = corners_world[i].z(); 1215 | 1216 | frustum_marker.points.push_back(p_center); 1217 | frustum_marker.points.push_back(p_corner); 1218 | } 1219 | 1220 | // Add lines connecting 4 corners (rectangle) 1221 | for (int i = 0; i < 4; ++i) { 1222 | geometry_msgs::msg::Point p1, p2; 1223 | 1224 | p1.x = corners_world[i].x(); 1225 | p1.y = corners_world[i].y(); 1226 | p1.z = corners_world[i].z(); 1227 | 1228 | int next = (i + 1) % 4; 1229 | p2.x = corners_world[next].x(); 1230 | p2.y = corners_world[next].y(); 1231 | p2.z = corners_world[next].z(); 1232 | 1233 | frustum_marker.points.push_back(p1); 1234 | frustum_marker.points.push_back(p2); 1235 | } 1236 | 1237 | camera_frustum_pub_->publish(frustum_marker); 1238 | } 1239 | 1240 | // Subscribers 1241 | message_filters::Subscriber left_image_sub_; 1242 | message_filters::Subscriber right_image_sub_; 1243 | message_filters::Subscriber rgb_image_sub_; 1244 | message_filters::Subscriber depth_image_sub_; 1245 | rclcpp::Subscription::SharedPtr imu_sub_; 1246 | 1247 | // Publishers 1248 | rclcpp::Publisher::SharedPtr odom_pub_; 1249 | rclcpp::Publisher::SharedPtr pose_pub_; 1250 | rclcpp::Publisher::SharedPtr current_frame_points_pub_; 1251 | rclcpp::Publisher::SharedPtr keyframe_points_pub_; 1252 | rclcpp::Publisher::SharedPtr trajectory_pub_; 1253 | rclcpp::Publisher::SharedPtr camera_frustum_pub_; // ⭐ Camera frustum publisher 1254 | rclcpp::Publisher::SharedPtr tracking_image_pub_; // ⭐ Tracking image publisher 1255 | rclcpp::Publisher::SharedPtr dense_cloud_pub_; // ⭐ RGBD dense point cloud publisher 1256 | rclcpp::Publisher::SharedPtr depth_image_pub_; // ⭐ Normalized depth image publisher 1257 | 1258 | // TF broadcaster 1259 | std::shared_ptr tf_broadcaster_; 1260 | 1261 | // Trajectory storage 1262 | nav_msgs::msg::Path trajectory_; 1263 | 1264 | // Synchronizer for stereo images 1265 | std::shared_ptr>> sync_; 1269 | 1270 | // Synchronizer for RGBD images 1271 | std::shared_ptr>> rgbd_sync_; 1275 | 1276 | // IMU buffer and pivot 1277 | std::vector imu_buffer_; 1278 | size_t imu_pivot_index_; 1279 | double last_image_time_; 1280 | double imu_time_tolerance_; 1281 | 1282 | // Counters 1283 | size_t stereo_count_; 1284 | size_t rgbd_count_; 1285 | size_t imu_count_; 1286 | size_t left_count_; 1287 | size_t right_count_; 1288 | size_t dataset_id_; 1289 | 1290 | // Mode flag 1291 | bool is_rgbd_mode_; 1292 | 1293 | // Timestamps 1294 | double first_stereo_time_; 1295 | double last_stereo_time_; 1296 | double first_rgbd_time_; 1297 | double last_rgbd_time_; 1298 | double first_imu_time_; 1299 | double last_imu_time_; 1300 | 1301 | // VIO Estimator 1302 | std::shared_ptr estimator_; 1303 | 1304 | // ⭐ Producer-Consumer Pattern: Thread-safe Queues 1305 | ThreadSafeQueue image_queue_; // Image callback → Processing thread 1306 | ThreadSafeQueue result_queue_; // Processing thread → Publisher thread 1307 | 1308 | // ⭐ Worker Threads 1309 | std::thread processing_thread_; // VIO processing (heavy computation) 1310 | std::thread publisher_thread_; // ROS publishing (isolated from processing) 1311 | std::atomic running_; // Thread control flag 1312 | }; 1313 | 1314 | int main(int argc, char** argv) 1315 | { 1316 | rclcpp::init(argc, argv); 1317 | auto node = std::make_shared(); 1318 | rclcpp::spin(node); 1319 | rclcpp::shutdown(); 1320 | return 0; 1321 | } 1322 | --------------------------------------------------------------------------------