From 85da79138fa7e165761b5b24757960109cfd0c25 Mon Sep 17 00:00:00 2001 From: Bona Date: Mon, 15 Dec 2025 22:26:39 -0800 Subject: [PATCH] add fast_lio_ros2 A few things before you can run it: To enable auto-relocalization, edit localizer.yaml: auto_relocalize: true default_pcd_path: "/path/to/your/map.pcd" initial_x: 1.0 initial_y: 2.0 initial_z: 0.0 initial_yaw: 1.57 # radians Expected Log Output [INFO] [localizer_node]: [AUTO-RELOC] Enabled with map: /path/to/map.pcd [INFO] [localizer_node]: [AUTO-RELOC] Initial pose: x=1.000, y=2.000, z=0.000, yaw=1.570 ... [INFO] [localizer_node]: [AUTO-RELOC] Triggering relocalization... [INFO] [localizer_node]: [AUTO-RELOC] Map: /path/to/map.pcd [INFO] [localizer_node]: [AUTO-RELOC] Map loaded successfully [INFO] [localizer_node]: [AUTO-RELOC] Initial guess set, waiting for localization... [INFO] [localizer_node]: [RELOCALIZE] SUCCESS - Localization converged Usage: FASTLIO2 only (SLAM): ros2 launch fastlio2 lio_launch.py FASTLIO2 + Localizer (with relocalization): ros2 launch localizer localizer_launch.py Both launch files include rviz2 automatically. --- src/FASTLIO2_ROS2/README.md | 112 ++ src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt | 80 + src/FASTLIO2_ROS2/fastlio2/LICENSE | 17 + src/FASTLIO2_ROS2/fastlio2/config/lio.yaml | 34 + .../fastlio2/launch/lio_launch.py | 37 + src/FASTLIO2_ROS2/fastlio2/package.xml | 29 + src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz | 248 +++ src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp | 322 +++ .../fastlio2/src/map_builder/commons.cpp | 34 + .../fastlio2/src/map_builder/commons.h | 86 + .../fastlio2/src/map_builder/ieskf.cpp | 120 ++ .../fastlio2/src/map_builder/ieskf.h | 84 + .../fastlio2/src/map_builder/ikd_Tree.cpp | 1732 +++++++++++++++++ .../fastlio2/src/map_builder/ikd_Tree.h | 345 ++++ .../src/map_builder/imu_processor.cpp | 132 ++ .../fastlio2/src/map_builder/imu_processor.h | 24 + .../src/map_builder/lidar_processor.cpp | 268 +++ .../src/map_builder/lidar_processor.h | 48 + .../fastlio2/src/map_builder/map_builder.cpp | 29 + .../fastlio2/src/map_builder/map_builder.h | 27 + src/FASTLIO2_ROS2/fastlio2/src/utils.cpp | 39 + src/FASTLIO2_ROS2/fastlio2/src/utils.h | 29 + src/FASTLIO2_ROS2/hba/CMakeLists.txt | 68 + src/FASTLIO2_ROS2/hba/LICENSE | 17 + src/FASTLIO2_ROS2/hba/config/hba.yaml | 11 + src/FASTLIO2_ROS2/hba/launch/hba_launch.py | 32 + src/FASTLIO2_ROS2/hba/package.xml | 25 + src/FASTLIO2_ROS2/hba/rviz/hba.rviz | 170 ++ src/FASTLIO2_ROS2/hba/src/hba/blam.cpp | 458 +++++ src/FASTLIO2_ROS2/hba/src/hba/blam.h | 140 ++ src/FASTLIO2_ROS2/hba/src/hba/commons.cpp | 1 + src/FASTLIO2_ROS2/hba/src/hba/commons.h | 38 + src/FASTLIO2_ROS2/hba/src/hba/hba.cpp | 174 ++ src/FASTLIO2_ROS2/hba/src/hba/hba.h | 57 + src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp | 126 ++ src/FASTLIO2_ROS2/hba/src/hba_node.cpp | 222 +++ src/FASTLIO2_ROS2/interface/CMakeLists.txt | 36 + src/FASTLIO2_ROS2/interface/LICENSE | 17 + src/FASTLIO2_ROS2/interface/package.xml | 24 + src/FASTLIO2_ROS2/interface/srv/IsValid.srv | 3 + src/FASTLIO2_ROS2/interface/srv/RefineMap.srv | 4 + .../interface/srv/Relocalize.srv | 10 + src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv | 5 + src/FASTLIO2_ROS2/interface/srv/SavePoses.srv | 4 + src/FASTLIO2_ROS2/localizer/CMakeLists.txt | 65 + src/FASTLIO2_ROS2/localizer/LICENSE | 17 + .../localizer/config/localizer.yaml | 25 + .../localizer/launch/localizer_launch.py | 53 + src/FASTLIO2_ROS2/localizer/package.xml | 27 + .../localizer/rviz/localizer.rviz | 219 +++ .../localizer/src/localizer_node.cpp | 411 ++++ .../localizer/src/localizers/commons.cpp | 1 + .../localizer/src/localizers/commons.h | 15 + .../src/localizers/icp_localizer.cpp | 87 + .../localizer/src/localizers/icp_localizer.h | 46 + src/FASTLIO2_ROS2/pgo/CMakeLists.txt | 68 + src/FASTLIO2_ROS2/pgo/LICENSE | 17 + src/FASTLIO2_ROS2/pgo/config/pgo.yaml | 13 + src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py | 46 + src/FASTLIO2_ROS2/pgo/package.xml | 30 + src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz | 258 +++ src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp | 307 +++ src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp | 8 + src/FASTLIO2_ROS2/pgo/src/pgos/commons.h | 30 + src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp | 211 ++ src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h | 78 + 66 files changed, 7550 insertions(+) create mode 100644 src/FASTLIO2_ROS2/README.md create mode 100644 src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/fastlio2/LICENSE create mode 100644 src/FASTLIO2_ROS2/fastlio2/config/lio.yaml create mode 100644 src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py create mode 100644 src/FASTLIO2_ROS2/fastlio2/package.xml create mode 100644 src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/utils.cpp create mode 100644 src/FASTLIO2_ROS2/fastlio2/src/utils.h create mode 100644 src/FASTLIO2_ROS2/hba/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/hba/LICENSE create mode 100644 src/FASTLIO2_ROS2/hba/config/hba.yaml create mode 100644 src/FASTLIO2_ROS2/hba/launch/hba_launch.py create mode 100644 src/FASTLIO2_ROS2/hba/package.xml create mode 100644 src/FASTLIO2_ROS2/hba/rviz/hba.rviz create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/blam.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/blam.h create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/commons.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/commons.h create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/hba.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba/hba.h create mode 100644 src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp create mode 100644 src/FASTLIO2_ROS2/hba/src/hba_node.cpp create mode 100644 src/FASTLIO2_ROS2/interface/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/interface/LICENSE create mode 100644 src/FASTLIO2_ROS2/interface/package.xml create mode 100644 src/FASTLIO2_ROS2/interface/srv/IsValid.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/RefineMap.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/Relocalize.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv create mode 100644 src/FASTLIO2_ROS2/interface/srv/SavePoses.srv create mode 100644 src/FASTLIO2_ROS2/localizer/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/localizer/LICENSE create mode 100644 src/FASTLIO2_ROS2/localizer/config/localizer.yaml create mode 100644 src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py create mode 100644 src/FASTLIO2_ROS2/localizer/package.xml create mode 100644 src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/commons.h create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp create mode 100644 src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h create mode 100644 src/FASTLIO2_ROS2/pgo/CMakeLists.txt create mode 100644 src/FASTLIO2_ROS2/pgo/LICENSE create mode 100644 src/FASTLIO2_ROS2/pgo/config/pgo.yaml create mode 100644 src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py create mode 100644 src/FASTLIO2_ROS2/pgo/package.xml create mode 100644 src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/commons.h create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp create mode 100644 src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h diff --git a/src/FASTLIO2_ROS2/README.md b/src/FASTLIO2_ROS2/README.md new file mode 100644 index 00000000..379ec385 --- /dev/null +++ b/src/FASTLIO2_ROS2/README.md @@ -0,0 +1,112 @@ +# FASTLIO2 ROS2 +## 主要工作 +1. 重构[FASTLIO2](https://github.com/hku-mars/FAST_LIO) 适配ROS2 +2. 添加回环节点，基于位置先验+ICP进行回环检测，基于GTSAM进行位姿图优化 +3. 添加重定位节点，基于由粗到细两阶段ICP进行重定位 +4. 增加一致性地图优化，基于[BLAM](https://github.com/hku-mars/BALM) (小场景地图) 和[HBA](https://github.com/hku-mars/HBA) (大场景地图) + +## 环境依赖 +1. Ubuntu 22.04 +2. ROS2 Humble + +## 编译依赖 +```text +pcl +Eigen +sophus +gtsam +livox_ros_driver2 +``` + +## 详细说明 +### 1.编译 LIVOX-SDK2 +```shell +git clone https://github.com/Livox-SDK/Livox-SDK2.git +cd ./Livox-SDK2/ +mkdir build +cd build +cmake .. && make -j +sudo make install +``` + +### 2.编译 livox_ros_driver2 +```shell +mkdir -r ws_livox/src +git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 +cd ws_livox/src/livox_ros_driver2 +source /opt/ros/humble/setup.sh +./build.sh humble +``` + +### 3.编译 Sophus +```shell +git clone https://github.com/strasdat/Sophus.git +cd Sophus +git checkout 1.22.10 +mkdir build && cd build +cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON +make +sudo make install +``` + +**新的Sophus依赖fmt，可以在CMakeLists.txt中添加add_compile_definitions(SOPHUS_USE_BASIC_LOGGING)去除，否则会报错** + + +## 实例数据集 +```text +链接: https://pan.baidu.com/s/1rTTUlVwxi1ZNo7ZmcpEZ7A?pwd=t6yb 提取码: t6yb +--来自百度网盘超级会员v7的分享 +``` + +## 部分脚本 + +### 1.激光惯性里程计 +```shell +ros2 launch fastlio2 lio_launch.py +ros2 bag play your_bag_file +``` + +### 2.里程计加回环 +#### 启动回环节点 +```shell +ros2 launch pgo pgo_launch.py +ros2 bag play your_bag_file +``` +#### 保存地图 +```shell +ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}" +``` + +### 3.里程计加重定位 +#### 启动重定位节点 +```shell +ros2 launch localizer localizer_launch.py +ros2 bag play your_bag_file // 可选 +``` +#### 设置重定位初始值 +```shell +ros2 service call /localizer/relocalize interface/srv/Relocalize "{"pcd_path": "your_map.pcd", "x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0, "pitch": 0.0, "roll": 0.0}" +``` +#### 检查重定位结果 +```shell +ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}" +``` + +### 4.一致性地图优化 +#### 启动一致性地图优化节点 +```shell +ros2 launch hba hba_launch.py +``` +#### 调用优化服务 +```shell +ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}" +``` +**如果需要调用优化服务，保存地图时需要设置save_patches为true** + +## 特别感谢 +1. [FASTLIO2](https://github.com/hku-mars/FAST_LIO) +2. [BLAM](https://github.com/hku-mars/BALM) +3. [HBA](https://github.com/hku-mars/HBA) +## 性能相关的问题 +该代码主要使用timerCB作为频率触发主函数，由于ROS2中的timer、subscriber以及service的回调实际上运行在同一个线程上，在电脑性能不是好的时候，会出现调用阻塞的情况，建议使用线程并发的方式将耗时的回调独立出来(如timerCB)来提升性能 + diff --git a/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt b/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt new file mode 100644 index 00000000..7fd37a73 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.8) +project(fastlio2) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_definitions(SOPHUS_USE_BASIC_LOGGING) + +add_definitions(-DMP_EN) +add_definitions(-DMP_PROC_NUM=2) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(livox_ros_driver2 REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(OpenMP QUIET) +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Sophus REQUIRED) +find_package(yaml-cpp REQUIRED) + + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +set(SRC_LIST src/map_builder/commons.cpp + src/map_builder/ieskf.cpp + src/map_builder/imu_processor.cpp + src/map_builder/ikd_Tree.cpp + src/map_builder/lidar_processor.cpp + src/map_builder/map_builder.cpp + src/utils.cpp) + +add_executable(lio_node src/lio_node.cpp ${SRC_LIST}) +ament_target_dependencies(lio_node rclcpp std_msgs tf2 tf2_ros nav_msgs sensor_msgs livox_ros_driver2 pcl_conversions geometry_msgs) +target_link_libraries(lio_node + yaml-cpp + ${PCL_LIBRARIES} +) + + +install(TARGETS lio_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/fastlio2/LICENSE b/src/FASTLIO2_ROS2/fastlio2/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml b/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml new file mode 100644 index 00000000..5d77ae3c --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/config/lio.yaml @@ -0,0 +1,34 @@ +imu_topic: /livox/imu +lidar_topic: /livox/lidar +body_frame: body +world_frame: lidar +print_time_cost: false +print_odom: true +odom_log_interval: 0.5 # seconds between odom logs (2Hz) + +lidar_filter_num: 6 +lidar_min_range: 0.5 +lidar_max_range: 30.0 +scan_resolution: 0.15 +map_resolution: 0.3 + +cube_len: 300 +det_range: 60 +move_thresh: 1.5 + +na: 0.01 +ng: 0.01 +nba: 0.0001 +nbg: 0.0001 + +imu_init_num: 20 +near_search_num: 5 +ieskf_max_iter: 5 + +gravity_align: true +esti_il: false + +r_il: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] +t_il: [-0.011, -0.02329, 0.04412] + +lidar_cov_inv: 1000.0 diff --git a/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py b/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py new file mode 100644 index 00000000..754b5818 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/launch/lio_launch.py @@ -0,0 +1,37 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + rviz_cfg = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "rviz", "fastlio2.rviz"] + ) + + config_path = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "config", "lio.yaml"] + ) + + + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[{"config_path": config_path.perform(launch.LaunchContext())}] + ), + launch_ros.actions.Node( + package="rviz2", + namespace="fastlio2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ), + ] + ) diff --git a/src/FASTLIO2_ROS2/fastlio2/package.xml b/src/FASTLIO2_ROS2/fastlio2/package.xml new file mode 100644 index 00000000..4bd70f57 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/package.xml @@ -0,0 +1,29 @@ + + + + fastlio2 + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + + rclcpp + std_msgs + tf2 + tf2_ros + nav_msgs + sensor_msgs + pcl_conversions + livox_ros_driver2 + geometry_msgs + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz b/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz new file mode 100644 index 00000000..0b3e4a93 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/rviz/fastlio2.rviz @@ -0,0 +1,248 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud22 + - /Axes1 + - /Axes2 + - /Path1 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/body_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 200 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/world_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: lidar + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: body + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/lio_path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: lidar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.265907287597656 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8947967290878296 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.220390319824219 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp b/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp new file mode 100644 index 00000000..ef006ffa --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/lio_node.cpp @@ -0,0 +1,322 @@ +#include +#include +#include +#include +#include +#include +// #include +#include +#include +#include + +#include "utils.h" +#include "map_builder/commons.h" +#include "map_builder/map_builder.h" + +#include +#include "tf2_ros/transform_broadcaster.h" +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +struct NodeConfig +{ + std::string imu_topic = "/livox/imu"; + std::string lidar_topic = "/livox/lidar"; + std::string body_frame = "body"; + std::string world_frame = "lidar"; + bool print_time_cost = false; + bool print_odom = false; + double odom_log_interval = 5.0; // seconds between odom logs +}; +struct StateData +{ + bool lidar_pushed = false; + std::mutex imu_mutex; + std::mutex lidar_mutex; + double last_lidar_time = -1.0; + double last_imu_time = -1.0; + double last_odom_log_time = -1.0; + std::deque imu_buffer; + std::deque::Ptr>> lidar_buffer; + nav_msgs::msg::Path path; +}; + +class LIONode : public rclcpp::Node +{ +public: + LIONode() : Node("lio_node") + { + RCLCPP_INFO(this->get_logger(), "LIO Node Started"); + loadParameters(); + + m_imu_sub = this->create_subscription(m_node_config.imu_topic, 10, std::bind(&LIONode::imuCB, this, std::placeholders::_1)); + m_lidar_sub = this->create_subscription(m_node_config.lidar_topic, 10, std::bind(&LIONode::lidarCB, this, std::placeholders::_1)); + + m_body_cloud_pub = this->create_publisher("body_cloud", 10000); + m_world_cloud_pub = this->create_publisher("world_cloud", 10000); + m_path_pub = this->create_publisher("lio_path", 10000); + m_odom_pub = this->create_publisher("lio_odom", 10000); + m_tf_broadcaster = std::make_shared(*this); + + m_state_data.path.poses.clear(); + m_state_data.path.header.frame_id = m_node_config.world_frame; + + m_kf = std::make_shared(); + m_builder = std::make_shared(m_builder_config, m_kf); + m_timer = this->create_wall_timer(20ms, std::bind(&LIONode::timerCB, this)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + + m_node_config.imu_topic = config["imu_topic"].as(); + m_node_config.lidar_topic = config["lidar_topic"].as(); + m_node_config.body_frame = config["body_frame"].as(); + m_node_config.world_frame = config["world_frame"].as(); + m_node_config.print_time_cost = config["print_time_cost"].as(); + // Optional: load odom logging config with defaults + if (config["print_odom"]) + m_node_config.print_odom = config["print_odom"].as(); + if (config["odom_log_interval"]) + m_node_config.odom_log_interval = config["odom_log_interval"].as(); + + m_builder_config.lidar_filter_num = config["lidar_filter_num"].as(); + m_builder_config.lidar_min_range = config["lidar_min_range"].as(); + m_builder_config.lidar_max_range = config["lidar_max_range"].as(); + m_builder_config.scan_resolution = config["scan_resolution"].as(); + m_builder_config.map_resolution = config["map_resolution"].as(); + m_builder_config.cube_len = config["cube_len"].as(); + m_builder_config.det_range = config["det_range"].as(); + m_builder_config.move_thresh = config["move_thresh"].as(); + m_builder_config.na = config["na"].as(); + m_builder_config.ng = config["ng"].as(); + m_builder_config.nba = config["nba"].as(); + m_builder_config.nbg = config["nbg"].as(); + + m_builder_config.imu_init_num = config["imu_init_num"].as(); + m_builder_config.near_search_num = config["near_search_num"].as(); + m_builder_config.ieskf_max_iter = config["ieskf_max_iter"].as(); + m_builder_config.gravity_align = config["gravity_align"].as(); + m_builder_config.esti_il = config["esti_il"].as(); + std::vector t_il_vec = config["t_il"].as>(); + std::vector r_il_vec = config["r_il"].as>(); + m_builder_config.t_il << t_il_vec[0], t_il_vec[1], t_il_vec[2]; + m_builder_config.r_il << r_il_vec[0], r_il_vec[1], r_il_vec[2], r_il_vec[3], r_il_vec[4], r_il_vec[5], r_il_vec[6], r_il_vec[7], r_il_vec[8]; + m_builder_config.lidar_cov_inv = config["lidar_cov_inv"].as(); + } + + void imuCB(const sensor_msgs::msg::Imu::SharedPtr msg) + { + std::lock_guard lock(m_state_data.imu_mutex); + double timestamp = Utils::getSec(msg->header); + if (timestamp < m_state_data.last_imu_time) + { + RCLCPP_WARN(this->get_logger(), "IMU Message is out of order"); + std::deque().swap(m_state_data.imu_buffer); + } + m_state_data.imu_buffer.emplace_back(V3D(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z) * 10.0, + V3D(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z), + timestamp); + m_state_data.last_imu_time = timestamp; + } + void lidarCB(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg) + { + CloudType::Ptr cloud = Utils::livox2PCL(msg, m_builder_config.lidar_filter_num, m_builder_config.lidar_min_range, m_builder_config.lidar_max_range); + std::lock_guard lock(m_state_data.lidar_mutex); + double timestamp = Utils::getSec(msg->header); + if (timestamp < m_state_data.last_lidar_time) + { + RCLCPP_WARN(this->get_logger(), "Lidar Message is out of order"); + std::deque::Ptr>>().swap(m_state_data.lidar_buffer); + } + m_state_data.lidar_buffer.emplace_back(timestamp, cloud); + m_state_data.last_lidar_time = timestamp; + } + + bool syncPackage() + { + if (m_state_data.imu_buffer.empty() || m_state_data.lidar_buffer.empty()) + return false; + if (!m_state_data.lidar_pushed) + { + m_package.cloud = m_state_data.lidar_buffer.front().second; + std::sort(m_package.cloud->points.begin(), m_package.cloud->points.end(), [](PointType &p1, PointType &p2) + { return p1.curvature < p2.curvature; }); + m_package.cloud_start_time = m_state_data.lidar_buffer.front().first; + m_package.cloud_end_time = m_package.cloud_start_time + m_package.cloud->points.back().curvature / 1000.0; + m_state_data.lidar_pushed = true; + } + if (m_state_data.last_imu_time < m_package.cloud_end_time) + return false; + + Vec().swap(m_package.imus); + while (!m_state_data.imu_buffer.empty() && m_state_data.imu_buffer.front().time < m_package.cloud_end_time) + { + m_package.imus.emplace_back(m_state_data.imu_buffer.front()); + m_state_data.imu_buffer.pop_front(); + } + m_state_data.lidar_buffer.pop_front(); + m_state_data.lidar_pushed = false; + return true; + } + + void publishCloud(rclcpp::Publisher::SharedPtr pub, CloudType::Ptr cloud, std::string frame_id, const double &time) + { + if (pub->get_subscription_count() <= 0) + return; + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = frame_id; + cloud_msg.header.stamp = Utils::getTime(time); + pub->publish(cloud_msg); + } + + void publishOdometry(rclcpp::Publisher::SharedPtr odom_pub, std::string frame_id, std::string child_frame, const double &time) + { + nav_msgs::msg::Odometry odom; + odom.header.frame_id = frame_id; + odom.header.stamp = Utils::getTime(time); + odom.child_frame_id = child_frame; + odom.pose.pose.position.x = m_kf->x().t_wi.x(); + odom.pose.pose.position.y = m_kf->x().t_wi.y(); + odom.pose.pose.position.z = m_kf->x().t_wi.z(); + Eigen::Quaterniond q(m_kf->x().r_wi); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + + V3D vel = m_kf->x().r_wi.transpose() * m_kf->x().v; + odom.twist.twist.linear.x = vel.x(); + odom.twist.twist.linear.y = vel.y(); + odom.twist.twist.linear.z = vel.z(); + + // Throttled odometry logging (logs at odom_log_interval seconds) + bool should_log = (m_state_data.last_odom_log_time < 0) || // First time + ((time - m_state_data.last_odom_log_time) >= m_node_config.odom_log_interval); + if (m_node_config.print_odom && should_log) + { + m_state_data.last_odom_log_time = time; + double yaw = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); + double speed = std::sqrt(vel.x() * vel.x() + vel.y() * vel.y() + vel.z() * vel.z()); + RCLCPP_INFO(this->get_logger(), "[ODOM] pos: x=%.3f, y=%.3f, z=%.3f | yaw=%.3f rad (%.1f deg) | speed=%.3f m/s", + m_kf->x().t_wi.x(), m_kf->x().t_wi.y(), m_kf->x().t_wi.z(), + yaw, yaw * 180.0 / M_PI, speed); + } + + if (odom_pub->get_subscription_count() > 0) + odom_pub->publish(odom); + } + + void publishPath(rclcpp::Publisher::SharedPtr path_pub, std::string frame_id, const double &time) + { + if (path_pub->get_subscription_count() <= 0) + return; + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = frame_id; + pose.header.stamp = Utils::getTime(time); + pose.pose.position.x = m_kf->x().t_wi.x(); + pose.pose.position.y = m_kf->x().t_wi.y(); + pose.pose.position.z = m_kf->x().t_wi.z(); + Eigen::Quaterniond q(m_kf->x().r_wi); + pose.pose.orientation.x = q.x(); + pose.pose.orientation.y = q.y(); + pose.pose.orientation.z = q.z(); + pose.pose.orientation.w = q.w(); + m_state_data.path.poses.push_back(pose); + path_pub->publish(m_state_data.path); + } + + void broadCastTF(std::shared_ptr broad_caster, std::string frame_id, std::string child_frame, const double &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = frame_id; + transformStamped.child_frame_id = child_frame; + transformStamped.header.stamp = Utils::getTime(time); + Eigen::Quaterniond q(m_kf->x().r_wi); + V3D t = m_kf->x().t_wi; + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + broad_caster->sendTransform(transformStamped); + } + + void timerCB() + { + if (!syncPackage()) + return; + auto t1 = std::chrono::high_resolution_clock::now(); + m_builder->process(m_package); + auto t2 = std::chrono::high_resolution_clock::now(); + + if (m_node_config.print_time_cost) + { + auto time_used = std::chrono::duration_cast>(t2 - t1).count() * 1000; + RCLCPP_WARN(this->get_logger(), "Time cost: %.2f ms", time_used); + } + + if (m_builder->status() != BuilderStatus::MAPPING) + return; + + broadCastTF(m_tf_broadcaster, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); + + publishOdometry(m_odom_pub, m_node_config.world_frame, m_node_config.body_frame, m_package.cloud_end_time); + + CloudType::Ptr body_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_kf->x().r_il, m_kf->x().t_il); + + publishCloud(m_body_cloud_pub, body_cloud, m_node_config.body_frame, m_package.cloud_end_time); + + CloudType::Ptr world_cloud = m_builder->lidar_processor()->transformCloud(m_package.cloud, m_builder->lidar_processor()->r_wl(), m_builder->lidar_processor()->t_wl()); + + publishCloud(m_world_cloud_pub, world_cloud, m_node_config.world_frame, m_package.cloud_end_time); + + publishPath(m_path_pub, m_node_config.world_frame, m_package.cloud_end_time); + } + +private: + rclcpp::Subscription::SharedPtr m_lidar_sub; + rclcpp::Subscription::SharedPtr m_imu_sub; + + rclcpp::Publisher::SharedPtr m_body_cloud_pub; + rclcpp::Publisher::SharedPtr m_world_cloud_pub; + rclcpp::Publisher::SharedPtr m_path_pub; + rclcpp::Publisher::SharedPtr m_odom_pub; + + rclcpp::TimerBase::SharedPtr m_timer; + StateData m_state_data; + SyncPackage m_package; + NodeConfig m_node_config; + Config m_builder_config; + std::shared_ptr m_kf; + std::shared_ptr m_builder; + std::shared_ptr m_tf_broadcaster; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp new file mode 100644 index 00000000..6625b0c4 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.cpp @@ -0,0 +1,34 @@ +#include "commons.h" +bool esti_plane(PointVec &points, const double &thresh, V4D &out) +{ + Eigen::MatrixXd A(points.size(), 3); + Eigen::MatrixXd b(points.size(), 1); + A.setZero(); + b.setOnes(); + b *= -1.0; + for (size_t i = 0; i < points.size(); i++) + { + A(i, 0) = points[i].x; + A(i, 1) = points[i].y; + A(i, 2) = points[i].z; + } + V3D normvec = A.colPivHouseholderQr().solve(b); + double norm = normvec.norm(); + out[0] = normvec(0) / norm; + out[1] = normvec(1) / norm; + out[2] = normvec(2) / norm; + out[3] = 1.0 / norm; + for (size_t j = 0; j < points.size(); j++) + { + if (std::fabs(out(0) * points[j].x + out(1) * points[j].y + out(2) * points[j].z + out(3)) > thresh) + { + return false; + } + } + return true; +} + +float sq_dist(const PointType &p1, const PointType &p2) +{ + return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h new file mode 100644 index 00000000..419f2d59 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/commons.h @@ -0,0 +1,86 @@ +#pragma once +#include +#include +#include + +using PointType = pcl::PointXYZINormal; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M2D = Eigen::Matrix2d; +using V2D = Eigen::Vector2d; +using M2F = Eigen::Matrix2f; +using V2F = Eigen::Vector2f; +using M4D = Eigen::Matrix4d; +using V4D = Eigen::Vector4d; + + +template +using Vec = std::vector; + + +bool esti_plane(PointVec &points, const double &thresh, V4D &out); + +float sq_dist(const PointType &p1, const PointType &p2); + +struct Config +{ + int lidar_filter_num = 3; + double lidar_min_range = 0.5; + double lidar_max_range = 20.0; + double scan_resolution = 0.15; + double map_resolution = 0.3; + + double cube_len = 300; + double det_range = 60; + double move_thresh = 1.5; + + double na = 0.01; + double ng = 0.01; + double nba = 0.0001; + double nbg = 0.0001; + int imu_init_num = 20; + int near_search_num = 5; + int ieskf_max_iter = 5; + bool gravity_align = true; + bool esti_il = false; + M3D r_il = M3D::Identity(); + V3D t_il = V3D::Zero(); + + double lidar_cov_inv = 1000.0; +}; + +struct IMUData +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + V3D acc; + V3D gyro; + double time; + IMUData() = default; + IMUData(const V3D &a, const V3D &g, double &t) : acc(a), gyro(g), time(t) {} +}; + +struct Pose +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + double offset; + V3D acc; + V3D gyro; + V3D vel; + V3D trans; + M3D rot; + Pose() = default; + Pose(double t, const V3D &a, const V3D &g, const V3D &v, const V3D &p, const M3D &r) : offset(t), acc(a), gyro(g), vel(v), trans(p), rot(r) {} +}; + +struct SyncPackage +{ + Vec imus; + CloudType::Ptr cloud; + double cloud_start_time = 0.0; + double cloud_end_time = 0.0; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp new file mode 100644 index 00000000..53b5ef95 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.cpp @@ -0,0 +1,120 @@ +#include "ieskf.h" + +double State::gravity = 9.81; + +M3D Jr(const V3D &inp) +{ + return Sophus::SO3d::leftJacobian(inp).transpose(); +} +M3D JrInv(const V3D &inp) +{ + return Sophus::SO3d::leftJacobianInverse(inp).transpose(); +} + +void State::operator+=(const V21D &delta) +{ + r_wi *= Sophus::SO3d::exp(delta.segment<3>(0)).matrix(); + t_wi += delta.segment<3>(3); + r_il *= Sophus::SO3d::exp(delta.segment<3>(6)).matrix(); + t_il += delta.segment<3>(9); + v += delta.segment<3>(12); + bg += delta.segment<3>(15); + ba += delta.segment<3>(18); +} + +V21D State::operator-(const State &other) const +{ + V21D delta = V21D::Zero(); + delta.segment<3>(0) = Sophus::SO3d(other.r_wi.transpose() * r_wi).log(); + delta.segment<3>(3) = t_wi - other.t_wi; + delta.segment<3>(6) = Sophus::SO3d(other.r_il.transpose() * r_il).log(); + delta.segment<3>(9) = t_il - other.t_il; + delta.segment<3>(12) = v - other.v; + delta.segment<3>(15) = bg - other.bg; + delta.segment<3>(18) = ba - other.ba; + return delta; +} + +std::ostream &operator<<(std::ostream &os, const State &state) +{ + os << "==============START===============" << std::endl; + os << "r_wi: " << state.r_wi.eulerAngles(2, 1, 0).transpose() << std::endl; + os << "t_il: " << state.t_il.transpose() << std::endl; + os << "r_il: " << state.r_il.eulerAngles(2, 1, 0).transpose() << std::endl; + os << "t_wi: " << state.t_wi.transpose() << std::endl; + os << "v: " << state.v.transpose() << std::endl; + os << "bg: " << state.bg.transpose() << std::endl; + os << "ba: " << state.ba.transpose() << std::endl; + os << "g: " << state.g.transpose() << std::endl; + os << "===============END================" << std::endl; + + return os; +} + +void IESKF::predict(const Input &inp, double dt, const M12D &Q) +{ + V21D delta = V21D::Zero(); + delta.segment<3>(0) = (inp.gyro - m_x.bg) * dt; + delta.segment<3>(3) = m_x.v * dt; + delta.segment<3>(12) = (m_x.r_wi * (inp.acc - m_x.ba) + m_x.g) * dt; + + m_F.setIdentity(); + m_F.block<3, 3>(0, 0) = Sophus::SO3d::exp(-(inp.gyro - m_x.bg) * dt).matrix(); + m_F.block<3, 3>(0, 15) = -Jr((inp.gyro - m_x.bg) * dt) * dt; + m_F.block<3, 3>(3, 12) = Eigen::Matrix3d::Identity() * dt; + m_F.block<3, 3>(12, 0) = -m_x.r_wi * Sophus::SO3d::hat(inp.acc - m_x.ba) * dt; + m_F.block<3, 3>(12, 18) = -m_x.r_wi * dt; + + m_G.setZero(); + m_G.block<3, 3>(0, 0) = -Jr((inp.gyro - m_x.bg) * dt) * dt; + m_G.block<3, 3>(12, 3) = -m_x.r_wi * dt; + m_G.block<3, 3>(15, 6) = Eigen::Matrix3d::Identity() * dt; + m_G.block<3, 3>(18, 9) = Eigen::Matrix3d::Identity() * dt; + + m_x += delta; + m_P = m_F * m_P * m_F.transpose() + m_G * Q * m_G.transpose(); +} + +void IESKF::update() +{ + State predict_x = m_x; + SharedState shared_data; + shared_data.iter_num = 0; + shared_data.res = 1e10; + V21D delta = V21D::Zero(); + M21D H = M21D::Identity(); + V21D b; + + for (size_t i = 0; i < m_max_iter; i++) + { + m_loss_func(m_x, shared_data); + if (!shared_data.valid) + break; + H.setZero(); + b.setZero(); + delta = m_x - predict_x; + M21D J = M21D::Identity(); + J.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0)); + J.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6)); + H += J.transpose() * m_P.inverse() * J; + b += J.transpose() * m_P.inverse() * delta; + + H.block<12, 12>(0, 0) += shared_data.H; + b.block<12, 1>(0, 0) += shared_data.b; + + delta = -H.inverse() * b; + + m_x += delta; + shared_data.iter_num += 1; + + if (m_stop_func(delta)) + break; + } + + M21D L = M21D::Identity(); + // L.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0)); + // L.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6)); + L.block<3, 3>(0, 0) = Jr(delta.segment<3>(0)); + L.block<3, 3>(6, 6) = Jr(delta.segment<3>(6)); + m_P = L * H.inverse() * L.transpose(); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h new file mode 100644 index 00000000..b5232cf7 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ieskf.h @@ -0,0 +1,84 @@ +#pragma once +#include +#include +#include "commons.h" + +using M12D = Eigen::Matrix; +using M21D = Eigen::Matrix; + +using V12D = Eigen::Matrix; +using V21D = Eigen::Matrix; +using M21X12D = Eigen::Matrix; + +M3D Jr(const V3D &inp); +M3D JrInv(const V3D &inp); + +struct SharedState +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + M12D H; + V12D b; + double res = 1e10; + bool valid = false; + size_t iter_num = 0; +}; +struct Input +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + V3D acc; + V3D gyro; + Input() = default; + Input(V3D &a, V3D &g) : acc(a), gyro(g) {} + Input(double a1, double a2, double a3, double g1, double g2, double g3) : acc(a1, a2, a3), gyro(g1, g2, g3) {} +}; +struct State +{ + static double gravity; + M3D r_wi = M3D::Identity(); + V3D t_wi = V3D::Zero(); + M3D r_il = M3D::Identity(); + V3D t_il = V3D::Zero(); + V3D v = V3D::Zero(); + V3D bg = V3D::Zero(); + V3D ba = V3D::Zero(); + V3D g = V3D(0.0, 0.0, -9.81); + + void initGravityDir(const V3D &gravity_dir) { g = gravity_dir.normalized() * State::gravity; } + + void operator+=(const V21D &delta); + + V21D operator-(const State &other) const; + + friend std::ostream &operator<<(std::ostream &os, const State &state); +}; + +using loss_func = std::function; +using stop_func = std::function; + +class IESKF +{ +public: + IESKF() = default; + void setMaxIter(size_t iter) { m_max_iter = iter; } + void setLossFunction(loss_func func) { m_loss_func = func; } + void setStopFunction(stop_func func) { m_stop_func = func; } + + void predict(const Input &inp, double dt, const M12D &Q); + + void update(); + + State &x() { return m_x; } + + M21D &P() { return m_P; } + +private: + size_t m_max_iter = 10; + State m_x; + M21D m_P; + loss_func m_loss_func; + stop_func m_stop_func; + M21D m_F; + M21X12D m_G; +}; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp new file mode 100644 index 00000000..022414ff --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.cpp @@ -0,0 +1,1732 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) +{ + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) +{ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE *root) +{ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->radius_sq = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock), NULL); +} + +template +int KD_TREE::size() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + return Root_Node->TreeSize; + } + else + { + return 0; + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range() +{ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } + else + { + memset(&range, 0, sizeof(range)); + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } + else + { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize - Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) +{ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } + else + { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread() +{ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread() +{ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) + pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void *KD_TREE::multi_thread_ptr(void *arg) +{ + KD_TREE *handle = (KD_TREE *)arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild() +{ + bool terminated = false; + KD_TREE_NODE *father_ptr, **new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated) + { + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr) + { + /* Traverse and copy */ + if (!Rebuild_Logger.empty()) + { + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) + { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE *new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0) + { + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()) + { + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter++; + if (tmp_counter % 10 == 0) + usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) + { + father_ptr->left_son_ptr = new_root_node; + } + else if (father_ptr->right_son_ptr == *Rebuild_Ptr) + { + father_ptr->right_son_ptr = new_root_node; + } + else + { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) + new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) + Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE *update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node) + { + update_root = update_root->father_ptr; + if (update_root->working_flag) + break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) + break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) + break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } + else + { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) +{ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) + (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) + (*root)->invalid_point_num = (*root)->TreeSize; + else + (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud) +{ + if (Root_Node != nullptr) + { + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) + return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist) +{ + MANUAL_HEAP q(2 * k_nearest); + q.clear(); + vector().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Search(Root_Node, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest, int(q.size())); + PointVector().swap(Nearest_Points); + vector().swap(Point_Distance); + for (int i = 0; i < k_found; i++) + { + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) +{ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i = 0; i < PointToAdd.size(); i++) + { + if (downsample_switch) + { + Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; + Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; + Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; + Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; + Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; + Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; + mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; + mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; + mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; + PointVector().swap(Downsample_Storage); + Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); + min_dist = calc_dist(PointToAdd[i], mid_point); + downsample_result = PointToAdd[i]; + for (int index = 0; index < Downsample_Storage.size(); index++) + { + tmp_dist = calc_dist(Downsample_Storage[index], mid_point); + if (tmp_dist < min_dist) + { + min_dist = tmp_dist; + downsample_result = Downsample_Storage[index]; + } + } + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter++; + } + } + else + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, false, true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter++; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) + Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector &BoxPoints) +{ + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_range(&Root_Node, BoxPoints[i], true); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node, BoxPoints[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector &PointToDel) +{ + for (int i = 0; i < PointToDel.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Delete_by_point(&Root_Node, PointToDel[i], true); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToDel[i]; + operation.op = DELETE_POINT; + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&Root_Node, PointToDel[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) +{ + int tmp_counter = 0; + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector &removed_points) +{ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size(); i++) + { + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size(); i++) + { + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) +{ + if (l > r) + return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l + r) >> 1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0, 0, 0}; + for (i = l; i <= r; i++) + { + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i = 0; i < 3; i++) + dim_range[i] = max_value[i] - min_value[i]; + for (i = 1; i < 3; i++) + if (dim_range[i] > dim_range[div_axis]) + div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); + break; + default: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; + BuildTree(&left_son, l, mid - 1, Storage); + BuildTree(&right_son, mid + 1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE **root) +{ + KD_TREE_NODE *father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) + { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) + { + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) + { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } + else + { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); + if (*root != nullptr) + (*root)->father_ptr = father_ptr; + if (*root == Root_Node) + STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample) + { + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) + (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) + delete_box_log.op = DOWNSAMPLE_DELETE; + else + delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) + { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) + (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) +{ + if ((*root) == nullptr) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) +{ + if (*root == nullptr) + { + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) +{ + if (root == nullptr || root->tree_deleted) + return; + float cur_dist = calc_box_dist(root, point); + float max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) + return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) + { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0) + { + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + else + { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted) + { + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) + { + if (q.size() >= k_nearest) + q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) + { + if (dist_left_node <= dist_right_node) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } + else + { + if (dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) + { + if (!root->point_deleted) + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) + return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius) + { + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) +{ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) + { + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num) / root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); + if (delete_evaluation > delete_criterion_param) + { + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) + { + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root) +{ + if (root == nullptr) + return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE *root) +{ + KD_TREE_NODE *left_son_ptr = root->left_son_ptr; + KD_TREE_NODE *right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (left_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (right_son_ptr != nullptr) + { + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else + { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted ? 1 : 0); + root->down_del_num = (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); + memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); + memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L * x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) + left_son_ptr->father_ptr = root; + if (right_son_ptr != nullptr) + right_son_ptr->father_ptr = root; + if (root == Root_Node && root->TreeSize > 3) + { + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); + root->alpha_del = float(root->invalid_point_num) / root->TreeSize; + root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) +{ + if (root == nullptr) + return; + Push_Down(root); + if (!root->point_deleted) + { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) +{ + if (*root == nullptr) + return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy(&(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b) +{ + return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) +{ + if (node == nullptr) + return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) + min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) + min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) + min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) + min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) + min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) + min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); + return min_dist; +} +template +bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } +template +bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } +template +bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } + +template +void KD_TREE::ClearTree() +{ + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} +// Manual heap + +// manual queue + +// Manual Instatiations +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h new file mode 100644 index 00000000..275dd301 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/ikd_Tree.h @@ -0,0 +1,345 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +// typedef pcl::PointXYZINormal PointType; +// typedef vector> PointVector; + +struct BoxPointType +{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set +{ + ADD_POINT, + DELETE_POINT, + DELETE_BOX, + ADD_BOX, + DOWNSAMPLE_DELETE, + PUSH_DOWN +}; + +enum delete_point_storage_set +{ + NOT_RECORD, + DELETE_POINTS_REC, + MULTI_THREAD_REC +}; + +template +class KD_TREE +{ + // using MANUAL_Q_ = MANUAL_Q; + // using PointVector = std::vector; + + // using MANUAL_Q_ = MANUAL_Q; +public: + using PointVector = std::vector>; + using Ptr = std::shared_ptr>; + + struct KD_TREE_NODE + { + PointType point; + int division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + float radius_sq; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type + { + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + // static const PointType zeroP; + + struct PointType_CMP + { + PointType point; + float dist = 0.0; + PointType_CMP(PointType p = PointType(), float d = INFINITY) + { + this->point = p; + this->dist = d; + }; + bool operator<(const PointType_CMP &a) const + { + if (fabs(dist - a.dist) < 1e-10) + return point.x < a.point.x; + else + return dist < a.dist; + } + }; + + class MANUAL_HEAP + { + + public: + MANUAL_HEAP(int max_capacity = 100) + + { + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP() + { + delete[] heap; + } + void pop() + { + if (heap_size == 0) + return; + heap[0] = heap[heap_size - 1]; + heap_size--; + MoveDown(0); + return; + } + PointType_CMP top() + { + return heap[0]; + } + void push(PointType_CMP point) + { + if (heap_size >= cap) + return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + int size() + { + return heap_size; + } + void clear() + { + heap_size = 0; + return; + } + + private: + PointType_CMP *heap; + void MoveDown(int heap_index) + { + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size) + { + if (l + 1 < heap_size && heap[l] < heap[l + 1]) + l++; + if (tmp < heap[l]) + { + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + void FloatUp(int heap_index) + { + int ancestor = (heap_index - 1) / 2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0) + { + if (heap[ancestor] < tmp) + { + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index - 1) / 2; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + int heap_size = 0; + int cap = 0; + }; + + class MANUAL_Q + { + private: + int head = 0, tail = 0, counter = 0; + Operation_Logger_Type q[Q_LEN]; + bool is_empty; + + public: + void pop() + { + if (counter == 0) + return; + head++; + head %= Q_LEN; + counter--; + if (counter == 0) + is_empty = true; + return; + } + Operation_Logger_Type front() + { + return q[head]; + } + Operation_Logger_Type back() + { + return q[tail]; + } + void clear() + { + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; + } + void push(Operation_Logger_Type op) + { + q[tail] = op; + counter++; + if (is_empty) + is_empty = false; + tail++; + tail %= Q_LEN; + } + bool empty() + { + return is_empty; + } + int size() + { + return counter; + } + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE **Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void *multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE *root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); + void Rebuild(KD_TREE_NODE **root); + int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE *root); + void Push_Down(KD_TREE_NODE *root); + void Update(KD_TREE_NODE *root); + void delete_tree_nodes(KD_TREE_NODE **root); + void downsample(KD_TREE_NODE **root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE *node, PointType point); + void ClearTree(); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param) + { + delete_criterion_param = delete_param; + } + void Set_balance_criterion_param(float balance_param) + { + balance_criterion_param = balance_param; + } + void set_downsample_param(float downsample_param) + { + downsample_size = downsample_param; + } + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector &PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector &BoxPoints); + void Delete_Points(PointVector &PointToDel); + int Delete_Point_Boxes(vector &BoxPoints); + void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector &removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE *Root_Node = nullptr; + int max_queue_size = 0; +}; + +// template +// PointType KD_TREE::zeroP = PointType(0,0,0); diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp new file mode 100644 index 00000000..d5bc04a8 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.cpp @@ -0,0 +1,132 @@ +#include "imu_processor.h" + +IMUProcessor::IMUProcessor(Config &config, std::shared_ptr kf) : m_config(config), m_kf(kf) +{ + m_Q.setIdentity(); + m_Q.block<3, 3>(0, 0) = M3D::Identity() * m_config.ng; + m_Q.block<3, 3>(3, 3) = M3D::Identity() * m_config.na; + m_Q.block<3, 3>(6, 6) = M3D::Identity() * m_config.nbg; + m_Q.block<3, 3>(9, 9) = M3D::Identity() * m_config.nba; + m_last_acc.setZero(); + m_last_gyro.setZero(); + m_imu_cache.clear(); + m_poses_cache.clear(); +} + +bool IMUProcessor::initialize(SyncPackage &package) +{ + m_imu_cache.insert(m_imu_cache.end(), package.imus.begin(), package.imus.end()); + if (m_imu_cache.size() < static_cast(m_config.imu_init_num)) + return false; + V3D acc_mean = V3D::Zero(); + V3D gyro_mean = V3D::Zero(); + for (const auto &imu : m_imu_cache) + { + acc_mean += imu.acc; + gyro_mean += imu.gyro; + } + acc_mean /= static_cast(m_imu_cache.size()); + gyro_mean /= static_cast(m_imu_cache.size()); + m_kf->x().r_il = m_config.r_il; + m_kf->x().t_il = m_config.t_il; + m_kf->x().bg = gyro_mean; + if (m_config.gravity_align) + { + m_kf->x().r_wi = (Eigen::Quaterniond::FromTwoVectors((-acc_mean).normalized(), V3D(0.0, 0.0, -1.0)).matrix()); + m_kf->x().initGravityDir(V3D(0, 0, -1.0)); + } + else + m_kf->x().initGravityDir(-acc_mean); + m_kf->P().setIdentity(); + m_kf->P().block<3, 3>(6, 6) = M3D::Identity() * 0.00001; + m_kf->P().block<3, 3>(9, 9) = M3D::Identity() * 0.00001; + m_kf->P().block<3, 3>(15, 15) = M3D::Identity() * 0.0001; + m_kf->P().block<3, 3>(18, 18) = M3D::Identity() * 0.0001; + + m_last_imu = m_imu_cache.back(); + m_last_propagate_end_time = package.cloud_end_time; + return true; +} + +void IMUProcessor::undistort(SyncPackage &package) +{ + + m_imu_cache.clear(); + m_imu_cache.push_back(m_last_imu); + m_imu_cache.insert(m_imu_cache.end(), package.imus.begin(), package.imus.end()); + + // const double imu_time_begin = m_imu_cache.front().time; + const double imu_time_end = m_imu_cache.back().time; + + const double cloud_time_begin = package.cloud_start_time; + const double propagate_time_end = package.cloud_end_time; + + m_poses_cache.clear(); + m_poses_cache.emplace_back(0.0, m_last_acc, m_last_gyro, m_kf->x().v, m_kf->x().t_wi, m_kf->x().r_wi); + + V3D acc_val, gyro_val; + double dt = 0.0; + Input inp; + inp.acc = m_imu_cache.back().acc; + inp.gyro = m_imu_cache.back().gyro; + for (auto it_imu = m_imu_cache.begin(); it_imu < (m_imu_cache.end() - 1); it_imu++) + { + IMUData &head = *it_imu; + IMUData &tail = *(it_imu + 1); + if (tail.time < m_last_propagate_end_time) + continue; + gyro_val = 0.5 * (head.gyro + tail.gyro); + acc_val = 0.5 * (head.acc + tail.acc); + + if (head.time < m_last_propagate_end_time) + dt = tail.time - m_last_propagate_end_time; + else + dt = tail.time - head.time; + + inp.acc = acc_val; + inp.gyro = gyro_val; + m_kf->predict(inp, dt, m_Q); + + m_last_gyro = gyro_val - m_kf->x().bg; + m_last_acc = m_kf->x().r_wi * (acc_val - m_kf->x().ba) + m_kf->x().g; + double offset = tail.time - cloud_time_begin; + m_poses_cache.emplace_back(offset, m_last_acc, m_last_gyro, m_kf->x().v, m_kf->x().t_wi, m_kf->x().r_wi); + } + + dt = propagate_time_end - imu_time_end; + m_kf->predict(inp, dt, m_Q); + m_last_imu = m_imu_cache.back(); + m_last_propagate_end_time = propagate_time_end; + + M3D cur_r_wi = m_kf->x().r_wi; + V3D cur_t_wi = m_kf->x().t_wi; + M3D cur_r_il = m_kf->x().r_il; + V3D cur_t_il = m_kf->x().t_il; + auto it_pcl = package.cloud->points.end() - 1; + + for (auto it_kp = m_poses_cache.end() - 1; it_kp != m_poses_cache.begin(); it_kp--) + { + auto head = it_kp - 1; + auto tail = it_kp; + + M3D imu_r_wi = head->rot; + V3D imu_t_wi = head->trans; + V3D imu_vel = head->vel; + V3D imu_acc = tail->acc; + V3D imu_gyro = tail->gyro; + + for (; it_pcl->curvature / double(1000) > head->offset; it_pcl--) + { + dt = it_pcl->curvature / double(1000) - head->offset; + V3D point(it_pcl->x, it_pcl->y, it_pcl->z); + M3D point_rot = imu_r_wi * Sophus::SO3d::exp(imu_gyro * dt).matrix(); + V3D point_pos = imu_t_wi + imu_vel * dt + 0.5 * imu_acc * dt * dt; + V3D p_compensate = cur_r_il.transpose() * (cur_r_wi.transpose() * (point_rot * (cur_r_il * point + cur_t_il) + point_pos - cur_t_wi) - cur_t_il); + it_pcl->x = p_compensate(0); + it_pcl->y = p_compensate(1); + it_pcl->z = p_compensate(2); + if (it_pcl == package.cloud->points.begin()) + break; + } + } +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h new file mode 100644 index 00000000..0751dfbb --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/imu_processor.h @@ -0,0 +1,24 @@ +#pragma once +#include "ieskf.h" +#include "commons.h" + +class IMUProcessor +{ +public: + IMUProcessor(Config &config, std::shared_ptr kf); + + bool initialize(SyncPackage &package); + + void undistort(SyncPackage &package); + +private: + Config m_config; + std::shared_ptr m_kf; + double m_last_propagate_end_time; + Vec m_imu_cache; + Vec m_poses_cache; + V3D m_last_acc; + V3D m_last_gyro; + M12D m_Q; + IMUData m_last_imu; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp new file mode 100644 index 00000000..674d35bc --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.cpp @@ -0,0 +1,268 @@ +#include "lidar_processor.h" + +LidarProcessor::LidarProcessor(Config &config, std::shared_ptr kf) : m_config(config), m_kf(kf) +{ + m_ikdtree = std::make_shared>(); + m_ikdtree->set_downsample_param(m_config.map_resolution); + m_cloud_down_lidar.reset(new CloudType); + m_cloud_down_world.reset(new CloudType(10000, 1)); + m_norm_vec.reset(new CloudType(10000, 1)); + m_effect_cloud_lidar.reset(new CloudType(10000, 1)); + m_effect_norm_vec.reset(new CloudType(10000, 1)); + m_nearest_points.resize(10000); + m_point_selected_flag.resize(10000, false); + + if (m_config.scan_resolution > 0.0) + { + m_scan_filter.setLeafSize(m_config.scan_resolution, m_config.scan_resolution, m_config.scan_resolution); + } + + m_kf->setLossFunction([&](State &s, SharedState &d) + { updateLossFunc(s, d); }); + m_kf->setStopFunction([&](const V21D &delta) -> bool + { V3D rot_delta = delta.block<3, 1>(0, 0); + V3D t_delta = delta.block<3, 1>(3, 0); + return (rot_delta.norm() * 57.3 < 0.01) && (t_delta.norm() * 100 < 0.015); }); +} + +void LidarProcessor::trimCloudMap() +{ + m_local_map.cub_to_rm.clear(); + const State &state = m_kf->x(); + Eigen::Vector3d pos_lidar = state.t_wi + state.r_wi * state.t_il; + + if (!m_local_map.initialed) + { + for (int i = 0; i < 3; i++) + { + m_local_map.local_map_corner.vertex_min[i] = pos_lidar[i] - m_config.cube_len / 2.0; + m_local_map.local_map_corner.vertex_max[i] = pos_lidar[i] + m_config.cube_len / 2.0; + } + m_local_map.initialed = true; + return; + } + float dist_to_map_edge[3][2]; + bool need_move = false; + double det_thresh = m_config.move_thresh * m_config.det_range; + for (int i = 0; i < 3; i++) + { + dist_to_map_edge[i][0] = fabs(pos_lidar(i) - m_local_map.local_map_corner.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_lidar(i) - m_local_map.local_map_corner.vertex_max[i]); + + if (dist_to_map_edge[i][0] <= det_thresh || dist_to_map_edge[i][1] <= det_thresh) + need_move = true; + } + if (!need_move) + return; + BoxPointType new_corner, temp_corner; + new_corner = m_local_map.local_map_corner; + float mov_dist = std::max((m_config.cube_len - 2.0 * m_config.move_thresh * m_config.det_range) * 0.5 * 0.9, double(m_config.det_range * (m_config.move_thresh - 1))); + + for (int i = 0; i < 3; i++) + { + temp_corner = m_local_map.local_map_corner; + if (dist_to_map_edge[i][0] <= det_thresh) + { + new_corner.vertex_max[i] -= mov_dist; + new_corner.vertex_min[i] -= mov_dist; + temp_corner.vertex_min[i] = m_local_map.local_map_corner.vertex_max[i] - mov_dist; + m_local_map.cub_to_rm.push_back(temp_corner); + } + else if (dist_to_map_edge[i][1] <= det_thresh) + { + new_corner.vertex_max[i] += mov_dist; + new_corner.vertex_min[i] += mov_dist; + temp_corner.vertex_max[i] = m_local_map.local_map_corner.vertex_min[i] + mov_dist; + m_local_map.cub_to_rm.push_back(temp_corner); + } + } + m_local_map.local_map_corner = new_corner; + + PointVec points_history; + m_ikdtree->acquire_removed_points(points_history); + + // 删除局部地图之外的点云 + if (m_local_map.cub_to_rm.size() > 0) + m_ikdtree->Delete_Point_Boxes(m_local_map.cub_to_rm); + return; +} + +void LidarProcessor::incrCloudMap() +{ + if (m_cloud_down_lidar->empty()) + return; + const State &state = m_kf->x(); + int size = m_cloud_down_lidar->size(); + PointVec point_to_add; + PointVec point_no_need_downsample; + for (int i = 0; i < size; i++) + { + const PointType &p = m_cloud_down_lidar->points[i]; + Eigen::Vector3d point(p.x, p.y, p.z); + point = state.r_wi * (state.r_il * point + state.t_il) + state.t_wi; + m_cloud_down_world->points[i].x = point(0); + m_cloud_down_world->points[i].y = point(1); + m_cloud_down_world->points[i].z = point(2); + m_cloud_down_world->points[i].intensity = m_cloud_down_lidar->points[i].intensity; + // 如果该点附近没有近邻点则需要添加到地图中 + if (m_nearest_points[i].empty()) + { + point_to_add.push_back(m_cloud_down_world->points[i]); + continue; + } + + const PointVec &points_near = m_nearest_points[i]; + bool need_add = true; + PointType downsample_result, mid_point; + mid_point.x = std::floor(m_cloud_down_world->points[i].x / m_config.map_resolution) * m_config.map_resolution + 0.5 * m_config.map_resolution; + mid_point.y = std::floor(m_cloud_down_world->points[i].y / m_config.map_resolution) * m_config.map_resolution + 0.5 * m_config.map_resolution; + mid_point.z = std::floor(m_cloud_down_world->points[i].z / m_config.map_resolution) * m_config.map_resolution + 0.5 * m_config.map_resolution; + + // 如果该点所在的voxel没有点，则直接加入地图，且不需要降采样 + if (fabs(points_near[0].x - mid_point.x) > 0.5 * m_config.map_resolution && fabs(points_near[0].y - mid_point.y) > 0.5 * m_config.map_resolution && fabs(points_near[0].z - mid_point.z) > 0.5 * m_config.map_resolution) + { + point_no_need_downsample.push_back(m_cloud_down_world->points[i]); + continue; + } + float dist = sq_dist(m_cloud_down_world->points[i], mid_point); + + for (int readd_i = 0; readd_i < m_config.near_search_num; readd_i++) + { + // 如果该点的近邻点较少，则需要加入到地图中 + if (points_near.size() < static_cast(m_config.near_search_num)) + break; + // 如果该点的近邻点距离voxel中心点的距离比该点距离voxel中心点更近，则不需要加入该点 + if (sq_dist(points_near[readd_i], mid_point) < dist) + { + need_add = false; + break; + } + } + if (need_add) + point_to_add.push_back(m_cloud_down_world->points[i]); + } + m_ikdtree->Add_Points(point_to_add, true); + m_ikdtree->Add_Points(point_no_need_downsample, false); +} + +void LidarProcessor::initCloudMap(PointVec &point_vec) +{ + m_ikdtree->Build(point_vec); +} + +void LidarProcessor::process(SyncPackage &package) +{ + // m_kf->setLossFunction([&](State &s, SharedState &d) + // { updateLossFunc(s, d); }); + // m_kf->setStopFunction([&](const V21D &delta) -> bool + // { V3D rot_delta = delta.block<3, 1>(0, 0); + // V3D t_delta = delta.block<3, 1>(3, 0); + // return (rot_delta.norm() * 57.3 < 0.01) && (t_delta.norm() * 100 < 0.015); }); + if (m_config.scan_resolution > 0.0) + { + m_scan_filter.setInputCloud(package.cloud); + m_scan_filter.filter(*m_cloud_down_lidar); + } + else + { + pcl::copyPointCloud(*package.cloud, *m_cloud_down_lidar); + } + trimCloudMap(); + m_kf->update(); + incrCloudMap(); +} + +void LidarProcessor::updateLossFunc(State &state, SharedState &share_data) +{ + int size = m_cloud_down_lidar->size(); +#ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); +#pragma omp parallel for +#endif + for (int i = 0; i < size; i++) + { + PointType &point_body = m_cloud_down_lidar->points[i]; + PointType &point_world = m_cloud_down_world->points[i]; + Eigen::Vector3d point_body_vec(point_body.x, point_body.y, point_body.z); + Eigen::Vector3d point_world_vec = state.r_wi * (state.r_il * point_body_vec + state.t_il) + state.t_wi; + point_world.x = point_world_vec(0); + point_world.y = point_world_vec(1); + point_world.z = point_world_vec(2); + point_world.intensity = point_body.intensity; + std::vector point_sq_dist(m_config.near_search_num); + auto &points_near = m_nearest_points[i]; + m_ikdtree->Nearest_Search(point_world, m_config.near_search_num, points_near, point_sq_dist); + if (points_near.size() >= static_cast(m_config.near_search_num) && point_sq_dist[m_config.near_search_num - 1] <= 5) + m_point_selected_flag[i] = true; + else + m_point_selected_flag[i] = false; + if (!m_point_selected_flag[i]) + continue; + + Eigen::Vector4d pabcd; + m_point_selected_flag[i] = false; + if (esti_plane(points_near, 0.1, pabcd)) + { + double pd2 = pabcd(0) * point_world_vec(0) + pabcd(1) * point_world_vec(1) + pabcd(2) * point_world_vec(2) + pabcd(3); + double s = 1 - 0.9 * std::fabs(pd2) / std::sqrt(point_body_vec.norm()); + if (s > 0.9) + { + m_point_selected_flag[i] = true; + m_norm_vec->points[i].x = pabcd(0); + m_norm_vec->points[i].y = pabcd(1); + m_norm_vec->points[i].z = pabcd(2); + m_norm_vec->points[i].intensity = pd2; + } + } + } + + int effect_feat_num = 0; + for (int i = 0; i < size; i++) + { + if (!m_point_selected_flag[i]) + continue; + m_effect_cloud_lidar->points[effect_feat_num] = m_cloud_down_lidar->points[i]; + m_effect_norm_vec->points[effect_feat_num] = m_norm_vec->points[i]; + effect_feat_num++; + } + if (effect_feat_num < 1) + { + share_data.valid = false; + std::cerr << "NO Effective Points!" << std::endl; + return; + } + share_data.valid = true; + share_data.H.setZero(); + share_data.b.setZero(); + Eigen::Matrix J; + for (int i = 0; i < effect_feat_num; i++) + { + J.setZero(); + const PointType &laser_p = m_effect_cloud_lidar->points[i]; + const PointType &norm_p = m_effect_norm_vec->points[i]; + Eigen::Vector3d laser_p_vec(laser_p.x, laser_p.y, laser_p.z); + Eigen::Vector3d norm_vec(norm_p.x, norm_p.y, norm_p.z); + Eigen::Matrix B = -norm_vec.transpose() * state.r_wi * Sophus::SO3d::hat(state.r_il * laser_p_vec + state.t_wi); + J.block<1, 3>(0, 0) = B; + J.block<1, 3>(0, 3) = norm_vec.transpose(); + if (m_config.esti_il) + { + Eigen::Matrix C = -norm_vec.transpose() * state.r_wi * state.r_il * Sophus::SO3d::hat(laser_p_vec); + Eigen::Matrix D = norm_vec.transpose() * state.r_wi; + J.block<1, 3>(0, 6) = C; + J.block<1, 3>(0, 9) = D; + } + share_data.H += J.transpose() * m_config.lidar_cov_inv * J; + share_data.b += J.transpose() * m_config.lidar_cov_inv * norm_p.intensity; + } +} + +CloudType::Ptr LidarProcessor::transformCloud(CloudType::Ptr inp, const M3D &r, const V3D &t) +{ + Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); + transform.block<3, 3>(0, 0) = r.cast(); + transform.block<3, 1>(0, 3) = t.cast(); + CloudType::Ptr ret(new CloudType); + pcl::transformPointCloud(*inp, *ret, transform); + return ret; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h new file mode 100644 index 00000000..34826940 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/lidar_processor.h @@ -0,0 +1,48 @@ +#pragma once +#include "commons.h" +#include "ieskf.h" +#include "ikd_Tree.h" +#include +#include + +struct LocalMap +{ + bool initialed = false; + BoxPointType local_map_corner; + Vec cub_to_rm; +}; + +class LidarProcessor +{ +public: + LidarProcessor(Config &config, std::shared_ptr kf); + + void trimCloudMap(); + + void incrCloudMap(); + + void initCloudMap(PointVec &point_vec); + + void process(SyncPackage &package); + + void updateLossFunc(State &state, SharedState &share_data); + + static CloudType::Ptr transformCloud(CloudType::Ptr inp, const M3D &r, const V3D &t); + M3D r_wl() { return m_kf->x().r_wi * m_kf->x().r_il; } + V3D t_wl() { return m_kf->x().t_wi + m_kf->x().r_wi * m_kf->x().t_il; } + +private: + Config m_config; + LocalMap m_local_map; + std::shared_ptr m_kf; + std::shared_ptr> m_ikdtree; + CloudType::Ptr m_cloud_lidar; + CloudType::Ptr m_cloud_down_lidar; + CloudType::Ptr m_cloud_down_world; + std::vector m_point_selected_flag; + CloudType::Ptr m_norm_vec; + CloudType::Ptr m_effect_cloud_lidar; + CloudType::Ptr m_effect_norm_vec; + std::vector m_nearest_points; + pcl::VoxelGrid m_scan_filter; +}; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp new file mode 100644 index 00000000..74a9329d --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.cpp @@ -0,0 +1,29 @@ +#include "map_builder.h" +MapBuilder::MapBuilder(Config &config, std::shared_ptr kf) : m_config(config), m_kf(kf) +{ + m_imu_processor = std::make_shared(config, kf); + m_lidar_processor = std::make_shared(config, kf); + m_status = BuilderStatus::IMU_INIT; +} + +void MapBuilder::process(SyncPackage &package) +{ + if (m_status == BuilderStatus::IMU_INIT) + { + if (m_imu_processor->initialize(package)) + m_status = BuilderStatus::MAP_INIT; + return; + } + + m_imu_processor->undistort(package); + + if (m_status == BuilderStatus::MAP_INIT) + { + CloudType::Ptr cloud_world = LidarProcessor::transformCloud(package.cloud, m_lidar_processor->r_wl(), m_lidar_processor->t_wl()); + m_lidar_processor->initCloudMap(cloud_world->points); + m_status = BuilderStatus::MAPPING; + return; + } + + m_lidar_processor->process(package); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h new file mode 100644 index 00000000..baa0fba2 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/map_builder/map_builder.h @@ -0,0 +1,27 @@ +#pragma once +#include "imu_processor.h" +#include "lidar_processor.h" + +enum BuilderStatus +{ + IMU_INIT, + MAP_INIT, + MAPPING +}; + +class MapBuilder +{ +public: + MapBuilder(Config &config, std::shared_ptr kf); + + void process(SyncPackage &package); + BuilderStatus status() { return m_status; } + std::shared_ptr lidar_processor(){return m_lidar_processor;} + +private: + Config m_config; + BuilderStatus m_status; + std::shared_ptr m_kf; + std::shared_ptr m_imu_processor; + std::shared_ptr m_lidar_processor; +}; diff --git a/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp b/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp new file mode 100644 index 00000000..1e358e62 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/utils.cpp @@ -0,0 +1,39 @@ +#include "utils.h" +pcl::PointCloud::Ptr Utils::livox2PCL(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg, int filter_num, double min_range, double max_range) +{ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + int point_num = msg->point_num; + cloud->reserve(point_num / filter_num + 1); + for (int i = 0; i < point_num; i += filter_num) + { + if ((msg->points[i].line < 4) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + + float x = msg->points[i].x; + float y = msg->points[i].y; + float z = msg->points[i].z; + if (x * x + y * y + z * z < min_range * min_range || x * x + y * y + z * z > max_range * max_range) + continue; + pcl::PointXYZINormal p; + p.x = x; + p.y = y; + p.z = z; + p.intensity = msg->points[i].reflectivity; + p.curvature = msg->points[i].offset_time / 1000000.0f; + cloud->push_back(p); + } + } + return cloud; +} + +double Utils::getSec(std_msgs::msg::Header &header) +{ + return static_cast(header.stamp.sec) + static_cast(header.stamp.nanosec) * 1e-9; +} +builtin_interfaces::msg::Time Utils::getTime(const double &sec) +{ + builtin_interfaces::msg::Time time_msg; + time_msg.sec = static_cast(sec); + time_msg.nanosec = static_cast((sec - time_msg.sec) * 1e9); + return time_msg; +} diff --git a/src/FASTLIO2_ROS2/fastlio2/src/utils.h b/src/FASTLIO2_ROS2/fastlio2/src/utils.h new file mode 100644 index 00000000..0fe11d69 --- /dev/null +++ b/src/FASTLIO2_ROS2/fastlio2/src/utils.h @@ -0,0 +1,29 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +#define RESET "\033[0m" +#define BLACK "\033[30m" /* Black */ +#define RED "\033[31m" /* Red */ +#define GREEN "\033[32m" /* Green */ +#define YELLOW "\033[33m" /* Yellow */ +#define BLUE "\033[34m" /* Blue */ +#define PURPLE "\033[35m" /* Purple */ +#define CYAN "\033[36m" /* Cyan */ +#define WHITE "\033[37m" /* White */ + +class Utils +{ +public: + // static pcl::PointCloud::Ptr convertToPCL(const sensor_msgs::msg::PointCloud2 &msg); + // static sensor_msgs::msg::PointCloud2 convertToROS(const pcl::PointCloud::Ptr &cloud); + static double getSec(std_msgs::msg::Header &header); + static pcl::PointCloud::Ptr livox2PCL(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg, int filter_num, double min_range = 0.5, double max_range = 20.0); + static builtin_interfaces::msg::Time getTime(const double& sec); +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/CMakeLists.txt b/src/FASTLIO2_ROS2/hba/CMakeLists.txt new file mode 100644 index 00000000..98c76275 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(hba) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_definitions(SOPHUS_USE_BASIC_LOGGING) +add_definitions(-DMP_EN) +add_definitions(-DMP_PROC_NUM=4) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(interface REQUIRED) +find_package(pcl_conversions REQUIRED) + + +find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(OpenMP QUIET) +find_package(Sophus REQUIRED) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + +set(SRC_LIST src/hba/commons.cpp + src/hba/blam.cpp + src/hba/hba.cpp) + +add_executable(hba_node src/hba_node.cpp ${SRC_LIST}) +ament_target_dependencies(hba_node rclcpp std_msgs sensor_msgs interface pcl_conversions GTSAM) +target_link_libraries(hba_node ${PCL_LIBRARIES} gtsam yaml-cpp) + +install(TARGETS hba_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/hba/LICENSE b/src/FASTLIO2_ROS2/hba/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/src/FASTLIO2_ROS2/hba/config/hba.yaml b/src/FASTLIO2_ROS2/hba/config/hba.yaml new file mode 100644 index 00000000..714a470d --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/config/hba.yaml @@ -0,0 +1,11 @@ +scan_resolution: 0.1 + +window_size: 20 +stride: 10 +voxel_size: 0.5 +min_point_num: 10 +max_layer: 3 +plane_thresh: 0.01 +ba_max_iter: 10 +hba_iter: 5 +down_sample: 0.1 \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/launch/hba_launch.py b/src/FASTLIO2_ROS2/hba/launch/hba_launch.py new file mode 100644 index 00000000..b30137d6 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/launch/hba_launch.py @@ -0,0 +1,32 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + rviz_cfg = PathJoinSubstitution([FindPackageShare("hba"), "rviz", "hba.rviz"]) + config_path = PathJoinSubstitution([FindPackageShare("hba"), "config", "hba.yaml"]) + + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="hba", + namespace="hba", + executable="hba_node", + name="hba", + output="screen", + parameters=[ + {"config_path": config_path.perform(launch.LaunchContext())} + ], + ), + launch_ros.actions.Node( + package="rviz2", + namespace="hba", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ), + ] + ) diff --git a/src/FASTLIO2_ROS2/hba/package.xml b/src/FASTLIO2_ROS2/hba/package.xml new file mode 100644 index 00000000..c594cdf5 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/package.xml @@ -0,0 +1,25 @@ + + + + hba + 0.0.0 + TODO: Package description + liangheming + MIT + + ament_cmake + + rclcpp + std_msgs + GTSAM + sensor_msgs + interface + pcl_conversions + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/hba/rviz/hba.rviz b/src/FASTLIO2_ROS2/hba/rviz/hba.rviz new file mode 100644 index 00000000..1b26193e --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/rviz/hba.rviz @@ -0,0 +1,170 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.800000011920929 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /hba/map_points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: map + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0.2099999338388443 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 49.88230895996094 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 5.11167049407959 + Y: 2.8143327236175537 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp b/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp new file mode 100644 index 00000000..ac24a4a0 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/blam.cpp @@ -0,0 +1,458 @@ +#include "blam.h" +VoxelKey VoxelKey::index(double x, double y, double z, double resolution, double bias) +{ + V3D point(x, y, z); + V3D idx = (point / resolution + V3D(bias, bias, bias)).array().floor(); + return VoxelKey(static_cast(idx(0)), static_cast(idx(1)), static_cast(idx(2))); +} +OctoTree::OctoTree(int layer, int max_layer, int min_point_num, double plane_thresh, double quater_len, const V3D ¢er) + : m_layer(layer), m_max_layer(max_layer), m_min_point_num(min_point_num), m_plane_thresh(plane_thresh), m_quater_len(quater_len), m_is_valid(false), m_is_plane(false), m_center(center) +{ + m_leaves.resize(8, nullptr); +} +void OctoTree::insert(const PointType &point) +{ + m_points.push_back(point); +} +void OctoTree::buildPlanes() +{ + if (static_cast(m_points.size()) < m_min_point_num) + { + m_is_plane = false; + m_is_valid = false; + return; + } + + V3D mean = V3D::Zero(); + M3D cov = M3D::Zero(); + for (PointType &point : m_points) + { + V3D p_vec(point.x, point.y, point.z); + mean += p_vec; + cov += p_vec * p_vec.transpose(); + } + mean /= static_cast(m_points.size()); + cov = cov / static_cast(m_points.size()) - mean * mean.transpose(); + Eigen::SelfAdjointEigenSolver eigensolver(cov); + m_mean = mean; + m_eigen_val = eigensolver.eigenvalues(); + m_eigen_vec = eigensolver.eigenvectors(); + if (m_eigen_val[0] < m_plane_thresh) + { + m_mean = mean; + m_is_plane = true; + m_is_valid = true; + doMergePoints(); + return; + } + m_is_plane = false; + m_is_valid = splitPlanes(); +} +void OctoTree::doMergePoints() +{ + assert(m_is_valid && m_is_plane); + std::unordered_map id_point_map; + for (PointType &point : m_points) + { + uint32_t id = point.id; + if (id_point_map.find(id) == id_point_map.end()) + { + id_point_map[id] = point; + id_point_map[id].time = 1.0; + } + else + { + id_point_map[id].x += point.x; + id_point_map[id].y += point.y; + id_point_map[id].z += point.z; + id_point_map[id].intensity += point.intensity; + id_point_map[id].lx += point.lx; + id_point_map[id].ly += point.ly; + id_point_map[id].lz += point.lz; + id_point_map[id].time += 1.0; + } + } + + PointVec().swap(m_merged_points); + for (auto &iter : id_point_map) + { + PointType &point = iter.second; + point.x /= point.time; + point.y /= point.time; + point.z /= point.time; + point.intensity /= point.time; + point.lx /= point.time; + point.ly /= point.time; + point.lz /= point.time; + point.time = 1.0; + m_merged_points.push_back(point); + } +} +bool OctoTree::splitPlanes() +{ + if (m_layer >= m_max_layer - 1) + return false; + for (PointType &point : m_points) + { + int xyz[3] = {0, 0, 0}; + if (point.x > m_center.x()) + xyz[0] = 1; + if (point.y > m_center.y()) + xyz[1] = 1; + if (point.z > m_center.z()) + xyz[2] = 1; + + int leaf_num = 4 * xyz[0] + 2 * xyz[1] + xyz[2]; + if (m_leaves[leaf_num] == nullptr) + { + V3D shift((2 * xyz[0] - 1) * m_quater_len, (2 * xyz[1] - 1) * m_quater_len, (2 * xyz[2] - 1) * m_quater_len); + V3D center = m_center + shift; + m_leaves[leaf_num].reset(new OctoTree(m_layer + 1, m_max_layer, m_min_point_num, m_plane_thresh, m_quater_len / 2.0, center)); + } + m_leaves[leaf_num]->insert(point); + } + + PointVec().swap(m_points); + PointVec().swap(m_merged_points); + + bool has_sub_plane = false; + for (auto &leaf : m_leaves) + { + if (leaf != nullptr) + { + leaf->buildPlanes(); + if (leaf->isValid()) + has_sub_plane = true; + } + } + return has_sub_plane; +} +void OctoTree::getPlanes(Vec &planes) +{ + if (m_is_plane) + { + planes.push_back(this); + return; + } + if (!m_is_valid) + return; + + for (auto &leaf : m_leaves) + { + if (leaf != nullptr) + leaf->getPlanes(planes); + } +} +int OctoTree::planeCount() +{ + if (m_is_plane) + return 1; + + if (!m_is_valid) + return 0; + + int count = 0; + for (auto &leaf : m_leaves) + { + if (leaf != nullptr) + count += leaf->planeCount(); + } + return count; +} +double OctoTree::updateByPose(const Vec &poses) +{ + assert(m_is_valid && m_is_plane); + for (PointType &point : m_merged_points) + { + V3D p_vec(point.lx, point.ly, point.lz); + const Pose &pose = poses[point.id]; + p_vec = pose.r * p_vec + pose.t; + point.x = p_vec(0); + point.y = p_vec(1); + point.z = p_vec(2); + } + + V3D mean = V3D::Zero(); + M3D cov = M3D::Zero(); + for (PointType &point : m_points) + { + V3D p_vec(point.lx, point.ly, point.lz); + const Pose &pose = poses[point.id]; + p_vec = pose.r * p_vec + pose.t; + point.x = p_vec(0); + point.y = p_vec(1); + point.z = p_vec(2); + mean += p_vec; + cov += p_vec * p_vec.transpose(); + } + mean /= static_cast(m_points.size()); + cov = cov / static_cast(m_points.size()) - mean * mean.transpose(); + Eigen::SelfAdjointEigenSolver eigensolver(cov); + m_mean = mean; + m_eigen_val = eigensolver.eigenvalues(); + m_eigen_vec = eigensolver.eigenvectors(); + return m_eigen_val[0]; +} +double OctoTree::evalByPose(const Vec &poses) +{ + assert(m_is_valid && m_is_plane); + V3D mean = V3D::Zero(); + M3D cov = M3D::Zero(); + for (PointType &point : m_points) + { + V3D p_vec(point.lx, point.ly, point.lz); + const Pose &pose = poses[point.id]; + p_vec = pose.r * p_vec + pose.t; + point.x = p_vec(0); + point.y = p_vec(1); + point.z = p_vec(2); + mean += p_vec; + cov += p_vec * p_vec.transpose(); + } + mean /= static_cast(m_points.size()); + cov = cov / static_cast(m_points.size()) - mean * mean.transpose(); + Eigen::SelfAdjointEigenSolver eigensolver(cov); + return eigensolver.eigenvalues()[0]; +} +M3D OctoTree::fp(const V3D &p) +{ + M3D ret = M3D::Zero(); + ret.row(1) = (p - m_mean).transpose() * (m_eigen_vec.col(1) * m_eigen_vec.col(0).transpose() + m_eigen_vec.col(0) * m_eigen_vec.col(1).transpose()) / static_cast(m_points.size()) / (m_eigen_val(0) - m_eigen_val(1)); + ret.row(2) = (p - m_mean).transpose() * (m_eigen_vec.col(2) * m_eigen_vec.col(0).transpose() + m_eigen_vec.col(0) * m_eigen_vec.col(2).transpose()) / static_cast(m_points.size()) / (m_eigen_val(0) - m_eigen_val(2)); + return ret; +} +V3D OctoTree::dp(const V3D &p) +{ + return 2.0 / static_cast(m_points.size()) * ((p - m_mean).transpose() * m_eigen_vec.col(0) * m_eigen_vec.col(0).transpose()).transpose(); +} +M3D OctoTree::dp2(const V3D &p1, const V3D &p2, bool equal) +{ + double n = static_cast(m_points.size()); + V3D u0 = m_eigen_vec.col(0); + if (equal) + { + return 2.0 / n * ((n - 1) / n * u0 * u0.transpose() + u0 * (p1 - m_mean).transpose() * m_eigen_vec * fp(p2) + m_eigen_vec * fp(p2) * (u0.transpose() * (p1 - m_mean))); + } + else + { + return 2.0 / n * (-1.0 / n * u0 * u0.transpose() + u0 * (p1 - m_mean).transpose() * m_eigen_vec * fp(p2) + m_eigen_vec * fp(p2) * (u0.transpose() * (p1 - m_mean))); + } +} +BLAM::BLAM(const BLAMConfig &config) : m_config(config) +{ + Vec().swap(m_poses); + Vec::Ptr>().swap(m_clouds); +} +void BLAM::insert(pcl::PointCloud::Ptr &cloud, const Pose &pose) +{ + m_poses.push_back(pose); + m_clouds.push_back(cloud); +} + +void BLAM::addPoint(const PointType &point) +{ + VoxelKey loc = VoxelKey::index(point.x, point.y, point.z, m_config.voxel_size, 0.0); + if (m_voxel_map.find(loc) == m_voxel_map.end()) + { + V3D center = V3D((0.5 + loc.x) * m_config.voxel_size, (0.5 + loc.y) * m_config.voxel_size, (0.5 + loc.z) * m_config.voxel_size); + double quater_len = m_config.voxel_size / 4.0; + m_voxel_map[loc] = std::make_shared(0, m_config.max_layer, m_config.min_point_num, m_config.plane_thresh, quater_len, center); + } + m_voxel_map[loc]->insert(point); +} +void BLAM::buildVoxels() +{ + + Vec().swap(m_planes); + VoxelMap().swap(m_voxel_map); + for (size_t i = 0; i < m_clouds.size(); i++) + { + pcl::PointCloud::Ptr cloud = m_clouds[i]; + const Pose &pose = m_poses[i]; + for (pcl::PointXYZI &point : cloud->points) + { + PointType p; + V3D p_vec(point.x, point.y, point.z); + p_vec = pose.r * p_vec + pose.t; + p.x = static_cast(p_vec.x()); + p.y = static_cast(p_vec.y()); + p.z = static_cast(p_vec.z()); + p.lx = point.x; + p.ly = point.y; + p.lz = point.z; + p.intensity = point.intensity; + p.id = static_cast(i); + p.time = 1.0; + addPoint(p); + } + } + + for (auto &iter : m_voxel_map) + { + iter.second->buildPlanes(); + iter.second->getPlanes(m_planes); + } +} +void BLAM::updateJaccAndHess() +{ + m_H.setZero(); + m_J.setZero(); + for (size_t i = 0; i < m_planes.size(); i++) + { + OctoTree *plane_ptr = m_planes[i]; + const PointVec &points = plane_ptr->mergedPoints(); + Eigen::MatrixXd D_i = Eigen::MatrixXd::Zero(3 * points.size(), 6 * points.size()); + Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(3 * points.size(), 3 * points.size()); + Eigen::MatrixXd J_i = Eigen::MatrixXd::Zero(1, 3 * points.size()); + Vec idxs(points.size(), 0); + for (size_t m = 0; m < points.size(); m++) + { + const PointType &point1 = points[m]; + V3D p1(point1.x, point1.y, point1.z); + V3D pl1(point1.lx, point1.ly, point1.lz); + idxs[m] = point1.id; + Pose &pose = m_poses[point1.id]; + for (size_t n = 0; n < points.size(); n++) + { + const PointType &point2 = points[n]; + V3D p2(point2.x, point2.y, point2.z); + H_i.block<3, 3>(m * 3, n * 3) = plane_ptr->dp2(p1, p2, m == n); + } + J_i.block<1, 3>(0, m * 3) = plane_ptr->dp(p1).transpose(); + D_i.block<3, 3>(m * 3, m * 3) = -pose.r * Sophus::SO3d::hat(pl1); + D_i.block<3, 3>(m * 3, m * 3 + 3) = M3D::Identity(); + } + Eigen::MatrixXd H_bar = D_i.transpose() * H_i * D_i; // 6 n * 6 n + Eigen::VectorXd J_bar = (J_i * D_i).transpose(); + + for (size_t m = 0; m < idxs.size(); m++) + { + uint32_t p_id1 = idxs[m]; + for (size_t n = m; n < idxs.size(); n++) + { + uint32_t p_id2 = idxs[n]; + m_H.block<6, 6>(p_id1 * 6, p_id2 * 6) += H_bar.block<6, 6>(m * 6, n * 6); + if (m == n) + continue; + m_H.block<6, 6>(p_id2 * 6, p_id1 * 6) += H_bar.block<6, 6>(n * 6, m * 6); + } + m_J.block<6, 1>(p_id1 * 6, 0) += J_bar.block<6, 1>(m * 6, 0); + } + } +} +void BLAM::optimize() +{ + buildVoxels(); + Eigen::MatrixXd D; + D.resize(m_poses.size() * 6, m_poses.size() * 6); + m_H.resize(m_poses.size() * 6, m_poses.size() * 6); + m_J.resize(m_poses.size() * 6); + double residual = 0.0; + bool build_hess = true; + double u = 0.01, v = 2.0; + + for (size_t i = 0; i < m_config.max_iter; i++) + { + if (build_hess) + { + residual = updatePlanesByPoses(m_poses); + updateJaccAndHess(); + } + D = m_H.diagonal().asDiagonal(); + Eigen::MatrixXd Hess = m_H + u * D; + Eigen::VectorXd delta = Hess.colPivHouseholderQr().solve(-m_J); + Vec temp_pose(m_poses.begin(), m_poses.end()); + plusDelta(temp_pose, delta); + double residual_new = evalPlanesByPoses(temp_pose); + double pho = (residual - residual_new) / (0.5 * (delta.transpose() * (u * D * delta - m_J))[0]); + if (pho > 0) + { + build_hess = true; + // std::cout << "ITER : " << i << " LM ITER UPDATE" << std::endl; + m_poses = temp_pose; + v = 2.0; + double q = 1 - pow(2 * pho - 1, 3); + u *= (q < 0.33333333 ? 0.33333333 : q); + } + else + { + build_hess = false; + u *= v; + v *= 2; + } + if (std::abs(residual - residual_new) < 1e-9) + break; + } +} + +double BLAM::updatePlanesByPoses(const Vec &poses) +{ + double residual = 0.0; + for (size_t i = 0; i < m_planes.size(); i++) + { + OctoTree *plane = m_planes[i]; + residual += plane->updateByPose(poses); + } + return residual; +} + +double BLAM::evalPlanesByPoses(const Vec &poses) +{ + double residual = 0.0; + for (size_t i = 0; i < m_planes.size(); i++) + { + OctoTree *plane = m_planes[i]; + residual += plane->evalByPose(poses); + } + return residual; +} + +int BLAM::planeCount(bool with_sub_planes) +{ + int count = 0; + for (auto &iter : m_voxel_map) + { + if (with_sub_planes) + count += iter.second->planeCount(); + else if (iter.second->isPlane()) + count += 1; + } + return count; +} + +void BLAM::plusDelta(Vec &poses, const Eigen::VectorXd &x) +{ + assert(static_cast(x.rows()) == poses.size() * 6); + for (size_t i = 0; i < poses.size(); i++) + { + M3D &r = poses[i].r; + V3D &t = poses[i].t; + r = r * Sophus::SO3d::exp(x.segment<3>(i * 6)).matrix(); + t += x.segment<3>(i * 6 + 3); + } +} + +pcl::PointCloud::Ptr BLAM::getLocalCloud() +{ + pcl::PointCloud::Ptr ret(new pcl::PointCloud); + Pose &first_pose = m_poses[0]; + + for (OctoTree *plane : m_planes) + { + for (const PointType &point : plane->points()) + { + pcl::PointXYZI p; + Pose &cur_pose = m_poses[point.id]; + M3D r_fc = first_pose.r.transpose() * cur_pose.r; + V3D t_fc = first_pose.r.transpose() * (cur_pose.t - first_pose.t); + V3D p_vec(point.lx, point.ly, point.lz); + p_vec = r_fc * p_vec + t_fc; + p.x = p_vec[0]; + p.y = p_vec[1]; + p.z = p_vec[2]; + p.intensity = point.intensity; + ret->push_back(p); + } + } + return ret; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/blam.h b/src/FASTLIO2_ROS2/hba/src/hba/blam.h new file mode 100644 index 00000000..7dca2ee8 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/blam.h @@ -0,0 +1,140 @@ +#pragma once +#include +#include +#include +#include + +#include "commons.h" + +#define HASH_P 116101 +#define MAX_N 10000000000 + +struct Pose +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + M3D r; + V3D t; +}; + +class VoxelKey +{ +public: + int64_t x, y, z; + VoxelKey(int64_t _x = 0, int64_t _y = 0, int64_t _z = 0) : x(_x), y(_y), z(_z) {} + + static VoxelKey index(double x, double y, double z, double resolution, double bias = 0.0); + bool operator==(const VoxelKey &other) const + { + return (x == other.x && y == other.y && z == other.z); + } + struct Hasher + { + int64_t operator()(const VoxelKey &k) const + { + return ((((k.z) * HASH_P) % MAX_N + (k.y)) * HASH_P) % MAX_N + (k.x); + } + }; +}; + +class OctoTree +{ +public: + OctoTree(int layer, int max_layer, int min_point_num, double plane_thresh, double quater_len, const V3D ¢er); + bool isPlane() { return m_is_plane; } + bool isValid() { return m_is_valid; } + double &quaterLength() { return m_quater_len; } + V3D ¢er() { return m_center; } + V3D &mean() { return m_mean; } + V3D &eigenVal() { return m_eigen_val; } + M3D &eigenVec() { return m_eigen_vec; } + PointVec &points() { return m_points; } + PointVec &mergedPoints() { return m_merged_points; } + + void insert(const PointType &point); + + void buildPlanes(); + + bool splitPlanes(); + + void doMergePoints(); + + void getPlanes(Vec &planes); + + int planeCount(); + + double updateByPose(const Vec &poses); + + double evalByPose(const Vec &poses); + + M3D fp(const V3D &p); + V3D dp(const V3D &p); + M3D dp2(const V3D &p1, const V3D &p2, bool equal); + +private: + int m_layer; + int m_max_layer; + int m_min_point_num; + double m_plane_thresh; + double m_quater_len; + bool m_is_valid; + bool m_is_plane; + V3D m_center; + V3D m_mean; + V3D m_eigen_val; + M3D m_eigen_vec; + Vec> m_leaves; + PointVec m_points; + PointVec m_merged_points; +}; + +struct BLAMConfig +{ + double voxel_size = 1.0; + int min_point_num = 10; + int max_layer = 4; + double plane_thresh = 0.01; + size_t max_iter = 10; +}; + +using VoxelMap = std::unordered_map, VoxelKey::Hasher>; + +class BLAM +{ +public: + BLAM(const BLAMConfig &config); + BLAMConfig &config() { return m_config; } + VoxelMap &voxelMap() { return m_voxel_map; } + Vec &poses() { return m_poses; } + Vec &planes() { return m_planes; } + Eigen::MatrixXd &H() { return m_H; } + Eigen::VectorXd &J() { return m_J; } + + void insert(pcl::PointCloud::Ptr &cloud, const Pose &pose); + + void addPoint(const PointType &point); + + void buildVoxels(); + + void updateJaccAndHess(); + + void optimize(); + + double updatePlanesByPoses(const Vec &poses); + + double evalPlanesByPoses(const Vec &poses); + + int planeCount(bool with_sub_planes = false); + + static void plusDelta(Vec &poses, const Eigen::VectorXd &x); + + pcl::PointCloud::Ptr getLocalCloud(); + +private: + BLAMConfig m_config; + Vec m_poses; + VoxelMap m_voxel_map; + Vec m_planes; + Vec::Ptr> m_clouds; + Eigen::MatrixXd m_H; + Eigen::VectorXd m_J; +}; diff --git a/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp b/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp new file mode 100644 index 00000000..cb3a78a1 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/commons.cpp @@ -0,0 +1 @@ +#include "commons.h" \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/commons.h b/src/FASTLIO2_ROS2/hba/src/hba/commons.h new file mode 100644 index 00000000..30dc7efb --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/commons.h @@ -0,0 +1,38 @@ +#pragma once +#include +#include +#include + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M2D = Eigen::Matrix2d; +using V2D = Eigen::Vector2d; +using M2F = Eigen::Matrix2f; +using V2F = Eigen::Vector2f; +using M4D = Eigen::Matrix4d; +using V4D = Eigen::Vector4d; + +using M6D = Eigen::Matrix; + +template +using Vec = std::vector; + +struct PointXYZIDT +{ + PCL_ADD_POINT4D; + float lx; + float ly; + float lz; + float intensity; + uint32_t id; + double time; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIDT, + (float, x, x)(float, y, y)(float, z, z)(float, lx, lx)(float, ly, ly)(float, lz, lz)(float, intensity, intensity)(uint32_t, id, id)(double, time, time)) +using PointType = PointXYZIDT; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp b/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp new file mode 100644 index 00000000..bccf2d92 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/hba.cpp @@ -0,0 +1,174 @@ +#include "hba.h" +HBA::HBA(const HBAConfig config) : m_config(config) +{ + m_poses.clear(); + m_clouds.clear(); + m_lbas.clear(); +} +void HBA::insert(pcl::PointCloud::Ptr &cloud, const Pose &pose) +{ + m_clouds.push_back(cloud); + m_poses.push_back(pose); +} +void HBA::optimize() +{ + m_levels = calcLevels(); + gtsam::Values initial_estimates; + gtsam::LevenbergMarquardtParams lm_params; + gtsam::NonlinearFactorGraph graph; + + // for (size_t i = 0; i < m_config.hba_iter; i++) + // { + Vec>().swap(m_lbas); + m_lbas.resize(m_levels, Vec()); + Vec::Ptr> clouds = m_clouds; + Vec poses = m_poses; + initial_estimates.clear(); + graph.resize(0); + // 设置初始值 + + std::cout << "INIT POSE ESTIMATE" << std::endl; + for (size_t j = 0; j < m_poses.size(); j++) + { + initial_estimates.insert(j, gtsam::Pose3(gtsam::Rot3(poses[j].r), gtsam::Point3(poses[j].t))); + } + + for (int level = 0; level < m_levels; level++) + { + constructHierarchy(clouds, poses, level); + std::cout << "LBA OPTIMIZE LEVEL: " << level << std::endl; + + updateCloudsAndPose(clouds, poses, level); + } + Vec> between_factors_id; + Vec between_factors_pose; + Vec between_factors_info; + getAllFactors(between_factors_id, between_factors_pose, between_factors_info); + // 添加二元因子 + + std::cout << "CONSTRUCT BETWEEN FACTORS" << std::endl; + for (size_t j = 0; j < between_factors_id.size(); j++) + { + gtsam::BetweenFactor between_factor(between_factors_id[j].first, + between_factors_id[j].second, + gtsam::Pose3(gtsam::Rot3(between_factors_pose[j].r), gtsam::Point3(between_factors_pose[j].t)), + gtsam::noiseModel::Gaussian::Information(between_factors_info[j])); + graph.add(between_factor); + } + // 优化 + gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial_estimates, lm_params); + + std::cout << "LM OPTIMIZE " << std::endl; + gtsam::Values result = optimizer.optimize(); + // 更新位姿 + std::cout << "UPDATE POSE ESTIMATE" << std::endl; + for (size_t j = 0; j < m_poses.size(); j++) + { + gtsam::Pose3 pose = result.at(j); + m_poses[j].t = pose.translation().matrix().cast(); + m_poses[j].r = pose.rotation().matrix().cast(); + } + // } +} +void HBA::updateCloudsAndPose(Vec::Ptr> &clouds, Vec &poses, int level) +{ + Vec::Ptr>().swap(clouds); + Vec().swap(poses); + for (BLAM &lba : m_lbas[level]) + { + poses.push_back(lba.poses()[0]); + pcl::PointCloud::Ptr cloud = lba.getLocalCloud(); + if (m_config.down_sample > 0.0) + { + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(m_config.down_sample, m_config.down_sample, m_config.down_sample); + voxel_grid.setInputCloud(cloud); + voxel_grid.filter(*cloud); + } + clouds.push_back(cloud); + } +} +void HBA::constructHierarchy(Vec::Ptr> &clouds, Vec &poses, int level) +{ + BLAMConfig config; + config.voxel_size = m_config.voxel_size; + config.max_iter = m_config.ba_max_iter; + config.max_layer = m_config.max_layer; + config.min_point_num = m_config.min_point_num; + for (size_t i = 0; i < clouds.size(); i += m_config.stride) + { + m_lbas[level].emplace_back(config); + bool drop_last = false; + for (int j = 0; j < m_config.window_size; j++) + { + size_t idx = i + j; + if (i + j >= clouds.size()) + { + drop_last = true; + break; + } + m_lbas[level].back().insert(clouds[idx], poses[idx]); + } + // std::cout << "level: " << level << " window: " << i << std::endl; + m_lbas[level].back().optimize(); + if (drop_last) + break; + } +} +void HBA::getAllFactors(Vec> &ids, Vec &poses, Vec &infos) +{ + for (size_t level_idx = 0; level_idx < static_cast(m_levels); level_idx++) + { + for (size_t stride_idx = 0; stride_idx < m_lbas[level_idx].size(); stride_idx++) + { + BLAM &blam = m_lbas[level_idx][stride_idx]; + for (size_t pose_idx = 0; pose_idx < blam.poses().size() - 1; pose_idx++) + { + size_t from = pose_idx + stride_idx * m_config.stride; + size_t to = pose_idx + 1 + stride_idx * m_config.stride; + from = from * std::pow(m_config.stride, level_idx); + to = to * std::pow(m_config.stride, level_idx); + ids.emplace_back(std::make_pair(from, to)); + Pose &pose_from = blam.poses()[pose_idx]; + Pose &pose_to = blam.poses()[pose_idx + 1]; + Pose pose_fc; + pose_fc.r = pose_from.r.transpose() * pose_to.r; + pose_fc.t = pose_from.r.transpose() * (pose_to.t - pose_from.t); + // std::cout << "level: " << level_idx << " from: " << from << " to: " << to << std::endl; + poses.push_back(pose_fc); + infos.push_back(blam.H().block<6, 6>(6 * pose_idx, 6 * (pose_idx + 1))); + } + } + } +} +int HBA::calcLevels() +{ + + assert(m_poses.size() > 0 && m_config.window_size > 0 && m_config.stride > 0); + return static_cast(0.5 * std::log(3.0 * std::pow(static_cast(m_poses.size()), 2) * (std::pow(m_config.stride, 3) - m_config.stride) / std::pow(static_cast(m_config.window_size), 3)) / std::log(m_config.stride)); +} +pcl::PointCloud::Ptr HBA::getMapPoints() +{ + pcl::PointCloud::Ptr ret(new pcl::PointCloud); + for (size_t i = 0; i < m_poses.size(); i++) + { + Pose &pose = m_poses[i]; + pcl::PointCloud::Ptr cloud = m_clouds[i]; + pcl::PointCloud::Ptr transformed(new pcl::PointCloud); + pcl::transformPointCloud(*cloud, *transformed, pose.t, Eigen::Quaterniond(pose.r)); + *ret += *transformed; + } + return ret; +} +void HBA::writePoses(const std::string &path) +{ + std::ofstream txt_file(path); + for (size_t i = 0; i < m_poses.size(); i++) + { + Pose &pose = m_poses[i]; + V3D t = pose.t; + Eigen::Quaterniond q(pose.r); + txt_file << t.x() << " " << t.y() << " " << t.z() << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl; + } + txt_file.close(); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba/hba.h b/src/FASTLIO2_ROS2/hba/src/hba/hba.h new file mode 100644 index 00000000..cf03dfc0 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba/hba.h @@ -0,0 +1,57 @@ +#pragma once +#include "commons.h" +#include "blam.h" +#include +#include +#include +#include +#include +#include +#include +#include + + +struct HBAConfig +{ + int window_size = 20; + int stride = 10; + double voxel_size = 1.0; + int min_point_num = 10; + int max_layer = 4; + double plane_thresh = 0.01; + size_t ba_max_iter = 10; + size_t hba_iter = 2; + double down_sample = 0.1; +}; + +class HBA +{ +public: + HBA(const HBAConfig config); + void insert(pcl::PointCloud::Ptr &cloud, const Pose &pose); + + void optimize(); + + int calcLevels(); + + void constructHierarchy(Vec::Ptr> &clouds, Vec &poses, int level); + + void updateCloudsAndPose(Vec::Ptr> &clouds, Vec &poses, int level); + int levels() { return m_levels; } + HBAConfig &config() { return m_config; } + Vec &poses() { return m_poses; } + Vec::Ptr> &clouds() { return m_clouds; } + + void getAllFactors(Vec> &ids, Vec &poses, Vec &infos); + + pcl::PointCloud::Ptr getMapPoints(); + + void writePoses(const std::string &path); + +private: + int m_levels = 1; + HBAConfig m_config; + Vec m_poses; + Vec::Ptr> m_clouds; + Vec> m_lbas; +}; diff --git a/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp b/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp new file mode 100644 index 00000000..55e44396 --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba_node copy.cpp @@ -0,0 +1,126 @@ +#include +#include +#include +#include +#include +#include "hba/blam.h" +#include +#include +#include "hba/hba.h" + +void fromStr(const std::string &str, std::string &file_name, Pose &pose) +{ + std::stringstream ss(str); + std::vector tokens; + std::string token; + while (std::getline(ss, token, ' ')) + { + tokens.push_back(token); + } + assert(tokens.size() == 8); + file_name = tokens[0]; + pose.t = V3D(std::stod(tokens[1]), std::stod(tokens[2]), std::stod(tokens[3])); + pose.r = Eigen::Quaterniond(std::stod(tokens[4]), std::stod(tokens[5]), std::stod(tokens[6]), std::stod(tokens[7])).normalized().toRotationMatrix(); +} + +int main1(int argc, char *argv[]) +{ + pcl::PCDReader reader; + BLAMConfig config; + + double scan_resolution = 0.1; + size_t skip_num = 1; + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(scan_resolution, scan_resolution, scan_resolution); + + BLAM blam(config); + + std::filesystem::path p_dir(argv[1]); + std::filesystem::path txt_file = p_dir / "poses.txt"; + if (!std::filesystem::exists(txt_file)) + { + std::cout << "poses.txt not found" << std::endl; + return 0; + } + std::ifstream ifs(txt_file); + std::string line; + std::string file_name; + Pose pose; + + Vec::Ptr> raw_points; + + while (std::getline(ifs, line)) + { + + fromStr(line, file_name, pose); + std::filesystem::path pcd_file = p_dir / "patches" / file_name; + if (!std::filesystem::exists(pcd_file)) + { + std::cerr << "pcd file not found" << std::endl; + continue; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + reader.read(pcd_file, *cloud); + voxel_grid.setInputCloud(cloud); + voxel_grid.filter(*cloud); + blam.insert(cloud, pose); + } + + std::cout << "start optimize" << std::endl; + std::cout << blam.poses()[0].t.transpose() << std::endl; + blam.optimize(); + std::cout << "after optimize" << std::endl; + std::cout << blam.poses()[0].t.transpose() << std::endl; + std::cout << blam.H().block<12, 12>(0, 0) << std::endl; + + std::cout << "plane count with sub: " << blam.planeCount(true) << " :: " << blam.planes().size() << std::endl; + std::cout << "plane count no sub: " << blam.planeCount(false) << std::endl; + return 0; +} + +int main(int argc, char *argv[]) +{ + pcl::PCDReader reader; + HBAConfig config; + + double scan_resolution = 0.1; + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(scan_resolution, scan_resolution, scan_resolution); + + HBA hba(config); + + std::filesystem::path p_dir(argv[1]); + std::filesystem::path txt_file = p_dir / "poses.txt"; + if (!std::filesystem::exists(txt_file)) + { + std::cout << "poses.txt not found" << std::endl; + return 0; + } + std::ifstream ifs(txt_file); + std::string line; + std::string file_name; + Pose pose; + + Vec::Ptr> raw_points; + + while (std::getline(ifs, line)) + { + + fromStr(line, file_name, pose); + std::filesystem::path pcd_file = p_dir / "patches" / file_name; + if (!std::filesystem::exists(pcd_file)) + { + std::cerr << "pcd file not found" << std::endl; + continue; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + reader.read(pcd_file, *cloud); + voxel_grid.setInputCloud(cloud); + voxel_grid.filter(*cloud); + hba.insert(cloud, pose); + } + pcl::io::savePCDFileBinary("/home/zhouzhou/temp/before_opt.pcd", *hba.getMapPoints()); + hba.optimize(); + pcl::io::savePCDFileBinary("/home/zhouzhou/temp/after_opt.pcd", *hba.getMapPoints()); + hba.writePoses("/home/zhouzhou/temp/refined_poses.txt"); +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/hba/src/hba_node.cpp b/src/FASTLIO2_ROS2/hba/src/hba_node.cpp new file mode 100644 index 00000000..83b08c9e --- /dev/null +++ b/src/FASTLIO2_ROS2/hba/src/hba_node.cpp @@ -0,0 +1,222 @@ +#include +#include +#include +#include "hba/hba.h" +#include +#include +#include +#include +#include +#include + +#include "interface/srv/refine_map.hpp" +#include "interface/srv/save_poses.hpp" + +using namespace std::chrono_literals; +void fromStr(const std::string &str, std::string &file_name, Pose &pose) +{ + std::stringstream ss(str); + std::vector tokens; + std::string token; + while (std::getline(ss, token, ' ')) + { + tokens.push_back(token); + } + assert(tokens.size() == 8); + file_name = tokens[0]; + pose.t = V3D(std::stod(tokens[1]), std::stod(tokens[2]), std::stod(tokens[3])); + pose.r = Eigen::Quaterniond(std::stod(tokens[4]), std::stod(tokens[5]), std::stod(tokens[6]), std::stod(tokens[7])).normalized().toRotationMatrix(); +} + +struct NodeConfig +{ + double scan_resolution = 0.1; +}; + +class HBANode : public rclcpp::Node +{ +public: + HBANode() : Node("hba_node") + { + RCLCPP_INFO(this->get_logger(), "HBA node started"); + loadParameters(); + m_hba = std::make_shared(m_hba_config); + m_voxel_grid.setLeafSize(m_node_config.scan_resolution, m_node_config.scan_resolution, m_node_config.scan_resolution); + m_refine_map_srv = this->create_service( + "refine_map", + std::bind(&HBANode::refineMapCB, this, std::placeholders::_1, std::placeholders::_2)); + m_save_poses_srv = this->create_service( + "save_poses", + std::bind(&HBANode::savePosesCB, this, std::placeholders::_1, std::placeholders::_2)); + m_timer = this->create_wall_timer(100ms, std::bind(&HBANode::mainCB, this)); + m_cloud_pub = this->create_publisher("map_points", 10); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + + m_node_config.scan_resolution = config["scan_resolution"].as(); + m_hba_config.window_size = config["window_size"].as(); + m_hba_config.stride = config["stride"].as(); + m_hba_config.voxel_size = config["voxel_size"].as(); + m_hba_config.min_point_num = config["min_point_num"].as(); + m_hba_config.max_layer = config["max_layer"].as(); + m_hba_config.plane_thresh = config["plane_thresh"].as(); + m_hba_config.ba_max_iter = config["ba_max_iter"].as(); + m_hba_config.hba_iter = config["hba_iter"].as(); + m_hba_config.down_sample = config["down_sample"].as(); + } + + void refineMapCB(const std::shared_ptr request, std::shared_ptr response) + { + std::lock_guard lock(m_service_mutex); + if (!std::filesystem::exists(request->maps_path)) + { + response->success = false; + response->message = "maps_path not exists"; + return; + } + + std::filesystem::path p_dir(request->maps_path); + std::filesystem::path pcd_dir = p_dir / "patches"; + if (!std::filesystem::exists(pcd_dir)) + { + response->success = false; + response->message = pcd_dir.string() + " not exists"; + return; + } + + std::filesystem::path txt_file = p_dir / "poses.txt"; + + if (!std::filesystem::exists(txt_file)) + { + response->success = false; + response->message = txt_file.string() + " not exists"; + return; + } + + std::ifstream ifs(txt_file); + std::string line; + std::string file_name; + Pose pose; + pcl::PCDReader reader; + while (std::getline(ifs, line)) + { + + fromStr(line, file_name, pose); + std::filesystem::path pcd_file = p_dir / "patches" / file_name; + if (!std::filesystem::exists(pcd_file)) + { + std::cerr << "pcd file not found" << std::endl; + continue; + } + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + reader.read(pcd_file, *cloud); + m_voxel_grid.setInputCloud(cloud); + m_voxel_grid.filter(*cloud); + m_hba->insert(cloud, pose); + } + RCLCPP_INFO(this->get_logger(), "LOAD POSE %lu;", m_hba->poses().size()); + response->success = true; + response->message = "load poses success!"; + m_do_optimize = true; + return; + } + + void savePosesCB(const std::shared_ptr request, std::shared_ptr response) + { + std::lock_guard lock(m_service_mutex); + std::filesystem::path file_path(request->file_path); + std::filesystem::path par_path = file_path.parent_path(); + if (!std::filesystem::exists(par_path)) + { + response->success = false; + response->message = "parent path not exists"; + return; + } + if (m_hba->poses().size() < 1) + { + response->success = false; + response->message = "poses is empty"; + return; + } + + if (std::filesystem::exists(file_path)) + { + std::filesystem::remove(file_path); + } + + m_hba->writePoses(file_path); + response->success = true; + response->message = "save poses success!"; + return; + } + void mainCB() + { + { + std::lock_guard lock(m_service_mutex); + if (!m_do_optimize) + return; + RCLCPP_WARN(this->get_logger(), "START OPTIMIZE"); + publishMap(); + for (size_t i = 0; i < m_hba_config.hba_iter; i++) + { + RCLCPP_INFO(this->get_logger(), "======HBA ITER %lu START======", i + 1); + m_hba->optimize(); + publishMap(); + RCLCPP_INFO(this->get_logger(), "======HBA ITER %lu END========", i + 1); + } + + m_do_optimize = false; + RCLCPP_WARN(this->get_logger(), "END OPTIMIZE"); + } + } + + void publishMap() + { + if (m_cloud_pub->get_subscription_count() < 1) + return; + if (m_hba->poses().size() < 1) + return; + pcl::PointCloud::Ptr cloud = m_hba->getMapPoints(); + m_voxel_grid.setInputCloud(cloud); + m_voxel_grid.filter(*cloud); + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = "map"; + cloud_msg.header.stamp = this->now(); + m_cloud_pub->publish(cloud_msg); + } + +private: + NodeConfig m_node_config; + HBAConfig m_hba_config; + std::shared_ptr m_hba; + rclcpp::Service::SharedPtr m_refine_map_srv; + rclcpp::Service::SharedPtr m_save_poses_srv; + + rclcpp::Publisher::SharedPtr m_cloud_pub; + + pcl::VoxelGrid m_voxel_grid; + std::mutex m_service_mutex; + bool m_do_optimize = false; + rclcpp::TimerBase::SharedPtr m_timer; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/CMakeLists.txt b/src/FASTLIO2_ROS2/interface/CMakeLists.txt new file mode 100644 index 00000000..8918f6de --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SaveMaps.srv" + "srv/Relocalize.srv" + "srv/IsValid.srv" + "srv/RefineMap.srv" + "srv/SavePoses.srv" + DEPENDENCIES std_msgs +) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/interface/LICENSE b/src/FASTLIO2_ROS2/interface/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/src/FASTLIO2_ROS2/interface/package.xml b/src/FASTLIO2_ROS2/interface/package.xml new file mode 100644 index 00000000..8ead288e --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/package.xml @@ -0,0 +1,24 @@ + + + + interface + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + rosidl_default_generators + + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/interface/srv/IsValid.srv b/src/FASTLIO2_ROS2/interface/srv/IsValid.srv new file mode 100644 index 00000000..a38eb5c0 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/IsValid.srv @@ -0,0 +1,3 @@ +int32 code +--- +bool valid \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv b/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv new file mode 100644 index 00000000..6450115e --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/RefineMap.srv @@ -0,0 +1,4 @@ +string maps_path +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv b/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv new file mode 100644 index 00000000..9cd10868 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/Relocalize.srv @@ -0,0 +1,10 @@ +string pcd_path +float32 x +float32 y +float32 z +float32 yaw +float32 pitch +float32 roll +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv b/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv new file mode 100644 index 00000000..13856556 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/SaveMaps.srv @@ -0,0 +1,5 @@ +string file_path +bool save_patches +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv b/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv new file mode 100644 index 00000000..00cb3840 --- /dev/null +++ b/src/FASTLIO2_ROS2/interface/srv/SavePoses.srv @@ -0,0 +1,4 @@ +string file_path +--- +bool success +string message \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/CMakeLists.txt b/src/FASTLIO2_ROS2/localizer/CMakeLists.txt new file mode 100644 index 00000000..52baf9cc --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(localizer) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(interface REQUIRED) +find_package(pcl_conversions REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(yaml-cpp REQUIRED) + + + +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + + +set(SRC_LIST src/localizers/commons.cpp + src/localizers/icp_localizer.cpp) + +add_executable(localizer_node src/localizer_node.cpp ${SRC_LIST}) +ament_target_dependencies(localizer_node rclcpp std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs interface) +target_link_libraries(localizer_node ${PCL_LIBRARIES} yaml-cpp) + + +install(TARGETS localizer_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/localizer/LICENSE b/src/FASTLIO2_ROS2/localizer/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/src/FASTLIO2_ROS2/localizer/config/localizer.yaml b/src/FASTLIO2_ROS2/localizer/config/localizer.yaml new file mode 100644 index 00000000..3e1fd012 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/config/localizer.yaml @@ -0,0 +1,25 @@ +cloud_topic: /fastlio2/body_cloud +odom_topic: /fastlio2/lio_odom +map_frame: map +local_frame: lidar +update_hz: 1.0 + +# Auto-relocalization on startup +auto_relocalize: false +default_pcd_path: "" +initial_x: 0.0 +initial_y: 0.0 +initial_z: 0.0 +initial_yaw: 0.0 +initial_pitch: 0.0 +initial_roll: 0.0 + +rough_scan_resolution: 0.25 +rough_map_resolution: 0.25 +rough_max_iteration: 5 +rough_score_thresh: 0.2 + +refine_scan_resolution: 0.1 +refine_map_resolution: 0.1 +refine_max_iteration: 10 +refine_score_thresh: 0.1 \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py b/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py new file mode 100644 index 00000000..02166788 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/launch/localizer_launch.py @@ -0,0 +1,53 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + rviz_cfg = PathJoinSubstitution( + [FindPackageShare("localizer"), "rviz", "localizer.rviz"] + ) + localizer_config_path = PathJoinSubstitution( + [FindPackageShare("localizer"), "config", "localizer.yaml"] + ) + + lio_config_path = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "config", "lio.yaml"] + ) + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[ + {"config_path": lio_config_path.perform(launch.LaunchContext())} + ], + ), + launch_ros.actions.Node( + package="localizer", + namespace="localizer", + executable="localizer_node", + name="localizer_node", + output="screen", + parameters=[ + { + "config_path": localizer_config_path.perform( + launch.LaunchContext() + ) + } + ], + ), + launch_ros.actions.Node( + package="rviz2", + namespace="localizer", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ) + ] + ) diff --git a/src/FASTLIO2_ROS2/localizer/package.xml b/src/FASTLIO2_ROS2/localizer/package.xml new file mode 100644 index 00000000..c7b30624 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/package.xml @@ -0,0 +1,27 @@ + + + + localizer + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + + std_msgs + sensor_msgs + nav_msgs + tf2_ros + message_filters + pcl_conversions + geometry_msgs + interface + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz b/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz new file mode 100644 index 00000000..c1e6ffc5 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/rviz/localizer.rviz @@ -0,0 +1,219 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud22 + - /Axes1 + - /Axes2 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 158 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/body_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localizer/map_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: map + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: body + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 38.8297004699707 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.4930500984191895 + Y: 3.4114441871643066 + Z: -0.03380456194281578 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.4947973489761353 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.220401763916016 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ef000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006b0000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp b/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp new file mode 100644 index 00000000..cd883e71 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizer_node.cpp @@ -0,0 +1,411 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "localizers/commons.h" +#include "localizers/icp_localizer.h" +#include "interface/srv/relocalize.hpp" +#include "interface/srv/is_valid.hpp" +#include + +using namespace std::chrono_literals; + +struct NodeConfig +{ + std::string cloud_topic = "/fastlio2/body_cloud"; + std::string odom_topic = "/fastlio2/lio_odom"; + std::string map_frame = "map"; + std::string local_frame = "lidar"; + double update_hz = 1.0; + + // Auto-relocalization on startup + bool auto_relocalize = false; + std::string default_pcd_path = ""; + float initial_x = 0.0f; + float initial_y = 0.0f; + float initial_z = 0.0f; + float initial_yaw = 0.0f; + float initial_pitch = 0.0f; + float initial_roll = 0.0f; +}; + +struct NodeState +{ + std::mutex message_mutex; + std::mutex service_mutex; + + bool message_received = false; + bool service_received = false; + bool localize_success = false; + bool auto_reloc_triggered = false; // Track if auto-relocalization was triggered + rclcpp::Time last_send_tf_time = rclcpp::Clock().now(); + rclcpp::Time last_tf_log_time = rclcpp::Clock().now(); + builtin_interfaces::msg::Time last_message_time; + CloudType::Ptr last_cloud = std::make_shared(); + M3D last_r; // localmap_body_r + V3D last_t; // localmap_body_t + M3D last_offset_r = M3D::Identity(); // map_localmap_r + V3D last_offset_t = V3D::Zero(); // map_localmap_t + M4F initial_guess = M4F::Identity(); +}; + +class LocalizerNode : public rclcpp::Node +{ +public: + LocalizerNode() : Node("localizer_node") + { + RCLCPP_INFO(this->get_logger(), "Localizer Node Started"); + loadParameters(); + rclcpp::QoS qos = rclcpp::QoS(10); + m_cloud_sub.subscribe(this, m_config.cloud_topic, qos.get_rmw_qos_profile()); + m_odom_sub.subscribe(this, m_config.odom_topic, qos.get_rmw_qos_profile()); + + m_tf_broadcaster = std::make_shared(*this); + + m_sync = std::make_shared>>(message_filters::sync_policies::ApproximateTime(10), m_cloud_sub, m_odom_sub); + m_sync->setAgePenalty(0.1); + m_sync->registerCallback(std::bind(&LocalizerNode::syncCB, this, std::placeholders::_1, std::placeholders::_2)); + m_localizer = std::make_shared(m_localizer_config); + + m_reloc_srv = this->create_service("relocalize", std::bind(&LocalizerNode::relocCB, this, std::placeholders::_1, std::placeholders::_2)); + + m_reloc_check_srv = this->create_service("relocalize_check", std::bind(&LocalizerNode::relocCheckCB, this, std::placeholders::_1, std::placeholders::_2)); + + m_map_cloud_pub = this->create_publisher("map_cloud", 10); + + m_timer = this->create_wall_timer(10ms, std::bind(&LocalizerNode::timerCB, this)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + + m_config.cloud_topic = config["cloud_topic"].as(); + m_config.odom_topic = config["odom_topic"].as(); + m_config.map_frame = config["map_frame"].as(); + m_config.local_frame = config["local_frame"].as(); + m_config.update_hz = config["update_hz"].as(); + + // Load auto-relocalization config + if (config["auto_relocalize"]) + m_config.auto_relocalize = config["auto_relocalize"].as(); + if (config["default_pcd_path"]) + m_config.default_pcd_path = config["default_pcd_path"].as(); + if (config["initial_x"]) + m_config.initial_x = config["initial_x"].as(); + if (config["initial_y"]) + m_config.initial_y = config["initial_y"].as(); + if (config["initial_z"]) + m_config.initial_z = config["initial_z"].as(); + if (config["initial_yaw"]) + m_config.initial_yaw = config["initial_yaw"].as(); + if (config["initial_pitch"]) + m_config.initial_pitch = config["initial_pitch"].as(); + if (config["initial_roll"]) + m_config.initial_roll = config["initial_roll"].as(); + + if (m_config.auto_relocalize) + { + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Enabled with map: %s", m_config.default_pcd_path.c_str()); + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Initial pose: x=%.3f, y=%.3f, z=%.3f, yaw=%.3f", + m_config.initial_x, m_config.initial_y, m_config.initial_z, m_config.initial_yaw); + } + + m_localizer_config.rough_scan_resolution = config["rough_scan_resolution"].as(); + m_localizer_config.rough_map_resolution = config["rough_map_resolution"].as(); + m_localizer_config.rough_max_iteration = config["rough_max_iteration"].as(); + m_localizer_config.rough_score_thresh = config["rough_score_thresh"].as(); + + m_localizer_config.refine_scan_resolution = config["refine_scan_resolution"].as(); + m_localizer_config.refine_map_resolution = config["refine_map_resolution"].as(); + m_localizer_config.refine_max_iteration = config["refine_max_iteration"].as(); + m_localizer_config.refine_score_thresh = config["refine_score_thresh"].as(); + } + bool triggerAutoRelocalize() + { + if (m_config.default_pcd_path.empty()) + { + RCLCPP_ERROR(this->get_logger(), "[AUTO-RELOC] FAILED - No PCD path specified"); + return false; + } + + if (!std::filesystem::exists(m_config.default_pcd_path)) + { + RCLCPP_ERROR(this->get_logger(), "[AUTO-RELOC] FAILED - PCD file not found: %s", m_config.default_pcd_path.c_str()); + return false; + } + + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Triggering relocalization..."); + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Map: %s", m_config.default_pcd_path.c_str()); + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Initial pose: x=%.3f, y=%.3f, z=%.3f, roll=%.3f, pitch=%.3f, yaw=%.3f", + m_config.initial_x, m_config.initial_y, m_config.initial_z, + m_config.initial_roll, m_config.initial_pitch, m_config.initial_yaw); + + bool load_flag = m_localizer->loadMap(m_config.default_pcd_path); + if (!load_flag) + { + RCLCPP_ERROR(this->get_logger(), "[AUTO-RELOC] FAILED - Could not load map"); + return false; + } + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Map loaded successfully"); + + Eigen::AngleAxisd yaw_angle = Eigen::AngleAxisd(m_config.initial_yaw, Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd roll_angle = Eigen::AngleAxisd(m_config.initial_roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch_angle = Eigen::AngleAxisd(m_config.initial_pitch, Eigen::Vector3d::UnitY()); + + { + std::lock_guard(m_state.message_mutex); + m_state.initial_guess.setIdentity(); + m_state.initial_guess.block<3, 3>(0, 0) = (yaw_angle * roll_angle * pitch_angle).toRotationMatrix().cast(); + m_state.initial_guess.block<3, 1>(0, 3) = V3F(m_config.initial_x, m_config.initial_y, m_config.initial_z); + m_state.service_received = true; + m_state.localize_success = false; + } + + RCLCPP_INFO(this->get_logger(), "[AUTO-RELOC] Initial guess set, waiting for localization..."); + return true; + } + + void timerCB() + { + if (!m_state.message_received) + return; + + // Trigger auto-relocalization once when first message is received + if (m_config.auto_relocalize && !m_state.auto_reloc_triggered) + { + m_state.auto_reloc_triggered = true; + triggerAutoRelocalize(); + } + + rclcpp::Duration diff = rclcpp::Clock().now() - m_state.last_send_tf_time; + + bool update_tf = diff.seconds() > (1.0 / m_config.update_hz) && m_state.message_received; + + if (!update_tf) + { + sendBroadCastTF(m_state.last_message_time); + return; + } + + m_state.last_send_tf_time = rclcpp::Clock().now(); + + M4F initial_guess = M4F::Identity(); + if (m_state.service_received) + { + std::lock_guard(m_state.service_mutex); + initial_guess = m_state.initial_guess; + // m_state.service_received = false; + } + else + { + std::lock_guard(m_state.message_mutex); + initial_guess.block<3, 3>(0, 0) = (m_state.last_offset_r * m_state.last_r).cast(); + initial_guess.block<3, 1>(0, 3) = (m_state.last_offset_r * m_state.last_t + m_state.last_offset_t).cast(); + } + + M3D current_local_r; + V3D current_local_t; + builtin_interfaces::msg::Time current_time; + { + std::lock_guard(m_state.message_mutex); + current_local_r = m_state.last_r; + current_local_t = m_state.last_t; + current_time = m_state.last_message_time; + m_localizer->setInput(m_state.last_cloud); + } + + bool result = m_localizer->align(initial_guess); + if (result) + { + M3D map_body_r = initial_guess.block<3, 3>(0, 0).cast(); + V3D map_body_t = initial_guess.block<3, 1>(0, 3).cast(); + m_state.last_offset_r = map_body_r * current_local_r.transpose(); + m_state.last_offset_t = -map_body_r * current_local_r.transpose() * current_local_t + map_body_t; + if (!m_state.localize_success && m_state.service_received) + { + std::lock_guard(m_state.service_mutex); + m_state.localize_success = true; + m_state.service_received = false; + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] SUCCESS - Localization converged"); + Eigen::Quaterniond q_result(map_body_r); + // Convert quaternion to euler (yaw) + double yaw_result = std::atan2(2.0 * (q_result.w() * q_result.z() + q_result.x() * q_result.y()), + 1.0 - 2.0 * (q_result.y() * q_result.y() + q_result.z() * q_result.z())); + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Final pose in map: x=%.3f, y=%.3f, z=%.3f, yaw=%.3f rad (%.1f deg)", + map_body_t.x(), map_body_t.y(), map_body_t.z(), yaw_result, yaw_result * 180.0 / M_PI); + } + } + else + { + RCLCPP_DEBUG(this->get_logger(), "[RELOCALIZE] ICP alignment iteration - not yet converged"); + } + sendBroadCastTF(current_time); + publishMapCloud(current_time); + } + void syncCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud_msg, const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg) + { + + std::lock_guard(m_state.message_mutex); + + pcl::fromROSMsg(*cloud_msg, *m_state.last_cloud); + + m_state.last_r = Eigen::Quaterniond(odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z) + .toRotationMatrix(); + m_state.last_t = V3D(odom_msg->pose.pose.position.x, + odom_msg->pose.pose.position.y, + odom_msg->pose.pose.position.z); + m_state.last_message_time = cloud_msg->header.stamp; + if (!m_state.message_received) + { + m_state.message_received = true; + m_config.local_frame = odom_msg->header.frame_id; + } + } + + void sendBroadCastTF(builtin_interfaces::msg::Time &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = m_config.map_frame; + transformStamped.child_frame_id = m_config.local_frame; + transformStamped.header.stamp = time; + Eigen::Quaterniond q(m_state.last_offset_r); + V3D t = m_state.last_offset_t; + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + m_tf_broadcaster->sendTransform(transformStamped); + + // Throttled TF logging (every 5 seconds) + rclcpp::Duration tf_log_diff = rclcpp::Clock().now() - m_state.last_tf_log_time; + if (tf_log_diff.seconds() > 5.0) + { + m_state.last_tf_log_time = rclcpp::Clock().now(); + double yaw = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); + RCLCPP_INFO(this->get_logger(), "[TF] %s -> %s: x=%.3f, y=%.3f, z=%.3f, yaw=%.3f rad (%.1f deg)", + m_config.map_frame.c_str(), m_config.local_frame.c_str(), + t.x(), t.y(), t.z(), yaw, yaw * 180.0 / M_PI); + } + } + + void relocCB(const std::shared_ptr request, std::shared_ptr response) + { + std::string pcd_path = request->pcd_path; + float x = request->x; + float y = request->y; + float z = request->z; + float yaw = request->yaw; + float roll = request->roll; + float pitch = request->pitch; + + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Service called - Initial pose: x=%.3f, y=%.3f, z=%.3f, roll=%.3f, pitch=%.3f, yaw=%.3f", + x, y, z, roll, pitch, yaw); + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Map path: %s", pcd_path.c_str()); + + if (!std::filesystem::exists(pcd_path)) + { + RCLCPP_ERROR(this->get_logger(), "[RELOCALIZE] FAILED - PCD file not found: %s", pcd_path.c_str()); + response->success = false; + response->message = "pcd file not found"; + return; + } + + Eigen::AngleAxisd yaw_angle = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd roll_angle = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch_angle = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()); + bool load_flag = m_localizer->loadMap(pcd_path); + if (!load_flag) + { + RCLCPP_ERROR(this->get_logger(), "[RELOCALIZE] FAILED - Could not load map from: %s", pcd_path.c_str()); + response->success = false; + response->message = "load map failed"; + return; + } + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Map loaded successfully"); + { + std::lock_guard(m_state.message_mutex); + m_state.initial_guess.setIdentity(); + m_state.initial_guess.block<3, 3>(0, 0) = (yaw_angle * roll_angle * pitch_angle).toRotationMatrix().cast(); + m_state.initial_guess.block<3, 1>(0, 3) = V3F(x, y, z); + m_state.service_received = true; + m_state.localize_success = false; + } + + RCLCPP_INFO(this->get_logger(), "[RELOCALIZE] Initial guess set, waiting for localization..."); + response->success = true; + response->message = "relocalize success"; + return; + } + + void relocCheckCB(const std::shared_ptr request, std::shared_ptr response) + { + std::lock_guard(m_state.service_mutex); + if (request->code == 1) + response->valid = true; + else + response->valid = m_state.localize_success; + return; + } + void publishMapCloud(builtin_interfaces::msg::Time &time) + { + if (m_map_cloud_pub->get_subscription_count() < 1) + return; + CloudType::Ptr map_cloud = m_localizer->refineMap(); + if (map_cloud->size() < 1) + return; + sensor_msgs::msg::PointCloud2 map_cloud_msg; + pcl::toROSMsg(*map_cloud, map_cloud_msg); + map_cloud_msg.header.frame_id = m_config.map_frame; + map_cloud_msg.header.stamp = time; + m_map_cloud_pub->publish(map_cloud_msg); + } + +private: + NodeConfig m_config; + NodeState m_state; + + ICPConfig m_localizer_config; + std::shared_ptr m_localizer; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; + rclcpp::TimerBase::SharedPtr m_timer; + std::shared_ptr>> m_sync; + std::shared_ptr m_tf_broadcaster; + rclcpp::Service::SharedPtr m_reloc_srv; + rclcpp::Service::SharedPtr m_reloc_check_srv; + rclcpp::Publisher::SharedPtr m_map_cloud_pub; +}; +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp new file mode 100644 index 00000000..cb3a78a1 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.cpp @@ -0,0 +1 @@ +#include "commons.h" \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h new file mode 100644 index 00000000..e7e01325 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/commons.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include +#include + +using PointType = pcl::PointXYZI; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M4F = Eigen::Matrix4f; +using V4F = Eigen::Vector4f; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp new file mode 100644 index 00000000..c2f4f1f3 --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.cpp @@ -0,0 +1,87 @@ +#include "icp_localizer.h" + +ICPLocalizer::ICPLocalizer(const ICPConfig &config) : m_config(config) +{ + m_refine_inp.reset(new CloudType); + m_refine_tgt.reset(new CloudType); + m_rough_inp.reset(new CloudType); + m_rough_tgt.reset(new CloudType); +} +bool ICPLocalizer::loadMap(const std::string &path) +{ + if (!std::filesystem::exists(path)) + { + std::cerr << "Map file not found: " << path << std::endl; + return false; + } + pcl::PCDReader reader; + CloudType::Ptr cloud(new CloudType); + reader.read(path, *cloud); + if (m_config.refine_map_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.refine_map_resolution, m_config.refine_map_resolution, m_config.refine_map_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_refine_tgt); + } + else + { + pcl::copyPointCloud(*cloud, *m_refine_tgt); + } + + if (m_config.rough_map_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.rough_map_resolution, m_config.rough_map_resolution, m_config.rough_map_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_rough_tgt); + } + else + { + pcl::copyPointCloud(*cloud, *m_rough_tgt); + } + return true; +} +void ICPLocalizer::setInput(const CloudType::Ptr &cloud) +{ + if (m_config.refine_scan_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.refine_scan_resolution, m_config.refine_scan_resolution, m_config.refine_scan_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_refine_inp); + } + else + { + pcl::copyPointCloud(*cloud, *m_refine_inp); + } + + if (m_config.rough_scan_resolution > 0) + { + m_voxel_filter.setLeafSize(m_config.rough_scan_resolution, m_config.rough_scan_resolution, m_config.rough_scan_resolution); + m_voxel_filter.setInputCloud(cloud); + m_voxel_filter.filter(*m_rough_inp); + } + else + { + pcl::copyPointCloud(*cloud, *m_rough_inp); + } +} + +bool ICPLocalizer::align(M4F &guess) +{ + CloudType::Ptr aligned_cloud(new CloudType); + if (m_refine_tgt->size() == 0 || m_rough_tgt->size() == 0) + return false; + m_rough_icp.setMaximumIterations(m_config.rough_max_iteration); + m_rough_icp.setInputSource(m_rough_inp); + m_rough_icp.setInputTarget(m_rough_tgt); + m_rough_icp.align(*aligned_cloud, guess); + if (!m_rough_icp.hasConverged() || m_rough_icp.getFitnessScore() > m_config.rough_score_thresh) + return false; + m_refine_icp.setMaximumIterations(m_config.refine_max_iteration); + m_refine_icp.setInputSource(m_refine_inp); + m_refine_icp.setInputTarget(m_refine_tgt); + m_refine_icp.align(*aligned_cloud, m_rough_icp.getFinalTransformation()); + if (!m_refine_icp.hasConverged() || m_refine_icp.getFitnessScore() > m_config.refine_score_thresh) + return false; + guess = m_refine_icp.getFinalTransformation(); + return true; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h new file mode 100644 index 00000000..5fea433a --- /dev/null +++ b/src/FASTLIO2_ROS2/localizer/src/localizers/icp_localizer.h @@ -0,0 +1,46 @@ +#pragma once +#include "commons.h" +#include +#include +#include +#include + +struct ICPConfig +{ + double refine_scan_resolution = 0.1; + double refine_map_resolution = 0.1; + double refine_score_thresh = 0.1; + int refine_max_iteration = 10; + + double rough_scan_resolution = 0.25; + double rough_map_resolution = 0.25; + double rough_score_thresh = 0.2; + int rough_max_iteration = 5; +}; + +class ICPLocalizer +{ +public: + ICPLocalizer(const ICPConfig &config); + + bool loadMap(const std::string &path); + + void setInput(const CloudType::Ptr &cloud); + + bool align(M4F &guess); + ICPConfig &config() { return m_config; } + CloudType::Ptr roughMap() { return m_rough_tgt; } + CloudType::Ptr refineMap() { return m_refine_tgt; } + + +private: + ICPConfig m_config; + pcl::VoxelGrid m_voxel_filter; + pcl::IterativeClosestPoint m_refine_icp; + pcl::IterativeClosestPoint m_rough_icp; + CloudType::Ptr m_refine_inp; + CloudType::Ptr m_rough_inp; + CloudType::Ptr m_refine_tgt; + CloudType::Ptr m_rough_tgt; + std::string m_pcd_path; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/CMakeLists.txt b/src/FASTLIO2_ROS2/pgo/CMakeLists.txt new file mode 100644 index 00000000..d2e123c9 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(pgo) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED True) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(interface REQUIRED) +find_package(yaml-cpp REQUIRED) + + + +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + +set(SRC_LIST src/pgos/commons.cpp + src/pgos/simple_pgo.cpp) + +add_executable(pgo_node src/pgo_node.cpp ${SRC_LIST}) +ament_target_dependencies(pgo_node rclcpp std_msgs sensor_msgs nav_msgs message_filters pcl_conversions tf2_ros geometry_msgs visualization_msgs interface GTSAM) +target_link_libraries(pgo_node ${PCL_LIBRARIES} gtsam yaml-cpp) + + +install(TARGETS pgo_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/FASTLIO2_ROS2/pgo/LICENSE b/src/FASTLIO2_ROS2/pgo/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/src/FASTLIO2_ROS2/pgo/config/pgo.yaml b/src/FASTLIO2_ROS2/pgo/config/pgo.yaml new file mode 100644 index 00000000..4fc9087b --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/config/pgo.yaml @@ -0,0 +1,13 @@ +cloud_topic: /fastlio2/body_cloud +odom_topic: /fastlio2/lio_odom +map_frame: map +local_frame: lidar + +key_pose_delta_deg: 10 +key_pose_delta_trans: 0.5 +loop_search_radius: 1.0 +loop_time_tresh: 60.0 +loop_score_tresh: 0.15 +loop_submap_half_range: 5 +submap_resolution: 0.1 +min_loop_detect_duration: 5.0 \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py b/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py new file mode 100644 index 00000000..2ffc30ad --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/launch/pgo_launch.py @@ -0,0 +1,46 @@ +import launch +import launch_ros.actions +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + rviz_cfg = PathJoinSubstitution( + [FindPackageShare("pgo"), "rviz", "pgo.rviz"] + ) + pgo_config_path = PathJoinSubstitution( + [FindPackageShare("pgo"), "config", "pgo.yaml"] + ) + + lio_config_path = PathJoinSubstitution( + [FindPackageShare("fastlio2"), "config", "lio.yaml"] + ) + + + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="fastlio2", + namespace="fastlio2", + executable="lio_node", + name="lio_node", + output="screen", + parameters=[{"config_path": lio_config_path.perform(launch.LaunchContext())}] + ), + launch_ros.actions.Node( + package="pgo", + namespace="pgo", + executable="pgo_node", + name="pgo_node", + output="screen", + parameters=[{"config_path": pgo_config_path.perform(launch.LaunchContext())}] + ), + launch_ros.actions.Node( + package="rviz2", + namespace="pgo", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_cfg.perform(launch.LaunchContext())], + ) + ] + ) diff --git a/src/FASTLIO2_ROS2/pgo/package.xml b/src/FASTLIO2_ROS2/pgo/package.xml new file mode 100644 index 00000000..e077f042 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/package.xml @@ -0,0 +1,30 @@ + + + + pgo + 0.0.0 + TODO: Package description + zhouzhou + MIT + + ament_cmake + + std_msgs + rclcpp + nav_msgs + tf2_ros + pcl_conversions + sensor_msgs + message_filters + geometry_msgs + visualization_msgs + interface + GTSAM + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz b/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz new file mode 100644 index 00000000..6b4101c2 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/rviz/pgo.rviz @@ -0,0 +1,258 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud22 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 195 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/body_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 200 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 195 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/world_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: map + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: body + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fastlio2/lio_path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + pgo_edges: true + pgo_nodes: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pgo/loop_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 38.65398025512695 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.8247766494750977 + Y: -5.519836902618408 + Z: 5.7220458984375e-06 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.01979660987854 + Target Frame: + Value: XYOrbit (rviz_default_plugins) + Yaw: 4.747386932373047 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000749000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 27 diff --git a/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp b/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp new file mode 100644 index 00000000..4f8a10f1 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgo_node.cpp @@ -0,0 +1,307 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pgos/commons.h" +#include "pgos/simple_pgo.h" +#include "interface/srv/save_maps.hpp" +#include +#include +#include + +using namespace std::chrono_literals; + +struct NodeConfig +{ + std::string cloud_topic = "/lio/body_cloud"; + std::string odom_topic = "/lio/odom"; + std::string map_frame = "map"; + std::string local_frame = "lidar"; +}; + +struct NodeState +{ + std::mutex message_mutex; + std::queue cloud_buffer; + double last_message_time; +}; + +class PGONode : public rclcpp::Node +{ +public: + PGONode() : Node("pgo_node") + { + RCLCPP_INFO(this->get_logger(), "PGO node started"); + loadParameters(); + m_pgo = std::make_shared(m_pgo_config); + rclcpp::QoS qos = rclcpp::QoS(10); + m_cloud_sub.subscribe(this, m_node_config.cloud_topic, qos.get_rmw_qos_profile()); + m_odom_sub.subscribe(this, m_node_config.odom_topic, qos.get_rmw_qos_profile()); + m_loop_marker_pub = this->create_publisher("/pgo/loop_markers", 10000); + m_tf_broadcaster = std::make_shared(*this); + m_sync = std::make_shared>>(message_filters::sync_policies::ApproximateTime(10), m_cloud_sub, m_odom_sub); + m_sync->setAgePenalty(0.1); + m_sync->registerCallback(std::bind(&PGONode::syncCB, this, std::placeholders::_1, std::placeholders::_2)); + m_timer = this->create_wall_timer(50ms, std::bind(&PGONode::timerCB, this)); + m_save_map_srv = this->create_service("/pgo/save_maps", std::bind(&PGONode::saveMapsCB, this, std::placeholders::_1, std::placeholders::_2)); + } + + void loadParameters() + { + this->declare_parameter("config_path", ""); + std::string config_path; + this->get_parameter("config_path", config_path); + YAML::Node config = YAML::LoadFile(config_path); + if (!config) + { + RCLCPP_WARN(this->get_logger(), "FAIL TO LOAD YAML FILE!"); + return; + } + RCLCPP_INFO(this->get_logger(), "LOAD FROM YAML CONFIG PATH: %s", config_path.c_str()); + m_node_config.cloud_topic = config["cloud_topic"].as(); + m_node_config.odom_topic = config["odom_topic"].as(); + m_node_config.map_frame = config["map_frame"].as(); + m_node_config.local_frame = config["local_frame"].as(); + + m_pgo_config.key_pose_delta_deg = config["key_pose_delta_deg"].as(); + m_pgo_config.key_pose_delta_trans = config["key_pose_delta_trans"].as(); + m_pgo_config.loop_search_radius = config["loop_search_radius"].as(); + m_pgo_config.loop_time_tresh = config["loop_time_tresh"].as(); + m_pgo_config.loop_score_tresh = config["loop_score_tresh"].as(); + m_pgo_config.loop_submap_half_range = config["loop_submap_half_range"].as(); + m_pgo_config.submap_resolution = config["submap_resolution"].as(); + m_pgo_config.min_loop_detect_duration = config["min_loop_detect_duration"].as(); + } + void syncCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud_msg, const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg) + { + + std::lock_guard(m_state.message_mutex); + CloudWithPose cp; + cp.pose.setTime(cloud_msg->header.stamp.sec, cloud_msg->header.stamp.nanosec); + if (cp.pose.second < m_state.last_message_time) + { + RCLCPP_WARN(this->get_logger(), "Received out of order message"); + return; + } + m_state.last_message_time = cp.pose.second; + + cp.pose.r = Eigen::Quaterniond(odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z) + .toRotationMatrix(); + cp.pose.t = V3D(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z); + cp.cloud = CloudType::Ptr(new CloudType); + pcl::fromROSMsg(*cloud_msg, *cp.cloud); + m_state.cloud_buffer.push(cp); + } + + void sendBroadCastTF(builtin_interfaces::msg::Time &time) + { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.frame_id = m_node_config.map_frame; + transformStamped.child_frame_id = m_node_config.local_frame; + transformStamped.header.stamp = time; + Eigen::Quaterniond q(m_pgo->offsetR()); + V3D t = m_pgo->offsetT(); + transformStamped.transform.translation.x = t.x(); + transformStamped.transform.translation.y = t.y(); + transformStamped.transform.translation.z = t.z(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + m_tf_broadcaster->sendTransform(transformStamped); + } + + void publishLoopMarkers(builtin_interfaces::msg::Time &time) + { + if (m_loop_marker_pub->get_subscription_count() == 0) + return; + if (m_pgo->historyPairs().size() == 0) + return; + + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker nodes_marker; + visualization_msgs::msg::Marker edges_marker; + nodes_marker.header.frame_id = m_node_config.map_frame; + nodes_marker.header.stamp = time; + nodes_marker.ns = "pgo_nodes"; + nodes_marker.id = 0; + nodes_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + nodes_marker.action = visualization_msgs::msg::Marker::ADD; + nodes_marker.pose.orientation.w = 1.0; + nodes_marker.scale.x = 0.3; + nodes_marker.scale.y = 0.3; + nodes_marker.scale.z = 0.3; + nodes_marker.color.r = 1.0; + nodes_marker.color.g = 0.8; + nodes_marker.color.b = 0.0; + nodes_marker.color.a = 1.0; + + edges_marker.header.frame_id = m_node_config.map_frame; + edges_marker.header.stamp = time; + edges_marker.ns = "pgo_edges"; + edges_marker.id = 1; + edges_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + edges_marker.action = visualization_msgs::msg::Marker::ADD; + edges_marker.pose.orientation.w = 1.0; + edges_marker.scale.x = 0.1; + edges_marker.color.r = 0.0; + edges_marker.color.g = 0.8; + edges_marker.color.b = 0.0; + edges_marker.color.a = 1.0; + + std::vector &poses = m_pgo->keyPoses(); + std::vector> &pairs = m_pgo->historyPairs(); + for (size_t i = 0; i < pairs.size(); i++) + { + size_t i1 = pairs[i].first; + size_t i2 = pairs[i].second; + geometry_msgs::msg::Point p1, p2; + p1.x = poses[i1].t_global.x(); + p1.y = poses[i1].t_global.y(); + p1.z = poses[i1].t_global.z(); + + p2.x = poses[i2].t_global.x(); + p2.y = poses[i2].t_global.y(); + p2.z = poses[i2].t_global.z(); + + nodes_marker.points.push_back(p1); + nodes_marker.points.push_back(p2); + edges_marker.points.push_back(p1); + edges_marker.points.push_back(p2); + } + + marker_array.markers.push_back(nodes_marker); + marker_array.markers.push_back(edges_marker); + m_loop_marker_pub->publish(marker_array); + } + + void timerCB() + { + if (m_state.cloud_buffer.size() == 0) + return; + CloudWithPose cp = m_state.cloud_buffer.front(); + // 清理队列 + { + std::lock_guard(m_state.message_mutex); + while (!m_state.cloud_buffer.empty()) + { + m_state.cloud_buffer.pop(); + } + } + builtin_interfaces::msg::Time cur_time; + cur_time.sec = cp.pose.sec; + cur_time.nanosec = cp.pose.nsec; + if (!m_pgo->addKeyPose(cp)) + { + + sendBroadCastTF(cur_time); + return; + } + + m_pgo->searchForLoopPairs(); + + m_pgo->smoothAndUpdate(); + + sendBroadCastTF(cur_time); + + publishLoopMarkers(cur_time); + } + + void saveMapsCB(const std::shared_ptr request, std::shared_ptr response) + { + if (!std::filesystem::exists(request->file_path)) + { + response->success = false; + response->message = request->file_path + " IS NOT EXISTS!"; + return; + } + + if (m_pgo->keyPoses().size() == 0) + { + response->success = false; + response->message = "NO POSES!"; + return; + } + + std::filesystem::path p_dir(request->file_path); + std::filesystem::path patches_dir = p_dir / "patches"; + std::filesystem::path poses_txt_path = p_dir / "poses.txt"; + std::filesystem::path map_path = p_dir / "map.pcd"; + + if (request->save_patches) + { + if (std::filesystem::exists(patches_dir)) + { + std::filesystem::remove_all(patches_dir); + } + + std::filesystem::create_directories(patches_dir); + + if (std::filesystem::exists(poses_txt_path)) + { + std::filesystem::remove(poses_txt_path); + } + RCLCPP_INFO(this->get_logger(), "Patches Path: %s", patches_dir.string().c_str()); + } + RCLCPP_INFO(this->get_logger(), "SAVE MAP TO %s", map_path.string().c_str()); + + std::ofstream txt_file(poses_txt_path); + + CloudType::Ptr ret(new CloudType); + for (size_t i = 0; i < m_pgo->keyPoses().size(); i++) + { + + CloudType::Ptr body_cloud = m_pgo->keyPoses()[i].body_cloud; + if (request->save_patches) + { + std::string patch_name = std::to_string(i) + ".pcd"; + std::filesystem::path patch_path = patches_dir / patch_name; + pcl::io::savePCDFileBinary(patch_path.string(), *body_cloud); + Eigen::Quaterniond q(m_pgo->keyPoses()[i].r_global); + V3D t = m_pgo->keyPoses()[i].t_global; + txt_file << patch_name << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl; + } + CloudType::Ptr world_cloud(new CloudType); + pcl::transformPointCloud(*body_cloud, *world_cloud, m_pgo->keyPoses()[i].t_global, Eigen::Quaterniond(m_pgo->keyPoses()[i].r_global)); + *ret += *world_cloud; + } + txt_file.close(); + pcl::io::savePCDFileBinary(map_path.string(), *ret); + response->success = true; + response->message = "SAVE SUCCESS!"; + } + +private: + NodeConfig m_node_config; + Config m_pgo_config; + NodeState m_state; + std::shared_ptr m_pgo; + rclcpp::TimerBase::SharedPtr m_timer; + rclcpp::Publisher::SharedPtr m_loop_marker_pub; + rclcpp::Service::SharedPtr m_save_map_srv; + message_filters::Subscriber m_cloud_sub; + message_filters::Subscriber m_odom_sub; + std::shared_ptr m_tf_broadcaster; + std::shared_ptr>> m_sync; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp new file mode 100644 index 00000000..54fef5f6 --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.cpp @@ -0,0 +1,8 @@ +#include "commons.h" + +void PoseWithTime::setTime(int32_t _sec, uint32_t _nsec) +{ + sec = _sec; + nsec = _nsec; + second = static_cast(sec) + static_cast(nsec) / 1e9; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h new file mode 100644 index 00000000..12de288f --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/commons.h @@ -0,0 +1,30 @@ +#pragma once +#include +#include +#include + +using PointType = pcl::PointXYZI; +using CloudType = pcl::PointCloud; +using PointVec = std::vector>; + +using M3D = Eigen::Matrix3d; +using V3D = Eigen::Vector3d; +using M3F = Eigen::Matrix3f; +using V3F = Eigen::Vector3f; +using M4F = Eigen::Matrix4f; +using V4F = Eigen::Vector4f; + +struct PoseWithTime { + V3D t; + M3D r; + int32_t sec; + uint32_t nsec; + double second; + void setTime(int32_t sec, uint32_t nsec); + // double second() const; +}; + +struct CloudWithPose { + CloudType::Ptr cloud; + PoseWithTime pose; +}; \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp new file mode 100644 index 00000000..ee0f73bf --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.cpp @@ -0,0 +1,211 @@ +#include "simple_pgo.h" + +SimplePGO::SimplePGO(const Config &config) : m_config(config) +{ + gtsam::ISAM2Params isam2_params; + isam2_params.relinearizeThreshold = 0.01; + isam2_params.relinearizeSkip = 1; + m_isam2 = std::make_shared(isam2_params); + m_initial_values.clear(); + m_graph.resize(0); + m_r_offset.setIdentity(); + m_t_offset.setZero(); + + m_icp.setMaximumIterations(50); + m_icp.setMaxCorrespondenceDistance(10); + m_icp.setTransformationEpsilon(1e-6); + m_icp.setEuclideanFitnessEpsilon(1e-6); + m_icp.setRANSACIterations(0); +} + +bool SimplePGO::isKeyPose(const PoseWithTime &pose) +{ + if (m_key_poses.size() == 0) + return true; + const KeyPoseWithCloud &last_item = m_key_poses.back(); + double delta_trans = (pose.t - last_item.t_local).norm(); + double delta_deg = Eigen::Quaterniond(pose.r).angularDistance(Eigen::Quaterniond(last_item.r_local)) * 57.324; + if (delta_trans > m_config.key_pose_delta_trans || delta_deg > m_config.key_pose_delta_deg) + return true; + return false; +} +bool SimplePGO::addKeyPose(const CloudWithPose &cloud_with_pose) +{ + bool is_key_pose = isKeyPose(cloud_with_pose.pose); + if (!is_key_pose) + return false; + size_t idx = m_key_poses.size(); + M3D init_r = m_r_offset * cloud_with_pose.pose.r; + V3D init_t = m_r_offset * cloud_with_pose.pose.t + m_t_offset; + // 添加初始值 + m_initial_values.insert(idx, gtsam::Pose3(gtsam::Rot3(init_r), gtsam::Point3(init_t))); + if (idx == 0) + { + // 添加先验约束 + gtsam::noiseModel::Diagonal::shared_ptr noise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() * 1e-12); + m_graph.add(gtsam::PriorFactor(idx, gtsam::Pose3(gtsam::Rot3(init_r), gtsam::Point3(init_t)), noise)); + } + else + { + // 添加里程计约束 + const KeyPoseWithCloud &last_item = m_key_poses.back(); + M3D r_between = last_item.r_local.transpose() * cloud_with_pose.pose.r; + V3D t_between = last_item.r_local.transpose() * (cloud_with_pose.pose.t - last_item.t_local); + gtsam::noiseModel::Diagonal::shared_ptr noise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-6).finished()); + m_graph.add(gtsam::BetweenFactor(idx - 1, idx, gtsam::Pose3(gtsam::Rot3(r_between), gtsam::Point3(t_between)), noise)); + } + KeyPoseWithCloud item; + item.time = cloud_with_pose.pose.second; + item.r_local = cloud_with_pose.pose.r; + item.t_local = cloud_with_pose.pose.t; + item.body_cloud = cloud_with_pose.cloud; + item.r_global = init_r; + item.t_global = init_t; + m_key_poses.push_back(item); + return true; +} + +CloudType::Ptr SimplePGO::getSubMap(int idx, int half_range, double resolution) +{ + assert(idx >= 0 && idx < static_cast(m_key_poses.size())); + int min_idx = std::max(0, idx - half_range); + int max_idx = std::min(static_cast(m_key_poses.size()) - 1, idx + half_range); + + CloudType::Ptr ret(new CloudType); + for (int i = min_idx; i <= max_idx; i++) + { + + CloudType::Ptr body_cloud = m_key_poses[i].body_cloud; + CloudType::Ptr global_cloud(new CloudType); + pcl::transformPointCloud(*body_cloud, *global_cloud, m_key_poses[i].t_global, Eigen::Quaterniond(m_key_poses[i].r_global)); + *ret += *global_cloud; + } + if (resolution > 0) + { + pcl::VoxelGrid voxel_grid; + voxel_grid.setLeafSize(resolution, resolution, resolution); + voxel_grid.setInputCloud(ret); + voxel_grid.filter(*ret); + } + return ret; +} + +void SimplePGO::searchForLoopPairs() +{ + if (m_key_poses.size() < 10) + return; + if (m_config.min_loop_detect_duration > 0.0) + { + if (m_history_pairs.size() > 0) + { + double current_time = m_key_poses.back().time; + double last_time = m_key_poses[m_history_pairs.back().second].time; + if (current_time - last_time < m_config.min_loop_detect_duration) + return; + } + } + + size_t cur_idx = m_key_poses.size() - 1; + const KeyPoseWithCloud &last_item = m_key_poses.back(); + pcl::PointXYZ last_pose_pt; + last_pose_pt.x = last_item.t_global(0); + last_pose_pt.y = last_item.t_global(1); + last_pose_pt.z = last_item.t_global(2); + + pcl::PointCloud::Ptr key_poses_cloud(new pcl::PointCloud); + for (size_t i = 0; i < m_key_poses.size() - 1; i++) + { + pcl::PointXYZ pt; + pt.x = m_key_poses[i].t_global(0); + pt.y = m_key_poses[i].t_global(1); + pt.z = m_key_poses[i].t_global(2); + key_poses_cloud->push_back(pt); + } + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(key_poses_cloud); + std::vector ids; + std::vector sqdists; + int neighbors = kdtree.radiusSearch(last_pose_pt, m_config.loop_search_radius, ids, sqdists); + if (neighbors == 0) + return; + + int loop_idx = -1; + for (size_t i = 0; i < ids.size(); i++) + { + int idx = ids[i]; + if (std::abs(last_item.time - m_key_poses[idx].time) > m_config.loop_time_tresh) + { + loop_idx = idx; + break; + } + } + + if (loop_idx == -1) + return; + + CloudType::Ptr target_cloud = getSubMap(loop_idx, m_config.loop_submap_half_range, m_config.submap_resolution); + CloudType::Ptr source_cloud = getSubMap(m_key_poses.size() - 1, 0, m_config.submap_resolution); + CloudType::Ptr align_cloud(new CloudType); + + m_icp.setInputSource(source_cloud); + m_icp.setInputTarget(target_cloud); + m_icp.align(*align_cloud); + + if (!m_icp.hasConverged() || m_icp.getFitnessScore() > m_config.loop_score_tresh) + return; + + M4F loop_transform = m_icp.getFinalTransformation(); + + LoopPair one_pair; + one_pair.source_id = cur_idx; + one_pair.target_id = loop_idx; + one_pair.score = m_icp.getFitnessScore(); + M3D r_refined = loop_transform.block<3, 3>(0, 0).cast() * m_key_poses[cur_idx].r_global; + V3D t_refined = loop_transform.block<3, 3>(0, 0).cast() * m_key_poses[cur_idx].t_global + loop_transform.block<3, 1>(0, 3).cast(); + one_pair.r_offset = m_key_poses[loop_idx].r_global.transpose() * r_refined; + one_pair.t_offset = m_key_poses[loop_idx].r_global.transpose() * (t_refined - m_key_poses[loop_idx].t_global); + m_cache_pairs.push_back(one_pair); + m_history_pairs.emplace_back(one_pair.target_id, one_pair.source_id); +} + +void SimplePGO::smoothAndUpdate() +{ + bool has_loop = !m_cache_pairs.empty(); + // 添加回环因子 + if (has_loop) + { + for (LoopPair &pair : m_cache_pairs) + { + m_graph.add(gtsam::BetweenFactor(pair.target_id, pair.source_id, + gtsam::Pose3(gtsam::Rot3(pair.r_offset), + gtsam::Point3(pair.t_offset)), + gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() * pair.score))); + } + std::vector().swap(m_cache_pairs); + } + // smooth and mapping + m_isam2->update(m_graph, m_initial_values); + m_isam2->update(); + if (has_loop) + { + m_isam2->update(); + m_isam2->update(); + m_isam2->update(); + m_isam2->update(); + } + m_graph.resize(0); + m_initial_values.clear(); + + // update key poses + gtsam::Values estimate_values = m_isam2->calculateBestEstimate(); + for (size_t i = 0; i < m_key_poses.size(); i++) + { + gtsam::Pose3 pose = estimate_values.at(i); + m_key_poses[i].r_global = pose.rotation().matrix().cast(); + m_key_poses[i].t_global = pose.translation().matrix().cast(); + } + // update offset + const KeyPoseWithCloud &last_item = m_key_poses.back(); + m_r_offset = last_item.r_global * last_item.r_local.transpose(); + m_t_offset = last_item.t_global - m_r_offset * last_item.t_local; +} \ No newline at end of file diff --git a/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h new file mode 100644 index 00000000..fce824ad --- /dev/null +++ b/src/FASTLIO2_ROS2/pgo/src/pgos/simple_pgo.h @@ -0,0 +1,78 @@ +#pragma once +#include "commons.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct KeyPoseWithCloud +{ + M3D r_local; + V3D t_local; + M3D r_global; + V3D t_global; + double time; + CloudType::Ptr body_cloud; +}; +struct LoopPair +{ + size_t source_id; + size_t target_id; + M3D r_offset; + V3D t_offset; + double score; +}; + +struct Config +{ + double key_pose_delta_deg = 10; + double key_pose_delta_trans = 1.0; + double loop_search_radius = 1.0; + double loop_time_tresh = 60.0; + double loop_score_tresh = 0.15; + int loop_submap_half_range = 5; + double submap_resolution = 0.1; + double min_loop_detect_duration = 10.0; +}; + +class SimplePGO +{ +public: + SimplePGO(const Config &config); + + bool isKeyPose(const PoseWithTime &pose); + + bool addKeyPose(const CloudWithPose &cloud_with_pose); + + bool hasLoop(){return m_cache_pairs.size() > 0;} + + void searchForLoopPairs(); + + void smoothAndUpdate(); + + CloudType::Ptr getSubMap(int idx, int half_range, double resolution); + std::vector> &historyPairs() { return m_history_pairs; } + std::vector &keyPoses() { return m_key_poses; } + + M3D offsetR() { return m_r_offset; } + V3D offsetT() { return m_t_offset; } + +private: + Config m_config; + std::vector m_key_poses; + std::vector> m_history_pairs; + std::vector m_cache_pairs; + M3D m_r_offset; + V3D m_t_offset; + std::shared_ptr m_isam2; + gtsam::Values m_initial_values; + gtsam::NonlinearFactorGraph m_graph; + pcl::IterativeClosestPoint m_icp; +}; \ No newline at end of file