diff --git a/FAST_LIO b/FAST_LIO index 18418bc..ae3af82 160000 --- a/FAST_LIO +++ b/FAST_LIO @@ -1 +1 @@ -Subproject commit 18418bc5b15f16b4cb3d7f0f5c303c7698df8a22 +Subproject commit ae3af8229d217cadf0485dcbd9e89a046fda3dc7 diff --git a/README.md b/README.md index cc0123a..6cb416c 100644 --- a/README.md +++ b/README.md @@ -45,3 +45,8 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` ros2 service call /trigger_service std_srvs/srv/SetBool "{data: true}" + +重定位 +ros2 launch fast_lio localization_mid360.launch.py map:=path/test.pcd use_sim_time:=true +然后通过rviz2的 2D pose Estimate发布估计位置 +观察 /global_map_viz与 /cur_scan_in_map关系是否正确即可 diff --git a/livox_ros_driver2 b/livox_ros_driver2 index ded97ce..80d95bf 160000 --- a/livox_ros_driver2 +++ b/livox_ros_driver2 @@ -1 +1 @@ -Subproject commit ded97ce6604389fd6582f160af02c2d9c9bf61c0 +Subproject commit 80d95bf220aa954fef3d931991476f4eb75b2680 diff --git a/map_pub/CMakeLists.txt b/map_pub/CMakeLists.txt deleted file mode 100644 index 8206bcd..0000000 --- a/map_pub/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(map_pub) - -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(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(PCL REQUIRED) - -# 添加可执行文件 -add_executable(map_pub src/map_pub.cpp) - -# 添加依赖 -ament_target_dependencies(map_pub - rclcpp - sensor_msgs - pcl_conversions -) - -# 链接PCL库 -target_link_libraries(map_pub ${PCL_LIBRARIES}) - -# 安装 -install(TARGETS - map_pub - DESTINATION lib/${PROJECT_NAME} -) - -# 导出依赖 -# ament_export_dependencies( -# rclcpp -# sensor_msgs -# pcl_conversions -# ) - -# ament_package() - -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/map_pub/package.xml b/map_pub/package.xml deleted file mode 100644 index 4c43a2d..0000000 --- a/map_pub/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - map_pub - 0.0.0 - TODO: Package description - z - TODO: License declaration - - ament_cmake - - rclcpp - sensor_msgs - pcl_conversions - pcl_ros - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/map_pub/src/map_pub.cpp b/map_pub/src/map_pub.cpp deleted file mode 100644 index 68045a1..0000000 --- a/map_pub/src/map_pub.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -class PCDMapPublisher : public rclcpp::Node -{ -public: - PCDMapPublisher() : Node("pcd_map_publisher") - { - // 声明参数 - this->declare_parameter("pcd_file_path", "/home/z/rm_sim/src/rm_simulation/FAST_LIO/PCD/test.pcd"); - this->declare_parameter("frame_id", "map"); - this->declare_parameter("publish_rate", 1.0); - - // 获取参数 - std::string pcd_file_path = this->get_parameter("pcd_file_path").as_string(); - frame_id_ = this->get_parameter("frame_id").as_string(); - double publish_rate = this->get_parameter("publish_rate").as_double(); - - // 创建发布者 - publisher_ = this->create_publisher("/localizer/map_cloud", 10); - - // 加载PCD文件 - if (!loadPCDFile(pcd_file_path)) { - RCLCPP_ERROR(this->get_logger(), "Failed to load PCD file: %s", pcd_file_path.c_str()); - return; - } - - RCLCPP_INFO(this->get_logger(), "Successfully loaded PCD file: %s", pcd_file_path.c_str()); - RCLCPP_INFO(this->get_logger(), "Point cloud size: %ld points", cloud_->size()); - - // 创建定时器发布点云 - timer_ = this->create_wall_timer( - std::chrono::duration(1.0 / publish_rate), - std::bind(&PCDMapPublisher::publishPointCloud, this)); - - RCLCPP_INFO(this->get_logger(), "PCD map publisher started, publishing on topic: /localizer/map_cloud"); - } - -private: - bool loadPCDFile(const std::string& file_path) - { - cloud_.reset(new pcl::PointCloud()); - - if (pcl::io::loadPCDFile(file_path, *cloud_) == -1) { - return false; - } - - return true; - } - - void publishPointCloud() - { - if (cloud_->empty()) { - RCLCPP_WARN(this->get_logger(), "Point cloud is empty, skipping publish"); - return; - } - - // 转换为ROS2消息 - sensor_msgs::msg::PointCloud2 cloud_msg; - pcl::toROSMsg(*cloud_, cloud_msg); - cloud_msg.header.stamp = this->now(); - cloud_msg.header.frame_id = frame_id_; - - // 发布消息 - publisher_->publish(cloud_msg); - - RCLCPP_DEBUG(this->get_logger(), "Published point cloud with %ld points", cloud_->size()); - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - pcl::PointCloud::Ptr cloud_; - std::string frame_id_; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py b/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py index fbb6b70..df685e0 100644 --- a/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py +++ b/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py @@ -30,7 +30,8 @@ def get_world_config(world_type): }, WorldType.RMUL: { 'x': '4.3', - 'y': '3.35', + # 'y': '3.35', + 'y': '5.0', 'z': '1.16', 'yaw': '0.0', 'world_path': 'RMUL2024_world/RMUL2024_world.world' diff --git a/robot-relocalization/CMakeLists.txt b/robot-relocalization/CMakeLists.txt deleted file mode 100644 index 5c9c9ab..0000000 --- a/robot-relocalization/CMakeLists.txt +++ /dev/null @@ -1,138 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(bbs_based_initializer) - -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") -option(BUILD_CUDA "Build GPU version" ON) - -set(CMAKE_CUDA_HOST_COMPILER /usr/bin/g++-10) - - -# Set C++ standard -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -# Find required packages -set(PCL_DIR /usr/lib/x86_64-linux-gnu/cmake/pcl) -find_package(PCL REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -# Auto-detect CUDA availability -if(BUILD_CUDA) - # Check for CUDA compiler and runtime - find_package(CUDAToolkit QUIET) - if(CUDAToolkit_FOUND) - # Check if we can enable CUDA language - try_compile(CUDA_LANGUAGE_SUPPORTED - ${CMAKE_BINARY_DIR}/cuda_language_test - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/test_cuda_runtime.cpp - LINK_LIBRARIES CUDA::cudart - OUTPUT_VARIABLE CUDA_COMPILE_OUTPUT - ) - - if(CUDA_LANGUAGE_SUPPORTED) - enable_language(CUDA) - set(USE_CUDA ON) - add_definitions(-DUSE_CUDA) - - # Set CUDA architectures to avoid policy warning - if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) - set(CMAKE_CUDA_ARCHITECTURES "75;80;86" CACHE STRING "CUDA architectures") - endif() - - message(STATUS "CUDA toolkit detected, building GPU version") - message(STATUS "CUDA version: ${CUDAToolkit_VERSION}") - else() - set(USE_CUDA OFF) - message(STATUS "CUDA toolkit found but compilation test failed, building CPU version") - message(STATUS "CUDA compile output: ${CUDA_COMPILE_OUTPUT}") - endif() - else() - set(USE_CUDA OFF) - message(STATUS "CUDA toolkit not found, building CPU version") - endif() -else() - set(USE_CUDA OFF) - message(STATUS "CUDA disabled by BUILD_CUDA=OFF, building CPU version") -endif() - -# Define source files -set(BBS_INITIALIZER_SOURCES - src/bbs_based_initializer/bbs_based_initializer.cpp - src/bbs_based_initializer/bbs_based_initializer_node.cpp -) - -set(CPU_BBS3D_SOURCES - src/cpu_bbs3d/bbs3d.cpp - src/cpu_bbs3d/voxelmaps.cpp - src/cpu_bbs3d/voxelmaps_io.cpp -) - -set(GPU_BBS3D_SOURCES - src/gpu_bbs3d/bbs3d.cu - src/gpu_bbs3d/calc_score.cu - src/gpu_bbs3d/voxelmaps.cu - src/gpu_bbs3d/voxelmaps_io.cu - src/gpu_bbs3d/stream_manager/check_error.cu -) - -# Create main executable -if(USE_CUDA) - # GPU version - ament_auto_add_executable(bbs_based_initializer - ${BBS_INITIALIZER_SOURCES} - ${GPU_BBS3D_SOURCES} - ) - - set_property(TARGET bbs_based_initializer PROPERTY CUDA_STANDARD 17) - set_property(TARGET bbs_based_initializer PROPERTY CUDA_STANDARD_REQUIRED ON) - - # Link CUDA libraries - target_link_libraries(bbs_based_initializer - ${PCL_LIBRARIES} - yaml-cpp - CUDA::cudart - ) - - # Include CUDA headers - target_include_directories(bbs_based_initializer PRIVATE - ${CUDAToolkit_INCLUDE_DIRS} - ) -else() - # CPU version - ament_auto_add_executable(bbs_based_initializer - ${BBS_INITIALIZER_SOURCES} - ${CPU_BBS3D_SOURCES} - ) - - target_link_libraries(bbs_based_initializer - ${PCL_LIBRARIES} - yaml-cpp - ) -endif() - -# Include directories -target_include_directories(bbs_based_initializer - PUBLIC - ${PROJECT_SOURCE_DIR}/include - ${EIGEN3_INCLUDE_DIR} -) - -# OpenMP for CPU parallelization -find_package(OpenMP) -if(OpenMP_CXX_FOUND AND NOT USE_CUDA) - target_link_libraries(bbs_based_initializer OpenMP::OpenMP_CXX) -endif() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config - USE_SCOPED_HEADER_INSTALL_DIR -) diff --git a/robot-relocalization/cmake/Findgpu_bbs3d.cmake b/robot-relocalization/cmake/Findgpu_bbs3d.cmake deleted file mode 100644 index 4cc3447..0000000 --- a/robot-relocalization/cmake/Findgpu_bbs3d.cmake +++ /dev/null @@ -1,8 +0,0 @@ -find_path(gpu_bbs3d_INCLUDE_DIRS gpu_bbs3d - HINTS /usr/local/include) - -find_library(gpu_bbs3d_LIBRARY NAMES gpu_bbs3d - HINTS /usr/local/lib) - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(gpu_bbs3d DEFAULT_MSG gpu_bbs3d_INCLUDE_DIRS gpu_bbs3d_LIBRARY) \ No newline at end of file diff --git a/robot-relocalization/cmake/test_cuda_runtime.cpp b/robot-relocalization/cmake/test_cuda_runtime.cpp deleted file mode 100644 index 76854ab..0000000 --- a/robot-relocalization/cmake/test_cuda_runtime.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include -#include - -int main() { - int deviceCount = 0; - cudaError_t error = cudaGetDeviceCount(&deviceCount); - - if (error != cudaSuccess) { - std::cerr << "CUDA error: " << cudaGetErrorString(error) << std::endl; - return 1; - } - - if (deviceCount == 0) { - std::cerr << "No CUDA-capable devices found" << std::endl; - return 1; - } - - std::cout << "Found " << deviceCount << " CUDA device(s)" << std::endl; - - // Test basic CUDA functionality - for (int i = 0; i < deviceCount; ++i) { - cudaDeviceProp prop; - error = cudaGetDeviceProperties(&prop, i); - if (error != cudaSuccess) { - std::cerr << "Failed to get device properties for device " << i << std::endl; - return 1; - } - std::cout << "Device " << i << ": " << prop.name << std::endl; - } - - return 0; -} \ No newline at end of file diff --git a/robot-relocalization/config/bbs_based_initializer.param.yaml b/robot-relocalization/config/bbs_based_initializer.param.yaml deleted file mode 100644 index fba2228..0000000 --- a/robot-relocalization/config/bbs_based_initializer.param.yaml +++ /dev/null @@ -1,57 +0,0 @@ -################[Necessary]################ -# BBS-Based Initializer Configuration -# This config file supports both CPU and GPU modes (auto-detected) - -## topic name (for sub) -# sending a message (with data=true) will trigger localization -trigger_topic_name: "/bbs_localize" ## msg type: std_msgs/Bool - -map_topic_name: "/localizer/map_cloud" ## msg type: sensor_msgs/PointCloud2 -# lidar_topic_name: "/lio_point_body" ## msg type: sensor_msgs/PointCloud2 -lidar_topic_name: "/livox/lidar/pointcloud" ## msg type: sensor_msgs/PointCloud2 -imu_topic_name: "/livox/imu" ## msg type: sensor_msgs/Imu - -## topic name (for pub) -# note: PoseWithCovarianceStamped -# /initialpose is same topic when using rviz's 2d pose estimate func. -pose_topic_name: "/initialpose" - -## 3D-BBS parameters -# CPU mode: uses double precision, GPU mode: uses float precision (auto-converted) -min_level_res: 8.0 # [m], e.g. outdoor: 1.0, indoor: 0.5 -max_level: 16 # Hierarchy levels (more levels = more accuracy but slower) - -## Angular search range [rad] -# 6.28 input is converted to 2*M_PI -min_rpy: [-0.02,-0.02,0.0] # [roll, pitch, yaw] - small roll/pitch for robot constraint -max_rpy: [0.02,0.02,6.28] # [roll, pitch, yaw] - full yaw rotation allowed - -## Score threshold -# score_threshold = floor(src_points.size() * score_threshold_percentage) -# Higher values = more strict matching, Lower values = more tolerant -score_threshold_percentage: 0.9 - -################[Optional]################ -## Downsample target(map) clouds [m] -# Reduces map points for faster processing -map_leaf_size: 0.1 # off: 0.0 - -## Downsample source clouds [m] -# Reduces scan points for faster processing -src_leaf_size: 2.0 # off: 0.0 - -## Crop source clouds [m] -# Limit point cloud range for relevant data only -min_scan_range: 0.0 # [m] - minimum distance -max_scan_range: 100.0 # [m] - maximum distance -# off: set 0.0 to both min_scan_range and max_scan_range - -## Timeout [msec] -# Maximum time allowed for localization (0 = no timeout) -timeout_msec: 0 # off: 0 - -# Performance notes: -# - GPU mode: Better for large point clouds and complex scenes -# - CPU mode: Better for smaller point clouds, uses OpenMP parallelization -# - Adjust score_threshold_percentage based on your environment complexity - \ No newline at end of file diff --git a/robot-relocalization/config/bbs_based_initializer_fast.param.yaml b/robot-relocalization/config/bbs_based_initializer_fast.param.yaml deleted file mode 100644 index 01adb9b..0000000 --- a/robot-relocalization/config/bbs_based_initializer_fast.param.yaml +++ /dev/null @@ -1,55 +0,0 @@ -################[Necessary]################ -# BBS-Based Initializer Configuration - OPTIMIZED FOR CPU SPEED -# This config is tuned for fast CPU performance - -## topic name (for sub) -trigger_topic_name: "/bbs_localize" -map_topic_name: "/localizer/map_cloud" -lidar_topic_name: "/lio_point_body" -imu_topic_name: "/livox/imu" - -## topic name (for pub) -pose_topic_name: "/initialpose" - -## 3D-BBS parameters - OPTIMIZED FOR SPEED -# Reduced from 16 to 12 levels (25% faster, minimal accuracy loss) -min_level_res: 4.0 # Reduced from 8.0 - smaller search space -max_level: 12 # Reduced from 16 - fewer iterations - -## Angular search range [rad] -# Same constraints - robot on ground -min_rpy: [-0.02,-0.02,0.0] -max_rpy: [0.02,0.02,6.28] - -## Score threshold -# Slightly relaxed for faster convergence -score_threshold_percentage: 0.85 # Reduced from 0.9 - -################[Optional - SPEED OPTIMIZED]################ -## Downsample target(map) clouds [m] -# Increased for faster processing -map_leaf_size: 0.2 # Increased from 0.1 (fewer map points) - -## Downsample source clouds [m] -# Increased significantly for speed -src_leaf_size: 3.0 # Increased from 2.0 (fewer scan points = faster) - -## Crop source clouds [m] -# Limit range for faster processing -min_scan_range: 1.0 # Skip nearby noisy points -max_scan_range: 80.0 # Reduced from 100.0 - -## Timeout [msec] -# Set timeout to prevent hanging -timeout_msec: 30000 # 30 seconds max - -# Performance notes: -# This configuration prioritizes SPEED over accuracy -# Expected speedup: 3-5x faster than default -# Accuracy loss: ~5-10% (acceptable for initial localization) -# -# Optimization strategy: -# 1. Fewer voxel levels (12 vs 16) = 30% less iterations -# 2. Coarser voxels (4m vs 8m) = smaller search space -# 3. More downsampling (3m vs 2m) = fewer points to match -# 4. Tighter range limits (80m vs 100m) = less data diff --git a/robot-relocalization/include/bbs_based_initializer.hpp b/robot-relocalization/include/bbs_based_initializer.hpp deleted file mode 100644 index d084ec8..0000000 --- a/robot-relocalization/include/bbs_based_initializer.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -// ros2 -#include -#include -#include -#include -#include -#include -#include -#include -#include -// #include -#include -// pcl -#include -#include - -#ifdef USE_CUDA -#include -#else -#include -#endif - -class BbsBasedInitializer : public rclcpp::Node { -public: - BbsBasedInitializer(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()); - ~BbsBasedInitializer(); - -private: - bool load_config(const std::string& config); - // void localize_callback(const std_msgs::msg::Bool::SharedPtr msg); - int get_nearest_imu_index(const std::vector& imu_buffer, const builtin_interfaces::msg::Time& stamp); - void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void map_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); - void trigger_callback(const std::shared_ptr request, - const std::shared_ptr response); - - - // sub - rclcpp::Subscription::SharedPtr trigger_sub_; - rclcpp::Subscription::SharedPtr cloud_sub_; - rclcpp::Subscription::SharedPtr map_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Service::SharedPtr trigger_ser_; - - // pub - rclcpp::Publisher::SharedPtr pose_pub_; - - bool is_map_loaded; - - // msg buffer - sensor_msgs::msg::PointCloud2::SharedPtr source_cloud_msg_; - std::vector imu_buffer; - -#ifdef USE_CUDA - gpu::BBS3D gpu_bbs3d; -#else - cpu::BBS3D cpu_bbs3d; -#endif - - // topic name - std::string lidar_topic_name, map_topic_name, imu_topic_name; - std::string trigger_topic_name, pose_topic_name; - - // 3D-BBS parameters - double min_level_res; - int max_level; - - // angular search range - Eigen::Vector3d min_rpy; - Eigen::Vector3d max_rpy; - - // score threshold percentage - double score_threshold_percentage; - - // downsample - float map_leaf_size, src_leaf_size; - double min_scan_range, max_scan_range; - - // timeout - int timeout_msec; -}; diff --git a/robot-relocalization/include/cpu_bbs3d/bbs3d.hpp b/robot-relocalization/include/cpu_bbs3d/bbs3d.hpp deleted file mode 100644 index c74ebcb..0000000 --- a/robot-relocalization/include/cpu_bbs3d/bbs3d.hpp +++ /dev/null @@ -1,140 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include - -namespace cpu { -class VoxelMaps; - -struct AngularInfo { - Eigen::Vector3i num_division; - Eigen::Vector3d rpy_res; - Eigen::Vector3d min_rpy; -}; - -class BBS3D { -public: - BBS3D(); - ~BBS3D(); - - void set_tar_points(const std::vector& points, double min_level_res, int max_level); - - void set_tar_points(const std::vector& points, double min_level_res, int max_level); - - void set_src_points(const std::vector& points); - - void set_src_points(const std::vector& points); - - void set_trans_search_range(const std::vector& points); - - void set_trans_search_range(const std::vector& points); - - - void set_trans_search_range(const Eigen::Vector3d& min_xyz, const Eigen::Vector3d& max_xyz); - - void set_angular_search_range(const Eigen::Vector3d& min_rpy, const Eigen::Vector3d& max_rpy) { - min_rpy_ = min_rpy; - max_rpy_ = max_rpy; - } - - void set_voxel_expantion_rate(const double rate) { - v_rate_ = rate; - inv_v_rate_ = 1.0 / rate; - } - - void set_num_threads(const int num_threads) { num_threads_ = num_threads; } - - void set_score_threshold_percentage(double percentage) { score_threshold_percentage_ = percentage; } - - void enable_timeout() { use_timeout_ = true; } - - void disable_timeout() { use_timeout_ = false; } - - void set_timeout_duration_in_msec(const int msec); - - std::vector get_src_points() const { return src_points_; } - - bool set_voxelmaps_coords(const std::string& folder_path); - - std::pair get_trans_search_range() const { - return std::pair{min_xyz_, max_xyz_}; - } - - std::vector get_angular_search_range() const { return std::vector{min_rpy_, max_rpy_}; } - - Eigen::Matrix4d get_global_pose() const { return global_pose_; } - - int get_best_score() const { return best_score_; } - - double get_elapsed_time() const { return elapsed_time_; } - - double get_best_score_percentage() { - if (src_points_.size() == 0) - return 0.0; - else - return static_cast(best_score_) / src_points_.size(); - }; - - bool has_timed_out() { return has_timed_out_; } - - bool has_localized() { return has_localized_; } - - void localize(); - - // pcd iof - bool load_voxel_params(const std::string& voxelmaps_folder_path); - - bool set_multi_buckets(const std::string& folder_path); - - bool save_voxelmaps_pcd(const std::string& folder_path); - - bool save_voxel_params(const std::string& folder_path); - -private: - void calc_angular_info(std::vector& ang_info_vec); - - std::vector> create_init_transset(const AngularInfo& init_ang_info); - - void calc_score( - DiscreteTransformation& trans, - const double trans_res, - const Eigen::Vector3d& rpy_res, - const Eigen::Vector3d& min_rpy, - const std::vector& buckets, - const int max_bucket_scan_count, - const std::vector& points); - -private: - Eigen::Matrix4d global_pose_; - bool has_timed_out_, has_localized_; - double elapsed_time_; - - std::vector src_points_; - - std::unique_ptr voxelmaps_ptr_; - std::string voxelmaps_folder_name_; - - double v_rate_; // voxel expansion rate - double inv_v_rate_; - int num_threads_; - - int best_score_; - double score_threshold_percentage_; - bool use_timeout_; - std::chrono::milliseconds timeout_duration_; - Eigen::Vector3d min_xyz_; - Eigen::Vector3d max_xyz_; - Eigen::Vector3d min_rpy_; - Eigen::Vector3d max_rpy_; - std::pair init_tx_range_; - std::pair init_ty_range_; - std::pair init_tz_range_; -}; - -} // namespace cpu \ No newline at end of file diff --git a/robot-relocalization/include/cpu_bbs3d/voxelmaps.hpp b/robot-relocalization/include/cpu_bbs3d/voxelmaps.hpp deleted file mode 100644 index b83e3ff..0000000 --- a/robot-relocalization/include/cpu_bbs3d/voxelmaps.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace cpu { -class VoxelMaps { -public: - VoxelMaps(); - ~VoxelMaps(); - - struct VectorHash { - size_t operator()(const Eigen::Vector3i& x) const { - size_t seed = 0; - boost::hash_combine(seed, x[0]); - boost::hash_combine(seed, x[1]); - boost::hash_combine(seed, x[2]); - return seed; - } - }; - - struct VctorEqual { - bool operator()(const Eigen::Vector3i& v1, const Eigen::Vector3i& v2) const { return v1 == v2; } - }; - - using UnorderedVoxelMap = std::unordered_map; - using Buckets = std::vector; - - void set_min_res(double min_level_res) { min_level_res_ = min_level_res; } - - void set_max_level(int max_level) { max_level_ = max_level; } - - void set_max_bucket_scan_count(int max_bucket_scan_count) { max_bucket_scan_count_ = max_bucket_scan_count; } - - double get_min_res() const { return min_level_res_; } - - int get_max_level() const { return max_level_; } - - int get_max_bucket_scan_count() const { return max_bucket_scan_count_; } - - void create_voxelmaps(const std::vector& points, const int v_rate); - -private: - std::vector create_neighbor_coords(const Eigen::Vector3i& vec); - - Buckets create_hash_buckets(const UnorderedVoxelMap& unordered_voxelmap); - -public: - std::vector multi_buckets_; - - std::vector voxelmaps_res_; - -private: - double min_level_res_; - int max_level_, max_bucket_scan_count_; -}; -} // namespace cpu \ No newline at end of file diff --git a/robot-relocalization/include/discrete_transformation/discrete_transformation.hpp b/robot-relocalization/include/discrete_transformation/discrete_transformation.hpp deleted file mode 100644 index ab04dcc..0000000 --- a/robot-relocalization/include/discrete_transformation/discrete_transformation.hpp +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once -#include - -template -using Vector3 = Eigen::Matrix; - -template -using Matrix4 = Eigen::Matrix; - -template -class DiscreteTransformation { -public: - DiscreteTransformation() : score(0), level(0), x(0), y(0), z(0), roll(0), pitch(0), yaw(0) {} - - DiscreteTransformation(int score) : score(score), level(0), x(0), y(0), z(0), roll(0), pitch(0), yaw(0) {} - - DiscreteTransformation(int score, int level, int x, int y, int z, int roll, int pitch, int yaw) - : score(score), - level(level), - x(x), - y(y), - z(z), - roll(roll), - pitch(pitch), - yaw(yaw) {} - - ~DiscreteTransformation() {} - - bool operator<(const DiscreteTransformation& rhs) const { return score < rhs.score; } - - bool is_leaf() const { return level == 0; } - - Matrix4 create_matrix(const T trans_res, const Vector3& rpy_res, const Vector3& min_rpy) { - Eigen::Translation translation(x * trans_res, y * trans_res, z * trans_res); - Eigen::AngleAxis rollAngle(roll * rpy_res.x() + min_rpy.x(), Vector3::UnitX()); - Eigen::AngleAxis pitchAngle(pitch * rpy_res.y() + min_rpy.y(), Vector3::UnitY()); - Eigen::AngleAxis yawAngle(yaw * rpy_res.z() + min_rpy.z(), Vector3::UnitZ()); - return (translation * yawAngle * pitchAngle * rollAngle).matrix(); - } - - void branch(std::vector& b, const int child_level, const int v_rate, const Eigen::Vector3i& num_division) { - for (int i = 0; i < v_rate; i++) { - for (int j = 0; j < v_rate; j++) { - for (int k = 0; k < v_rate; k++) { - for (int l = 0; l < num_division.x(); l++) { - for (int m = 0; m < num_division.y(); m++) { - for (int n = 0; n < num_division.z(); n++) { - b.emplace_back(DiscreteTransformation( - 0, - child_level, - x * v_rate + i, - y * v_rate + j, - z * v_rate + k, - roll * num_division.x() + l, - pitch * num_division.y() + m, - yaw * num_division.z() + n)); - } - } - } - } - } - } - } - - std::vector branch(const int child_level, const int v_rate, const Eigen::Vector3i& num_division) { - std::vector b; - b.reserve(v_rate * v_rate * v_rate * num_division.x() * num_division.y() * num_division.z()); - for (int i = 0; i < v_rate; i++) { - for (int j = 0; j < v_rate; j++) { - for (int k = 0; k < v_rate; k++) { - for (int l = 0; l < num_division.x(); l++) { - for (int m = 0; m < num_division.y(); m++) { - for (int n = 0; n < num_division.z(); n++) { - b.emplace_back(DiscreteTransformation( - 0, - child_level, - x * v_rate + i, - y * v_rate + j, - z * v_rate + k, - roll * num_division.x() + l, - pitch * num_division.y() + m, - yaw * num_division.z() + n)); - } - } - } - } - } - } - return b; - } - -public: - int score; - int level; - int x; - int y; - int z; - int roll; - int pitch; - int yaw; -}; diff --git a/robot-relocalization/include/gpu_bbs3d/bbs3d.cuh b/robot-relocalization/include/gpu_bbs3d/bbs3d.cuh deleted file mode 100644 index f07f24d..0000000 --- a/robot-relocalization/include/gpu_bbs3d/bbs3d.cuh +++ /dev/null @@ -1,130 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include - -// thrust -#include -#include -#include - -namespace gpu { -class VoxelMaps; - -struct AngularInfo { - Eigen::Vector3i num_division; - Eigen::Vector3f rpy_res; - Eigen::Vector3f min_rpy; -}; - -class BBS3D { -public: - BBS3D(); - ~BBS3D(); - - void set_tar_points(const std::vector& points, float min_level_res, int max_level); - - void set_src_points(const std::vector& points); - - void set_trans_search_range(const std::vector& points); - - void set_trans_search_range(const Eigen::Vector3f& min_xyz, const Eigen::Vector3f& max_xyz); - - void set_angular_search_range(const Eigen::Vector3f& min_rpy, const Eigen::Vector3f& max_rpy) { - min_rpy_ = min_rpy; - max_rpy_ = max_rpy; - } - - void set_voxel_expantion_rate(const float rate) { - v_rate_ = rate; - inv_v_rate_ = 1.0f / rate; - } - - void set_branch_copy_size(int size) { branch_copy_size_ = size; } - - void set_score_threshold_percentage(float percentage) { score_threshold_percentage_ = percentage; } - - void enable_timeout() { use_timeout_ = true; } - - void disable_timeout() { use_timeout_ = false; } - - void set_timeout_duration_in_msec(const int msec); - - std::vector get_src_points() const { return src_points_; } - - bool set_voxelmaps_coords(const std::string& folder_path); - - std::pair get_trans_search_range() const { - return std::pair{min_xyz_, max_xyz_}; - } - - std::vector get_angular_search_range() const { return std::vector{min_rpy_, max_rpy_}; } - - Eigen::Matrix4f get_global_pose() const { return global_pose_; } - - int get_best_score() const { return best_score_; } - - double get_elapsed_time() const { return elapsed_time_; } - - float get_best_score_percentage() { - if (src_points_.size() == 0) - return 0.0f; - else - return static_cast(best_score_) / src_points_.size(); - }; - - bool has_timed_out() { return has_timed_out_; } - - bool has_localized() { return has_localized_; } - - void localize(); - -private: - void calc_angular_info(std::vector& ang_info_vec); - - std::vector> create_init_transset(const AngularInfo& init_ang_info); - - std::vector> calc_scores( - const std::vector>& h_transset, - thrust::device_vector& d_ang_info_vec); - - // pcd iof - bool load_voxel_params(const std::string& voxelmaps_folder_path); - - std::vector> set_multi_buckets(const std::string& voxelmaps_folder_path); - -private: - Eigen::Matrix4f global_pose_; - bool has_timed_out_, has_localized_; - double elapsed_time_; - - cudaStream_t stream; - - std::vector src_points_; - - thrust::device_vector d_src_points_; - - std::unique_ptr voxelmaps_ptr_; - std::string voxelmaps_folder_name_; - - float v_rate_; // voxel expansion rate - float inv_v_rate_; - - int branch_copy_size_, best_score_; - double score_threshold_percentage_; - bool use_timeout_; - std::chrono::milliseconds timeout_duration_; - Eigen::Vector3f min_xyz_; - Eigen::Vector3f max_xyz_; - Eigen::Vector3f min_rpy_; - Eigen::Vector3f max_rpy_; - std::pair init_tx_range_; - std::pair init_ty_range_; - std::pair init_tz_range_; -}; -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/include/gpu_bbs3d/stream_manager/check_error.cuh b/robot-relocalization/include/gpu_bbs3d/stream_manager/check_error.cuh deleted file mode 100644 index dfdd307..0000000 --- a/robot-relocalization/include/gpu_bbs3d/stream_manager/check_error.cuh +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace gpu { - -class CUDACheckError { -public: - void operator<<(cudaError_t error) const; -}; - -extern CUDACheckError check_error; - -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/include/gpu_bbs3d/voxelmaps.cuh b/robot-relocalization/include/gpu_bbs3d/voxelmaps.cuh deleted file mode 100644 index 6e40dad..0000000 --- a/robot-relocalization/include/gpu_bbs3d/voxelmaps.cuh +++ /dev/null @@ -1,74 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -namespace gpu { -struct VoxelMapInfo { - int num_buckets; - int max_bucket_scan_count; - float res; - float inv_res; -}; - -class VoxelMaps { -public: - VoxelMaps(); - ~VoxelMaps(); - - struct VectorHash { - size_t operator()(const Eigen::Vector3i& x) const { - size_t seed = 0; - boost::hash_combine(seed, x[0]); - boost::hash_combine(seed, x[1]); - boost::hash_combine(seed, x[2]); - return seed; - } - }; - - struct VctorEqual { - bool operator()(const Eigen::Vector3i& v1, const Eigen::Vector3i& v2) const { return v1 == v2; } - }; - - using UnorderedVoxelMap = std::unordered_map; - using Buckets = std::vector; - using DeviceBuckets = thrust::device_vector; - - void set_min_res(float min_level_res) { min_level_res_ = min_level_res; } - - void set_max_level(int max_level) { max_level_ = max_level; } - - void set_max_bucket_scan_count(int max_bucket_scan_count) { max_bucket_scan_count_ = max_bucket_scan_count; } - - float get_min_res() const { return min_level_res_; } - - int get_max_level() const { return max_level_; } - - int get_max_bucket_scan_count() const { return max_bucket_scan_count_; } - - void create_voxelmaps(const std::vector& points, const int v_rate, cudaStream_t stream); - - void set_buckets_on_device(const std::vector& multi_buckets, const int v_rate, cudaStream_t stream); - -private: - std::vector create_neighbor_coords(const Eigen::Vector3i& vec); - - Buckets create_hash_buckets(const UnorderedVoxelMap& unordered_voxelmap); - -public: - std::vector d_multi_buckets_; - thrust::device_vector d_multi_buckets_ptrs_; - - std::vector voxelmaps_info_; - thrust::device_vector d_voxelmaps_info_; - -private: - float min_level_res_; - int max_level_, max_bucket_scan_count_; -}; -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/include/pointcloud_iof/filter.hpp b/robot-relocalization/include/pointcloud_iof/filter.hpp deleted file mode 100644 index 9277337..0000000 --- a/robot-relocalization/include/pointcloud_iof/filter.hpp +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace pciof { -template -using Vector3 = Eigen::Matrix; - -// Declare T before using it in UnorderedVoxelMap -template -struct VectorHash { - size_t operator()(const Eigen::Vector3i& x) const { - size_t seed = 0; - boost::hash_combine(seed, x[0]); - boost::hash_combine(seed, x[1]); - boost::hash_combine(seed, x[2]); - return seed; - } -}; - -template -struct VctorEqual { - bool operator()(const Eigen::Vector3i& v1, const Eigen::Vector3i& v2) const { return v1 == v2; } -}; - -template -using UnorderedVoxelMap = std::unordered_map>, VectorHash, VctorEqual>; - -template -pciof::UnorderedVoxelMap create_voxel_map(const std::vector>& points3d, const T& voxel_width) { - pciof::UnorderedVoxelMap voxel_map; - const auto inv_voxel_width = 1 / voxel_width; - for (const auto& point : points3d) { - Eigen::Vector3i voxel = (point.array() * inv_voxel_width).template cast().array().floor(); - voxel_map[voxel].push_back(point); - } - return voxel_map; -} - -template -std::vector> filter(const std::vector>& points3d, const T& voxel_width) { - // if width == 0, return the original point cloud - if (voxel_width == 0) { - return points3d; - } - - // Create a map of voxels - const auto voxel_map = create_voxel_map(points3d, voxel_width); - - std::vector> filtered; - filtered.reserve(voxel_map.size()); - - // To avoid numerical errors, we subtract the first point from all the points - const Vector3 offset = points3d[0]; - - // Filter the point cloud - for (const auto& voxel : voxel_map) { - Vector3 average_point = Vector3::Zero(); - for (const auto& point : voxel.second) { - average_point += point - offset; - } - average_point /= static_cast(voxel.second.size()); - - average_point += offset; - filtered.push_back(average_point); - } - - return filtered; -} - -template -std::vector> narrow_scan_range(const std::vector>& points3d, const T& min_range, const T& max_range) { - if (min_range == max_range) { - return points3d; - } - - std::vector> filtered; - filtered.reserve(points3d.size()); - for (const auto& point : points3d) { - const auto range = point.norm(); - if (range < min_range || range > max_range) { - continue; - } - filtered.push_back(point); - } - return filtered; -} -} // namespace pciof diff --git a/robot-relocalization/include/pointcloud_iof/gravity_alignment.hpp b/robot-relocalization/include/pointcloud_iof/gravity_alignment.hpp deleted file mode 100644 index 75921d4..0000000 --- a/robot-relocalization/include/pointcloud_iof/gravity_alignment.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once -#include - -namespace pciof { -inline Eigen::Matrix4f calc_gravity_alignment_matrix(const Eigen::Vector3f& acc) { - Eigen::Vector3f th; - // 计算imu的俯仰角和横滚角 - th.x() = std::atan2(acc.y(), acc.z()); // roll - th.y() = std::atan2(-acc.x(), std::sqrt(acc.y() * acc.y() + acc.z() * acc.z())); // pitch - th.z() = 0.0f; // yaw - - // 构造一个旋转矩阵 - Eigen::Matrix3f rot; - rot = Eigen::AngleAxisf(th.x(), Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(th.y(), Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(th.z(), Eigen::Vector3f::UnitZ()); - Eigen::Matrix4f mat = Eigen::Matrix4f::Identity(); - mat.block<3, 3>(0, 0) = rot; - - return mat; -} -} // namespace pciof diff --git a/robot-relocalization/include/pointcloud_iof/pcd_io.hpp b/robot-relocalization/include/pointcloud_iof/pcd_io.hpp deleted file mode 100644 index 8423728..0000000 --- a/robot-relocalization/include/pointcloud_iof/pcd_io.hpp +++ /dev/null @@ -1,171 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace pciof { -template -using Vector3 = Eigen::Matrix; - -template -std::vector> read_pcd(const std::string& path) { - std::ifstream file(path, std::ios::binary); - if (!file.is_open()) { - std::cout << "Failed to open file: " << path << std::endl; - return {}; - } - - int points_size = 0; - Eigen::Vector3i xyz_cols = Eigen::Vector3i::Zero(); - - std::vector fields; - std::vector sizes; - std::vector types; - std::vector counts; - - // Skip the first 11 lines - for (int i = 0; i < 11; ++i) { - std::string line; - std::getline(file, line); - - size_t found = line.find("POINTS"); - if (found != std::string::npos) { - // Extract the number after "POINTS" - points_size = std::stoi(line.substr(found + 7)); - } - - size_t found1 = line.find("FIELDS"); - if (found1 != std::string::npos) { - std::istringstream iss(line); - std::string field; - while (iss >> field) { - if (field != "FIELDS" && field != "SIZE" && field != "TYPE" && field != "COUNT") { - fields.emplace_back(field); - } - } - - for (int j = 0; j < fields.size(); ++j) { - if (fields[j] == "x") - xyz_cols.x() = j; - else if (fields[j] == "y") - xyz_cols.y() = j; - else if (fields[j] == "z") - xyz_cols.z() = j; - } - } - - size_t found2 = line.find("SIZE"); - if (found2 != std::string::npos) { - std::istringstream iss(line); - std::string size; - while (iss >> size) { - if (size != "SIZE" && size != "TYPE" && size != "COUNT") { - sizes.emplace_back(std::stoi(size)); - } - } - } - - size_t found3 = line.find("TYPE"); - if (found3 != std::string::npos) { - std::istringstream iss(line); - std::string type; - while (iss >> type) { - if (type != "TYPE" && type != "COUNT") { - types.emplace_back(type); - } - } - } - - size_t found4 = line.find("COUNT"); - if (found4 != std::string::npos) { - std::istringstream iss(line); - std::string count; - while (iss >> count) { - if (count != "COUNT") { - counts.emplace_back(std::stoi(count)); - } - } - } - } - - std::vector> point_cloud; - point_cloud.reserve(points_size); - - // check if T and x y z sizez are the same - if (sizeof(T) != sizes[xyz_cols.x()] || sizeof(T) != sizes[xyz_cols.y()] || sizeof(T) != sizes[xyz_cols.z()]) { - std::cout << "T and x y z sizez are not the same" << std::endl; - return {}; - } - - size_t ignore_size_before_xyz = 0; - for (int i = 0; i < xyz_cols.x(); ++i) { - size_t count_temp = counts[i]; - ignore_size_before_xyz += sizes[i] * count_temp; - } - - size_t ignore_size_after_xyz = 0; - for (int i = xyz_cols.z() + 1; i < sizes.size(); ++i) { - size_t count_temp = counts[i]; - ignore_size_after_xyz += sizes[i] * count_temp; - } - - T x, y, z; - for (int i = 0; i < points_size; ++i) { - file.ignore(ignore_size_before_xyz); - file.read(reinterpret_cast(&x), sizeof(T)); - file.read(reinterpret_cast(&y), sizeof(T)); - file.read(reinterpret_cast(&z), sizeof(T)); - file.ignore(ignore_size_after_xyz); - - Vector3 point(x, y, z); - point_cloud.emplace_back(point); - } - file.close(); - - return point_cloud; -} - -template -bool save_pcd(const std::string& path, const std::vector>& point_cloud) { - std::ofstream file(path, std::ios::binary); - if (!file.is_open()) { - std::cout << "Failed to create file: " << path << std::endl; - return false; - } - - // Write PCD header - file << "# .PCD v.7 - Point Cloud Data file format\n"; - file << "VERSION .7\n"; - file << "FIELDS x y z\n"; - file << "SIZE " << sizeof(T) << " " << sizeof(T) << " " << sizeof(T) << "\n"; - - if (std::is_floating_point::value) { - file << "TYPE F F F\n"; - } else if (std::is_signed::value) { - file << "TYPE I I I\n"; - } else if (std::is_same::value) { - file << "TYPE D D D\n"; - } else { - file << "TYPE U U U\n"; - } - - file << "COUNT 1 1 1\n"; - file << "WIDTH " << point_cloud.size() << "\n"; - file << "HEIGHT 1\n"; - file << "VIEWPOINT 0 0 0 1 0 0 0\n"; - file << "POINTS " << point_cloud.size() << "\n"; - file << "DATA binary\n"; - - for (const auto& point : point_cloud) { - file.write(reinterpret_cast(&point[0]), sizeof(T)); - file.write(reinterpret_cast(&point[1]), sizeof(T)); - file.write(reinterpret_cast(&point[2]), sizeof(T)); - } - - file.close(); - - return true; -} - -} // namespace pciof diff --git a/robot-relocalization/include/pointcloud_iof/pcd_loader.hpp b/robot-relocalization/include/pointcloud_iof/pcd_loader.hpp deleted file mode 100644 index a8c437d..0000000 --- a/robot-relocalization/include/pointcloud_iof/pcd_loader.hpp +++ /dev/null @@ -1,133 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace pciof { -bool load_tar_clouds(const std::string& tar_path, const float tar_leaf_size, pcl::PointCloud::Ptr& tar_cloud_ptr) { - // Load pcd file - boost::filesystem::path dir(tar_path); - if (!boost::filesystem::exists(dir)) { - std::cout << "[ERROR] Can not open folder" << std::endl; - return false; - } - - pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); - for (const auto& file : boost::filesystem::directory_iterator(tar_path)) { - const std::string filename = file.path().c_str(); - const std::string extension = file.path().extension().string(); - if (extension != ".pcd" && extension != ".PCD") { - continue; - } - - // Check load pcd - pcl::PointCloud::Ptr cloud_temp_ptr(new pcl::PointCloud()); - if (pcl::io::loadPCDFile(filename, *cloud_temp_ptr) == -1) { - std::cout << "[WARN] Can not open pcd file: " << filename << std::endl; - continue; - } - *cloud_ptr += *cloud_temp_ptr; - } - - // Downsample - if (tar_leaf_size != 0.0f) { - pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); - pcl::ApproximateVoxelGrid filter; - filter.setLeafSize(tar_leaf_size, tar_leaf_size, tar_leaf_size); - filter.setInputCloud(cloud_ptr); - filter.filter(*filtered_cloud_ptr); - *tar_cloud_ptr = *filtered_cloud_ptr; - } else { - *tar_cloud_ptr = *cloud_ptr; - } - - return true; -} - -inline bool can_convert_to_int(const std::vector>& name_vec) { - for (const auto& str : name_vec) { - try { - std::stoi(str.second); - } catch (const std::invalid_argument& e) { - return false; - } catch (const std::out_of_range& e) { - return false; - } - } - return true; -} - -bool load_src_points_with_filename( - const std::string& src_path, - const double min_scan_range, - const double max_scan_range, - const float src_leaf_size, - std::vector::Ptr>>& cloud_set) { - boost::filesystem::path dir(src_path); - if (!boost::filesystem::exists(dir)) { - std::cout << "[ERROR] Can not open folder" << std::endl; - return false; - } - - std::vector> pcd_files; - for (const auto& file : boost::filesystem::directory_iterator(src_path)) { - const std::string extension = file.path().extension().string(); - if (extension != ".pcd" && extension != ".PCD") { - continue; - } - pcd_files.emplace_back(file.path().string(), file.path().stem().string()); - } - - if (can_convert_to_int(pcd_files)) { - std::sort(pcd_files.begin(), pcd_files.end(), [](const std::pair& a, const std::pair& b) { - return std::stoi(a.second) < std::stoi(b.second); - }); - } - - cloud_set.reserve(pcd_files.size()); - - for (const auto& file : pcd_files) { - // check load pcd - pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); - if (pcl::io::loadPCDFile(file.first, *cloud_ptr) == -1) { - std::cout << "[WARN] Can not open pcd file: " << file.first << std::endl; - continue; - } - - // Cut scan range - pcl::PointCloud::Ptr src_cloud_ptr(new pcl::PointCloud); - if (!(min_scan_range == 0.0 && max_scan_range == 0.0)) { - pcl::PointCloud::Ptr cropped_cloud_ptr(new pcl::PointCloud); - for (size_t i = 0; i < cloud_ptr->points.size(); ++i) { - pcl::PointXYZ point = cloud_ptr->points[i]; - double norm = pcl::euclideanDistance(point, pcl::PointXYZ(0, 0, 0)); - - if (norm >= min_scan_range && norm <= max_scan_range) { - cropped_cloud_ptr->points.push_back(point); - } - } - *src_cloud_ptr = *cropped_cloud_ptr; - } else { - *src_cloud_ptr = *cloud_ptr; - } - - // Downsample - if (src_leaf_size != 0.0f) { - pcl::VoxelGrid filter; - filter.setLeafSize(src_leaf_size, src_leaf_size, src_leaf_size); - filter.setInputCloud(src_cloud_ptr); - filter.filter(*src_cloud_ptr); - } - - cloud_set.emplace_back(file.second, src_cloud_ptr); - } - return true; -} -} // namespace pciof diff --git a/robot-relocalization/include/pointcloud_iof/pcd_loader_without_pcl.hpp b/robot-relocalization/include/pointcloud_iof/pcd_loader_without_pcl.hpp deleted file mode 100644 index 13ae33e..0000000 --- a/robot-relocalization/include/pointcloud_iof/pcd_loader_without_pcl.hpp +++ /dev/null @@ -1,134 +0,0 @@ -#pragma once -#include -#include -#include - -namespace pciof { -template -using Vector3 = Eigen::Matrix; - -template -bool load_tar_points(const std::string& tar_folder_path, const T& filter_voxel_width, std::vector>& points) { - boost::filesystem::path dir(tar_folder_path); - if (!boost::filesystem::exists(dir)) { - std::cout << "[ERROR] Can not open folder" << std::endl; - return false; - } - - for (const auto& file : boost::filesystem::directory_iterator(tar_folder_path)) { - const std::string file_name = file.path().c_str(); - const std::string extension = file.path().extension().string(); - if (extension != ".pcd" && extension != ".PCD") { - continue; - } - - const auto loaded_points = read_pcd(file_name); - - if (loaded_points.empty()) { - std::cout << "[ERROR] Failed to read point cloud from " << file_name << std::endl; - return false; - } - - const auto filtered_points = filter(loaded_points, filter_voxel_width); - - points.reserve(points.size() + filtered_points.size()); - points.insert(points.end(), filtered_points.begin(), filtered_points.end()); - } - return true; -} - -inline bool can_convert_to_int(const std::vector>& name_vec) { - for (const auto& str : name_vec) { - try { - std::stoi(str.second); - } catch (const std::invalid_argument& e) { - return false; - } catch (const std::out_of_range& e) { - return false; - } - } - return true; -} - -std::vector> load_pcd_file_paths(const std::string& folder_path) { - boost::filesystem::path dir(folder_path); - if (!boost::filesystem::exists(dir)) { - return {}; // output error - } - - std::vector> pcd_files; - for (const auto& file : boost::filesystem::directory_iterator(folder_path)) { - const std::string extension = file.path().extension().string(); - if (extension != ".pcd" && extension != ".PCD") { - continue; - } - pcd_files.emplace_back(file.path().string(), file.path().stem().string()); - } - - if (can_convert_to_int(pcd_files)) { - std::sort(pcd_files.begin(), pcd_files.end(), [](const std::pair& a, const std::pair& b) { - return std::stoi(a.second) < std::stoi(b.second); - }); - } - - return pcd_files; -} - -template -bool load_src_points(std::string src_folder_path, T min_range, T max_range, T voxel_width, std::vector>>& src_points_set) { - const auto pcd_files = load_pcd_file_paths(src_folder_path); - if (pcd_files.empty()) { - std::cout << "[ERROR] No pcd files in the folder" << std::endl; - return false; - } - - src_points_set.resize(pcd_files.size()); - - for (int i = 0; i < pcd_files.size(); i++) { - const auto& pcd_file = pcd_files[i]; - const auto loaded_points = read_pcd(pcd_file.first); - if (loaded_points.empty()) { - std::cout << "[ERROR] Failed to read point cloud from " << pcd_file.first << std::endl; - return false; - } - - const auto narrow_scan_range_points = narrow_scan_range(loaded_points, min_range, max_range); - const auto filtered_points = filter(narrow_scan_range_points, voxel_width); - - src_points_set[i].reserve(filtered_points.size()); - src_points_set[i].insert(src_points_set[i].end(), filtered_points.begin(), filtered_points.end()); - } -} - -template -bool load_src_points_with_filename( - std::string src_folder_path, - T min_range, - T max_range, - T voxel_width, - std::pair>>& src_points_set) { - const auto pcd_files = load_pcd_file_paths(src_folder_path); - if (pcd_files.empty()) { - std::cout << "[ERROR] No pcd files in the folder" << std::endl; - return false; - } - - src_points_set.resize(pcd_files.size()); - - for (int i = 0; i < pcd_files.size(); i++) { - const auto& pcd_file = pcd_files[i]; - const auto loaded_points = read_pcd(pcd_file.first); - if (loaded_points.empty()) { - std::cout << "[ERROR] Failed to read point cloud from " << pcd_file.first << std::endl; - return false; - } - - const auto narrow_scan_range_points = narrow_scan_range(loaded_points, min_range, max_range); - const auto filtered_points = filter(narrow_scan_range_points, voxel_width); - - src_points_set[i].first = pcd_file.second; - src_points_set[i].second.reserve(filtered_points.size()); - src_points_set[i].second.insert(src_points_set[i].second.end(), filtered_points.begin(), filtered_points.end()); - } -} -} // namespace pciof \ No newline at end of file diff --git a/robot-relocalization/include/pointcloud_iof/pcl_eigen_converter.hpp b/robot-relocalization/include/pointcloud_iof/pcl_eigen_converter.hpp deleted file mode 100644 index d1e5288..0000000 --- a/robot-relocalization/include/pointcloud_iof/pcl_eigen_converter.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace pciof { -template -void pcl_to_eigen(const pcl::PointCloud::Ptr& cloud_ptr, std::vector& points) { - // nullptr check - if (!cloud_ptr) { - return; - } - - points.resize(cloud_ptr->size()); - std::transform(cloud_ptr->begin(), cloud_ptr->end(), points.begin(), [](const pcl::PointXYZ& p) { return T(p.x, p.y, p.z); }); -} - -inline void eigen_to_pcl(const std::vector& points, pcl::PointCloud::Ptr& cloud_ptr) { - cloud_ptr->points.resize(points.size()); - cloud_ptr->width = points.size(); - cloud_ptr->height = 1; - cloud_ptr->is_dense = true; - - std::transform(points.begin(), points.end(), cloud_ptr->points.begin(), [](const Eigen::Vector3d& v) { - pcl::PointXYZ point; - Eigen::Vector3f vf = v.cast(); - point.x = vf.x(); - point.y = vf.y(); - point.z = vf.z(); - return point; - }); -} - -inline void eigen_to_pcl(const std::vector& points, pcl::PointCloud::Ptr& cloud_ptr) { - cloud_ptr->points.resize(points.size()); - cloud_ptr->width = points.size(); - cloud_ptr->height = 1; - cloud_ptr->is_dense = true; - - std::transform(points.begin(), points.end(), cloud_ptr->points.begin(), [](const Eigen::Vector3f& v) { - pcl::PointXYZ point; - point.x = v.x(); - point.y = v.y(); - point.z = v.z(); - return point; - }); -} -} // namespace pciof diff --git a/robot-relocalization/launch/bbs_based_initializer.launch.xml b/robot-relocalization/launch/bbs_based_initializer.launch.xml deleted file mode 100644 index fa65d23..0000000 --- a/robot-relocalization/launch/bbs_based_initializer.launch.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/robot-relocalization/package.xml b/robot-relocalization/package.xml deleted file mode 100644 index 54ba215..0000000 --- a/robot-relocalization/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - bbs_based_initializer - 1.0.0 - location initializer using 3D-BBS - liuli - Apache License 2.0 - - ament_cmake_auto - rclcpp - sensor_msgs - std_msgs - geometry_msgs - tf2_ros - pcl_conversions - std_srvs - ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/robot-relocalization/src/bbs_based_initializer/bbs_based_initializer.cpp b/robot-relocalization/src/bbs_based_initializer/bbs_based_initializer.cpp deleted file mode 100644 index 57af37d..0000000 --- a/robot-relocalization/src/bbs_based_initializer/bbs_based_initializer.cpp +++ /dev/null @@ -1,395 +0,0 @@ -#include - -#include -#include -#include -#include -// #include -#include -// pcl -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -// Define BBS3D instance based on compilation mode -#ifdef USE_CUDA -#define BBS3D_INSTANCE gpu_bbs3d -using BBS3D_TYPE = float; -using BBS3D_VECTOR3 = Eigen::Vector3f; -using BBS3D_MATRIX4 = Eigen::Matrix4f; -#else -#define BBS3D_INSTANCE cpu_bbs3d -using BBS3D_TYPE = double; -using BBS3D_VECTOR3 = Eigen::Vector3d; -using BBS3D_MATRIX4 = Eigen::Matrix4d; -#endif - -// Type conversion helper functions -template -T convert_eigen_vector(const Eigen::Vector3d& vec) { - if constexpr (std::is_same_v) { - return vec.template cast(); - } else { - return vec; - } -} - -template -Eigen::Vector3d convert_to_double_vector(const T& vec) { - if constexpr (std::is_same_v) { - return vec.template cast(); - } else { - return vec; - } -} - -Eigen::Vector3d to_eigen(const std::vector &vec) { - Eigen::Vector3d e_vec; - for (int i = 0; i < 3; ++i) { - if (vec[i] == 6.28) { - e_vec(i) = 2 * M_PI; - } else { - e_vec(i) = vec[i]; - } - } - return e_vec; -} - -BbsBasedInitializer::BbsBasedInitializer(const rclcpp::NodeOptions &node_options) : Node( - "bbs_based_initializer", node_options) { - // ==== Load config file ==== - std::cout << "[ROS2] Loading config file..." << std::endl; - - // Display compilation mode -#ifdef USE_CUDA - std::cout << "[INFO] Running in GPU mode with CUDA acceleration" << std::endl; -#else - std::cout << "[INFO] Running in CPU mode" << std::endl; -#endif - - // 加载函数的配置文件 - std::string config = this->declare_parameter("config"); - if (!load_config(config)) { - std::cout << "[ERROR] Loading config file failed" << std::endl; - }; - - // ==== ROS 2 ser ==== - // 创建了一个 trigger_service bool 类型的服务 - trigger_ser_ = this->create_service("trigger_service", - std::bind(&BbsBasedInitializer::trigger_callback, - this, std::placeholders::_1, - std::placeholders::_2)); - // ==== ROS 2 sub ==== - cloud_sub_ = this->create_subscription( - lidar_topic_name, - 100, - std::bind(&BbsBasedInitializer::cloud_callback, this, std::placeholders::_1)); - - map_sub_ = this->create_subscription( - map_topic_name, - 100, - std::bind(&BbsBasedInitializer::map_callback, this, std::placeholders::_1)); - - imu_sub_ = this->create_subscription(imu_topic_name, 100, - std::bind(&BbsBasedInitializer::imu_callback, this, - std::placeholders::_1)); - - pose_pub_ = this->create_publisher(pose_topic_name, 10); - - - // initialize map load status - is_map_loaded = false; -} - -BbsBasedInitializer::~BbsBasedInitializer() { -} - -void BbsBasedInitializer::trigger_callback(const std::shared_ptr request, - const std::shared_ptr response) { - std::cout << "[Trigger] Service called with data: " << (request->data ? "true" : "false") << std::endl; - - // 检查是否存在 map - if (!is_map_loaded) { - std::cout << "[Trigger] ERROR: Map not loaded yet!" << std::endl; - response->success = false; - response->message = "Map not loaded"; - return; - } - - if (!request->data) { - std::cout << "[Trigger] Service called with data=false, ignoring" << std::endl; - response->success = false; - response->message = "Service called with data=false"; - return; - } - - if (!source_cloud_msg_) { - std::cout << "[Trigger] ERROR: Point cloud message not received!" << std::endl; - std::cout << "[Trigger] Please check if LiDAR topic '" << lidar_topic_name << "' is publishing" << std::endl; - response->success = false; - response->message = "Point cloud message not received"; - return; - } - - if (imu_buffer.empty()) { - std::cout << "[Trigger] ERROR: IMU message not received!" << std::endl; - std::cout << "[Trigger] Please check if IMU topic '" << imu_topic_name << "' is publishing" << std::endl; - response->success = false; - response->message = "IMU message not received"; - return; - } - - std::cout << "[Trigger] All prerequisites met, starting localization..." << std::endl; - std::cout << "[Trigger] Point cloud size: " << source_cloud_msg_->width * source_cloud_msg_->height << " points" << std::endl; - std::cout << "[Trigger] IMU buffer size: " << imu_buffer.size() << " messages" << std::endl; - // 点云数据预处理 - pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*source_cloud_msg_, *src_cloud); - - // filter - // 降采样 - if (src_leaf_size != 0.0f) { - pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); - pcl::VoxelGrid filter; - filter.setLeafSize(src_leaf_size, src_leaf_size, src_leaf_size); - filter.setInputCloud(src_cloud); - filter.filter(*filtered_cloud_ptr); - *src_cloud = *filtered_cloud_ptr; - } - - // Cut scan range - // 限制扫描范围 过滤掉里传感器很近或者很远的点 也就是只用适中范围的点做匹配 - if (!(min_scan_range == 0.0 && max_scan_range == 0.0)) { - pcl::PointCloud::Ptr cut_cloud_ptr(new pcl::PointCloud); - for (size_t i = 0; i < src_cloud->points.size(); ++i) { - pcl::PointXYZ point = src_cloud->points[i]; - double norm = pcl::euclideanDistance(point, pcl::PointXYZ(0.0f, 0.0f, 0.0f)); - - if (norm >= min_scan_range && norm <= max_scan_range) { - cut_cloud_ptr->points.push_back(point); - } - } - *src_cloud = *cut_cloud_ptr; - } - - // 根据imu的加速度 对点云做旋转 使其在地图坐标系下正方向一致 - int imu_index = get_nearest_imu_index(imu_buffer, source_cloud_msg_->header.stamp); - const auto imu_msg = imu_buffer[imu_index]; - const Eigen::Vector3d acc = { - imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z - }; - pcl::transformPointCloud(*src_cloud, *src_cloud, pciof::calc_gravity_alignment_matrix(acc.cast())); - - std::vector src_points; - pciof::pcl_to_eigen(src_cloud, src_points); - BBS3D_INSTANCE.set_src_points(src_points); - - std::cout << "[Localize] start" << std::endl; -#ifdef USE_CUDA - std::cout << "[Localize] Using GPU acceleration for localization" << std::endl; -#else - std::cout << "[Localize] Using CPU for localization" << std::endl; -#endif - BBS3D_INSTANCE.localize(); // gloal localization - - if (!BBS3D_INSTANCE.has_localized()) { - if (BBS3D_INSTANCE.has_timed_out()) { - std::cout << "[Failed] Localization timed out." << std::endl; - response->success = false; - response->message = "Localization timed out"; - } else { - std::cout << "[Failed] Score is below the threshold." << std::endl; - std::cout << "[Failed] Best score: " << BBS3D_INSTANCE.get_best_score() << std::endl; - std::cout << "[Failed] Score percentage: " << BBS3D_INSTANCE.get_best_score_percentage() << std::endl; - response->success = false; - response->message = "Score below threshold - best score: " + std::to_string(BBS3D_INSTANCE.get_best_score()); - } - return; - } - - std::cout << "[Localize] Execution time: " << BBS3D_INSTANCE.get_elapsed_time() << "[msec] " << std::endl; - std::cout << "[Localize] score: " << BBS3D_INSTANCE.get_best_score() << std::endl; - - auto estimated_pose = BBS3D_INSTANCE.get_global_pose(); - - // generate ros message - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose_to_pub( - new geometry_msgs::msg::PoseWithCovarianceStamped); - // PoseWithCovarianceStamped - // - header - // - pose (PoseWithCovariance) - // - pose (Pose) - // - position - // - orientation - pose_to_pub->header.frame_id = "map"; - pose_to_pub->header.stamp = source_cloud_msg_->header.stamp; // timestamp of lidar topic - pose_to_pub->pose.pose.position.x = static_cast(estimated_pose(0, 3)); - pose_to_pub->pose.pose.position.y = static_cast(estimated_pose(1, 3)); - pose_to_pub->pose.pose.position.z = static_cast(estimated_pose(2, 3)); - - // Convert rotation matrix to quaternion (handle both float and double types) - Eigen::Matrix3f rotation_matrix_f = estimated_pose.block<3, 3>(0, 0).template cast(); - Eigen::Quaternionf q(rotation_matrix_f); - pose_to_pub->pose.pose.orientation.x = q.x(); - pose_to_pub->pose.pose.orientation.y = q.y(); - pose_to_pub->pose.pose.orientation.z = q.z(); - pose_to_pub->pose.pose.orientation.w = q.w(); - // FIXME: whether to initialize covariance? - pose_pub_->publish(*pose_to_pub); - - response->success = true; - response->message = "Service processed successfully"; -} - - -// get imu message which has closest timestamp with given timestamp -int BbsBasedInitializer::get_nearest_imu_index(const std::vector &imu_buffer, - const builtin_interfaces::msg::Time &stamp) { - int imu_index = 0; - double min_diff = 1000; - for (int i = 0; i < imu_buffer.size(); ++i) { - double diff = std::abs( - imu_buffer[i].header.stamp.sec + imu_buffer[i].header.stamp.nanosec * 1e-9 - source_cloud_msg_->header.stamp - .sec - - source_cloud_msg_->header.stamp.nanosec * 1e-9); - if (diff < min_diff) { - imu_index = i; - min_diff = diff; - } - } - return imu_index; -} - -void BbsBasedInitializer::cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - if (!msg) { - std::cout << "[LiDAR] Received null point cloud message" << std::endl; - return; - } - source_cloud_msg_ = msg; - std::cout << "[LiDAR] Received point cloud with " << msg->width * msg->height << " points" << std::endl; -} - -void BbsBasedInitializer::map_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - std::cout << "[Map] Map callback triggered" << std::endl; - - if (is_map_loaded) { - std::cout << "[Map] Map already loaded, skipping..." << std::endl; - return; - } - - if (!msg) { - std::cout << "[Map] ERROR: Received null map message!" << std::endl; - return; - } - - std::cout << "[Map] Received map with " << msg->width * msg->height << " points" << std::endl; - - // convert ROS message - pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); - pcl::fromROSMsg(*msg, *cloud_ptr); - // do downsample when leaf size is provided - if (map_leaf_size != 0.0f) { - pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); - pcl::ApproximateVoxelGrid filter; - filter.setLeafSize(map_leaf_size, map_leaf_size, map_leaf_size); - filter.setInputCloud(cloud_ptr); - filter.filter(*filtered_cloud_ptr); - *cloud_ptr = *filtered_cloud_ptr; - } - - // pcl to eigen - std::vector map_points; - pciof::pcl_to_eigen(cloud_ptr, map_points); - - std::cout << "[BBS3D:init] Creating hierarchical voxel map..." << std::endl; -#ifdef USE_CUDA - std::cout << "[BBS3D:init] Using GPU acceleration for voxel map creation" << std::endl; -#else - std::cout << "[BBS3D:init] Using CPU for voxel map creation" << std::endl; -#endif - BBS3D_INSTANCE.set_tar_points(map_points, static_cast(min_level_res), max_level); - BBS3D_INSTANCE.set_trans_search_range(map_points); - - BBS3D_INSTANCE.set_angular_search_range(convert_eigen_vector(min_rpy), convert_eigen_vector(max_rpy)); - BBS3D_INSTANCE.set_score_threshold_percentage(static_cast(score_threshold_percentage)); - if (timeout_msec > 0) { - BBS3D_INSTANCE.enable_timeout(); - BBS3D_INSTANCE.set_timeout_duration_in_msec(timeout_msec); - } - - std::cout << "[BBS3D:init] READY!" << std::endl; - - is_map_loaded = true; -} - -void BbsBasedInitializer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { - if (!msg) { - std::cout << "[IMU] Received null IMU message" << std::endl; - return; - } - imu_buffer.emplace_back(*msg); - if (imu_buffer.size() > 30) { - imu_buffer.erase(imu_buffer.begin()); - } - - // Only print IMU info occasionally to avoid spam - static int imu_count = 0; - if (++imu_count % 50 == 0) { // Print every 50th message - std::cout << "[IMU] Received IMU data, buffer size: " << imu_buffer.size() << std::endl; - } -} - -bool BbsBasedInitializer::load_config(const std::string &config) { - YAML::Node conf = YAML::LoadFile(config); - - std::cout << "[YAML] Loading topic name..." << std::endl; - lidar_topic_name = conf["lidar_topic_name"].as(); - map_topic_name = conf["map_topic_name"].as(); - imu_topic_name = conf["imu_topic_name"].as(); - std::cout << "[YAML] LiDAR topic name..." << lidar_topic_name << std::endl; - std::cout << "[YAML] imu topic name..." << imu_topic_name << std::endl; - - trigger_topic_name = conf["trigger_topic_name"].as(); - // for pub - pose_topic_name = conf["pose_topic_name"].as(); - - std::cout << "[YAML] Loading 3D-BBS parameters..." << std::endl; - min_level_res = conf["min_level_res"].as(); - max_level = conf["max_level"].as(); - - if (min_level_res == 0.0 || max_level == 0) { - std::cout << "[ERROR] Set min_level and num_layers except for 0" << std::endl; - return false; - } - - std::cout << "[YAML] Loading angular search range..." << std::endl; - std::vector min_rpy_temp = conf["min_rpy"].as >(); - std::vector max_rpy_temp = conf["max_rpy"].as >(); - if (min_rpy_temp.size() == 3 && max_rpy_temp.size() == 3) { - min_rpy = to_eigen(min_rpy_temp); - max_rpy = to_eigen(max_rpy_temp); - } else { - std::cout << "[ERROR] Set min_rpy and max_rpy correctly" << std::endl; - return false; - } - - std::cout << "[YAML] Loading score threshold percentage..." << std::endl; - score_threshold_percentage = conf["score_threshold_percentage"].as(); - - std::cout << "[YAML] Loading downsample parameters..." << std::endl; - map_leaf_size = conf["map_leaf_size"].as(); - src_leaf_size = conf["src_leaf_size"].as(); - min_scan_range = conf["min_scan_range"].as(); - max_scan_range = conf["max_scan_range"].as(); - - timeout_msec = conf["timeout_msec"].as(); - - return true; -} diff --git a/robot-relocalization/src/bbs_based_initializer/bbs_based_initializer_node.cpp b/robot-relocalization/src/bbs_based_initializer/bbs_based_initializer_node.cpp deleted file mode 100644 index e59f09b..0000000 --- a/robot-relocalization/src/bbs_based_initializer/bbs_based_initializer_node.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/robot-relocalization/src/cpu_bbs3d/bbs3d.cpp b/robot-relocalization/src/cpu_bbs3d/bbs3d.cpp deleted file mode 100644 index 98a5187..0000000 --- a/robot-relocalization/src/cpu_bbs3d/bbs3d.cpp +++ /dev/null @@ -1,310 +0,0 @@ -#include -#include -#include -#include - -namespace cpu { -BBS3D::BBS3D() -: v_rate_(2.0), - num_threads_(std::max(1, static_cast(std::thread::hardware_concurrency()))), // Auto-detect CPU cores - score_threshold_percentage_(0.0), - use_timeout_(false), - timeout_duration_(10000), - has_timed_out_(false), - has_localized_(false), - voxelmaps_folder_name_("voxelmaps_coords") { - inv_v_rate_ = 1.0 / v_rate_; - min_rpy_ << -0.02, -0.02, 0.0; - max_rpy_ << 0.02, 0.02, 2 * M_PI; - - // Print thread info for debugging - std::cout << "[CPU BBS3D] Using " << num_threads_ << " threads for parallel computation" << std::endl; -} - -BBS3D::~BBS3D() {} - -void BBS3D::set_timeout_duration_in_msec(const int msec) { - timeout_duration_ = std::chrono::milliseconds(msec); -} - -void BBS3D::set_tar_points(const std::vector& points, double min_level_res, int max_level) { - voxelmaps_ptr_.reset(new VoxelMaps); - voxelmaps_ptr_->set_min_res(min_level_res); - voxelmaps_ptr_->set_max_level(max_level); - voxelmaps_ptr_->create_voxelmaps(points, v_rate_); -} - - // 新增：处理 float 类型点云的实现 - void BBS3D::set_tar_points(const std::vector& points, double min_level_res, int max_level) { - // 将 float 点云转换为 double 类型 - std::vector points_double; - points_double.reserve(points.size()); - - std::transform( - points.begin(), points.end(), - std::back_inserter(points_double), - [](const Eigen::Vector3f& p) { return p.cast(); } - ); - - // 调用原始函数处理转换后的点云 - set_tar_points(points_double, min_level_res, max_level); -} - -void BBS3D::set_src_points(const std::vector& points) { - src_points_.clear(); - src_points_.shrink_to_fit(); - src_points_.resize(points.size()); - std::copy(points.begin(), points.end(), src_points_.begin()); -} - - // 新增：处理 float 类型点云的实现 - void BBS3D::set_src_points(const std::vector& points) { - src_points_.clear(); - src_points_.shrink_to_fit(); - src_points_.resize(points.size()); - - // 在复制过程中自动将 float 转换为 double - std::transform( - points.begin(), points.end(), - src_points_.begin(), - [](const Eigen::Vector3f& p) { return p.cast(); } - ); -} - -void BBS3D::set_trans_search_range(const std::vector& points) { - // The minimum and maximum x, y, z values are selected from the 3D coordinate vector. - Eigen::Vector3d min_xyz = Eigen::Vector3d::Constant(std::numeric_limits::max()); - Eigen::Vector3d max_xyz = Eigen::Vector3d::Constant(std::numeric_limits::lowest()); - - for (const auto& point : points) { - min_xyz = min_xyz.cwiseMin(point); - max_xyz = max_xyz.cwiseMax(point); - } - - set_trans_search_range(min_xyz, max_xyz); -} - -void BBS3D::set_trans_search_range(const Eigen::Vector3d& min_xyz, const Eigen::Vector3d& max_xyz) { - min_xyz_ = min_xyz; - max_xyz_ = max_xyz; - - const int max_level = voxelmaps_ptr_->get_max_level(); - const double top_res = voxelmaps_ptr_->voxelmaps_res_[max_level]; - init_tx_range_ = std::make_pair(std::floor(min_xyz.x() / top_res), std::ceil(max_xyz.x() / top_res)); - init_ty_range_ = std::make_pair(std::floor(min_xyz.y() / top_res), std::ceil(max_xyz.y() / top_res)); - init_tz_range_ = std::make_pair(std::floor(min_xyz.z() / top_res), std::ceil(max_xyz.z() / top_res)); -} - - // 新增：处理 float 类型点云的实现 - void BBS3D::set_trans_search_range(const std::vector& points) { - // 转换为 double 类型 - std::vector points_double; - points_double.reserve(points.size()); - - std::transform( - points.begin(), points.end(), - std::back_inserter(points_double), - [](const Eigen::Vector3f& p) { return p.cast(); } - ); - - // 调用原始函数处理转换后的点云 - set_trans_search_range(points_double); -} - -void BBS3D::calc_angular_info(std::vector& ang_info_vec) { - double max_norm = src_points_[0].norm(); - for (const auto& point : src_points_) { - double norm = point.norm(); - if (norm > max_norm) { - max_norm = norm; - } - } - - const int max_level = voxelmaps_ptr_->get_max_level(); - for (int i = max_level; i >= 0; i--) { - const double cosine = 1 - (std::pow(voxelmaps_ptr_->voxelmaps_res_[i], 2) / std::pow(max_norm, 2)) * 0.5; - double ori_res = std::acos(std::max(cosine, -1.0)); - ori_res = std::floor(ori_res * 10000) / 10000; - Eigen::Vector3d rpy_res_temp; - rpy_res_temp.x() = ori_res <= std::abs(max_rpy_.x() - min_rpy_.x()) ? ori_res : 0.0; - rpy_res_temp.y() = ori_res <= std::abs(max_rpy_.y() - min_rpy_.y()) ? ori_res : 0.0; - rpy_res_temp.z() = ori_res <= std::abs(max_rpy_.z() - min_rpy_.z()) ? ori_res : 0.0; - - Eigen::Vector3d max_rpy_piece; - if (i == max_level) { - max_rpy_piece = max_rpy_ - min_rpy_; - } else { - max_rpy_piece.x() = ang_info_vec[i + 1].rpy_res.x() != 0.0 ? ang_info_vec[i + 1].rpy_res.x() : max_rpy_.x() - min_rpy_.x(); - max_rpy_piece.y() = ang_info_vec[i + 1].rpy_res.y() != 0.0 ? ang_info_vec[i + 1].rpy_res.y() : max_rpy_.y() - min_rpy_.y(); - max_rpy_piece.z() = ang_info_vec[i + 1].rpy_res.z() != 0.0 ? ang_info_vec[i + 1].rpy_res.z() : max_rpy_.z() - min_rpy_.z(); - } - - // Angle division number - Eigen::Vector3i num_division; - num_division.x() = rpy_res_temp.x() == 0.0 ? 1 : std::ceil(max_rpy_piece.x() / rpy_res_temp.x()); - num_division.y() = rpy_res_temp.y() == 0.0 ? 1 : std::ceil(max_rpy_piece.y() / rpy_res_temp.y()); - num_division.z() = rpy_res_temp.z() == 0.0 ? 1 : std::ceil(max_rpy_piece.z() / rpy_res_temp.z()); - ang_info_vec[i].num_division = num_division; - - // Bisect an angle - ang_info_vec[i].rpy_res.x() = num_division.x() == 1 ? 0.0 : max_rpy_piece.x() / num_division.x(); - ang_info_vec[i].rpy_res.y() = num_division.y() == 1 ? 0.0 : max_rpy_piece.y() / num_division.y(); - ang_info_vec[i].rpy_res.z() = num_division.z() == 1 ? 0.0 : max_rpy_piece.z() / num_division.z(); - - ang_info_vec[i].min_rpy.x() = ang_info_vec[i].rpy_res.x() == 0.0 ? 0.0 : min_rpy_.x(); - ang_info_vec[i].min_rpy.y() = ang_info_vec[i].rpy_res.y() == 0.0 ? 0.0 : min_rpy_.y(); - ang_info_vec[i].min_rpy.z() = ang_info_vec[i].rpy_res.z() == 0.0 ? 0.0 : min_rpy_.z(); - } -} - -std::vector> BBS3D::create_init_transset(const AngularInfo& init_ang_info) { - const int init_transset_size = (init_tx_range_.second - init_tx_range_.first + 1) * (init_ty_range_.second - init_ty_range_.first + 1) * - (init_tz_range_.second - init_tz_range_.first + 1) * (init_ang_info.num_division.x()) * - (init_ang_info.num_division.y()) * (init_ang_info.num_division.z()); - - const int max_level = voxelmaps_ptr_->get_max_level(); - - std::vector> transset; - transset.reserve(init_transset_size); - for (int tx = init_tx_range_.first; tx <= init_tx_range_.second; tx++) { - for (int ty = init_ty_range_.first; ty <= init_ty_range_.second; ty++) { - for (int tz = init_tz_range_.first; tz <= init_tz_range_.second; tz++) { - for (int roll = 0; roll < init_ang_info.num_division.x(); roll++) { - for (int pitch = 0; pitch < init_ang_info.num_division.y(); pitch++) { - for (int yaw = 0; yaw < init_ang_info.num_division.z(); yaw++) { - transset.emplace_back(DiscreteTransformation(0, max_level, tx, ty, tz, roll, pitch, yaw)); - } - } - } - } - } - } - return transset; -} - -void BBS3D::calc_score( - DiscreteTransformation& trans, - const double trans_res, - const Eigen::Vector3d& rpy_res, - const Eigen::Vector3d& min_rpy, - const std::vector& buckets, - const int max_bucket_scan_count, - const std::vector& points) { - const int num_buckets = buckets.size(); - const double inv_res = 1.0 / trans_res; - Eigen::Transform transform; - transform = trans.create_matrix(trans_res, rpy_res, min_rpy); - - for (int i = 0; i < points.size(); i++) { - const Eigen::Vector3d transed_point = transform * points[i]; - const Eigen::Vector3i coord = (transed_point.array() * inv_res).floor().cast(); - const std::uint32_t hash = (coord[0] * 73856093) ^ (coord[1] * 19349669) ^ (coord[2] * 83492791); - - for (int j = 0; j < max_bucket_scan_count; j++) { - const std::uint32_t bucket_index = (hash + j) % num_buckets; - const Eigen::Vector4i& bucket = buckets[bucket_index]; - - if (bucket.x() != coord.x() || bucket.y() != coord.y() || bucket.z() != coord.z()) { - continue; - } - - if (bucket.w() == 1) { - trans.score++; - break; - } - } - } -} - -void BBS3D::localize() { - // Calc localize time limit - const auto start_time = std::chrono::system_clock::now(); - // 设置超时的阈值 - const auto time_limit = start_time + timeout_duration_; - has_timed_out_ = false; - - best_score_ = 0; - const int score_threshold = std::floor(src_points_.size() * score_threshold_percentage_); - int best_score = score_threshold; - DiscreteTransformation best_trans(best_score); - - // Preapre initial transset - const int max_bucket_scan_count = voxelmaps_ptr_->get_max_bucket_scan_count(); - const int max_level = voxelmaps_ptr_->get_max_level(); - std::vector ang_info_vec(max_level + 1); - calc_angular_info(ang_info_vec); - auto init_transset = create_init_transset(ang_info_vec[max_level]); - - // Calc initial transset scores - const auto& top_buckets = voxelmaps_ptr_->multi_buckets_[max_level]; - double init_trans_res = voxelmaps_ptr_->voxelmaps_res_[max_level]; - const Eigen::Vector3d& rpy_res = ang_info_vec[max_level].rpy_res; - const Eigen::Vector3d& min_rpy = ang_info_vec[max_level].min_rpy; -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < init_transset.size(); i++) { - calc_score(init_transset[i], init_trans_res, rpy_res, min_rpy, top_buckets, max_bucket_scan_count, src_points_); - } - - std::priority_queue> trans_queue(init_transset.begin(), init_transset.end()); - - while (!trans_queue.empty()) { - if (use_timeout_ && std::chrono::system_clock::now() > time_limit) { - has_timed_out_ = true; - break; - } - - auto trans = trans_queue.top(); - trans_queue.pop(); - - // pruning - if (trans.score < best_score) { - continue; - } - - if (trans.is_leaf()) { - best_trans = trans; - best_score = trans.score; - } else { - const int child_level = trans.level - 1; - const Eigen::Vector3i& num_division = ang_info_vec[child_level].num_division; - const Eigen::Vector3d& rpy_res = ang_info_vec[child_level].rpy_res; - const Eigen::Vector3d& min_rpy = ang_info_vec[child_level].min_rpy; - auto children = trans.branch(child_level, static_cast(v_rate_), num_division); - - const auto& buckets = voxelmaps_ptr_->multi_buckets_[child_level]; - double trans_res = voxelmaps_ptr_->voxelmaps_res_[child_level]; - -#pragma omp parallel for num_threads(num_threads_) - for (int i = 0; i < children.size(); i++) { - calc_score(children[i], trans_res, rpy_res, min_rpy, buckets, max_bucket_scan_count, src_points_); - } - - for (const auto& child : children) { - // pruning - if (child.score < best_score) { - continue; - } - - trans_queue.push(child); - } - } - } - - // Calc localization elapsed time - const auto end_time = std::chrono::system_clock::now(); - elapsed_time_ = std::chrono::duration_cast(end_time - start_time).count() / 1e6; - - // Not localized - if (best_score == score_threshold || has_timed_out_) { - has_localized_ = false; - return; - } - - double min_res = voxelmaps_ptr_->get_min_res(); - global_pose_ = best_trans.create_matrix(min_res, ang_info_vec[0].rpy_res, ang_info_vec[0].min_rpy); - best_score_ = best_score; - has_timed_out_ = false; - has_localized_ = true; -} - -} // namespace cpu \ No newline at end of file diff --git a/robot-relocalization/src/cpu_bbs3d/voxelmaps.cpp b/robot-relocalization/src/cpu_bbs3d/voxelmaps.cpp deleted file mode 100644 index 7425c13..0000000 --- a/robot-relocalization/src/cpu_bbs3d/voxelmaps.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include - -namespace cpu { - -VoxelMaps::VoxelMaps() : min_level_res_(1.0), max_level_(6), max_bucket_scan_count_(10) {} - -VoxelMaps::~VoxelMaps() {} - -void VoxelMaps::create_voxelmaps(const std::vector& points, const int v_rate) { - const int multi_buckets_size = max_level_ + 1; - multi_buckets_.resize(multi_buckets_size); - voxelmaps_res_.resize(multi_buckets_size); - - double resolution = min_level_res_; - for (int i = 0; i < multi_buckets_size; i++) { - UnorderedVoxelMap unordered_voxelmap; - std::vector coords(points.size()); - std::transform(points.begin(), points.end(), coords.begin(), [&](const Eigen::Vector3d& point) { - return (point.array() / resolution).floor().cast(); - }); - - // 0: empty, 1: voxel, -1: neighbor - for (const auto& coord : coords) { - if (unordered_voxelmap.count(coord) == 0) { - // if the coord is not in the map - unordered_voxelmap[coord] = 1; - } else if (unordered_voxelmap[coord] == -1) { - // if the coord is exist as a neighbor - unordered_voxelmap[coord] = 1; - } else if (unordered_voxelmap[coord] == 1) { - // if the coord is exist as a voxel - continue; - } - - const auto neighbor_coords = create_neighbor_coords(coord); - for (const auto& neighbor_coord : neighbor_coords) { - if (unordered_voxelmap.count(neighbor_coord) == 0) { - // add neighbor coords - unordered_voxelmap[neighbor_coord] = -1; - } - } - } - - const Buckets& buckets = create_hash_buckets(unordered_voxelmap); - multi_buckets_[i] = buckets; - voxelmaps_res_[i] = resolution; - - resolution = resolution * v_rate; - } -} - -std::vector VoxelMaps::create_neighbor_coords(const Eigen::Vector3i& vec) { - std::vector neighbor_coord; - neighbor_coord.reserve(7); - neighbor_coord.emplace_back(vec.x() - 1, vec.y() - 1, vec.z()); - neighbor_coord.emplace_back(vec.x() - 1, vec.y(), vec.z()); - neighbor_coord.emplace_back(vec.x(), vec.y() - 1, vec.z()); - neighbor_coord.emplace_back(vec.x() - 1, vec.y() - 1, vec.z() - 1); - neighbor_coord.emplace_back(vec.x() - 1, vec.y(), vec.z() - 1); - neighbor_coord.emplace_back(vec.x(), vec.y() - 1, vec.z() - 1); - neighbor_coord.emplace_back(vec.x(), vec.y(), vec.z() - 1); - return neighbor_coord; -} - -VoxelMaps::Buckets VoxelMaps::create_hash_buckets(const UnorderedVoxelMap& unordered_voxelmap) { - // Loop until the success rate exceeds the threshold - std::vector buckets; - for (int num_buckets = unordered_voxelmap.size(); num_buckets <= unordered_voxelmap.size() * 16; num_buckets *= 2) { - buckets.resize(num_buckets); - std::fill(buckets.begin(), buckets.end(), Eigen::Vector4i::Zero()); - - int success_count = 0; - for (const auto& voxel : unordered_voxelmap) { - Eigen::Vector4i coord; - coord << voxel.first.x(), voxel.first.y(), voxel.first.z(), 1; - const std::uint32_t hash = (coord[0] * 73856093) ^ (coord[1] * 19349669) ^ (coord[2] * 83492791); - - // Find empty bucket - for (int i = 0; i < max_bucket_scan_count_; i++) { - const std::uint32_t bucket_index = (hash + i) % num_buckets; - - if (buckets[bucket_index].w() == 0) { - buckets[bucket_index] = coord; - success_count++; - break; - } - } - } - - const double success_rate = static_cast(success_count) / unordered_voxelmap.size(); - if (success_rate > 0.999) { - break; - } - } - - return buckets; -} -} // namespace cpu diff --git a/robot-relocalization/src/cpu_bbs3d/voxelmaps_io.cpp b/robot-relocalization/src/cpu_bbs3d/voxelmaps_io.cpp deleted file mode 100644 index 138070d..0000000 --- a/robot-relocalization/src/cpu_bbs3d/voxelmaps_io.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#include -#include -#include - -#include - -namespace cpu { - -bool BBS3D::set_voxelmaps_coords(const std::string& folder_path) { - const std::string voxelmaps_folder_path = folder_path + "/" + voxelmaps_folder_name_; - if (!boost::filesystem::exists(voxelmaps_folder_path)) { - return false; - } - - if (!load_voxel_params(voxelmaps_folder_path)) return false; - - if (!set_multi_buckets(voxelmaps_folder_path)) return false; - return true; -} - -bool BBS3D::load_voxel_params(const std::string& voxelmaps_folder_path) { - std::ifstream ifs(voxelmaps_folder_path + "/voxel_params.txt"); - if (!ifs) return false; - std::string str; - std::getline(ifs, str); - const double min_level_res = std::stod(str.substr(str.find(" ") + 1)); - - std::getline(ifs, str); - const int max_level = std::stoi(str.substr(str.find(" ") + 1)); - - std::getline(ifs, str); - set_voxel_expantion_rate(std::stod(str.substr(str.find(" ") + 1))); - - voxelmaps_ptr_.reset(new VoxelMaps); - voxelmaps_ptr_->set_min_res(min_level_res); - voxelmaps_ptr_->set_max_level(max_level); - return true; -} - -bool BBS3D::set_multi_buckets(const std::string& voxelmaps_folder_path) { - const auto pcd_files = pciof::load_pcd_file_paths(voxelmaps_folder_path); - voxelmaps_ptr_->multi_buckets_.resize(pcd_files.size()); - voxelmaps_ptr_->voxelmaps_res_.resize(pcd_files.size()); - - for (int i = 0; i < pcd_files.size(); i++) { - const auto& file = pcd_files[i]; - const auto coords = pciof::read_pcd(file.first); - if (coords.empty()) return false; - std::vector coords4i(coords.size()); - for (int j = 0; j < coords.size(); j++) { - coords4i[j] << coords[j], 1; - } - - voxelmaps_ptr_->multi_buckets_[i] = coords4i; - voxelmaps_ptr_->voxelmaps_res_[i] = voxelmaps_ptr_->get_min_res() * std::pow(v_rate_, i); - } - - // The minimum and maximum x, y, z values are selected from the 3D coordinate vector. - Eigen::Vector4i min_xyz = Eigen::Vector4i::Constant(std::numeric_limits::max()); - Eigen::Vector4i max_xyz = Eigen::Vector4i::Constant(std::numeric_limits::lowest()); - - int max_level = voxelmaps_ptr_->get_max_level(); - const auto& top_buckets = voxelmaps_ptr_->multi_buckets_[max_level]; - for (const auto& point : top_buckets) { - min_xyz = min_xyz.cwiseMin(point); - max_xyz = max_xyz.cwiseMax(point); - } - - init_tx_range_ = std::make_pair(min_xyz.x(), max_xyz.x()); - init_ty_range_ = std::make_pair(min_xyz.y(), max_xyz.y()); - init_tz_range_ = std::make_pair(min_xyz.z(), max_xyz.z()); - - return true; -} - -bool BBS3D::save_voxel_params(const std::string& folder_path) { - boost::filesystem::path dir(folder_path); - if (!boost::filesystem::exists(dir)) { - std::cout << "[ERROR] Can not open folder" << std::endl; - return false; - } - - double min_level_res = voxelmaps_ptr_->get_min_res(); - int max_level = voxelmaps_ptr_->get_max_level(); - - // create voxelmaps coords folder - const std::string voxelmaps_folder_path = folder_path + "/" + voxelmaps_folder_name_; - boost::filesystem::path voxelmaps_coords_dir(voxelmaps_folder_path); - if (!boost::filesystem::exists(voxelmaps_coords_dir)) { - boost::filesystem::create_directory(voxelmaps_coords_dir); - } - - std::ofstream ofs(voxelmaps_folder_path + "/voxel_params.txt"); - ofs << "min_level_res " << min_level_res << std::endl; - ofs << "max_level " << max_level << std::endl; - ofs << "v_rate " << v_rate_ << std::endl; - ofs.close(); - - return true; -} - -bool BBS3D::save_voxelmaps_pcd(const std::string& folder_path) { - boost::filesystem::path dir(folder_path); - if (!boost::filesystem::exists(dir)) { - std::cout << "[ERROR] Can not open folder" << std::endl; - return false; - } - - // create voxelmaps coords folder - const std::string voxelmaps_folder_path = folder_path + "/" + voxelmaps_folder_name_; - boost::filesystem::path voxelmaps_coords_dir(voxelmaps_folder_path); - if (!boost::filesystem::exists(voxelmaps_coords_dir)) { - boost::filesystem::create_directory(voxelmaps_coords_dir); - } - - for (int i = 0; i < voxelmaps_ptr_->get_max_level() + 1; ++i) { - const std::string file_path = voxelmaps_folder_path + "/" + std::to_string(i) + ".pcd"; - const auto& coords = voxelmaps_ptr_->multi_buckets_[i]; - if (coords.empty()) { - std::cout << "[ERROR] Can not get coords" << std::endl; - return false; - } - - std::vector coords3i(coords.size()); - for (int i = 0; i < coords.size(); ++i) { - coords3i[i] = coords[i].head<3>(); - } - - if (!pciof::save_pcd(file_path, coords3i)) { - std::cout << "[ERROR] Can not save pcd file: " << file_path << std::endl; - return false; - } - } - return true; -} -} // namespace cpu \ No newline at end of file diff --git a/robot-relocalization/src/gpu_bbs3d/bbs3d.cu b/robot-relocalization/src/gpu_bbs3d/bbs3d.cu deleted file mode 100644 index 421c77c..0000000 --- a/robot-relocalization/src/gpu_bbs3d/bbs3d.cu +++ /dev/null @@ -1,241 +0,0 @@ -#include -#include -#include -#include - -namespace gpu { -BBS3D::BBS3D() -: v_rate_(2.0f), - branch_copy_size_(10000), - score_threshold_percentage_(0.0), - use_timeout_(false), - timeout_duration_(10000), - has_timed_out_(false), - has_localized_(false), - voxelmaps_folder_name_("voxelmaps_coords") { - check_error << cudaStreamCreate(&stream); - - inv_v_rate_ = 1.0f / v_rate_; - min_rpy_ << -0.02f, -0.02f, 0.0f; - max_rpy_ << 0.02f, 0.02f, 2 * M_PI; -} - -BBS3D::~BBS3D() { - check_error << cudaStreamDestroy(stream); -} - -void BBS3D::set_timeout_duration_in_msec(const int msec) { - timeout_duration_ = std::chrono::milliseconds(msec); -} - -void BBS3D::set_tar_points(const std::vector& points, float min_level_res, int max_level) { - voxelmaps_ptr_.reset(new VoxelMaps); - voxelmaps_ptr_->set_min_res(min_level_res); - voxelmaps_ptr_->set_max_level(max_level); - voxelmaps_ptr_->create_voxelmaps(points, v_rate_, stream); -} - -void BBS3D::set_src_points(const std::vector& points) { - int src_size = points.size(); - src_points_.clear(); - src_points_.shrink_to_fit(); - src_points_.resize(src_size); - std::copy(points.begin(), points.end(), src_points_.begin()); - - d_src_points_.clear(); - d_src_points_.shrink_to_fit(); - d_src_points_.resize(src_size); - check_error << cudaMemcpyAsync( - thrust::raw_pointer_cast(d_src_points_.data()), - points.data(), - sizeof(Eigen::Vector3f) * src_size, - cudaMemcpyHostToDevice, - stream); -} - -void BBS3D::set_trans_search_range(const std::vector& points) { - // The minimum and maximum x, y, z values are selected from the 3D coordinate vector. - Eigen::Vector3f min_xyz = Eigen::Vector3f::Constant(std::numeric_limits::max()); - Eigen::Vector3f max_xyz = Eigen::Vector3f::Constant(std::numeric_limits::lowest()); - - for (const auto& point : points) { - min_xyz = min_xyz.cwiseMin(point); - max_xyz = max_xyz.cwiseMax(point); - } - - set_trans_search_range(min_xyz, max_xyz); -} - -void BBS3D::set_trans_search_range(const Eigen::Vector3f& min_xyz, const Eigen::Vector3f& max_xyz) { - min_xyz_ = min_xyz; - max_xyz_ = max_xyz; - const int max_level = voxelmaps_ptr_->get_max_level(); - const float top_res = voxelmaps_ptr_->voxelmaps_info_[max_level].res; - init_tx_range_ = std::make_pair(std::floor(min_xyz.x() / top_res), std::ceil(max_xyz.x() / top_res)); - init_ty_range_ = std::make_pair(std::floor(min_xyz.y() / top_res), std::ceil(max_xyz.y() / top_res)); - init_tz_range_ = std::make_pair(std::floor(min_xyz.z() / top_res), std::ceil(max_xyz.z() / top_res)); -} - -void BBS3D::calc_angular_info(std::vector& ang_info_vec) { - float max_norm = src_points_[0].norm(); - for (const auto& point : src_points_) { - float norm = point.norm(); - if (norm > max_norm) { - max_norm = norm; - } - } - - const int max_level = voxelmaps_ptr_->get_max_level(); - for (int i = max_level; i >= 0; i--) { - const float cosine = 1 - (std::pow(voxelmaps_ptr_->voxelmaps_info_[i].res, 2) / std::pow(max_norm, 2)) * 0.5f; - float ori_res = std::acos(max(cosine, -1.0f)); - ori_res = std::floor(ori_res * 10000) / 10000; - Eigen::Vector3f rpy_res_temp; - rpy_res_temp.x() = ori_res <= std::abs(max_rpy_.x() - min_rpy_.x()) ? ori_res : 0.0f; - rpy_res_temp.y() = ori_res <= std::abs(max_rpy_.y() - min_rpy_.y()) ? ori_res : 0.0f; - rpy_res_temp.z() = ori_res <= std::abs(max_rpy_.z() - min_rpy_.z()) ? ori_res : 0.0f; - - Eigen::Vector3f max_rpy_piece; - if (i == max_level) { - max_rpy_piece = max_rpy_ - min_rpy_; - } else { - max_rpy_piece.x() = ang_info_vec[i + 1].rpy_res.x() != 0.0f ? ang_info_vec[i + 1].rpy_res.x() : max_rpy_.x() - min_rpy_.x(); - max_rpy_piece.y() = ang_info_vec[i + 1].rpy_res.y() != 0.0f ? ang_info_vec[i + 1].rpy_res.y() : max_rpy_.y() - min_rpy_.y(); - max_rpy_piece.z() = ang_info_vec[i + 1].rpy_res.z() != 0.0f ? ang_info_vec[i + 1].rpy_res.z() : max_rpy_.z() - min_rpy_.z(); - } - - // Angle division number - Eigen::Vector3i num_division; - num_division.x() = rpy_res_temp.x() == 0.0f ? 1 : std::ceil(max_rpy_piece.x() / rpy_res_temp.x()); - num_division.y() = rpy_res_temp.y() == 0.0f ? 1 : std::ceil(max_rpy_piece.y() / rpy_res_temp.y()); - num_division.z() = rpy_res_temp.z() == 0.0f ? 1 : std::ceil(max_rpy_piece.z() / rpy_res_temp.z()); - ang_info_vec[i].num_division = num_division; - - // Bisect an angle - ang_info_vec[i].rpy_res.x() = num_division.x() == 1 ? 0.0f : max_rpy_piece.x() / num_division.x(); - ang_info_vec[i].rpy_res.y() = num_division.y() == 1 ? 0.0f : max_rpy_piece.y() / num_division.y(); - ang_info_vec[i].rpy_res.z() = num_division.z() == 1 ? 0.0f : max_rpy_piece.z() / num_division.z(); - - ang_info_vec[i].min_rpy.x() = ang_info_vec[i].rpy_res.x() == 0.0f ? 0.0f : min_rpy_.x(); - ang_info_vec[i].min_rpy.y() = ang_info_vec[i].rpy_res.y() == 0.0f ? 0.0f : min_rpy_.y(); - ang_info_vec[i].min_rpy.z() = ang_info_vec[i].rpy_res.z() == 0.0f ? 0.0f : min_rpy_.z(); - } -} - -std::vector> BBS3D::create_init_transset(const AngularInfo& init_ang_info) { - const int init_transset_size = (init_tx_range_.second - init_tx_range_.first + 1) * (init_ty_range_.second - init_ty_range_.first + 1) * - (init_tz_range_.second - init_tz_range_.first + 1) * (init_ang_info.num_division.x()) * - (init_ang_info.num_division.y()) * (init_ang_info.num_division.z()); - - const int max_level = voxelmaps_ptr_->get_max_level(); - - std::vector> transset; - transset.reserve(init_transset_size); - for (int tx = init_tx_range_.first; tx <= init_tx_range_.second; tx++) { - for (int ty = init_ty_range_.first; ty <= init_ty_range_.second; ty++) { - for (int tz = init_tz_range_.first; tz <= init_tz_range_.second; tz++) { - for (int roll = 0; roll < init_ang_info.num_division.x(); roll++) { - for (int pitch = 0; pitch < init_ang_info.num_division.y(); pitch++) { - for (int yaw = 0; yaw < init_ang_info.num_division.z(); yaw++) { - transset.emplace_back(DiscreteTransformation(0, max_level, tx, ty, tz, roll, pitch, yaw)); - } - } - } - } - } - } - return transset; -} - -void BBS3D::localize() { - // Calc localize time limit - const auto start_time = std::chrono::system_clock::now(); - const auto time_limit = start_time + timeout_duration_; - has_timed_out_ = false; - - // Initialize best score - best_score_ = 0; - const int score_threshold = std::floor(src_points_.size() * score_threshold_percentage_); - int best_score = score_threshold; - DiscreteTransformation best_trans(best_score); - - // Preapre initial transset - const int max_level = voxelmaps_ptr_->get_max_level(); - std::vector ang_info_vec(max_level + 1); - calc_angular_info(ang_info_vec); - thrust::device_vector d_ang_info_vec(ang_info_vec.size()); - check_error << cudaMemcpyAsync( - thrust::raw_pointer_cast(d_ang_info_vec.data()), - ang_info_vec.data(), - sizeof(AngularInfo) * ang_info_vec.size(), - cudaMemcpyHostToDevice, - stream); - - const auto init_transset = create_init_transset(ang_info_vec[max_level]); - - // Calc initial transset scores - const auto init_transset_output = calc_scores(init_transset, d_ang_info_vec); - - std::priority_queue> trans_queue(init_transset_output.begin(), init_transset_output.end()); - - std::vector> branch_stock; - branch_stock.reserve(branch_copy_size_); - while (!trans_queue.empty()) { - if (use_timeout_ && std::chrono::system_clock::now() > time_limit) { - has_timed_out_ = true; - break; - } - - auto trans = trans_queue.top(); - trans_queue.pop(); - - // Calculate remaining branch_stock when queue is empty - if (trans_queue.empty() && !branch_stock.empty()) { - const auto transset_output = calc_scores(branch_stock, d_ang_info_vec); - for (const auto& output : transset_output) { - if (output.score < best_score) continue; // pruning - trans_queue.push(output); - } - branch_stock.clear(); - } - - // pruning - if (trans.score < best_score) { - continue; - } - - if (trans.is_leaf()) { - best_trans = trans; - best_score = trans.score; - } else { - const int child_level = trans.level - 1; - trans.branch(branch_stock, child_level, static_cast(v_rate_), ang_info_vec[child_level].num_division); - } - - if (branch_stock.size() >= branch_copy_size_) { - const auto transset_output = calc_scores(branch_stock, d_ang_info_vec); - for (const auto& output : transset_output) { - if (output.score < best_score) continue; // pruning - trans_queue.push(output); - } - branch_stock.clear(); - } - } - - // Calc localization elapsed time - const auto end_time = std::chrono::system_clock::now(); - elapsed_time_ = std::chrono::duration_cast(end_time - start_time).count() / 1e6; - - // Not localized - if (best_score == score_threshold || has_timed_out_) { - has_localized_ = false; - return; - } - - float min_res = voxelmaps_ptr_->get_min_res(); - global_pose_ = best_trans.create_matrix(min_res, ang_info_vec[0].rpy_res, ang_info_vec[0].min_rpy); - best_score_ = best_score; - has_timed_out_ = false; - has_localized_ = true; -} -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/src/gpu_bbs3d/calc_score.cu b/robot-relocalization/src/gpu_bbs3d/calc_score.cu deleted file mode 100644 index 829bbcb..0000000 --- a/robot-relocalization/src/gpu_bbs3d/calc_score.cu +++ /dev/null @@ -1,93 +0,0 @@ -#include -#include -#include -#include - -namespace gpu { -__global__ void calc_scores_kernel( - const thrust::device_ptr multi_buckets_ptrs, - const thrust::device_ptr voxelmap_info_ptr, - const thrust::device_ptr d_ang_info_vec_ptr, - thrust::device_ptr> trans_ptr, - size_t index_size, - const thrust::device_ptr points_ptr, - size_t num_points) { - const size_t pose_index = threadIdx.x + blockIdx.x * blockDim.x; - if (pose_index > index_size) { - return; - } - - DiscreteTransformation& trans = *thrust::raw_pointer_cast(trans_ptr + pose_index); - const VoxelMapInfo& voxelmap_info = *thrust::raw_pointer_cast(voxelmap_info_ptr + trans.level); - const AngularInfo& ang_info = *thrust::raw_pointer_cast(d_ang_info_vec_ptr + trans.level); - const Eigen::Vector4i* buckets = thrust::raw_pointer_cast(multi_buckets_ptrs)[trans.level]; - - int score = 0; - for (size_t i = 0; i < num_points; i++) { - const Eigen::Vector3f& point = thrust::raw_pointer_cast(points_ptr)[i]; - - const Eigen::Vector3f translation(trans.x * voxelmap_info.res, trans.y * voxelmap_info.res, trans.z * voxelmap_info.res); - Eigen::Matrix3f rotation; - rotation = Eigen::AngleAxisf(trans.yaw * ang_info.rpy_res.z() + ang_info.min_rpy.z(), Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(trans.pitch * ang_info.rpy_res.y() + ang_info.min_rpy.y(), Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(trans.roll * ang_info.rpy_res.x() + ang_info.min_rpy.x(), Eigen::Vector3f::UnitX()); - const Eigen::Vector3f transed_point = rotation * point + translation; - - // coord to hash - const Eigen::Vector3i coord = (transed_point.array() * voxelmap_info.inv_res).floor().cast(); - const std::uint32_t hash = (coord[0] * 73856093) ^ (coord[1] * 19349669) ^ (coord[2] * 83492791); - - // open addressing - for (int j = 0; j < voxelmap_info.max_bucket_scan_count; j++) { - const std::uint32_t bucket_index = (hash + j) % voxelmap_info.num_buckets; - const Eigen::Vector4i bucket = buckets[bucket_index]; - - if (bucket.x() != coord.x() || bucket.y() != coord.y() || bucket.z() != coord.z()) { - continue; - } - - if (bucket.w() == 1) { - score++; - break; - } - } - } - trans.score = score; -} - -std::vector> BBS3D::calc_scores( - const std::vector>& h_transset, - thrust::device_vector& d_ang_info_vec) { - size_t transset_size = h_transset.size(); - thrust::device_vector> d_transset(transset_size); - check_error << cudaMemcpyAsync( - thrust::raw_pointer_cast(d_transset.data()), - h_transset.data(), - sizeof(DiscreteTransformation) * transset_size, - cudaMemcpyHostToDevice, - stream); - - const size_t block_size = 32; - const size_t num_blocks = (transset_size + (block_size - 1)) / block_size; - - calc_scores_kernel<<>>( - voxelmaps_ptr_->d_multi_buckets_ptrs_.data(), - voxelmaps_ptr_->d_voxelmaps_info_.data(), - d_ang_info_vec.data(), - d_transset.data(), - transset_size - 1, - d_src_points_.data(), - src_points_.size()); - - std::vector> h_output(transset_size); - check_error << cudaMemcpyAsync( - h_output.data(), - thrust::raw_pointer_cast(d_transset.data()), - sizeof(DiscreteTransformation) * transset_size, - cudaMemcpyDeviceToHost, - stream); - - check_error << cudaStreamSynchronize(stream); - return h_output; -} -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/src/gpu_bbs3d/stream_manager/check_error.cu b/robot-relocalization/src/gpu_bbs3d/stream_manager/check_error.cu deleted file mode 100644 index 6d7d445..0000000 --- a/robot-relocalization/src/gpu_bbs3d/stream_manager/check_error.cu +++ /dev/null @@ -1,19 +0,0 @@ -#include - -namespace gpu { - -void CUDACheckError::operator<<(cudaError_t error) const { - if (error == cudaSuccess) { - return; - } - - const std::string error_name = cudaGetErrorName(error); - const std::string error_string = cudaGetErrorString(error); - - std::cerr << "warning: " << error_name << std::endl; - std::cerr << " : " << error_string << std::endl; -} - -CUDACheckError check_error; - -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/src/gpu_bbs3d/voxelmaps.cu b/robot-relocalization/src/gpu_bbs3d/voxelmaps.cu deleted file mode 100644 index 13b9ce7..0000000 --- a/robot-relocalization/src/gpu_bbs3d/voxelmaps.cu +++ /dev/null @@ -1,145 +0,0 @@ -#include -#include - -namespace gpu { - -VoxelMaps::VoxelMaps() : min_level_res_(1.0f), max_level_(6), max_bucket_scan_count_(10) {} - -VoxelMaps::~VoxelMaps() {} - -void VoxelMaps::create_voxelmaps(const std::vector& points, const int v_rate, cudaStream_t stream) { - const int multi_buckets_size = max_level_ + 1; - std::vector multi_buckets(multi_buckets_size); - - float resolution = min_level_res_; - for (int i = 0; i < multi_buckets_size; i++) { - UnorderedVoxelMap unordered_voxelmap; - std::vector coords(points.size()); - std::transform(points.begin(), points.end(), coords.begin(), [&](const Eigen::Vector3f& point) { - return (point.array() / resolution).floor().cast(); - }); - - // 0: empty, 1: voxel, -1: neighbor - for (const auto& coord : coords) { - if (unordered_voxelmap.count(coord) == 0) { - // if the coord is not in the map - unordered_voxelmap[coord] = 1; - } else if (unordered_voxelmap[coord] == -1) { - // if the coord is exist as a neighbor - unordered_voxelmap[coord] = 1; - } else if (unordered_voxelmap[coord] == 1) { - // if the coord is exist as a voxel - continue; - } - - const auto neighbor_coords = create_neighbor_coords(coord); - for (const auto& neighbor_coord : neighbor_coords) { - if (unordered_voxelmap.count(neighbor_coord) == 0) { - // add neighbor coords - unordered_voxelmap[neighbor_coord] = -1; - } - } - } - - const Buckets& buckets = create_hash_buckets(unordered_voxelmap); - multi_buckets[i] = buckets; - - resolution = resolution * v_rate; - } - - // host to device - set_buckets_on_device(multi_buckets, v_rate, stream); -} - -std::vector VoxelMaps::create_neighbor_coords(const Eigen::Vector3i& vec) { - std::vector neighbor_coord; - neighbor_coord.reserve(7); - neighbor_coord.emplace_back(vec.x() - 1, vec.y() - 1, vec.z()); - neighbor_coord.emplace_back(vec.x() - 1, vec.y(), vec.z()); - neighbor_coord.emplace_back(vec.x(), vec.y() - 1, vec.z()); - neighbor_coord.emplace_back(vec.x() - 1, vec.y() - 1, vec.z() - 1); - neighbor_coord.emplace_back(vec.x() - 1, vec.y(), vec.z() - 1); - neighbor_coord.emplace_back(vec.x(), vec.y() - 1, vec.z() - 1); - neighbor_coord.emplace_back(vec.x(), vec.y(), vec.z() - 1); - return neighbor_coord; -} - -VoxelMaps::Buckets VoxelMaps::create_hash_buckets(const UnorderedVoxelMap& unordered_voxelmap) { - // Loop until the success rate exceeds the threshold - std::vector buckets; - for (int num_buckets = unordered_voxelmap.size(); num_buckets <= unordered_voxelmap.size() * 16; num_buckets *= 2) { - buckets.resize(num_buckets); - std::fill(buckets.begin(), buckets.end(), Eigen::Vector4i::Zero()); - - int success_count = 0; - for (const auto& voxel : unordered_voxelmap) { - Eigen::Vector4i coord; - coord << voxel.first.x(), voxel.first.y(), voxel.first.z(), 1; - const std::uint32_t hash = (coord[0] * 73856093) ^ (coord[1] * 19349669) ^ (coord[2] * 83492791); - - // Find empty bucket - for (int i = 0; i < max_bucket_scan_count_; i++) { - const std::uint32_t bucket_index = (hash + i) % num_buckets; - - if (buckets[bucket_index].w() == 0) { - buckets[bucket_index] = coord; - success_count++; - break; - } - } - } - - const double success_rate = static_cast(success_count) / unordered_voxelmap.size(); - if (success_rate > 0.999) { - break; - } - } - - return buckets; -} - -void VoxelMaps::set_buckets_on_device(const std::vector& multi_buckets, const int v_rate, cudaStream_t stream) { - d_multi_buckets_.resize(multi_buckets.size()); - voxelmaps_info_.resize(multi_buckets.size()); - - float resolution = min_level_res_; - for (int i = 0; i < multi_buckets.size(); i++) { - const int buckets_size = multi_buckets[i].size(); - const int buckets_datasize = sizeof(Eigen::Vector4i) * buckets_size; - DeviceBuckets d_buckets(buckets_size); - check_error - << cudaMemcpyAsync(thrust::raw_pointer_cast(d_buckets.data()), multi_buckets[i].data(), buckets_datasize, cudaMemcpyHostToDevice, stream); - d_multi_buckets_[i] = d_buckets; - - // create voxel map info - VoxelMapInfo info; - info.res = resolution; - info.inv_res = 1.0f / resolution; - info.max_bucket_scan_count = max_bucket_scan_count_; - info.num_buckets = buckets_size; - voxelmaps_info_[i] = info; - - resolution = resolution * v_rate; - } - - // Extract d_multi_buckets_ pointers - std::vector ptrs; - ptrs.reserve(multi_buckets.size()); - for (int i = 0; i < multi_buckets.size(); i++) { - ptrs.emplace_back(thrust::raw_pointer_cast(d_multi_buckets_[i].data())); - } - - // Copy host to device (ptrs) - d_multi_buckets_ptrs_.resize(multi_buckets.size()); - const int ptrs_datasize = sizeof(Eigen::Vector4i*) * multi_buckets.size(); - check_error << cudaMemcpyAsync(thrust::raw_pointer_cast(d_multi_buckets_ptrs_.data()), ptrs.data(), ptrs_datasize, cudaMemcpyHostToDevice, stream); - - // Copy host to device (voxel map info) - d_voxelmaps_info_.resize(voxelmaps_info_.size()); - const int info_datasize = sizeof(VoxelMapInfo) * voxelmaps_info_.size(); - check_error - << cudaMemcpyAsync(thrust::raw_pointer_cast(d_voxelmaps_info_.data()), voxelmaps_info_.data(), info_datasize, cudaMemcpyHostToDevice, stream); - - check_error << cudaStreamSynchronize(stream); -} -} // namespace gpu \ No newline at end of file diff --git a/robot-relocalization/src/gpu_bbs3d/voxelmaps_io.cu b/robot-relocalization/src/gpu_bbs3d/voxelmaps_io.cu deleted file mode 100644 index 539cbf2..0000000 --- a/robot-relocalization/src/gpu_bbs3d/voxelmaps_io.cu +++ /dev/null @@ -1,77 +0,0 @@ -#include -#include -#include -#include - -namespace gpu { - -bool BBS3D::set_voxelmaps_coords(const std::string& folder_path) { - const std::string voxelmaps_folder_path = folder_path + "/" + voxelmaps_folder_name_; - if (!boost::filesystem::exists(voxelmaps_folder_path)) { - return false; - } - - if (!load_voxel_params(voxelmaps_folder_path)) return false; - - const auto multi_buckets = set_multi_buckets(voxelmaps_folder_path); - if (multi_buckets.empty()) return false; - - voxelmaps_ptr_->set_buckets_on_device(multi_buckets, v_rate_, stream); - - return true; -} - -bool BBS3D::load_voxel_params(const std::string& voxelmaps_folder_path) { - std::ifstream ifs(voxelmaps_folder_path + "/voxel_params.txt"); - if (!ifs) return false; - std::string str; - std::getline(ifs, str); - const float min_level_res = std::stof(str.substr(str.find(" ") + 1)); - - std::getline(ifs, str); - const int max_level = std::stoi(str.substr(str.find(" ") + 1)); - - std::getline(ifs, str); - set_voxel_expantion_rate(std::stof(str.substr(str.find(" ") + 1))); - - voxelmaps_ptr_.reset(new VoxelMaps); - voxelmaps_ptr_->set_min_res(min_level_res); - voxelmaps_ptr_->set_max_level(max_level); - return true; -} - -std::vector> BBS3D::set_multi_buckets(const std::string& voxelmaps_folder_path) { - const auto pcd_files = pciof::load_pcd_file_paths(voxelmaps_folder_path); - - std::vector> multi_buckets; - multi_buckets.reserve(pcd_files.size()); - - for (const auto& file : pcd_files) { - const auto coords = pciof::read_pcd(file.first); - if (coords.empty()) return {}; - std::vector coords4i(coords.size()); - for (int i = 0; i < coords.size(); i++) { - coords4i[i] << coords[i], 1; - } - - multi_buckets.emplace_back(coords4i); - } - - // The minimum and maximum x, y, z values are selected from the 3D coordinate vector. - Eigen::Vector4i min_xyz = Eigen::Vector4i::Constant(std::numeric_limits::max()); - Eigen::Vector4i max_xyz = Eigen::Vector4i::Constant(std::numeric_limits::lowest()); - - int max_level = voxelmaps_ptr_->get_max_level(); - const auto& top_buckets = multi_buckets[max_level]; - for (const auto& point : top_buckets) { - min_xyz = min_xyz.cwiseMin(point); - max_xyz = max_xyz.cwiseMax(point); - } - - init_tx_range_ = std::make_pair(min_xyz.x(), max_xyz.x()); - init_ty_range_ = std::make_pair(min_xyz.y(), max_xyz.y()); - init_tz_range_ = std::make_pair(min_xyz.z(), max_xyz.z()); - - return multi_buckets; -} -} // namespace gpu \ No newline at end of file diff --git a/terrain_analysis/src/terrainAnalysis.cpp b/terrain_analysis/src/terrainAnalysis.cpp index aec6571..11a9314 100644 --- a/terrain_analysis/src/terrainAnalysis.cpp +++ b/terrain_analysis/src/terrainAnalysis.cpp @@ -257,7 +257,7 @@ int main(int argc, char **argv) { nh->get_parameter("maxRelZ", maxRelZ); nh->get_parameter("disRatioZ", disRatioZ); - auto subOdometry = nh->create_subscription("/odom", 5, odometryHandler); + auto subOdometry = nh->create_subscription("/Odometry", 5, odometryHandler); auto subLaserCloud = nh->create_subscription("/registered_scan", 5, laserCloudHandler); diff --git a/vehicle_simulator/src/vehicleSimulator.cpp b/vehicle_simulator/src/vehicleSimulator.cpp index c957333..0b3898b 100644 --- a/vehicle_simulator/src/vehicleSimulator.cpp +++ b/vehicle_simulator/src/vehicleSimulator.cpp @@ -180,7 +180,7 @@ void scanHandler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanIn) { // 1. 获取 map->odom geometry_msgs::msg::TransformStamped tfMapToOdom = - tfBuffer->lookupTransform("map", "odom", nh->get_clock()->now(), rclcpp::Duration::from_seconds(0.1)); + tfBuffer->lookupTransform("map", "odom", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); // 2. 反转得到 odom->map（也即 camera_init->map） geometry_msgs::msg::TransformStamped tfOdomToMap; @@ -211,13 +211,12 @@ void scanHandler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanIn) - int main(int argc, char** argv) { rclcpp::init(argc,argv); nh = rclcpp::Node::make_shared("vehicleSimulator"); - nh->set_parameter(rclcpp::Parameter("use_sim_time", false)); + nh->set_parameter(rclcpp::Parameter("use_sim_time", true)); // 初始化 TF Buffer 和 Listener tfBuffer = std::make_shared(nh->get_clock()); @@ -230,6 +229,7 @@ int main(int argc, char** argv) pubScanPointer = nh->create_publisher("/registered_scan",2); auto subOdometry = nh->create_subscription("/Odometry",5,odometryHandle); + // auto subOdometry = nh->create_subscription("/map_to_odom",5,odometryHandle);