├── .gitmodules ├── .vscode └── settings.json ├── .gitignore ├── package.xml ├── launch └── lidar_odometry.launch.py ├── CMakeLists.txt ├── README.md ├── rviz └── lidar_odometry.rviz ├── scripts └── kitti_to_rosbag.py └── src └── lidar_odometry_ros.cpp /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lidar_odometry"] 2 | path = lidar_odometry 3 | url = https://github.com/93won/lidar_odometry.git 4 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "cmake.sourceDirectory": "/home/eugene/source/lidar_odometry_ros_wrapper/lidar_odometry" 3 | } -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # ROS2 Build artifacts 2 | build/ 3 | install/ 4 | log/ 5 | 6 | # CMake 7 | CMakeLists.txt.user 8 | CMakeCache.txt 9 | CMakeFiles 10 | CMakeScripts 11 | Testing 12 | Makefile 13 | cmake_install.cmake 14 | install_manifest.txt 15 | compile_commands.json 16 | CTestTestfile.cmake 17 | _deps 18 | 19 | # C++ artifacts 20 | *.so 21 | *.dylib 22 | *.dll 23 | *.a 24 | *.lib 25 | *.o 26 | *.obj 27 | *.exe 28 | 29 | # IDE and editor files 30 | .vscode/ 31 | .idea/ 32 | *.swp 33 | *.swo 34 | *~ 35 | .DS_Store 36 | 37 | # Python 38 | __pycache__/ 39 | *.py[cod] 40 | *$py.class 41 | *.so 42 | .Python 43 | env/ 44 | venv/ 45 | ENV/ 46 | env.bak/ 47 | venv.bak/ 48 | 49 | # ROS 50 | devel/ 51 | logs/ 52 | .catkin_workspace 53 | *.bag 54 | *.db3 55 | 56 | # System files 57 | Thumbs.db 58 | .directory 59 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_odometry_ros 5 | 1.0.0 6 | ROS2 wrapper for LiDAR odometry with Probabilistic Kernel Optimization (PKO) 7 | 8 | Eugene 9 | MIT 10 | 11 | Seungwon Choi 12 | 13 | ament_cmake 14 | cmake 15 | 16 | 17 | rclcpp 18 | sensor_msgs 19 | nav_msgs 20 | geometry_msgs 21 | visualization_msgs 22 | tf2 23 | tf2_ros 24 | tf2_geometry_msgs 25 | pcl_ros 26 | pcl_conversions 27 | 28 | 29 | eigen3_cmake_module 30 | libpcl-all-dev 31 | libceres-dev 32 | 33 | 34 | pkg-config 35 | 36 | 37 | ament_lint_auto 38 | ament_lint_common 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /launch/lidar_odometry.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument, LogInfo 5 | from launch.conditions import IfCondition 6 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 7 | from launch_ros.actions import Node 8 | from launch_ros.substitutions import FindPackageShare 9 | 10 | def generate_launch_description(): 11 | # Launch arguments 12 | config_file_arg = DeclareLaunchArgument( 13 | 'config_file', 14 | description='Path to configuration YAML file (REQUIRED)' 15 | ) 16 | 17 | base_frame_arg = DeclareLaunchArgument( 18 | 'base_frame', 19 | default_value='base_link', 20 | description='Base frame ID' 21 | ) 22 | 23 | odom_frame_arg = DeclareLaunchArgument( 24 | 'odom_frame', 25 | default_value='odom', 26 | description='Odometry frame ID' 27 | ) 28 | 29 | map_frame_arg = DeclareLaunchArgument( 30 | 'map_frame', 31 | default_value='map', 32 | description='Map frame ID' 33 | ) 34 | 35 | publish_tf_arg = DeclareLaunchArgument( 36 | 'publish_tf', 37 | default_value='true', 38 | description='Whether to publish TF transforms' 39 | ) 40 | 41 | enable_viewer_arg = DeclareLaunchArgument( 42 | 'enable_viewer', 43 | default_value='false', 44 | description='Whether to enable Pangolin viewer' 45 | ) 46 | 47 | publish_features_arg = DeclareLaunchArgument( 48 | 'publish_features', 49 | default_value='false', 50 | description='Whether to publish feature points' 51 | ) 52 | 53 | max_range_arg = DeclareLaunchArgument( 54 | 'max_range', 55 | default_value='100.0', 56 | description='Maximum range for point cloud filtering' 57 | ) 58 | 59 | min_range_arg = DeclareLaunchArgument( 60 | 'min_range', 61 | default_value='1.0', 62 | description='Minimum range for point cloud filtering' 63 | ) 64 | 65 | pointcloud_topic_arg = DeclareLaunchArgument( 66 | 'pointcloud_topic', 67 | default_value='/velodyne_points', 68 | description='Input point cloud topic name' 69 | ) 70 | 71 | # RViz argument 72 | enable_rviz_arg = DeclareLaunchArgument( 73 | 'enable_rviz', 74 | default_value='true', 75 | description='Whether to launch RViz' 76 | ) 77 | 78 | # Simulation time argument for rosbag playback 79 | use_sim_time_arg = DeclareLaunchArgument( 80 | 'use_sim_time', 81 | default_value='true', 82 | description='Use simulation time (needed for rosbag playback)' 83 | ) 84 | 85 | # LiDAR Odometry Node 86 | lidar_odometry_node = Node( 87 | package='lidar_odometry_ros', 88 | executable='lidar_odometry_node', 89 | name='simple_lidar_odometry', 90 | output='screen', 91 | parameters=[{ 92 | 'config_file': LaunchConfiguration('config_file'), 93 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 94 | }], 95 | remappings=[ 96 | ('velodyne_points', LaunchConfiguration('pointcloud_topic')), 97 | ] 98 | ) 99 | 100 | # RViz node 101 | rviz_config_file = PathJoinSubstitution([ 102 | FindPackageShare('lidar_odometry_ros'), 103 | 'rviz', 104 | 'lidar_odometry.rviz' 105 | ]) 106 | 107 | rviz_node = Node( 108 | package='rviz2', 109 | executable='rviz2', 110 | name='rviz2', 111 | arguments=['-d', rviz_config_file, '--ros-args', '--log-level', 'FATAL'], 112 | output='screen', 113 | parameters=[{ 114 | 'use_sim_time': LaunchConfiguration('use_sim_time'), 115 | }], 116 | additional_env={'RCUTILS_LOGGING_SEVERITY_THRESHOLD': 'FATAL'}, 117 | condition=IfCondition(LaunchConfiguration('enable_rviz')) 118 | ) 119 | 120 | return LaunchDescription([ 121 | config_file_arg, 122 | base_frame_arg, 123 | odom_frame_arg, 124 | map_frame_arg, 125 | publish_tf_arg, 126 | enable_viewer_arg, 127 | publish_features_arg, 128 | max_range_arg, 129 | min_range_arg, 130 | pointcloud_topic_arg, 131 | enable_rviz_arg, 132 | use_sim_time_arg, 133 | LogInfo(msg="Starting LiDAR Odometry ROS Node..."), 134 | lidar_odometry_node, 135 | rviz_node, 136 | ]) 137 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(lidar_odometry_ros) 3 | 4 | # Set default build type to Release if not specified 5 | if(NOT CMAKE_BUILD_TYPE) 6 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) 7 | endif() 8 | 9 | # Add compiler flags to avoid GCC internal compiler error with Ceres 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | # Reduce optimization for problematic files to avoid compiler crash 13 | set(CMAKE_CXX_FLAGS_RELEASE "-O2 -DNDEBUG -fno-strict-aliasing") 14 | # Limit parallel compilation to avoid memory issues 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pipe") 16 | endif() 17 | 18 | # Set C++ standard 19 | set(CMAKE_CXX_STANDARD 17) 20 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 21 | 22 | # find dependencies 23 | find_package(ament_cmake REQUIRED) 24 | find_package(rclcpp REQUIRED) 25 | find_package(sensor_msgs REQUIRED) 26 | find_package(nav_msgs REQUIRED) 27 | find_package(geometry_msgs REQUIRED) 28 | find_package(visualization_msgs REQUIRED) 29 | find_package(tf2 REQUIRED) 30 | find_package(tf2_ros REQUIRED) 31 | find_package(tf2_geometry_msgs REQUIRED) 32 | find_package(std_msgs REQUIRED) 33 | find_package(fmt REQUIRED) 34 | find_package(Eigen3 REQUIRED) 35 | find_package(OpenCV REQUIRED) 36 | 37 | # Force system spdlog to avoid conflicts 38 | find_package(PkgConfig REQUIRED) 39 | pkg_check_modules(SPDLOG REQUIRED spdlog) 40 | 41 | # Thirdparty directories 42 | set(THIRDPARTY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lidar_odometry/thirdparty) 43 | set(LIDAR_ODOMETRY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lidar_odometry) 44 | 45 | # Build Ceres from thirdparty (needed for lidar_odometry library) 46 | set(CERES_DIR ${THIRDPARTY_DIR}/ceres-solver) 47 | set(BUILD_EXAMPLES OFF CACHE BOOL "Build Ceres examples" FORCE) 48 | set(BUILD_BENCHMARKS OFF CACHE BOOL "Build Ceres benchmarks" FORCE) 49 | set(BUILD_TESTING OFF CACHE BOOL "Build Ceres tests" FORCE) 50 | set(PROVIDE_UNINSTALL_TARGET OFF CACHE BOOL "Disable Ceres uninstall target" FORCE) 51 | set(TBB OFF CACHE BOOL "Disable TBB for Ceres" FORCE) 52 | set(SUITESPARSE OFF CACHE BOOL "Disable SuiteSparse for Ceres" FORCE) 53 | set(CMAKE_DISABLE_FIND_PACKAGE_TBB ON CACHE BOOL "Disable TBB package finding" FORCE) 54 | set(CMAKE_DISABLE_FIND_PACKAGE_SuiteSparse ON CACHE BOOL "Disable SuiteSparse package finding" FORCE) 55 | add_subdirectory(${CERES_DIR} ${CMAKE_BINARY_DIR}/ceres-solver EXCLUDE_FROM_ALL) 56 | 57 | # Include directories for lidar_odometry 58 | include_directories( 59 | ${LIDAR_ODOMETRY_DIR}/src 60 | ${LIDAR_ODOMETRY_DIR}/app 61 | ${THIRDPARTY_DIR}/Sophus 62 | ${THIRDPARTY_DIR}/nanoflann/include 63 | ${THIRDPARTY_DIR}/LidarIris 64 | ${THIRDPARTY_DIR}/LidarIris/fftm 65 | ${EIGEN3_INCLUDE_DIR} 66 | ) 67 | 68 | # Direct static linking approach - much simpler! 69 | set(LIDAR_ODOMETRY_SOURCES 70 | ${LIDAR_ODOMETRY_DIR}/src/database/LidarFrame.cpp 71 | ${LIDAR_ODOMETRY_DIR}/src/processing/FeatureExtractor.cpp 72 | ${LIDAR_ODOMETRY_DIR}/src/processing/Estimator.cpp 73 | ${LIDAR_ODOMETRY_DIR}/src/processing/LoopClosureDetector.cpp 74 | ${LIDAR_ODOMETRY_DIR}/src/optimization/IterativeClosestPointOptimizer.cpp 75 | ${LIDAR_ODOMETRY_DIR}/src/optimization/PoseGraphOptimizer.cpp 76 | ${LIDAR_ODOMETRY_DIR}/src/optimization/Parameters.cpp 77 | ${LIDAR_ODOMETRY_DIR}/src/optimization/Factors.cpp 78 | ${LIDAR_ODOMETRY_DIR}/src/optimization/AdaptiveMEstimator.cpp 79 | ${LIDAR_ODOMETRY_DIR}/src/util/Config.cpp 80 | ${LIDAR_ODOMETRY_DIR}/src/util/MathUtils.cpp 81 | ${LIDAR_ODOMETRY_DIR}/src/util/PointCloudUtils.cpp 82 | ${THIRDPARTY_DIR}/LidarIris/LidarIris.cpp 83 | ${THIRDPARTY_DIR}/LidarIris/fftm/fftm.cpp 84 | ) 85 | 86 | # Simple direct linking node 87 | add_executable(lidar_odometry_node 88 | src/lidar_odometry_ros.cpp 89 | ${LIDAR_ODOMETRY_SOURCES} 90 | ) 91 | 92 | ament_target_dependencies(lidar_odometry_node 93 | rclcpp 94 | sensor_msgs 95 | nav_msgs 96 | geometry_msgs 97 | tf2_ros 98 | std_msgs 99 | ) 100 | 101 | target_link_libraries(lidar_odometry_node 102 | ceres 103 | fmt::fmt 104 | ${SPDLOG_LIBRARIES} 105 | ${OpenCV_LIBS} 106 | ) 107 | 108 | target_include_directories(lidar_odometry_node PRIVATE 109 | ${SPDLOG_INCLUDE_DIRS} 110 | ${OpenCV_INCLUDE_DIRS} 111 | ) 112 | 113 | if(TARGET Eigen3::Eigen) 114 | target_link_libraries(lidar_odometry_node Eigen3::Eigen) 115 | else() 116 | target_include_directories(lidar_odometry_node PRIVATE ${EIGEN3_INCLUDE_DIR}) 117 | endif() 118 | 119 | # Install executables 120 | install(TARGETS 121 | lidar_odometry_node 122 | DESTINATION lib/${PROJECT_NAME} 123 | ) 124 | 125 | install(DIRECTORY 126 | launch/ 127 | DESTINATION share/${PROJECT_NAME}/launch 128 | ) 129 | 130 | install(DIRECTORY 131 | lidar_odometry/config/ 132 | DESTINATION share/${PROJECT_NAME}/config 133 | ) 134 | 135 | install(DIRECTORY 136 | rviz/ 137 | DESTINATION share/${PROJECT_NAME}/rviz 138 | ) 139 | 140 | if(BUILD_TESTING) 141 | find_package(ament_lint_auto REQUIRED) 142 | ament_lint_auto_find_test_dependencies() 143 | endif() 144 | 145 | ament_package() 146 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LiDAR Odometry ROS2 Wrapper 2 | 3 | This package provides a ROS2 wrapper for the LiDAR Odometry system with Probabilistic Kernel Optimization (PKO). It enables real-time LiDAR-based odometry estimation in ROS2 environments. 4 | 5 | ## Demo Video 6 | 7 | [![KITTI LiDAR Odometry Demo](https://img.youtube.com/vi/swrJY2EStrs/0.jpg)](https://www.youtube.com/watch?v=swrJY2EStrs) 8 | [![Mid360 LiDAR Odometry Demo](https://img.youtube.com/vi/HDPA_ILxCrE/0.jpg)](https://youtu.be/HDPA_ILxCrE) 9 | 10 | ## Features 11 | 12 | - ⚡ Real-time LiDAR odometry processing 13 | - 🎯 Feature-based point cloud registration 14 | - 🔧 Ceres Solver-based optimization with PKO 15 | - 📈 ROS2 native implementation 16 | - 🌐 TF2 transform broadcasting 17 | - 📊 Trajectory visualization 18 | - 🎮 Optional Pangolin viewer integration 19 | 20 | ## Dependencies 21 | 22 | ### ROS2 Dependencies 23 | - `rclcpp` 24 | - `sensor_msgs` 25 | - `nav_msgs` 26 | - `geometry_msgs` 27 | - `visualization_msgs` 28 | - `tf2` and `tf2_ros` 29 | - `pcl_ros` and `pcl_conversions` 30 | 31 | ### System Dependencies 32 | - Eigen3 33 | - PCL (Point Cloud Library) 34 | - Ceres Solver 35 | - OpenGL and GLEW 36 | - Pangolin (included as submodule) 37 | 38 | ## Installation 39 | 40 | ### 1. Setup Workspace and Clone Repository 41 | ```bash 42 | # Create a new ROS2 workspace 43 | mkdir -p lidar_odom_ws/src 44 | cd lidar_odom_ws/src 45 | 46 | # Clone the repository 47 | git clone https://github.com/93won/lidar_odometry_ros_wrapper.git 48 | cd lidar_odometry_ros_wrapper 49 | 50 | # Initialize and download submodules 51 | git submodule update --init --recursive 52 | ``` 53 | 54 | ### 2. Install System Dependencies 55 | ```bash 56 | # Ubuntu 22.04 57 | sudo apt update 58 | sudo apt install -y \ 59 | libeigen3-dev \ 60 | libpcl-dev \ 61 | libceres-dev \ 62 | libgl1-mesa-dev \ 63 | libglew-dev \ 64 | pkg-config 65 | ``` 66 | 67 | ### 3. Build the Package 68 | ```bash 69 | cd ../../ # Go back to lidar_odom_ws root 70 | colcon build --packages-select lidar_odometry_ros 71 | source install/setup.bash 72 | ``` 73 | 74 | ## Usage 75 | 76 | ### Basic Usage 77 | ```bash 78 | # Launch with default Velodyne topic 79 | ros2 launch lidar_odometry_ros lidar_odometry.launch.py \ 80 | config_file:=/path/to/your/workspace/lidar_odometry_ros_wrapper/lidar_odometry/config/kitti.yaml 81 | 82 | # Launch with custom topic (e.g., Livox) 83 | ros2 launch lidar_odometry_ros lidar_odometry.launch.py \ 84 | config_file:=/path/to/your/workspace/lidar_odometry_ros_wrapper/lidar_odometry/config/kitti.yaml \ 85 | pointcloud_topic:=/livox/pointcloud \ 86 | use_sim_time:=true 87 | ``` 88 | 89 | ### Quick Start 90 | 91 | #### Option 1: KITTI Sample Data 92 | Download and play the KITTI sample ROS bag file: 93 | ```bash 94 | # Download KITTI sample bag 95 | # https://drive.google.com/file/d/1U0tRSsc1PbEj_QThOHcD8l3qFkma3zjc/view?usp=sharing 96 | 97 | # Terminal 1: Launch odometry system 98 | ros2 launch lidar_odometry_ros lidar_odometry.launch.py \ 99 | config_file:=/path/to/your/workspace/lidar_odometry_ros_wrapper/lidar_odometry/config/kitti.yaml \ 100 | use_sim_time:=true 101 | 102 | # Terminal 2: Play KITTI bag file 103 | ros2 bag play /path/to/kitti_sample.bag --clock 104 | ``` 105 | 106 | #### Option 2: Livox MID360 Sample Data 107 | Download and play the Livox MID360 sample ROS bag file: 108 | ```bash 109 | # Download Livox MID360 sample bag 110 | # https://drive.google.com/file/d/1UI6Qc5cdY8R61Odc7A6IU-jRWZgnCx2g/view?usp=sharing 111 | # Source: https://www.youtube.com/watch?v=u8siB0KLFLc 112 | 113 | # Terminal 1: Launch odometry system for Livox 114 | ros2 launch lidar_odometry_ros lidar_odometry.launch.py \ 115 | config_file:=/path/to/your/workspace/lidar_odometry_ros_wrapper/lidar_odometry/config/kitti.yaml \ 116 | use_sim_time:=true \ 117 | pointcloud_topic:=/livox/pointcloud 118 | 119 | # Terminal 2: Play Livox bag file 120 | ros2 bag play /path/to/livox_mid360_sample.bag --clock 121 | ``` 122 | 123 | **Note**: The Livox sample data uses standard `sensor_msgs/PointCloud2` messages, not Livox custom message format. 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | ## KITTI Dataset Usage 132 | 133 | ### 1. Download KITTI Dataset 134 | ```bash 135 | # Create data directory 136 | mkdir -p ~/kitti_data 137 | cd ~/kitti_data 138 | 139 | # Download KITTI Odometry Dataset (example: sequence 00) 140 | # Visit: https://www.cvlibs.net/datasets/kitti/eval_odometry.php 141 | # Download velodyne laser data and poses 142 | 143 | # Expected structure: 144 | # ~/kitti_data/ 145 | # ├── sequences/ 146 | # │ └── 00/ 147 | # │ ├── velodyne/ 148 | # │ │ ├── 000000.bin 149 | # │ │ ├── 000001.bin 150 | # │ │ └── ... 151 | # │ └── poses.txt 152 | ``` 153 | 154 | ### 2. Convert KITTI to ROS2 Bag 155 | ```bash 156 | # Use the provided conversion script 157 | cd ~/ros2_ws/src/lidar_odometry_ros_wrapper/scripts 158 | 159 | python3 kitti_to_rosbag.py \ 160 | --kitti_dir ~/kitti_data/sequences/07 \ 161 | --output_bag ~/kitti_data/kitti_seq07.db3 \ 162 | --topic_name /velodyne_points \ 163 | --frame_id velodyne 164 | ``` 165 | 166 | ### 3. Run Examples 167 | 168 | #### Option A: KITTI Dataset 169 | ```bash 170 | # Terminal 1: Launch odometry system for KITTI 171 | ros2 launch lidar_odometry_ros lidar_odometry.launch.py \ 172 | config_file:=$(pwd)/lidar_odometry/config/kitti.yaml \ 173 | use_sim_time:=true \ 174 | pointcloud_topic:=/velodyne_points 175 | 176 | # Terminal 2: Play KITTI bag file 177 | ros2 bag play ~/kitti_data/kitti_seq07.db3 --clock 178 | ``` 179 | 180 | #### Option B: Livox MID360 Dataset 181 | ```bash 182 | # Terminal 1: Launch odometry system for Livox MID360 183 | ros2 launch lidar_odometry_ros lidar_odometry.launch.py \ 184 | config_file:=$(pwd)/lidar_odometry/config/kitti.yaml \ 185 | use_sim_time:=true \ 186 | pointcloud_topic:=/livox/pointcloud 187 | 188 | # Terminal 2: Play Livox bag file 189 | ros2 bag play /path/to/your/livox_bag --clock 190 | ``` 191 | 192 | #### Launch Parameters 193 | - `config_file`: Path to YAML configuration file (required) 194 | - `use_sim_time`: Enable simulation time for bag playback (default: true) 195 | - `pointcloud_topic`: Input point cloud topic name (default: /velodyne_points) 196 | - `enable_rviz`: Launch RViz for visualization (default: true) 197 | 198 | 199 | ## License 200 | 201 | This project is released under the MIT License. See the original [LiDAR Odometry repository](https://github.com/93won/lidar_odometry) for more details. 202 | 203 | ## Citation 204 | 205 | If you use this work, please cite: 206 | 207 | ```bibtex 208 | @article{choi2025probabilistic, 209 | title={Probabilistic Kernel Optimization for Robust State Estimation}, 210 | author={Choi, Seungwon and Kim, Tae-Wan}, 211 | journal={IEEE Robotics and Automation Letters}, 212 | volume={10}, 213 | number={3}, 214 | pages={2998--3005}, 215 | year={2025}, 216 | publisher={IEEE}, 217 | doi={10.1109/LRA.2025.3536294} 218 | } 219 | ``` 220 | 221 | ## Contributing 222 | 223 | Contributions are welcome! Please feel free to submit a Pull Request. 224 | -------------------------------------------------------------------------------- /rviz/lidar_odometry.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 | - /Status1 9 | - /PointCloud22 10 | - /PointCloud23 11 | - /Path1 12 | Splitter Ratio: 0.5 13 | Tree Height: 2300 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: PointCloud2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 0.8999999761581421 54 | Autocompute Intensity Bounds: true 55 | Autocompute Value Bounds: 56 | Max Value: 10 57 | Min Value: -10 58 | Value: true 59 | Axis: Z 60 | Channel Name: intensity 61 | Class: rviz_default_plugins/PointCloud2 62 | Color: 255; 255; 255 63 | Color Transformer: Intensity 64 | Decay Time: 0 65 | Enabled: true 66 | Invert Rainbow: false 67 | Max Color: 255; 255; 255 68 | Max Intensity: 4096 69 | Min Color: 0; 0; 0 70 | Min Intensity: 0 71 | Name: PointCloud2 72 | Position Transformer: XYZ 73 | Selectable: true 74 | Size (Pixels): 1 75 | Size (m): 0.009999999776482582 76 | Style: Points 77 | Topic: 78 | Depth: 5 79 | Durability Policy: Volatile 80 | Filter size: 10 81 | History Policy: Keep Last 82 | Reliability Policy: Reliable 83 | Value: /map_points 84 | Use Fixed Frame: true 85 | Use rainbow: true 86 | Value: true 87 | - Alpha: 1 88 | Autocompute Intensity Bounds: true 89 | Autocompute Value Bounds: 90 | Max Value: -71.85707092285156 91 | Min Value: -103.41136169433594 92 | Value: true 93 | Axis: X 94 | Channel Name: intensity 95 | Class: rviz_default_plugins/PointCloud2 96 | Color: 46; 194; 126 97 | Color Transformer: AxisColor 98 | Decay Time: 0 99 | Enabled: true 100 | Invert Rainbow: false 101 | Max Color: 255; 255; 255 102 | Max Intensity: 4096 103 | Min Color: 0; 0; 0 104 | Min Intensity: 0 105 | Name: PointCloud2 106 | Position Transformer: XYZ 107 | Selectable: true 108 | Size (Pixels): 1 109 | Size (m): 0.009999999776482582 110 | Style: Points 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | Filter size: 10 115 | History Policy: Keep Last 116 | Reliability Policy: Reliable 117 | Value: /current_cloud 118 | Use Fixed Frame: true 119 | Use rainbow: true 120 | Value: true 121 | - Alpha: 1 122 | Autocompute Intensity Bounds: true 123 | Autocompute Value Bounds: 124 | Max Value: 10 125 | Min Value: -10 126 | Value: true 127 | Axis: Z 128 | Channel Name: intensity 129 | Class: rviz_default_plugins/PointCloud2 130 | Color: 237; 51; 59 131 | Color Transformer: FlatColor 132 | Decay Time: 0 133 | Enabled: true 134 | Invert Rainbow: false 135 | Max Color: 255; 255; 255 136 | Max Intensity: 4096 137 | Min Color: 0; 0; 0 138 | Min Intensity: 0 139 | Name: PointCloud2 140 | Position Transformer: XYZ 141 | Selectable: true 142 | Size (Pixels): 8 143 | Size (m): 0.009999999776482582 144 | Style: Points 145 | Topic: 146 | Depth: 5 147 | Durability Policy: Volatile 148 | Filter size: 10 149 | History Policy: Keep Last 150 | Reliability Policy: Reliable 151 | Value: /feature_points 152 | Use Fixed Frame: true 153 | Use rainbow: true 154 | Value: true 155 | - Alpha: 10 156 | Buffer Length: 1 157 | Class: rviz_default_plugins/Path 158 | Color: 248; 228; 92 159 | Enabled: true 160 | Head Diameter: 0.30000001192092896 161 | Head Length: 0.20000000298023224 162 | Length: 0.30000001192092896 163 | Line Style: Billboards 164 | Line Width: 0.30000001192092896 165 | Name: Path 166 | Offset: 167 | X: 0 168 | Y: 0 169 | Z: 0 170 | Pose Color: 255; 85; 255 171 | Pose Style: None 172 | Radius: 0.029999999329447746 173 | Shaft Diameter: 0.10000000149011612 174 | Shaft Length: 0.10000000149011612 175 | Topic: 176 | Depth: 5 177 | Durability Policy: Volatile 178 | Filter size: 10 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /trajectory 182 | Value: true 183 | Enabled: true 184 | Global Options: 185 | Background Color: 48; 48; 48 186 | Fixed Frame: map 187 | Frame Rate: 30 188 | Name: root 189 | Tools: 190 | - Class: rviz_default_plugins/Interact 191 | Hide Inactive Objects: true 192 | - Class: rviz_default_plugins/MoveCamera 193 | - Class: rviz_default_plugins/Select 194 | - Class: rviz_default_plugins/FocusCamera 195 | - Class: rviz_default_plugins/Measure 196 | Line color: 128; 128; 0 197 | - Class: rviz_default_plugins/SetInitialPose 198 | Covariance x: 0.25 199 | Covariance y: 0.25 200 | Covariance yaw: 0.06853891909122467 201 | Topic: 202 | Depth: 5 203 | Durability Policy: Volatile 204 | History Policy: Keep Last 205 | Reliability Policy: Reliable 206 | Value: /initialpose 207 | - Class: rviz_default_plugins/SetGoal 208 | Topic: 209 | Depth: 5 210 | Durability Policy: Volatile 211 | History Policy: Keep Last 212 | Reliability Policy: Reliable 213 | Value: /goal_pose 214 | - Class: rviz_default_plugins/PublishPoint 215 | Single click: true 216 | Topic: 217 | Depth: 5 218 | Durability Policy: Volatile 219 | History Policy: Keep Last 220 | Reliability Policy: Reliable 221 | Value: /clicked_point 222 | Transformation: 223 | Current: 224 | Class: rviz_default_plugins/TF 225 | Value: true 226 | Views: 227 | Current: 228 | Class: rviz_default_plugins/Orbit 229 | Distance: 176.23416137695312 230 | Enable Stereo Rendering: 231 | Stereo Eye Separation: 0.05999999865889549 232 | Stereo Focal Distance: 1 233 | Swap Stereo Eyes: false 234 | Value: false 235 | Focal Point: 236 | X: 2.3664491176605225 237 | Y: -1.7221742868423462 238 | Z: 0 239 | Focal Shape Fixed Size: true 240 | Focal Shape Size: 0.05000000074505806 241 | Invert Z Axis: false 242 | Name: Current View 243 | Near Clip Distance: 0.009999999776482582 244 | Pitch: 1.5697963237762451 245 | Target Frame: base_link 246 | Value: Orbit (rviz_default_plugins) 247 | Yaw: 4.71238899230957 248 | Saved: ~ 249 | Window Geometry: 250 | Displays: 251 | collapsed: false 252 | Height: 2806 253 | Hide Left Dock: false 254 | Hide Right Dock: false 255 | QMainWindow State: 000000ff00000000fd0000000400000000000004ae000009f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000009f40000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000033c000009f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000009f40000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000014000000005afc0100000002fb0000000800540069006d00650100000000000014000000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000bfe000009f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 256 | Selection: 257 | collapsed: false 258 | Time: 259 | collapsed: false 260 | Tool Properties: 261 | collapsed: false 262 | Views: 263 | collapsed: false 264 | Width: 5120 265 | X: 5120 266 | Y: 0 267 | -------------------------------------------------------------------------------- /scripts/kitti_to_rosbag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | @file kitti_to_rosbag.py 4 | @brief KITTI Velodyne data to ROS2 bag converter 5 | @author Seungwon Choi 6 | @date 2025-09-28 7 | @copyright Copyright (c) 2025 Seungwon Choi. All rights reserved. 8 | 9 | @par License 10 | This project is released under the MIT License. 11 | 12 | @par Usage 13 | python3 kitti_to_rosbag.py /path/to/KITTI/Velodyne/07 14 | 15 | @par Description 16 | Converts KITTI .bin files to ROS2 bag format for LiDAR odometry testing. 17 | Reads Velodyne binary point cloud files from KITTI dataset and converts them 18 | to ROS2 bag format with configurable frequency for odometry algorithm testing. 19 | """ 20 | 21 | import os 22 | import sys 23 | import struct 24 | import argparse 25 | import shutil 26 | from pathlib import Path 27 | import numpy as np 28 | from typing import List, Tuple 29 | 30 | # ROS2 imports 31 | import rclpy 32 | from rclpy.node import Node 33 | from rclpy.serialization import serialize_message 34 | from sensor_msgs.msg import PointCloud2, PointField 35 | from std_msgs.msg import Header 36 | import rosbag2_py 37 | from builtin_interfaces.msg import Time 38 | 39 | 40 | class KittiToBag: 41 | """Convert KITTI Velodyne data to ROS2 bag""" 42 | 43 | def __init__(self, kitti_path: str, output_bag_path: str = None, frequency: float = 10.0): 44 | """ 45 | Initialize converter 46 | 47 | Args: 48 | kitti_path: Path to KITTI sequence directory (e.g., /data/KITTI/Velodyne/07) 49 | output_bag_path: Output bag path (default: sequence_name.bag in same directory) 50 | frequency: Recording frequency in Hz (default: 10.0) 51 | """ 52 | self.kitti_path = Path(kitti_path) 53 | self.sequence_name = self.kitti_path.name 54 | 55 | # Set output bag path 56 | if output_bag_path is None: 57 | # Create rosbag directory and put data.bag inside 58 | rosbag_dir = self.kitti_path / "rosbag" 59 | rosbag_dir.mkdir(exist_ok=True) 60 | self.output_bag_path = rosbag_dir 61 | else: 62 | self.output_bag_path = Path(output_bag_path) 63 | 64 | self.frequency = frequency 65 | self.dt = 1.0 / frequency # Time interval between frames 66 | 67 | # Velodyne data directory 68 | self.velodyne_dir = self.kitti_path / "velodyne" 69 | 70 | if not self.velodyne_dir.exists(): 71 | raise FileNotFoundError(f"Velodyne directory not found: {self.velodyne_dir}") 72 | 73 | print(f"🚗 KITTI to ROS2 Bag Converter") 74 | print(f"📁 Input: {self.kitti_path}") 75 | print(f"📦 Output: {self.output_bag_path}") 76 | print(f"📊 Frequency: {self.frequency} Hz") 77 | 78 | def load_kitti_point_cloud(self, bin_file: Path) -> np.ndarray: 79 | """ 80 | Load KITTI binary point cloud file 81 | 82 | Args: 83 | bin_file: Path to .bin file 84 | 85 | Returns: 86 | numpy array of shape (N, 4) with [x, y, z, intensity] 87 | """ 88 | if not bin_file.exists(): 89 | raise FileNotFoundError(f"Binary file not found: {bin_file}") 90 | 91 | # KITTI Velodyne format: float32 x 4 (x, y, z, intensity) 92 | points = np.fromfile(str(bin_file), dtype=np.float32).reshape(-1, 4) 93 | return points 94 | 95 | def create_pointcloud2_msg(self, points: np.ndarray, timestamp: float, frame_id: str = "velodyne") -> PointCloud2: 96 | """ 97 | Convert numpy point array to ROS2 PointCloud2 message 98 | 99 | Args: 100 | points: numpy array of shape (N, 4) with [x, y, z, intensity] 101 | timestamp: timestamp in seconds 102 | frame_id: frame ID for the point cloud 103 | 104 | Returns: 105 | PointCloud2 message 106 | """ 107 | # Create header 108 | header = Header() 109 | header.frame_id = frame_id 110 | 111 | # Convert timestamp to ROS2 Time 112 | sec = int(timestamp) 113 | nanosec = int((timestamp - sec) * 1e9) 114 | header.stamp = Time(sec=sec, nanosec=nanosec) 115 | 116 | # Create PointCloud2 message 117 | msg = PointCloud2() 118 | msg.header = header 119 | msg.height = 1 120 | msg.width = len(points) 121 | msg.is_bigendian = False 122 | msg.is_dense = False 123 | 124 | # Define point fields (x, y, z, intensity) 125 | msg.fields = [ 126 | PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), 127 | PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), 128 | PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), 129 | PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1), 130 | ] 131 | 132 | msg.point_step = 16 # 4 fields * 4 bytes each 133 | msg.row_step = msg.point_step * msg.width 134 | 135 | # Pack point data 136 | msg.data = points.astype(np.float32).tobytes() 137 | 138 | return msg 139 | 140 | def get_bin_files(self) -> List[Path]: 141 | """Get sorted list of .bin files""" 142 | bin_files = sorted(self.velodyne_dir.glob("*.bin")) 143 | if not bin_files: 144 | raise FileNotFoundError(f"No .bin files found in {self.velodyne_dir}") 145 | return bin_files 146 | 147 | def convert_to_bag(self) -> bool: 148 | """ 149 | Convert KITTI data to ROS2 bag 150 | 151 | Returns: 152 | True if successful, False otherwise 153 | """ 154 | try: 155 | # Get list of binary files 156 | bin_files = self.get_bin_files() 157 | print(f"📋 Found {len(bin_files)} point cloud files") 158 | 159 | # Remove existing bag directory if it exists 160 | if self.output_bag_path.exists(): 161 | print(f"🗑️ Removing existing bag: {self.output_bag_path}") 162 | if self.output_bag_path.is_dir(): 163 | shutil.rmtree(self.output_bag_path) 164 | else: 165 | self.output_bag_path.unlink() 166 | 167 | # Initialize rosbag2 168 | writer = rosbag2_py.SequentialWriter() 169 | 170 | # Storage options (use sqlite3 instead of mcap) 171 | storage_options = rosbag2_py.StorageOptions(uri=str(self.output_bag_path), storage_id='sqlite3') 172 | 173 | # Converter options 174 | converter_options = rosbag2_py.ConverterOptions( 175 | input_serialization_format='cdr', 176 | output_serialization_format='cdr' 177 | ) 178 | 179 | writer.open(storage_options, converter_options) 180 | 181 | # Create topic info 182 | topic_metadata = rosbag2_py.TopicMetadata( 183 | name='/velodyne_points', 184 | type='sensor_msgs/msg/PointCloud2', 185 | serialization_format='cdr' 186 | ) 187 | writer.create_topic(topic_metadata) 188 | 189 | print(f"🎬 Starting conversion...") 190 | 191 | # Process each binary file 192 | for i, bin_file in enumerate(bin_files): 193 | try: 194 | # Load point cloud 195 | points = self.load_kitti_point_cloud(bin_file) 196 | 197 | # Calculate timestamp (starting from 0) 198 | timestamp = i * self.dt 199 | 200 | # Create ROS2 message 201 | msg = self.create_pointcloud2_msg(points, timestamp) 202 | 203 | # Write to bag 204 | timestamp_ns = int(timestamp * 1e9) 205 | writer.write('/velodyne_points', serialize_message(msg), timestamp_ns) 206 | 207 | # Progress update 208 | if (i + 1) % 50 == 0 or i == len(bin_files) - 1: 209 | progress = (i + 1) / len(bin_files) * 100 210 | print(f"⏳ Progress: {progress:.1f}% ({i+1}/{len(bin_files)}) - {bin_file.name}") 211 | 212 | except Exception as e: 213 | print(f"❌ Error processing {bin_file}: {e}") 214 | continue 215 | 216 | # Close writer 217 | writer.close() 218 | 219 | print(f"✅ Conversion completed!") 220 | print(f"📦 Output bag: {self.output_bag_path}") 221 | print(f"📊 Total frames: {len(bin_files)}") 222 | print(f"⏱️ Duration: {len(bin_files) * self.dt:.1f} seconds") 223 | 224 | return True 225 | 226 | except Exception as e: 227 | print(f"❌ Conversion failed: {e}") 228 | return False 229 | 230 | 231 | def main(): 232 | """Main function""" 233 | parser = argparse.ArgumentParser( 234 | description="Convert KITTI Velodyne data to ROS2 bag format", 235 | formatter_class=argparse.RawDescriptionHelpFormatter, 236 | epilog=""" 237 | Examples: 238 | python3 kitti_to_rosbag.py /home/eugene/data/KITTI/Velodyne/07 239 | python3 kitti_to_rosbag.py /data/KITTI/00 --output /tmp/kitti_00.bag --freq 20 240 | """ 241 | ) 242 | 243 | parser.add_argument('kitti_path', type=str, 244 | help='Path to KITTI sequence directory (e.g., /data/KITTI/Velodyne/07)') 245 | parser.add_argument('--output', '-o', type=str, default=None, 246 | help='Output bag file path (default: sequence_name.bag in input directory)') 247 | parser.add_argument('--freq', '-f', type=float, default=10.0, 248 | help='Recording frequency in Hz (default: 10.0)') 249 | 250 | args = parser.parse_args() 251 | 252 | # Check if input path exists 253 | if not Path(args.kitti_path).exists(): 254 | print(f"❌ Error: Path does not exist: {args.kitti_path}") 255 | sys.exit(1) 256 | 257 | try: 258 | # Initialize converter 259 | converter = KittiToBag( 260 | kitti_path=args.kitti_path, 261 | output_bag_path=args.output, 262 | frequency=args.freq 263 | ) 264 | 265 | # Convert to bag 266 | success = converter.convert_to_bag() 267 | 268 | if success: 269 | print(f"\n🎉 Successfully converted KITTI data to ROS2 bag!") 270 | print(f"🚀 You can now play it with:") 271 | print(f" ros2 bag play {converter.output_bag_path}") 272 | else: 273 | print(f"\n❌ Conversion failed!") 274 | sys.exit(1) 275 | 276 | except KeyboardInterrupt: 277 | print(f"\n⛔ Conversion interrupted by user") 278 | sys.exit(1) 279 | except Exception as e: 280 | print(f"\n❌ Unexpected error: {e}") 281 | sys.exit(1) 282 | 283 | 284 | if __name__ == "__main__": 285 | main() 286 | -------------------------------------------------------------------------------- /src/lidar_odometry_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include // for memcpy 9 | #include // for std::isfinite 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // Direct include - no more dynamic loading! 17 | #include "../lidar_odometry/src/processing/Estimator.h" 18 | #include "../lidar_odometry/src/database/LidarFrame.h" 19 | #include "../lidar_odometry/src/util/Config.h" 20 | 21 | class LidarOdometryRosWrapper : public rclcpp::Node { 22 | public: 23 | LidarOdometryRosWrapper() : Node("simple_lidar_odometry") { 24 | 25 | RCLCPP_INFO(this->get_logger(), "=== Simple LiDAR Odometry Started ==="); 26 | 27 | // Declare parameters 28 | this->declare_parameter("config_file", ""); 29 | 30 | // Load config 31 | load_config(); 32 | 33 | // Create estimator directly - delay creation to avoid constructor issues 34 | // estimator_ will be created on first frame 35 | estimator_ = nullptr; 36 | 37 | // Publishers - using lidar_odometry namespace 38 | odom_pub_ = create_publisher("odometry", 10); 39 | map_pub_ = create_publisher("map_points", 10); 40 | features_pub_ = create_publisher("feature_points", 10); 41 | trajectory_pub_ = create_publisher("trajectory", 10); 42 | current_cloud_pub_ = create_publisher("current_cloud", 10); 43 | 44 | // TF broadcaster 45 | tf_broadcaster_ = std::make_shared(*this); 46 | 47 | // Subscriber 48 | cloud_sub_ = create_subscription( 49 | "/velodyne_points", 10, 50 | std::bind(&LidarOdometryRosWrapper::cloud_callback, this, std::placeholders::_1)); 51 | 52 | // Start processing thread 53 | processing_thread_ = std::thread(&LidarOdometryRosWrapper::processing_loop, this); 54 | 55 | RCLCPP_INFO(this->get_logger(), "Simple LiDAR Odometry Ready!"); 56 | } 57 | 58 | ~LidarOdometryRosWrapper() { 59 | // Stop processing thread 60 | should_stop_ = true; 61 | queue_cv_.notify_all(); 62 | if (processing_thread_.joinable()) { 63 | processing_thread_.join(); 64 | } 65 | } 66 | 67 | private: 68 | /** 69 | * @brief Convert ROS PointCloud2 message to internal point cloud format 70 | */ 71 | lidar_odometry::util::PointCloudPtr convert_ros_to_internal(const sensor_msgs::msg::PointCloud2::SharedPtr& ros_cloud) { 72 | auto internal_cloud = std::make_shared(); 73 | 74 | // Parse the PointCloud2 message fields 75 | int x_offset = -1, y_offset = -1, z_offset = -1; 76 | for (const auto& field : ros_cloud->fields) { 77 | if (field.name == "x") x_offset = field.offset; 78 | else if (field.name == "y") y_offset = field.offset; 79 | else if (field.name == "z") z_offset = field.offset; 80 | } 81 | 82 | if (x_offset < 0 || y_offset < 0 || z_offset < 0) { 83 | RCLCPP_ERROR(this->get_logger(), "Invalid PointCloud2 format: missing x, y, or z fields"); 84 | return internal_cloud; 85 | } 86 | 87 | internal_cloud->reserve(ros_cloud->width * ros_cloud->height); 88 | 89 | // Extract points from the data buffer 90 | const uint8_t* data_ptr = ros_cloud->data.data(); 91 | for (size_t i = 0; i < ros_cloud->width * ros_cloud->height; ++i) { 92 | const uint8_t* point_data = data_ptr + i * ros_cloud->point_step; 93 | 94 | float x, y, z; 95 | memcpy(&x, point_data + x_offset, sizeof(float)); 96 | memcpy(&y, point_data + y_offset, sizeof(float)); 97 | memcpy(&z, point_data + z_offset, sizeof(float)); 98 | 99 | // Filter out invalid points 100 | if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { 101 | internal_cloud->push_back(x, y, z); 102 | } 103 | } 104 | 105 | return internal_cloud; 106 | } 107 | 108 | /** 109 | * @brief Convert internal point cloud to ROS PointCloud2 message directly (no PCL) 110 | */ 111 | sensor_msgs::msg::PointCloud2 convert_internal_to_ros( 112 | const lidar_odometry::util::PointCloudPtr& internal_cloud, 113 | const std::string& frame_id = "base_link") { 114 | 115 | sensor_msgs::msg::PointCloud2 cloud_msg; 116 | 117 | if (!internal_cloud || internal_cloud->empty()) { 118 | cloud_msg.header.frame_id = frame_id; 119 | cloud_msg.height = 1; 120 | cloud_msg.width = 0; 121 | cloud_msg.is_dense = true; 122 | return cloud_msg; 123 | } 124 | 125 | // Set up the PointCloud2 message structure 126 | cloud_msg.header.frame_id = frame_id; 127 | cloud_msg.height = 1; 128 | cloud_msg.width = internal_cloud->size(); 129 | cloud_msg.is_dense = true; 130 | 131 | // Define fields for x, y, z coordinates 132 | cloud_msg.fields.resize(3); 133 | 134 | cloud_msg.fields[0].name = "x"; 135 | cloud_msg.fields[0].offset = 0; 136 | cloud_msg.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; 137 | cloud_msg.fields[0].count = 1; 138 | 139 | cloud_msg.fields[1].name = "y"; 140 | cloud_msg.fields[1].offset = 4; 141 | cloud_msg.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; 142 | cloud_msg.fields[1].count = 1; 143 | 144 | cloud_msg.fields[2].name = "z"; 145 | cloud_msg.fields[2].offset = 8; 146 | cloud_msg.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; 147 | cloud_msg.fields[2].count = 1; 148 | 149 | cloud_msg.point_step = 12; // 3 * sizeof(float32) 150 | cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width; 151 | 152 | // Allocate memory for the data 153 | cloud_msg.data.resize(cloud_msg.row_step); 154 | 155 | // Copy the point data 156 | uint8_t* data_ptr = cloud_msg.data.data(); 157 | for (size_t i = 0; i < internal_cloud->size(); ++i) { 158 | const auto& point = (*internal_cloud)[i]; 159 | 160 | // Copy x, y, z coordinates as float32 161 | memcpy(data_ptr + i * cloud_msg.point_step + 0, &point.x, sizeof(float)); 162 | memcpy(data_ptr + i * cloud_msg.point_step + 4, &point.y, sizeof(float)); 163 | memcpy(data_ptr + i * cloud_msg.point_step + 8, &point.z, sizeof(float)); 164 | } 165 | 166 | return cloud_msg; 167 | } 168 | 169 | void load_config() { 170 | // Get config file parameter 171 | std::string config_file = this->get_parameter("config_file").as_string(); 172 | 173 | if (config_file.empty()) { 174 | RCLCPP_WARN(this->get_logger(), "No config file specified, using default parameters"); 175 | } 176 | 177 | RCLCPP_INFO(this->get_logger(), "Attempting to load config from: %s", config_file.c_str()); 178 | 179 | try { 180 | if (!config_file.empty()) { 181 | // Use ConfigManager to load YAML configuration 182 | lidar_odometry::util::ConfigManager::instance().load_from_file(config_file); 183 | config_ = lidar_odometry::util::ConfigManager::instance().get_config(); 184 | 185 | RCLCPP_INFO(this->get_logger(), "Successfully loaded config from YAML file"); 186 | } else { 187 | // Use default configuration 188 | config_ = lidar_odometry::util::SystemConfig{}; 189 | RCLCPP_WARN(this->get_logger(), "Using default config parameters"); 190 | } 191 | 192 | } catch (const std::exception& e) { 193 | RCLCPP_ERROR(this->get_logger(), "Failed to load YAML config: %s", e.what()); 194 | RCLCPP_WARN(this->get_logger(), "Using default config parameters as fallback"); 195 | 196 | // Fallback to default config 197 | config_ = lidar_odometry::util::SystemConfig{}; 198 | } 199 | } 200 | 201 | void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { 202 | // Simply add message to queue - fast callback 203 | { 204 | std::lock_guard lock(queue_mutex_); 205 | msg_queue_.push_back(msg); 206 | 207 | // No frame dropping - process all frames 208 | if (msg_queue_.size() > 50) { 209 | RCLCPP_WARN(this->get_logger(), "Queue size large: %zu - consider optimizing processing", msg_queue_.size()); 210 | } 211 | } 212 | 213 | // Notify processing thread 214 | queue_cv_.notify_one(); 215 | } 216 | 217 | void processing_loop() { 218 | RCLCPP_INFO(this->get_logger(), "Processing thread started"); 219 | 220 | while (!should_stop_) { 221 | sensor_msgs::msg::PointCloud2::SharedPtr msg; 222 | 223 | // Wait for new message 224 | { 225 | std::unique_lock lock(queue_mutex_); 226 | queue_cv_.wait(lock, [this] { return !msg_queue_.empty() || should_stop_; }); 227 | 228 | if (should_stop_) break; 229 | 230 | msg = msg_queue_.front(); 231 | msg_queue_.pop_front(); 232 | } 233 | 234 | // Process the frame (original callback logic) 235 | process_frame(msg); 236 | } 237 | 238 | RCLCPP_INFO(this->get_logger(), "Processing thread stopped"); 239 | } 240 | 241 | void process_frame(const sensor_msgs::msg::PointCloud2::SharedPtr& msg) { 242 | frame_count_++; 243 | 244 | // Lazy initialization of estimator 245 | if (!estimator_) { 246 | estimator_ = std::make_shared(config_); 247 | RCLCPP_INFO(this->get_logger(), "Estimator initialized on first frame"); 248 | } 249 | 250 | // Convert ROS message to internal format directly 251 | auto internal_cloud = convert_ros_to_internal(msg); 252 | 253 | RCLCPP_DEBUG(this->get_logger(), "Frame #%d - %zu points", frame_count_, internal_cloud->size()); 254 | 255 | // Create LidarFrame with internal cloud format 256 | double timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; 257 | auto frame = std::make_shared( 258 | frame_count_, timestamp, internal_cloud); 259 | 260 | // Store current frame for feature publishing 261 | current_frame_ = frame; 262 | 263 | // Process frame 264 | bool success = estimator_->process_frame(frame); 265 | 266 | if (success) { 267 | // Get current pose 268 | const auto& pose = estimator_->get_current_pose(); 269 | 270 | // Publish odometry 271 | publish_odometry(msg, pose); 272 | 273 | // Publish visualization data 274 | spdlog::debug("Publishing visualization data"); 275 | publish_map_points(msg->header); 276 | publish_current_cloud(msg); 277 | publish_features(msg->header); 278 | publish_trajectory(msg->header); 279 | 280 | RCLCPP_DEBUG(this->get_logger(), "Processing successful - pose: [%.3f, %.3f, %.3f]", 281 | pose.translation().x(), pose.translation().y(), pose.translation().z()); 282 | } else { 283 | // Even if processing failed, publish current cloud for debugging 284 | publish_current_cloud(msg); 285 | RCLCPP_WARN(this->get_logger(), "Processing failed"); 286 | } 287 | } 288 | 289 | void publish_odometry(const sensor_msgs::msg::PointCloud2::SharedPtr& msg, 290 | const lidar_odometry::util::SE3f& pose) { 291 | nav_msgs::msg::Odometry odom_msg; 292 | odom_msg.header = msg->header; 293 | odom_msg.header.frame_id = "odom"; 294 | odom_msg.child_frame_id = "base_link"; 295 | 296 | // Position 297 | const auto& t = pose.translation(); 298 | odom_msg.pose.pose.position.x = t.x(); 299 | odom_msg.pose.pose.position.y = t.y(); 300 | odom_msg.pose.pose.position.z = t.z(); 301 | 302 | // Orientation 303 | const auto q = pose.unit_quaternion(); 304 | odom_msg.pose.pose.orientation.w = q.w(); 305 | odom_msg.pose.pose.orientation.x = q.x(); 306 | odom_msg.pose.pose.orientation.y = q.y(); 307 | odom_msg.pose.pose.orientation.z = q.z(); 308 | 309 | odom_pub_->publish(odom_msg); 310 | 311 | // Publish TF transforms using rosbag timestamp 312 | geometry_msgs::msg::TransformStamped odom_to_base_tf; 313 | odom_to_base_tf.header = odom_msg.header; 314 | odom_to_base_tf.child_frame_id = odom_msg.child_frame_id; 315 | odom_to_base_tf.transform.translation.x = t.x(); 316 | odom_to_base_tf.transform.translation.y = t.y(); 317 | odom_to_base_tf.transform.translation.z = t.z(); 318 | odom_to_base_tf.transform.rotation = odom_msg.pose.pose.orientation; 319 | 320 | // Also publish map -> odom transform (identity for now) 321 | geometry_msgs::msg::TransformStamped map_to_odom_tf; 322 | map_to_odom_tf.header.stamp = msg->header.stamp; 323 | map_to_odom_tf.header.frame_id = "map"; 324 | map_to_odom_tf.child_frame_id = "odom"; 325 | map_to_odom_tf.transform.translation.x = 0.0; 326 | map_to_odom_tf.transform.translation.y = 0.0; 327 | map_to_odom_tf.transform.translation.z = 0.0; 328 | map_to_odom_tf.transform.rotation.w = 1.0; 329 | map_to_odom_tf.transform.rotation.x = 0.0; 330 | map_to_odom_tf.transform.rotation.y = 0.0; 331 | map_to_odom_tf.transform.rotation.z = 0.0; 332 | 333 | // Send both transforms at once 334 | std::vector transforms = {map_to_odom_tf, odom_to_base_tf}; 335 | tf_broadcaster_->sendTransform(transforms); 336 | } 337 | 338 | void publish_map_points(const std_msgs::msg::Header& header) { 339 | auto map_cloud = estimator_->get_local_map(); 340 | 341 | spdlog::debug("Publishing map points - count: {}", map_cloud ? map_cloud->size() : 0); 342 | if (map_cloud && !map_cloud->empty()) { 343 | // Convert internal cloud to ROS message directly (no PCL) 344 | auto map_msg = convert_internal_to_ros( 345 | std::const_pointer_cast(map_cloud), 346 | "odom"); 347 | map_msg.header = header; 348 | map_msg.header.frame_id = "odom"; 349 | 350 | map_pub_->publish(map_msg); 351 | RCLCPP_INFO(this->get_logger(), "Published map points: %zu", map_cloud->size()); 352 | } else { 353 | RCLCPP_DEBUG(this->get_logger(), "No map points to publish"); 354 | } 355 | } 356 | 357 | void publish_current_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr& msg) { 358 | // Republish current cloud with odom frame for visualization 359 | auto current_msg = *msg; 360 | current_msg.header.frame_id = "base_link"; 361 | current_cloud_pub_->publish(current_msg); 362 | } 363 | 364 | void publish_features(const std_msgs::msg::Header& header) { 365 | // Get feature cloud from current frame 366 | if (!current_frame_) { 367 | RCLCPP_DEBUG(this->get_logger(), "No current frame available for features"); 368 | return; 369 | } 370 | 371 | auto feature_cloud = current_frame_->get_feature_cloud(); 372 | if (!feature_cloud || feature_cloud->empty()) { 373 | RCLCPP_DEBUG(this->get_logger(), "No feature points available"); 374 | return; 375 | } 376 | 377 | // Transform features to world coordinates 378 | Eigen::Matrix4f transform_matrix = current_frame_->get_pose().matrix().cast(); 379 | 380 | auto world_features = std::make_shared(); 381 | world_features->reserve(feature_cloud->size()); 382 | 383 | for (size_t i = 0; i < feature_cloud->size(); ++i) { 384 | const auto& point = (*feature_cloud)[i]; 385 | // Transform point to world coordinates 386 | Eigen::Vector4f local_point(point.x, point.y, point.z, 1.0f); 387 | Eigen::Vector4f world_point = transform_matrix * local_point; 388 | 389 | world_features->push_back(world_point.x(), world_point.y(), world_point.z()); 390 | } 391 | 392 | // Convert to ROS message directly (no PCL) 393 | auto feature_msg = convert_internal_to_ros(world_features, "odom"); 394 | feature_msg.header = header; 395 | feature_msg.header.frame_id = "odom"; // World coordinate frame 396 | features_pub_->publish(feature_msg); 397 | 398 | RCLCPP_DEBUG(this->get_logger(), "Published feature points: %zu", world_features->size()); 399 | } 400 | 401 | void publish_trajectory(const std_msgs::msg::Header& header) { 402 | // Get all keyframes from estimator to build complete trajectory 403 | nav_msgs::msg::Path path; 404 | path.header = header; 405 | path.header.frame_id = "odom"; 406 | 407 | // Get all keyframes using existing functions 408 | size_t keyframe_count = estimator_->get_keyframe_count(); 409 | 410 | std::vector trajectory_poses; 411 | trajectory_poses.reserve(keyframe_count); 412 | 413 | for (size_t i = 0; i < keyframe_count; ++i) { 414 | auto keyframe = estimator_->get_keyframe(i); 415 | if (!keyframe) continue; 416 | 417 | const auto& pose = keyframe->get_pose(); 418 | 419 | geometry_msgs::msg::PoseStamped pose_stamped; 420 | pose_stamped.header = path.header; 421 | pose_stamped.header.stamp = rclcpp::Time(static_cast(keyframe->get_timestamp() * 1e9)); 422 | 423 | const auto& t = pose.translation(); 424 | const auto q = pose.unit_quaternion(); 425 | 426 | pose_stamped.pose.position.x = t.x(); 427 | pose_stamped.pose.position.y = t.y(); 428 | pose_stamped.pose.position.z = t.z(); 429 | pose_stamped.pose.orientation.w = q.w(); 430 | pose_stamped.pose.orientation.x = q.x(); 431 | pose_stamped.pose.orientation.y = q.y(); 432 | pose_stamped.pose.orientation.z = q.z(); 433 | 434 | trajectory_poses.push_back(pose_stamped); 435 | } 436 | 437 | path.poses = trajectory_poses; 438 | trajectory_pub_->publish(path); 439 | } 440 | 441 | // Core components 442 | std::shared_ptr estimator_; 443 | lidar_odometry::util::SystemConfig config_; 444 | 445 | // Store current frame for feature publishing 446 | std::shared_ptr current_frame_; 447 | 448 | // ROS components 449 | rclcpp::Subscription::SharedPtr cloud_sub_; 450 | rclcpp::Publisher::SharedPtr odom_pub_; 451 | rclcpp::Publisher::SharedPtr map_pub_; 452 | rclcpp::Publisher::SharedPtr features_pub_; 453 | rclcpp::Publisher::SharedPtr current_cloud_pub_; 454 | rclcpp::Publisher::SharedPtr trajectory_pub_; 455 | 456 | std::shared_ptr tf_broadcaster_; 457 | 458 | // Threading components 459 | std::deque msg_queue_; 460 | std::mutex queue_mutex_; 461 | std::condition_variable queue_cv_; 462 | std::thread processing_thread_; 463 | std::atomic should_stop_{false}; 464 | 465 | int frame_count_ = 0; 466 | }; 467 | 468 | int main(int argc, char** argv) { 469 | 470 | rclcpp::init(argc, argv); 471 | 472 | auto node = std::make_shared(); 473 | 474 | rclcpp::spin(node); 475 | rclcpp::shutdown(); 476 | return 0; 477 | } 478 | --------------------------------------------------------------------------------