├── CMakeLists.txt ├── README.md ├── README_CN.md ├── config └── localization_config.yaml ├── include ├── error_estimater │ ├── error_estimater.h │ └── voxelset.h ├── global_localization │ ├── bbs_localization.h │ ├── global_localization.h │ └── occupancy_gridmap.h ├── laser_odometry │ └── laser_odometry.h └── localization │ ├── UKF.h │ ├── localization.h │ └── tools.h ├── launch ├── kitti_test.py ├── localization.py └── single_laser.py ├── map ├── frame.pcd ├── map.pcd └── pos.txt ├── package.xml ├── src ├── error_estimater │ ├── error_estimater.cpp │ └── voxelset.cpp ├── global_localization │ ├── bbs_localization.cpp │ └── global_localization.cpp ├── laser_odometry │ └── laser_odometry.cpp ├── localization │ ├── UKF.cpp │ ├── localization.cpp │ └── tools.cpp └── main.cpp └── test ├── read_map.cpp ├── test_error.cpp ├── test_global_localization.cpp ├── test_kitti.cpp ├── test_laser_odom.cpp └── test_odom.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(laser_localization) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | #set(CMAKE_BUILD_TYPE "Release") 15 | #set(CMAKE_BUILD_TYPE "Debug") 16 | 17 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 19 | 20 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 21 | add_compile_options(-Wall -Wextra -Wpedantic) 22 | endif() 23 | add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 24 | set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 25 | find_package(OpenMP) 26 | if (OPENMP_FOUND) 27 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 28 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 29 | endif() 30 | 31 | # find dependencies 32 | find_package(ament_cmake REQUIRED) 33 | find_package(rclcpp REQUIRED) 34 | #find_package(agv_msgs REQUIRED) 35 | find_package(visualization_msgs REQUIRED) 36 | find_package(tf2_ros REQUIRED) 37 | find_package(tf2 REQUIRED) 38 | find_package(nav_msgs REQUIRED) 39 | find_package(sensor_msgs REQUIRED) 40 | find_package(pcl_conversions REQUIRED) 41 | find_package(teaserpp REQUIRED) 42 | find_package(Boost 1.58 REQUIRED) 43 | find_package(PCL REQUIRED COMPONENTS common features kdtree visualization) 44 | find_package(OpenCV REQUIRED) 45 | find_package(std_srvs REQUIRED) 46 | # 包含头文件目录 47 | include_directories(${PCL_INCLUDE_DIRS}) 48 | # 设置依赖库链接目录 49 | link_directories(${PCL_LIBRARY_DIRS}) 50 | # 添加预处理器和编译器标记 51 | add_definitions(${PCL_DEFINITIONS}) 52 | 53 | 54 | include_directories( 55 | include 56 | include/global_localization 57 | include/laser_odometry 58 | ) 59 | 60 | if(BUILD_TESTING) 61 | find_package(ament_lint_auto REQUIRED) 62 | # the following line skips the linter which checks for copyrights 63 | # uncomment the line when a copyright and license is not present in all source files 64 | #set(ament_cmake_copyright_FOUND TRUE) 65 | # the following line skips cpplint (only works in a git repo) 66 | # uncomment the line when this package is not in a git repo 67 | #set(ament_cmake_cpplint_FOUND TRUE) 68 | ament_lint_auto_find_test_dependencies() 69 | endif() 70 | 71 | 72 | ament_package() 73 | 74 | add_library( 75 | localization_lib 76 | src/localization/localization.cpp 77 | src/error_estimater/error_estimater.cpp 78 | src/localization/UKF.cpp 79 | src/global_localization/bbs_localization.cpp 80 | src/global_localization/global_localization.cpp 81 | src/laser_odometry/laser_odometry.cpp 82 | src/error_estimater/voxelset.cpp 83 | src/localization/tools.cpp 84 | ) 85 | ament_target_dependencies(localization_lib rclcpp visualization_msgs tf2_ros tf2 std_srvs nav_msgs sensor_msgs pcl_conversions) 86 | target_link_libraries( 87 | localization_lib 88 | ${PCL_LIBRARIES} 89 | Eigen3::Eigen 90 | ${OpenCV_LIBS} 91 | ) 92 | 93 | 94 | add_executable(laser_localization src/main.cpp) 95 | target_link_libraries(laser_localization 96 | ${PCL_LIBRARIES} 97 | Eigen3::Eigen 98 | ${OpenCV_LIBS} 99 | localization_lib 100 | ) 101 | install(TARGETS 102 | laser_localization 103 | DESTINATION lib/${PROJECT_NAME} 104 | LIBRARY DESTINATION lib/${PROJECT_NAME} 105 | ) 106 | ### test 107 | 108 | add_executable(test_laser_odom test/test_laser_odom.cpp) 109 | target_link_libraries(test_laser_odom 110 | ${PCL_LIBRARIES} 111 | Eigen3::Eigen 112 | localization_lib 113 | ) 114 | 115 | add_executable(test_global_localization test/test_global_localization.cpp) 116 | target_link_libraries(test_global_localization 117 | ${PCL_LIBRARIES} 118 | Eigen3::Eigen 119 | localization_lib 120 | ${OpenCV_LIBS} 121 | ) 122 | 123 | add_executable(test_error test/test_error.cpp) 124 | target_link_libraries(test_error 125 | ${PCL_LIBRARIES} 126 | Eigen3::Eigen 127 | localization_lib 128 | ${OpenCV_LIBS} 129 | ) 130 | 131 | add_executable(read_map test/read_map.cpp) 132 | target_link_libraries(read_map ${PCL_LIBRARIES}) 133 | 134 | add_executable(odom test/test_odom.cpp) 135 | target_link_libraries(odom ${PCL_LIBRARIES}) 136 | ament_target_dependencies(odom rclcpp visualization_msgs tf2_ros tf2 nav_msgs sensor_msgs pcl_conversions) 137 | 138 | add_executable(kitti test/test_kitti.cpp) 139 | target_link_libraries(kitti ${PCL_LIBRARIES}) 140 | ament_target_dependencies(kitti rclcpp visualization_msgs tf2_ros tf2 nav_msgs sensor_msgs pcl_conversions) 141 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # laser_localization | [CN](https://github.com/linyicheng1/laser_localization/blob/main/README_CN.md) 2 | 3 | laser_localization is a 3D LiDAR localization algorithm applied to small area scenes, typical application scenarios are industrial parks, neighborhoods or substations, etc. It combines 3D laser point cloud, wheeled odometer and IMU angle information to achieve high precision real-time positioning. A branch-and-bound search algorithm is used for global positioning, while NDT matching is used for local point clouds and global maps. The wheeled odometer and IMU angle information are used as the motion a priori parameters of the matching algorithm to accelerate laser matching and avoid falling into local optimum. 4 | 5 | 6 | [效果展示](https://www.bilibili.com/video/BV12P4y1m7nH/?spm_id_from=333.999.0.0&vd_source=4dd69fa6d40221a0fa0733def5c4708a) 7 | 8 | [Demo Video](https://www.bilibili.com/video/BV12P4y1m7nH/?spm_id_from=333.999.0.0&vd_source=4dd69fa6d40221a0fa0733def5c4708a) 9 | 10 | 11 | | | | | 12 | |------------|--------------------|--------------------------| 13 | | **Scene Range** | 500m x 500m (without GPS) | \ (with GPS case not tested, theoretical in 1km above.) | 14 | | **Speed** | less than 0.8m/s (only 3d laser) | 1-2m/s (3d laser + Wheeled odometer) | 15 | | **Accuracy** | Typical 2-3cm | Typical 2-3cm | 16 | 17 | 18 | ## 1. Prerequisites 19 | 20 | #### 1.1 **Ubuntu** 和 **ROS**，Ubuntu 18.04. ROS Dashing && Foxy 21 | 22 | ``` 23 | sudo apt install ros-YOUR_DISTRO-desktop ros-YOUR_DISTRO-pcl 24 | ``` 25 | [ROS2 Install WIKI](https://docs.ros.org/en/dashing/Installation/Ubuntu-Development-Setup.html) 26 | 27 | ## 2. build laser_localization on ROS2 28 | 29 | Clone the repository and build: 30 | 31 | ```shell 32 | cd ~/catkin_ws/src 33 | git clone https://github.com/linyicheng1/laser_localization.git 34 | cd ../ 35 | colcon build 36 | source ~/catkin_ws/install/setup.bash 37 | ``` 38 | 39 | ## 3. Run with KITTI dataset 40 | 41 | Download KITTI dataset： 42 | [BaiDu Driver](https://pan.baidu.com/s/1BaVZKkQu8WT2Yo4k4Omjug?pwd=6hrm)|[Google Driver](https://drive.google.com/file/d/1_gsbxX-M7xfUJSB0JO8qOGX5Adfaw0q_/view?usp=sharing) 43 | 44 | Modify the path code in `./launch/kitti_test.py`： 45 | 46 | ```python 47 | 61| parameters=[ 48 | 62| {"kitti_path": "${YOUR_PATH}"}, 49 | 63| ] 50 | 51 | 40| {"global_map": "${YOUR_PATH}"}, 52 | ``` 53 | 54 | run cmd in shell： 55 | 56 | ```shell 57 | ros2 launch ~/catkin_ws/src/laser_localization/launch/kitti_test.py 58 | ``` 59 | 60 | ## 4. Run with your device 61 | 62 | #### 4.0 Build point cloud map 63 | 64 | Point cloud maps can be constructed using the current mainstream 3D laser SLAM algorithm and saved at `. /map/map.pcd`. 65 | 66 | Recommended 3D point cloud algorithm: [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam)、[ALOAM](https://github.com/tops666/Aloam)、[LIO-SAM](https://github.com/TixiaoShan/LIO-SAM) 67 | 68 | #### 4.1 Laser only mode 69 | 70 | The single laser mode requires only one 3d radar and can be used for rapid deployment to operate in low speed carts. The relative transformation relationship of `base_link->laser` in the `tf` transformation tree needs to be provided, and the 3d point cloud data topic `pointcloud2` needs to be provided. 71 | 72 | run cmd: 73 | 74 | ```shell 75 | ros2 launch ~/catkin_ws/src/laser_localization/launch/single_laser.py 76 | ``` 77 | 78 | #### 4.2 Multi-sensor fusion localization 79 | 80 | Multi-sensor fusion positioning mode improves accuracy and speed, but requires more information. 81 | 82 | 1. The odometer data, `odom->base_link` in the `tf` transformation tree, is calculated for the wheel odometer. The calculation of the odometer requires calibration of the robot's wheel radius, calibration of the wheel distance over 10m straight ahead, and multiple rotations and calibration of the axis distance. 83 | 84 | Run in shell: 85 | 86 | ```shell 87 | ros2 launch ~/catkin_ws/src/laser_localization/launch/localization.py 88 | ``` 89 | 90 | ## 5. Advantages 91 | 92 | - Laser odometer + laser point cloud with map matching, laser positioning solution using only 3d radar, can reach about 10-15hz output, very easy to deploy at low speed using directly. 93 | 94 | - Laser positioning is separated from wheel odometer, which can selectively use wheel odometer information to provide a priori for laser matching, effectively avoiding the influence of wheel slip and other disturbances. 95 | 96 | - Provide matching confidence information and real-time feedback on current positioning status. 97 | 98 | - Provide global positioning to obtain the initial value method, and include the function of program power down to save the current position. 99 | 100 | ## 6. Licence 101 | 102 | The source code is released under [GPLv3](http://www.gnu.org/licenses/) license. 103 | -------------------------------------------------------------------------------- /README_CN.md: -------------------------------------------------------------------------------- 1 | # laser_localization 2 | 3 | laser_localization 是一个应用于小范围场景的3D激光雷达定位算法，典型应用场景为工业园区、小区或者变电站等。融合3D激光点云，轮式里程计以及IMU角度信息，实现高精实时定位。采用分支定界搜索算法进行全局定位，而局部点云和全局地图采用NDT匹配方法。轮式里程计和IMU角度信息作为匹配算法的运动先验参数，加速激光匹配，避免陷入局部最优。 4 | 5 | 6 | [效果展示](https://www.bilibili.com/video/BV12P4y1m7nH/?spm_id_from=333.999.0.0&vd_source=4dd69fa6d40221a0fa0733def5c4708a) 7 | 8 | [演示视频](https://www.bilibili.com/video/BV12P4y1m7nH/?spm_id_from=333.999.0.0&vd_source=4dd69fa6d40221a0fa0733def5c4708a) 9 | 10 | 11 | | | | | 12 | |------------|--------------------|--------------------------| 13 | | **适用定位范围** | 500m x 500m (无GPS) | \ (有GPS 情况下未测试，理论在1km以上) | 14 | | **运行速度** | 小于 0.8m/s (单激光) | 1-2m/s (雷达 + 轮式里程计) | 15 | | **定位精度** | 典型值 2-3cm | 典型值 2-3cm | 16 | 17 | 18 | ## 1. 准备环境 19 | 20 | 1.1 **Ubuntu** 和 **ROS**，Ubuntu 18.04. ROS Dashing && Foxy 21 | 22 | ``` 23 | sudo apt install ros-YOUR_DISTRO-desktop ros-YOUR_DISTRO-pcl 24 | ``` 25 | [ROS2 安装 WIKI](https://docs.ros.org/en/dashing/Installation/Ubuntu-Development-Setup.html) 26 | 27 | ## 2. ROS2 中编译 laser_localization 28 | 29 | 克隆本项目然后编译： 30 | 31 | ```shell 32 | cd ~/catkin_ws/src 33 | git clone https://github.com/linyicheng1/laser_localization.git 34 | cd ../ 35 | colcon build 36 | source ~/catkin_ws/install/setup.bash 37 | ``` 38 | 39 | ## 3. KITTI 数据集上运行 40 | 41 | 下载KITTI数据集： 42 | [百度网盘]()|[谷歌网盘]() 43 | 44 | 修改代码 `./launch/kitti_test.py` 中的路径： 45 | 46 | ```python 47 | 61| parameters=[ 48 | 62| {"kitti_path": "${YOUR_PATH}"}, 49 | 63| ] 50 | 51 | 40| {"global_map": "${YOUR_PATH}"}, 52 | ``` 53 | 54 | 运行代码： 55 | 56 | ```shell 57 | ros2 launch ~/catkin_ws/src/laser_localization/launch/kitti_test.py 58 | ``` 59 | 60 | ## 4. 在你的设备上运行 61 | 62 | #### 4.0 点云地图构建 63 | 64 | 可采用目前主流的3D激光SLAM算法构建点云地图并保存在`./map/map.pcd`处。 65 | 推荐的3D点云算法：[hdl_graph_slam](https://github.com/koide3/hdl_graph_slam)、[ALOAM](https://github.com/tops666/Aloam)、[LIO-SAM](https://github.com/TixiaoShan/LIO-SAM) 66 | 67 | #### 4.1 单激光模式 68 | 69 | 单激光模式仅需要一个3D雷达即可，可用于快速部署在低速小车中运行。需要提供 `tf` 变换树中 `base_link->laser` 的相对变换关系，并提供3d点云数据话题`pointcloud2`。 70 | 71 | 运行代码： 72 | 73 | ```shell 74 | ros2 launch ~/catkin_ws/src/laser_localization/launch/single_laser.py 75 | ``` 76 | 77 | #### 4.2 多传感器融合定位 78 | 79 | 多传感器融合定位模式下精度和运行速度均能得以提升，但需要提供更多信息。 80 | 1. 里程计数据，`tf`变换树中 `odom->base_link`, 为轮式里程计计算得到。里程计的计算需要对机器人轮子半径进行标定，直走10m以上标定轮距，旋转多圈并标定轴距。 81 | 82 | 运行代码： 83 | 84 | ```shell 85 | ros2 launch ~/catkin_ws/src/laser_localization/launch/localization.py 86 | ``` 87 | 88 | ## 5. 定位算法优势 89 | 90 | 91 | - 激光里程计 + 激光点云与地图匹配，只采用3d雷达的激光定位方案，可以达到10-15hz左右的输出，在低速时直接使用部署非常简单。 92 | 93 | - 激光定位与轮式里程计分离，可选择性的采用轮式里程计信息为激光匹配提供先验，有效避免车轮打滑等扰动影响。 94 | 95 | - 提供匹配置信度信息，实时反馈当前定位状态。 96 | 97 | - 提供全局定位获得初始值方法，并包含程序掉电保存当前位置的功能。 98 | 99 | ## 6. 许可证 100 | 101 | 本源代码在[GPLv3](http://www.gnu.org/licenses/)许可证下发布 102 | -------------------------------------------------------------------------------- /config/localization_config.yaml: -------------------------------------------------------------------------------- 1 | # 1 --- use laser only 2 | # 2 --- laser with wheel odometer 3 | # 3 --- fake localization 4 | localization/mode: 1 5 | global_map: "/home/hy/code/robot_ws/src/inspection_3d_robot/localization/map/map.pcd" 6 | pose_save: "/home/hy/code/robot_ws/src/inspection_3d_robot/localization/map/pos.txt" 7 | initial_pose_save: "/home/hy/code/robot_ws/src/inspection_3d_robot/localization/map/pos.txt" 8 | global_resolution: 1.2 9 | global_view_resolution: 2 10 | localization/ndt_resolution: 1 11 | localization/ndt_step_size: 0.1 12 | localization/ndt_epsilon: 0.01 13 | filter/k1: 0.95 14 | 15 | odom_frame: "odom" 16 | base_link_frame: "base_link" 17 | laser_frame: "os_sensor" 18 | laser_topic: "/points" 19 | #### 20 | 21 | bbs/map_filter_resolution: 1 22 | bbs/scan_filter_resolution: 1 23 | bbs/max_range: 15.0 24 | # transformation search range 25 | bbs/min_tx: -200.0 26 | bbs/max_tx: 200.0 27 | bbs/min_ty: -200.0 28 | bbs/max_ty: 200.0 29 | bbs/min_theta: -3.14 30 | bbs/max_theta: 3.14 31 | 32 | # global map config 33 | # slice height range (map coordinate) 34 | bbs/map_min_z: 2.0 35 | bbs/map_max_z: 2.4 36 | 37 | # 2D map params 38 | bbs/map_width: 512 39 | bbs/map_height: 1024 40 | bbs/map_resolution: 0.5 41 | bbs/max_points_pre_cell: 5 42 | 43 | # precomp map pyramid 44 | bbs/map_pyramid_level: 6 45 | 46 | # scan slice height range (sensor coordinate) 47 | bbs/scan_min_z: -0.2 48 | bbs/scan_max_z: 0.2 49 | 50 | 51 | #### 52 | 53 | ndt/resolution: 1.0 54 | ndt/step_size: 0.1 55 | ndt/epsilon: 0.01 56 | ndt/max_iterations: 30.0 57 | ndt/frame_resolution: 1.0 58 | odom/kf_distance: 2.0 59 | odom/local_map_size: 20 60 | ndt/local_map_resolution: 1.0 -------------------------------------------------------------------------------- /include/error_estimater/error_estimater.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_ERROR_ESTIMATER_H 2 | #define LOCALIZATION_ERROR_ESTIMATER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "error_estimater/voxelset.h" 8 | 9 | namespace match_error 10 | { 11 | class MatchingCostEvaluaterFlann 12 | { 13 | public: 14 | MatchingCostEvaluaterFlann() {} 15 | ~MatchingCostEvaluaterFlann(){} 16 | 17 | void set_target(pcl::PointCloud::ConstPtr cloud, double max_correspondence_distance); 18 | 19 | double calc_matching_error(const pcl::PointCloud& cloud, const Eigen::Matrix4f& transformation, double* inlier_fraction); 20 | 21 | private: 22 | double max_correspondence_distance_sq; 23 | pcl::KdTreeFLANN::Ptr tree; 24 | pcl::PointCloud::ConstPtr map; 25 | 26 | std::unique_ptr voxels; 27 | }; 28 | } 29 | 30 | #endif //LOCALIZATION_ERROR_ESTIMATER_H 31 | -------------------------------------------------------------------------------- /include/error_estimater/voxelset.h: -------------------------------------------------------------------------------- 1 | #ifndef LASER_LOCALIZATION_VOXELSET_H 2 | #define LASER_LOCALIZATION_VOXELSET_H 3 | #include 4 | #include 5 | #include 6 | 7 | namespace match_error{ 8 | class Vector3iHash { 9 | public: 10 | size_t operator()(const Eigen::Vector3i& x) const; 11 | }; 12 | 13 | class VoxelSet { 14 | public: 15 | VoxelSet(double max_correspondence_distance); 16 | 17 | void set_cloud(pcl::PointCloud::ConstPtr cloud); 18 | double matching_error(const pcl::PointCloud& cloud, double* inlier_fraction) const; 19 | 20 | private: 21 | Eigen::Vector3i voxel_coord(const Eigen::Vector4f& x) const; 22 | Eigen::Vector4f voxel_center(const Eigen::Vector3i& coord) const; 23 | 24 | private: 25 | double resolution; 26 | 27 | using Voxels = std::unordered_set, Eigen::aligned_allocator>; 28 | Voxels voxels; 29 | }; 30 | } 31 | 32 | #endif //LASER_LOCALIZATION_VOXELSET_H 33 | -------------------------------------------------------------------------------- /include/global_localization/bbs_localization.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_BBS_LOCALIZATION_H 2 | #define LOCALIZATION_BBS_LOCALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace global_localization { 13 | 14 | class OccupancyGridMap; 15 | 16 | struct DiscreteTransformation { 17 | public: 18 | using Points = std::vector>; 19 | 20 | DiscreteTransformation(); 21 | DiscreteTransformation(int level, int x, int y, int theta); 22 | ~DiscreteTransformation(); 23 | 24 | bool operator<(const DiscreteTransformation& rhs) const; 25 | 26 | bool is_leaf() const; 27 | 28 | Eigen::Isometry2f transformation(double theta_resolution, const std::vector>& gridmap_pyramid); 29 | 30 | Points transform(const Points& points, double trans_resolution, double theta_resolution); 31 | 32 | double calc_score(const Points& points, double theta_resolution, const std::vector>& gridmap_pyramid); 33 | 34 | std::vector branch(); 35 | 36 | public: 37 | double score; 38 | int level; 39 | int x; 40 | int y; 41 | int theta; 42 | }; 43 | 44 | struct BBSParams { 45 | BBSParams() { 46 | max_range = 15.0; 47 | min_tx = min_ty = -50.0; 48 | max_tx = max_ty = 50.0; 49 | min_theta = -M_PI; 50 | max_theta = M_PI; 51 | } 52 | 53 | double max_range; 54 | double min_tx; 55 | double max_tx; 56 | double min_ty; 57 | double max_ty; 58 | double min_theta; 59 | double max_theta; 60 | }; 61 | 62 | class BBSLocalization { 63 | public: 64 | using Points = std::vector>; 65 | 66 | BBSLocalization(const BBSParams& params = BBSParams()); 67 | ~BBSLocalization(); 68 | 69 | void set_map(const Points& map_points, double resolution, int width, int height, int pyramid_levels, int max_points_per_cell); 70 | 71 | boost::optional localize(const Points& scan_points, double min_score, double* best_score = nullptr); 72 | 73 | std::shared_ptr gridmap() const; 74 | 75 | private: 76 | std::priority_queue create_init_transset(const Points& scan_points) const; 77 | 78 | private: 79 | BBSParams params; 80 | 81 | double theta_resolution; 82 | std::vector> gridmap_pyramid; 83 | }; 84 | 85 | } // namespace global_localization 86 | 87 | #endif //LOCALIZATION_BBS_LOCALIZATION_H 88 | -------------------------------------------------------------------------------- /include/global_localization/global_localization.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_GLOBAL_LOCALIZATION_H 2 | #define LOCALIZATION_GLOBAL_LOCALIZATION_H 3 | 4 | #include "bbs_localization.h" 5 | #include 6 | #include 7 | #include "rclcpp/rclcpp.hpp" 8 | #include 9 | 10 | struct GlobalLocalizationResult { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | using Ptr = std::shared_ptr; 13 | 14 | GlobalLocalizationResult(double error, double inlier_fraction, const Eigen::Isometry3f& pose) : error(error), inlier_fraction(inlier_fraction), pose(pose) {} 15 | 16 | double error; 17 | double inlier_fraction; 18 | Eigen::Isometry3f pose; 19 | }; 20 | 21 | struct GlobalLocalizationResults { 22 | GlobalLocalizationResults(const std::vector& results) : results(results) {} 23 | 24 | GlobalLocalizationResults& sort(int max_num_candidates) { 25 | auto remove_loc = std::remove_if(results.begin(), results.end(), [](const auto& result) { return result == nullptr; }); 26 | results.erase(remove_loc, results.end()); 27 | 28 | std::cout << "valid solutions:" << results.size() << std::endl; 29 | 30 | // std::sort(results.begin(), results.end(), [](const auto& lhs, const auto& rhs) { return lhs->error < rhs->error; }); 31 | std::sort(results.begin(), results.end(), [](const auto& lhs, const auto& rhs) { return lhs->inlier_fraction > rhs->inlier_fraction; }); 32 | if (results.size() > max_num_candidates) { 33 | results.erase(results.begin() + max_num_candidates, results.end()); 34 | } 35 | return *this; 36 | } 37 | std::vector results; 38 | }; 39 | 40 | namespace global_localization { 41 | class BBSLocalization; 42 | 43 | class GlobalLocalizationBBS { 44 | public: 45 | GlobalLocalizationBBS(rclcpp::Node *node); 46 | ~GlobalLocalizationBBS() ; 47 | // add api 48 | 49 | void set_global_map(pcl::PointCloud::ConstPtr cloud); 50 | 51 | bool globalLocalization(pcl::PointCloud::ConstPtr cloud, const std::string& predict_txt, Eigen::Matrix4f& result); 52 | bool globalLocalization(pcl::PointCloud::ConstPtr cloud, const Eigen::Matrix4f& predict_pose, Eigen::Matrix4f& result); 53 | bool globalLocalization(pcl::PointCloud::ConstPtr cloud, Eigen::Matrix4f& result); 54 | 55 | void set_match_map(pcl::PointCloud::ConstPtr cloud); 56 | 57 | GlobalLocalizationResults query(pcl::PointCloud::ConstPtr cloud); 58 | 59 | private: 60 | using Points2D = std::vector>; 61 | Points2D slice(const pcl::PointCloud& cloud, double min_z, double max_z) const; 62 | 63 | pcl::PointCloud::Ptr unslice(const Points2D& points); 64 | // raw map 65 | pcl::PointCloud::ConstPtr map_; 66 | pcl::NormalDistributionsTransform ndt; 67 | protected: 68 | std::unique_ptr bbs; 69 | rclcpp::Node *node_; 70 | 71 | }; 72 | } // namespace global_localization 73 | 74 | 75 | #endif //LOCALIZATION_GLOBAL_LOCALIZATION_H 76 | -------------------------------------------------------------------------------- /include/global_localization/occupancy_gridmap.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_OCCUPANCY_GRIDMAP_H 2 | #define LOCALIZATION_OCCUPANCY_GRIDMAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace global_localization { 10 | 11 | class OccupancyGridMap { 12 | public: 13 | using Ptr = std::shared_ptr; 14 | 15 | OccupancyGridMap(double resolution, const cv::Mat& values) { 16 | this->resolution = resolution; 17 | this->values = values; 18 | } 19 | 20 | OccupancyGridMap(double resolution, int width, int height) { 21 | this->resolution = resolution; 22 | values = cv::Mat1f(height, width, 0.0f); 23 | } 24 | 25 | double grid_resolution() const { return resolution; } 26 | int width() const { return values.cols; } 27 | int height() const { return values.rows; } 28 | const float* data() const { return reinterpret_cast(values.data); } 29 | 30 | void insert_points(const std::vector>& points, int min_points_per_grid) { 31 | for (const auto& pt : points) { 32 | auto loc = grid_loc(pt); 33 | if (in_map(loc)) { 34 | value(loc) += 1; 35 | } 36 | } 37 | 38 | values /= min_points_per_grid; 39 | for (auto& x : values) { 40 | x = std::min(x, 1.0f); 41 | } 42 | } 43 | 44 | double calc_score(const std::vector>& points) const { 45 | double sum_dists = 0.0; 46 | for (const auto& pt : points) { 47 | auto loc = grid_loc(pt); 48 | loc.x() = std::max(0, std::min(values.cols - 1, loc.x())); 49 | loc.y() = std::max(0, std::min(values.rows - 1, loc.y())); 50 | sum_dists += value(loc); 51 | } 52 | 53 | return sum_dists; 54 | } 55 | 56 | OccupancyGridMap::Ptr pyramid_up() const { 57 | cv::Mat1f small_map(values.rows / 2, values.cols / 2); 58 | for (int i = 0; i < values.rows / 2; i++) { 59 | for (int j = 0; j < values.cols / 2; j++) { 60 | float x = values.at(i * 2, j * 2); 61 | x = std::max(x, values.at(i * 2 + 1, j * 2)); 62 | x = std::max(x, values.at(i * 2, j * 2 + 1)); 63 | x = std::max(x, values.at(i * 2 + 1, j * 2 + 1)); 64 | small_map.at(i, j) = x; 65 | } 66 | } 67 | 68 | return std::make_shared(resolution * 2.0, small_map); 69 | } 70 | 71 | nav_msgs::msg::OccupancyGrid::ConstSharedPtr to_rosmsg() { 72 | nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg(new nav_msgs::msg::OccupancyGrid); 73 | // TODO 74 | } 75 | 76 | private: 77 | bool in_map(const Eigen::Vector2i& pix) const { 78 | bool left_bound = (pix.array() >= Eigen::Array2i::Zero()).all(); 79 | bool right_bound = (pix.array() < Eigen::Array2i(values.cols, values.rows)).all(); 80 | return left_bound && right_bound; 81 | } 82 | 83 | float value(const Eigen::Vector2i& loc) const { return values.at(loc.y(), loc.x()); } 84 | float& value(const Eigen::Vector2i& loc) { return values.at(loc.y(), loc.x()); } 85 | 86 | Eigen::Vector2i grid_loc(const Eigen::Vector2f& pt) const { 87 | Eigen::Vector2i loc = (pt / resolution).array().floor().cast(); 88 | Eigen::Vector2i offset = Eigen::Vector2i(values.cols / 2, values.rows / 2); 89 | return loc + offset; 90 | } 91 | 92 | private: 93 | double resolution; 94 | cv::Mat1f values; 95 | }; 96 | 97 | } // namespace global_localization 98 | 99 | #endif //LOCALIZATION_OCCUPANCY_GRIDMAP_H 100 | -------------------------------------------------------------------------------- /include/laser_odometry/laser_odometry.h: -------------------------------------------------------------------------------- 1 | #ifndef LASER_ODOMETRY_LASER_ODOMETRY_H 2 | #define LASER_ODOMETRY_LASER_ODOMETRY_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "rclcpp/rclcpp.hpp" 8 | 9 | class laserOdometry 10 | { 11 | public: 12 | laserOdometry(rclcpp::Node *node); 13 | Eigen::Matrix4f addFrame(const pcl::PointCloud::Ptr& point); 14 | Eigen::Matrix4f addFrame(const pcl::PointCloud::Ptr& point, const Eigen::Matrix4f& predict, bool &accept); 15 | pcl::PointCloud::Ptr downSample(const pcl::PointCloud::Ptr& cloud, float resolution); 16 | void updateLocalMap(const pcl::PointCloud::Ptr& keyFrame, const Eigen::Matrix4f& pose); 17 | pcl::PointCloud::Ptr getLocalMap(){return local_map_cloud;} 18 | 19 | private: 20 | 21 | std::deque::Ptr> local_map; 22 | std::deque local_map_pose; 23 | pcl::PointCloud::Ptr local_map_cloud; 24 | Eigen::Matrix4f odometry; 25 | Eigen::Matrix4f nextGuess; 26 | // pcl::PointCloud::Ptr lastKf; 27 | pcl::PointCloud::Ptr align; 28 | pcl::NormalDistributionsTransform ndt; 29 | rclcpp::Node *node_; 30 | }; 31 | 32 | #endif //LASER_ODOMETRY_LASER_ODOMETRY_H 33 | -------------------------------------------------------------------------------- /include/localization/UKF.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_UKF_H 2 | #define LOCALIZATION_UKF_H 3 | #include "eigen3/Eigen/Core" 4 | #include "eigen3/Eigen/Geometry" 5 | #include "rclcpp/rclcpp.hpp" 6 | namespace laser_localization 7 | { 8 | class filter 9 | { 10 | public: 11 | filter(rclcpp::Node *node); 12 | Eigen::Vector3f predict(const Eigen::Matrix4f& update); 13 | Eigen::Vector3f update(const Eigen::Vector3f& p, const Eigen::Quaternionf& a); 14 | void set_pos(const Eigen::Vector3f& p, const Eigen::Quaternionf& a); 15 | Eigen::Matrix4f get_pos(); 16 | private: 17 | float k1_; 18 | bool first_ = true; 19 | 20 | Eigen::Vector3f pos_; 21 | Eigen::Quaternionf angular_; 22 | rclcpp::Node *node_; 23 | }; 24 | 25 | class UKF 26 | { 27 | public: 28 | UKF(); 29 | 30 | }; 31 | } 32 | #endif //LOCALIZATION_UKF_H 33 | -------------------------------------------------------------------------------- /include/localization/localization.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCALIZATION_LOCALIZATION_H 2 | #define LOCALIZATION_LOCALIZATION_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "localization/UKF.h" 9 | #include "error_estimater/error_estimater.h" 10 | #include 11 | #include 12 | #include "rclcpp/rclcpp.hpp" 13 | 14 | namespace laser_localization 15 | { 16 | class localization 17 | { 18 | public: 19 | localization(rclcpp::Node *node); 20 | void update_local_map(const Eigen::Matrix4f& pose); 21 | void update_pos(const geometry_msgs::msg::Twist::SharedPtr& cmd, float dt); 22 | void predict(const Eigen::Matrix4f& update); 23 | bool correct(pcl::PointCloud::Ptr cloud, int times = 10); 24 | void init_localization(const Eigen::Matrix4f& laser_base); 25 | 26 | void set_inv_odom_base(Eigen::Matrix4f p){inv_odom_base_ = p;} 27 | geometry_msgs::msg::Transform get_pos(){return pos_;} 28 | Eigen::Matrix4f get_estimate_pos(){return estimate_.get_pos();} 29 | pcl::PointCloud::Ptr get_map(){return map_points_;} 30 | pcl::PointCloud::Ptr get_view_map(){return map_points_view_;} 31 | pcl::PointCloud::Ptr get_aligned(){return aligned_;} 32 | private: 33 | Eigen::Matrix4f ndt_match(pcl::PointCloud::ConstPtr cloud, Eigen::Matrix4f gauss, int times); 34 | pcl::Registration::Ptr create_registration() ; 35 | pcl::PointCloud::Ptr map_points_; 36 | pcl::PointCloud::Ptr local_map_points_; 37 | Eigen::Matrix4f local_map_pose_; 38 | pcl::PointCloud::Ptr map_points_view_; 39 | pcl::PointCloud::Ptr aligned_; 40 | geometry_msgs::msg::Transform pos_; 41 | geometry_msgs::msg::Transform Zero; 42 | filter estimate_; 43 | pcl::Registration::Ptr ndt_; 44 | Eigen::Matrix4f inv_odom_base_; 45 | rclcpp::Node *node_; 46 | 47 | Eigen::Vector3f last_map_pos; 48 | pcl::PointCloud::Ptr local_map; 49 | }; 50 | }// laser_localization 51 | 52 | #endif //LOCALIZATION_LOCALIZATION_H 53 | -------------------------------------------------------------------------------- /include/localization/tools.h: -------------------------------------------------------------------------------- 1 | #ifndef LASER_LOCALIZATION_TOOLS_H 2 | #define LASER_LOCALIZATION_TOOLS_H 3 | #include 4 | #include 5 | #include 6 | #include "fstream" 7 | #include "unistd.h" 8 | #include "sys/types.h" 9 | #include "sys/stat.h" 10 | #include 11 | 12 | bool read_position(const std::string& file, Eigen::Matrix4f& base); 13 | 14 | void write_position(const std::string& file,const Eigen::Matrix4f& position); 15 | 16 | pcl::PointCloud::Ptr downSample(pcl::PointCloud::ConstPtr cloud, double resolution); 17 | 18 | #endif //LASER_LOCALIZATION_TOOLS_H 19 | -------------------------------------------------------------------------------- /launch/kitti_test.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='laser_localization', 8 | node_executable='laser_localization', 9 | node_name='laser_localization', 10 | parameters=[ 11 | {"localization/mode": 1}, 12 | {"bbs/max_range": 15.0}, 13 | {"bbs/min_tx": -50.0}, 14 | {"bbs/max_tx": 50.0}, 15 | {"bbs/min_ty": -50.0}, 16 | {"bbs/max_ty": 50.0}, 17 | {"bbs/min_theta": -3.14}, 18 | {"bbs/max_theta": 3.14}, 19 | {"bbs/map_min_z": -0.2}, 20 | {"bbs/map_max_z": 1.2}, 21 | {"bbs/map_width": 512}, 22 | {"bbs/map_height": 512}, 23 | {"bbs/map_resolution": 0.5}, 24 | {"bbs/max_points_pre_cell": 5}, 25 | {"bbs/map_pyramid_level": 6}, 26 | {"bbs/scan_min_z": -0.2}, 27 | {"bbs/scan_max_z": 1.2}, 28 | {"bbs/map_filter_resolution": 0.5}, 29 | {"bbs/scan_filter_resolution": 0.5}, 30 | {"global_map_width": 100.0}, 31 | {"global_map_height": 100.0}, 32 | {"ndt/resolution": 0.8}, 33 | {"ndt/step_size": 0.2}, 34 | {"ndt/epsilon": 0.01}, 35 | {"ndt/max_iterations": 30.0}, 36 | {"ndt/frame_resolution": 0.5}, 37 | {"odom/kf_distance": 2.0}, 38 | {"odom/local_map_size": 10}, 39 | {"ndt/local_map_resolution": 0.5}, 40 | {"global_map": "../map/kitti.pcd"}, 41 | {"odom_frame": "odom"}, 42 | {"base_link_frame": "base_link"}, 43 | {"laser_frame": "os_sensor"}, 44 | {"laser_topic": "/points"}, 45 | {"pose_save": "../map/pos.txt"}, 46 | {"correct_count": 5}, 47 | {"global_resolution": 1.3}, 48 | {"global_frame_resolution": 1}, 49 | {"global_view_resolution": 3}, 50 | {"local_map_size": 100}, 51 | {"localization/ndt_resolution": 1}, 52 | {"localization/ndt_step_size": 0.1}, 53 | {"localization/ndt_epsilon": 0.01}, 54 | {"initial_pose_save": "../map/init_pos.txt"}, 55 | ] 56 | ), 57 | Node( 58 | package='laser_localization', 59 | node_executable='kitti', 60 | node_name='kitti', 61 | parameters=[ 62 | {"kitti_path": "/media/lyc/软件/1-10/05/velodyne/"}, 63 | ] 64 | ), 65 | ]) -------------------------------------------------------------------------------- /launch/localization.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='laser_localization', 8 | node_executable='laser_localization', 9 | node_name='laser_localization', 10 | parameters=[ 11 | {"localization/mode": 2}, 12 | {"bbs/max_range": 15.0}, 13 | {"bbs/min_tx": -50.0}, 14 | {"bbs/max_tx": 50.0}, 15 | {"bbs/min_ty": -50.0}, 16 | {"bbs/max_ty": 50.0}, 17 | {"bbs/min_theta": -3.14}, 18 | {"bbs/max_theta": 3.14}, 19 | {"bbs/map_min_z": -0.2}, 20 | {"bbs/map_max_z": 1.2}, 21 | {"bbs/map_width": 512}, 22 | {"bbs/map_height": 512}, 23 | {"bbs/map_resolution": 0.5}, 24 | {"bbs/max_points_pre_cell": 5}, 25 | {"bbs/map_pyramid_level": 6}, 26 | {"bbs/scan_min_z": -0.2}, 27 | {"bbs/scan_max_z": 1.2}, 28 | {"bbs/map_filter_resolution": 0.5}, 29 | {"bbs/scan_filter_resolution": 0.5}, 30 | {"global_map_width": 100.0}, 31 | {"global_map_height": 100.0}, 32 | {"ndt/resolution": 0.8}, 33 | {"ndt/step_size": 0.2}, 34 | {"ndt/epsilon": 0.01}, 35 | {"ndt/max_iterations": 30.0}, 36 | {"ndt/frame_resolution": 0.5}, 37 | {"odom/kf_distance": 2.0}, 38 | {"odom/local_map_size": 10}, 39 | {"ndt/local_map_resolution": 0.5}, 40 | {"global_map": "../map/map.pcd"}, 41 | {"odom_frame": "odom"}, 42 | {"base_link_frame": "base_link"}, 43 | {"laser_frame": "os_sensor"}, 44 | {"laser_topic": "/points"}, 45 | {"pose_save": "../map/pos.txt"}, 46 | {"correct_count": 5}, 47 | {"global_resolution": 1.3}, 48 | {"global_frame_resolution": 1}, 49 | {"global_view_resolution": 3}, 50 | {"local_map_size": 100}, 51 | {"localization/ndt_resolution": 1}, 52 | {"localization/ndt_step_size": 0.1}, 53 | {"localization/ndt_epsilon": 0.01}, 54 | {"initial_pose_save": "../map/init_pos.txt"}, 55 | ] 56 | ), 57 | ]) -------------------------------------------------------------------------------- /launch/single_laser.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='laser_localization', 8 | node_executable='laser_localization', 9 | node_name='laser_localization', 10 | parameters=[ 11 | {"localization/mode": 1}, 12 | {"bbs/max_range": 15.0}, 13 | {"bbs/min_tx": -50.0}, 14 | {"bbs/max_tx": 50.0}, 15 | {"bbs/min_ty": -50.0}, 16 | {"bbs/max_ty": 50.0}, 17 | {"bbs/min_theta": -3.14}, 18 | {"bbs/max_theta": 3.14}, 19 | {"bbs/map_min_z": -0.2}, 20 | {"bbs/map_max_z": 1.2}, 21 | {"bbs/map_width": 512}, 22 | {"bbs/map_height": 512}, 23 | {"bbs/map_resolution": 0.5}, 24 | {"bbs/max_points_pre_cell": 5}, 25 | {"bbs/map_pyramid_level": 6}, 26 | {"bbs/scan_min_z": -0.2}, 27 | {"bbs/scan_max_z": 1.2}, 28 | {"bbs/map_filter_resolution": 0.5}, 29 | {"bbs/scan_filter_resolution": 0.5}, 30 | {"global_map_width": 100.0}, 31 | {"global_map_height": 100.0}, 32 | {"ndt/resolution": 0.8}, 33 | {"ndt/step_size": 0.2}, 34 | {"ndt/epsilon": 0.01}, 35 | {"ndt/max_iterations": 30.0}, 36 | {"ndt/frame_resolution": 0.5}, 37 | {"odom/kf_distance": 2.0}, 38 | {"odom/local_map_size": 10}, 39 | {"ndt/local_map_resolution": 0.5}, 40 | {"global_map": "/home/sa/code/robot_ws/src/inspection_3d_robot/laser_localization/map/map.pcd"}, 41 | {"odom_frame": "odom"}, 42 | {"base_link_frame": "base_link"}, 43 | {"laser_frame": "os_sensor"}, 44 | {"laser_topic": "/points"}, 45 | {"pose_save": "/home/sa/code/robot_ws/src/inspection_3d_robot/laser_localization/map/pos.txt"}, 46 | {"correct_count": 5}, 47 | {"global_resolution": 1.3}, 48 | {"global_frame_resolution": 1}, 49 | {"global_view_resolution": 3}, 50 | {"local_map_size": 100}, 51 | {"localization/ndt_resolution": 1}, 52 | {"localization/ndt_step_size": 0.1}, 53 | {"localization/ndt_epsilon": 0.01}, 54 | {"initial_pose_save": "/home/sa/code/robot_ws/src/inspection_3d_robot/laser_localization/map/init_pos.txt"}, 55 | ] 56 | ), 57 | ]) -------------------------------------------------------------------------------- /map/map.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/linyicheng1/laser_localization/667855e6374c92c7d4af95fa464da2347f91746e/map/map.pcd -------------------------------------------------------------------------------- /map/pos.txt: -------------------------------------------------------------------------------- 1 | 0.924372,-0.381237,0.0139672,0,0.381013,0.924426,0.0162771,0,-0.0191171,-0.00972437,0.99977,0,-1.31551,0.35245,0.0542225,1, -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | laser_localization 5 | 0.0.0 6 | TODO: Package description 7 | lyc 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/error_estimater/error_estimater.cpp: -------------------------------------------------------------------------------- 1 | #include "error_estimater/error_estimater.h" 2 | #include 3 | #include 4 | 5 | 6 | namespace match_error 7 | { 8 | 9 | void MatchingCostEvaluaterFlann::set_target(pcl::PointCloud::ConstPtr cloud, 10 | double max_correspondence_distance) 11 | { 12 | map = cloud; 13 | max_correspondence_distance_sq = max_correspondence_distance * max_correspondence_distance; 14 | tree.reset(new pcl::KdTreeFLANN); 15 | tree->setInputCloud(cloud); 16 | 17 | // max_correspondence_distance_sq = max_correspondence_distance * max_correspondence_distance; 18 | // 19 | // voxels.reset(new VoxelSet(max_correspondence_distance)); 20 | // voxels->set_cloud(cloud); 21 | } 22 | 23 | double MatchingCostEvaluaterFlann::calc_matching_error(const pcl::PointCloud &cloud, 24 | const Eigen::Matrix4f &transformation, 25 | double *inlier_fraction) 26 | { 27 | int num_inliers = 0; 28 | double matching_error = 0.0; 29 | 30 | pcl::PointCloud::Ptr transformed(new pcl::PointCloud()); 31 | pcl::transformPointCloud(cloud, *transformed, transformation.inverse()); 32 | 33 | // pcl::visualization::PCLVisualizer viewer("pcd viewer"); 34 | // pcl::visualization::PointCloudColorHandlerCustom c1(transformed, 0, 255, 0); 35 | // pcl::visualization::PointCloudColorHandlerCustom c2(map, 255, 0, 0); 36 | // viewer.addPointCloud(map, c2, "map"); 37 | // viewer.addPointCloud(transformed, c1, "aligned"); 38 | // viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "aligned"); 39 | // viewer.spin(); 40 | 41 | std::vector indices; 42 | std::vector sq_dists; 43 | for (int i = 0; i < transformed->size(); i++) 44 | { 45 | tree->nearestKSearch(transformed->at(i), 1, indices, sq_dists); 46 | if (sq_dists[0] < max_correspondence_distance_sq) 47 | { 48 | num_inliers++; 49 | matching_error += sq_dists[0]; 50 | } 51 | } 52 | 53 | if (inlier_fraction) 54 | { 55 | *inlier_fraction = static_cast(num_inliers) / cloud.size(); 56 | } 57 | return num_inliers ? matching_error / num_inliers : std::numeric_limits::max(); 58 | // int num_inliers = 0; 59 | // double matching_error = 0.0; 60 | // 61 | // pcl::PointCloud transformed; 62 | // pcl::transformPointCloud(cloud, transformed, transformation); 63 | // 64 | // return voxels->matching_error(transformed, inlier_fraction); 65 | } 66 | } -------------------------------------------------------------------------------- /src/error_estimater/voxelset.cpp: -------------------------------------------------------------------------------- 1 | #include "error_estimater/voxelset.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace match_error { 10 | 11 | size_t Vector3iHash::operator()(const Eigen::Vector3i& x) const { 12 | size_t seed = 0; 13 | boost::hash_combine(seed, x[0]); 14 | boost::hash_combine(seed, x[1]); 15 | boost::hash_combine(seed, x[2]); 16 | return seed; 17 | } 18 | 19 | VoxelSet::VoxelSet(double max_correspondence_distance) : resolution(max_correspondence_distance) {} 20 | 21 | void VoxelSet::set_cloud(pcl::PointCloud::ConstPtr cloud) { 22 | voxels.clear(); 23 | 24 | std::vector> offsets; 25 | offsets.reserve(9); 26 | for (int i = -1; i <= 1; i++) { 27 | for (int j = -1; j <= 1; j++) { 28 | for (int k = -1; k <= 1; k++) { 29 | offsets.push_back(Eigen::Vector3i(i, j, k)); 30 | } 31 | } 32 | } 33 | 34 | for (const auto& pt : *cloud) { 35 | Eigen::Vector3i coord = voxel_coord(pt.getVector4fMap()); 36 | for (const auto& offset : offsets) { 37 | Eigen::Vector4f center = voxel_center(coord + offset); 38 | if ((center - pt.getVector4fMap()).squaredNorm() < resolution * resolution) { 39 | voxels.insert(coord + offset); 40 | } 41 | } 42 | } 43 | } 44 | 45 | double VoxelSet::matching_error(const pcl::PointCloud& cloud, double* inlier_fraction) const { 46 | int num_inliers = 0; 47 | double errors = 0.0; 48 | 49 | for (const auto& pt : cloud) { 50 | Eigen::Vector3i coord = voxel_coord(pt.getVector4fMap()); 51 | if (voxels.find(coord) != voxels.end()) { 52 | Eigen::Vector4f center = voxel_center(coord); 53 | num_inliers++; 54 | errors += (pt.getVector4fMap() - center).squaredNorm(); 55 | } 56 | } 57 | 58 | *inlier_fraction = static_cast(num_inliers) / cloud.size(); 59 | return num_inliers ? errors / num_inliers : std::numeric_limits::max(); 60 | } 61 | 62 | Eigen::Vector3i VoxelSet::voxel_coord(const Eigen::Vector4f& x) const { 63 | return (x.array() / resolution - 0.5).floor().cast().head<3>(); 64 | } 65 | 66 | Eigen::Vector4f VoxelSet::voxel_center(const Eigen::Vector3i& coord) const { 67 | Eigen::Vector3f origin = (coord.template cast().array() + 0.5) * resolution; 68 | Eigen::Vector3f offset = Eigen::Vector3f::Ones() * resolution / 2.0f; 69 | return (Eigen::Vector4f() << origin + offset, 1.0).finished(); 70 | } 71 | 72 | } // namespace hdl_global_localization 73 | -------------------------------------------------------------------------------- /src/global_localization/bbs_localization.cpp: -------------------------------------------------------------------------------- 1 | #include "bbs_localization.h" 2 | #include "occupancy_gridmap.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace global_localization { 9 | 10 | DiscreteTransformation::DiscreteTransformation() { 11 | this->level = -1; 12 | } 13 | 14 | DiscreteTransformation::DiscreteTransformation(int level, int x, int y, int theta) { 15 | this->level = level; 16 | this->x = x; 17 | this->y = y; 18 | this->theta = theta; 19 | this->score = 0.0; 20 | } 21 | 22 | DiscreteTransformation::~DiscreteTransformation() {} 23 | 24 | bool DiscreteTransformation::operator<(const DiscreteTransformation& rhs) const { 25 | return score < rhs.score; 26 | } 27 | 28 | bool DiscreteTransformation::is_leaf() const { 29 | return level == 0; 30 | } 31 | 32 | Eigen::Isometry2f DiscreteTransformation::transformation(double theta_resolution, const std::vector>& gridmap_pyramid) { 33 | double trans_resolution = gridmap_pyramid[level]->grid_resolution(); 34 | 35 | Eigen::Isometry2f trans = Eigen::Isometry2f::Identity(); 36 | trans.linear() = Eigen::Rotation2Df(theta_resolution * theta).toRotationMatrix(); 37 | trans.translation() = Eigen::Vector2f(trans_resolution * x, trans_resolution * y); 38 | 39 | return trans; 40 | } 41 | 42 | DiscreteTransformation::Points DiscreteTransformation::transform(const Points& points, double trans_resolution, double theta_resolution) { 43 | Points transformed(points.size()); 44 | 45 | Eigen::Map> src(points[0].data(), 2, points.size()); 46 | Eigen::Map> dst(transformed[0].data(), 2, points.size()); 47 | 48 | Eigen::Matrix2f rot = Eigen::Rotation2Df(theta_resolution * theta).toRotationMatrix(); 49 | Eigen::Vector2f trans(trans_resolution * x, trans_resolution * y); 50 | 51 | dst = (rot * src).colwise() + trans; 52 | 53 | return transformed; 54 | } 55 | 56 | double DiscreteTransformation::calc_score(const Points& points, double theta_resolution, const std::vector>& gridmap_pyramid) { 57 | const auto& gridmap = gridmap_pyramid[level]; 58 | auto transformed = transform(points, gridmap->grid_resolution(), theta_resolution); 59 | score = gridmap->calc_score(transformed); 60 | return score; 61 | } 62 | 63 | std::vector DiscreteTransformation::branch() { 64 | std::vector b; 65 | b.reserve(4); 66 | b.emplace_back(DiscreteTransformation(level - 1, x * 2, y * 2, theta)); 67 | b.emplace_back(DiscreteTransformation(level - 1, x * 2 + 1, y * 2, theta)); 68 | b.emplace_back(DiscreteTransformation(level - 1, x * 2, y * 2 + 1, theta)); 69 | b.emplace_back(DiscreteTransformation(level - 1, x * 2 + 1, y * 2 + 1, theta)); 70 | return b; 71 | } 72 | 73 | BBSLocalization::BBSLocalization(const BBSParams& params) : params(params) {} 74 | 75 | BBSLocalization::~BBSLocalization() {} 76 | 77 | void BBSLocalization::set_map(const BBSLocalization::Points& map_points, double resolution, int width, int height, int pyramid_levels, int max_points_per_cell) { 78 | gridmap_pyramid.resize(pyramid_levels); 79 | gridmap_pyramid[0].reset(new OccupancyGridMap(resolution, width, height)); 80 | gridmap_pyramid[0]->insert_points(map_points, max_points_per_cell); 81 | 82 | for (int i = 1; i < pyramid_levels; i++) { 83 | gridmap_pyramid[i] = gridmap_pyramid[i - 1]->pyramid_up(); 84 | } 85 | } 86 | 87 | boost::optional BBSLocalization::localize(const BBSLocalization::Points& scan_points, double min_score, double* best_score) { 88 | theta_resolution = std::acos(1 - std::pow(gridmap_pyramid[0]->grid_resolution(), 2) / (2 * std::pow(params.max_range, 2))); 89 | 90 | double best_score_storage; 91 | best_score = best_score ? best_score : &best_score_storage; 92 | 93 | *best_score = min_score; 94 | boost::optional best_trans; 95 | 96 | auto trans_queue = create_init_transset(scan_points); 97 | 98 | while (!trans_queue.empty()) { 99 | // std::cout << trans_queue.size() << std::endl; 100 | 101 | auto trans = trans_queue.top(); 102 | trans_queue.pop(); 103 | 104 | if (trans.score < *best_score) { 105 | continue; 106 | } 107 | 108 | if (trans.is_leaf()) { 109 | best_trans = trans; 110 | *best_score = trans.score; 111 | } else { 112 | auto children = trans.branch(); 113 | for (auto& child : children) { 114 | child.calc_score(scan_points, theta_resolution, gridmap_pyramid); 115 | trans_queue.push(child); 116 | } 117 | } 118 | } 119 | 120 | if (best_trans == boost::none) { 121 | return boost::none; 122 | } 123 | 124 | return best_trans->transformation(theta_resolution, gridmap_pyramid); 125 | } 126 | 127 | std::shared_ptr BBSLocalization::gridmap() const { 128 | return gridmap_pyramid[0]; 129 | } 130 | 131 | std::priority_queue BBSLocalization::create_init_transset(const Points& scan_points) const { 132 | double trans_res = gridmap_pyramid.back()->grid_resolution(); 133 | std::pair tx_range(std::floor(params.min_tx / trans_res), std::ceil(params.max_tx / trans_res)); 134 | std::pair ty_range(std::floor(params.min_ty / trans_res), std::ceil(params.max_ty / trans_res)); 135 | std::pair theta_range(std::floor(params.min_theta / theta_resolution), std::ceil(params.max_theta / theta_resolution)); 136 | 137 | std::vector transset; 138 | transset.reserve((tx_range.second - tx_range.first) * (ty_range.second - ty_range.first) * (theta_range.second - theta_range.first)); 139 | for (int tx = tx_range.first; tx <= tx_range.second; tx++) { 140 | for (int ty = ty_range.first; ty <= ty_range.second; ty++) { 141 | for (int theta = theta_range.first; theta <= theta_range.second; theta++) { 142 | int level = gridmap_pyramid.size() - 1; 143 | transset.emplace_back(DiscreteTransformation(level, tx, ty, theta)); 144 | } 145 | } 146 | } 147 | 148 | #pragma omp parallel for 149 | for (int i = 0; i < transset.size(); i++) { 150 | auto& trans = transset[i]; 151 | const auto& gridmap = gridmap_pyramid[trans.level]; 152 | trans.calc_score(scan_points, theta_resolution, gridmap_pyramid); 153 | } 154 | 155 | return std::priority_queue(transset.begin(), transset.end()); 156 | } 157 | 158 | } // namespace global_localization 159 | -------------------------------------------------------------------------------- /src/global_localization/global_localization.cpp: -------------------------------------------------------------------------------- 1 | #include "global_localization.h" 2 | #include "localization/tools.h" 3 | #include 4 | #include 5 | 6 | namespace global_localization { 7 | 8 | GlobalLocalizationBBS::GlobalLocalizationBBS(rclcpp::Node *node): node_(node) { 9 | float ndt_resolution, ndt_step_size, ndt_epsilon, ndt_max_iterations; 10 | node_->get_parameter("ndt/resolution", ndt_resolution); 11 | node_->get_parameter("ndt/step_size", ndt_step_size); 12 | node_->get_parameter("ndt/epsilon", ndt_epsilon); 13 | node_->get_parameter("ndt/max_iterations", ndt_max_iterations); 14 | ndt.setResolution(ndt_resolution); 15 | ndt.setStepSize(ndt_step_size); 16 | ndt.setTransformationEpsilon(ndt_epsilon); 17 | ndt.setMaximumIterations(ndt_max_iterations); 18 | } 19 | 20 | GlobalLocalizationBBS ::~GlobalLocalizationBBS() {} 21 | 22 | // raw map 23 | void GlobalLocalizationBBS::set_global_map(pcl::PointCloud::ConstPtr cloud){ 24 | map_ = cloud; 25 | } 26 | 27 | bool GlobalLocalizationBBS::globalLocalization(pcl::PointCloud::ConstPtr cloud, const std::string& predict_txt, Eigen::Matrix4f& result){ 28 | // 1. read path from txt 29 | Eigen::Matrix4f T; 30 | bool read_success = read_position(predict_txt, T); 31 | if (!read_success || 32 | T.block<3, 3>(0,0).determinant() < 0.8 || 33 | T.block<3, 3>(0,0).determinant() == NAN || 34 | T.block<3, 3>(0,0).determinant() > 1.2 || 35 | T(0, 3) == NAN || T(1, 3) == NAN || T(2, 3) == NAN ){ 36 | // T is not available 37 | return globalLocalization(cloud, result); 38 | } 39 | else{ 40 | // match by ndt 41 | pcl::PointCloud align; 42 | ndt.setInputTarget(map_); 43 | ndt.setInputSource(cloud); 44 | ndt.align(align, T); 45 | double score = ndt.getFitnessScore(); 46 | std::cout<<"score "<::ConstPtr cloud, const Eigen::Matrix4f& predict_pose, Eigen::Matrix4f& result){ 53 | pcl::PointCloud::Ptr new_map(new pcl::PointCloud); 54 | // trans 55 | Eigen::Isometry3f center_t = Eigen::Isometry3f::Identity(); 56 | center_t.translation() = -predict_pose.block<3, 1>(0, 3); 57 | pcl::transformPointCloud(*map_, *new_map, center_t); 58 | // filter 59 | float map_resolution, scan_resolution; 60 | node_->get_parameter("bbs/map_filter_resolution", map_resolution); 61 | node_->get_parameter("bbs/scan_filter_resolution", scan_resolution); 62 | new_map = downSample(new_map, map_resolution); 63 | auto filtered = downSample(cloud, scan_resolution); 64 | // set match map 65 | set_match_map(new_map); 66 | // query 67 | auto re = query(filtered); 68 | re.sort(1); 69 | Eigen::Matrix4f T = re.results.at(0)->pose.matrix(); 70 | T(2, 3) = 0; 71 | // match by ndt 72 | pcl::PointCloud align; 73 | ndt.setInputTarget(new_map); 74 | ndt.setInputSource(cloud); 75 | ndt.align(align, T); 76 | double score = ndt.getFitnessScore(); 77 | std::cout<<"score "<::ConstPtr cloud, Eigen::Matrix4f& result){ 83 | pcl::PointCloud::Ptr new_map(new pcl::PointCloud); 84 | // trans 85 | Eigen::Vector4f centroid; 86 | pcl::compute3DCentroid(*map_, centroid); 87 | Eigen::Isometry3f center_t = Eigen::Isometry3f::Identity(); 88 | center_t.translation() = -centroid.block<3, 1>(0, 0); 89 | pcl::transformPointCloud(*map_, *new_map, center_t); 90 | // filter 91 | float map_resolution, scan_resolution; 92 | node_->get_parameter("bbs/map_filter_resolution", map_resolution); 93 | node_->get_parameter("bbs/scan_filter_resolution", scan_resolution); 94 | new_map = downSample(new_map, map_resolution); 95 | auto filtered = downSample(cloud, scan_resolution); 96 | // set match map 97 | { 98 | BBSParams params; 99 | double width, height; 100 | node_->get_parameter("global_map_width", width); 101 | node_->get_parameter("global_map_height", height); 102 | node_->get_parameter("bbs/max_range", params.max_range); 103 | params.min_tx = -width/2; 104 | params.max_tx = width/2; 105 | params.min_ty = -height/2; 106 | params.max_ty = height/2; 107 | node_->get_parameter("bbs/min_theta", params.min_theta); 108 | node_->get_parameter("bbs/max_theta", params.max_theta); 109 | bbs.reset(new BBSLocalization(params)); 110 | 111 | double map_min_z = 2.0, map_max_z = 2.4; 112 | node_->get_parameter("bbs/map_min_z", map_min_z); 113 | node_->get_parameter("bbs/map_max_z", map_max_z); 114 | auto map_2d = slice(*new_map, map_min_z, map_max_z); 115 | 116 | if (map_2d.size() < 128) { 117 | printf("Num points in the sliced map is too small!!"); 118 | printf("Change the slice range parameters!!"); 119 | } 120 | 121 | int map_width = 512, map_height = 1024, map_pyramid_level = 6, max_points_per_cell = 5; 122 | double map_resolution; 123 | node_->get_parameter("bbs/map_width", map_width); 124 | node_->get_parameter("bbs/map_height", map_height); 125 | node_->get_parameter("bbs/map_resolution", map_resolution); 126 | node_->get_parameter("bbs/map_pyramid_level", map_pyramid_level); 127 | node_->get_parameter("bbs/max_points_per_cell", max_points_per_cell); 128 | 129 | bbs->set_map(map_2d, map_resolution, map_width, map_height, map_pyramid_level, max_points_per_cell); 130 | 131 | auto map_3d = unslice(map_2d); 132 | map_3d->header.frame_id = "map"; 133 | } 134 | // query 135 | auto re = query(filtered); 136 | re.sort(1); 137 | if (re.results.empty()) 138 | return false; 139 | 140 | Eigen::Matrix4f T = re.results.at(0)->pose.matrix(); 141 | T(2, 3) = 0; 142 | // match by ndt 143 | pcl::PointCloud align; 144 | ndt.setInputTarget(new_map); 145 | ndt.setInputSource(cloud); 146 | ndt.align(align, T); 147 | double score = ndt.getFitnessScore(); 148 | std::cout<<"score "<::ConstPtr cloud) { 154 | 155 | BBSParams params; 156 | node_->get_parameter("bbs/max_range", params.max_range); 157 | node_->get_parameter("bbs/min_tx", params.min_tx); 158 | node_->get_parameter("bbs/max_tx", params.max_tx); 159 | node_->get_parameter("bbs/min_ty", params.min_ty); 160 | node_->get_parameter("bbs/max_ty", params.max_ty); 161 | node_->get_parameter("bbs/min_theta", params.min_theta); 162 | node_->get_parameter("bbs/max_theta", params.max_theta); 163 | bbs.reset(new BBSLocalization(params)); 164 | 165 | double map_min_z = 2.0, map_max_z = 2.4; 166 | node_->get_parameter("bbs/map_min_z", map_min_z); 167 | node_->get_parameter("bbs/map_max_z", map_max_z); 168 | auto map_2d = slice(*cloud, map_min_z, map_max_z); 169 | 170 | if (map_2d.size() < 128) { 171 | printf("Num points in the sliced map is too small!!"); 172 | printf("Change the slice range parameters!!"); 173 | } 174 | 175 | int map_width = 512, map_height = 1024, map_pyramid_level = 6, max_points_per_cell = 5; 176 | double map_resolution; 177 | node_->get_parameter("bbs/map_width", map_width); 178 | node_->get_parameter("bbs/map_height", map_height); 179 | node_->get_parameter("bbs/map_resolution", map_resolution); 180 | node_->get_parameter("bbs/map_pyramid_level", map_pyramid_level); 181 | node_->get_parameter("bbs/max_points_per_cell", max_points_per_cell); 182 | 183 | bbs->set_map(map_2d, map_resolution, map_width, map_height, map_pyramid_level, max_points_per_cell); 184 | 185 | auto map_3d = unslice(map_2d); 186 | map_3d->header.frame_id = "map"; 187 | } 188 | 189 | GlobalLocalizationResults GlobalLocalizationBBS::query(pcl::PointCloud::ConstPtr cloud) { 190 | double scan_min_z = -0.2, scan_max_z = 0.2; 191 | node_->get_parameter("bbs/scan_min_z", scan_min_z); 192 | node_->get_parameter("bbs/scan_max_z", scan_max_z); 193 | auto scan_2d = slice(*cloud, scan_min_z, scan_max_z); 194 | 195 | std::vector results; 196 | 197 | if (scan_2d.size() < 32) { 198 | return GlobalLocalizationResults(results); 199 | } 200 | 201 | double best_score = 0.0; 202 | auto trans_2d = bbs->localize(scan_2d, 0.0, &best_score); 203 | if (trans_2d == boost::none) { 204 | return GlobalLocalizationResults(results); 205 | } 206 | 207 | Eigen::Isometry3f trans_3d = Eigen::Isometry3f::Identity(); 208 | trans_3d.linear().block<2, 2>(0, 0) = trans_2d->linear(); 209 | trans_3d.translation().head<2>() = trans_2d->translation(); 210 | 211 | results.resize(1); 212 | results[0].reset(new GlobalLocalizationResult(best_score, best_score, trans_3d)); 213 | 214 | return GlobalLocalizationResults(results); 215 | } 216 | 217 | GlobalLocalizationBBS::Points2D GlobalLocalizationBBS::slice(const pcl::PointCloud& cloud, double min_z, double max_z) const { 218 | Points2D points_2d; 219 | points_2d.reserve(cloud.size()); 220 | for (auto i : cloud) { 221 | if (min_z < i.z && i.z < max_z) { 222 | points_2d.push_back(i.getVector3fMap().head<2>()); 223 | } 224 | } 225 | return points_2d; 226 | } 227 | 228 | pcl::PointCloud::Ptr GlobalLocalizationBBS::unslice(const Points2D& points) { 229 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 230 | cloud->resize(points.size()); 231 | for (int i = 0; i < points.size(); i++) { 232 | cloud->at(i).getVector3fMap().head<2>() = points[i]; 233 | cloud->at(i).z = 0.0f; 234 | } 235 | 236 | return cloud; 237 | } 238 | } // namespace global_localization -------------------------------------------------------------------------------- /src/laser_odometry/laser_odometry.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_odometry.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | laserOdometry::laserOdometry(rclcpp::Node *node): 8 | align(new pcl::PointCloud()), 9 | local_map_cloud(new pcl::PointCloud()), 10 | node_(node) 11 | { 12 | float ndt_resolution, ndt_step_size, ndt_epsilon, ndt_max_iterations; 13 | node_->get_parameter("ndt/resolution", ndt_resolution); 14 | node_->get_parameter("ndt/step_size", ndt_step_size); 15 | node_->get_parameter("ndt/epsilon", ndt_epsilon); 16 | node_->get_parameter("ndt/max_iterations", ndt_max_iterations); 17 | 18 | ndt.setResolution(ndt_resolution); 19 | ndt.setStepSize(ndt_step_size); 20 | ndt.setTransformationEpsilon(ndt_epsilon); 21 | ndt.setMaximumIterations(ndt_max_iterations); 22 | } 23 | 24 | 25 | Eigen::Matrix4f laserOdometry::addFrame(const pcl::PointCloud::Ptr& point, const Eigen::Matrix4f& predict, bool &accept) 26 | { 27 | auto s = std::chrono::steady_clock::now(); 28 | float frame_resolution = 1; 29 | node_->get_parameter("ndt/frame_resolution", frame_resolution); 30 | 31 | auto filter = downSample(point, frame_resolution); 32 | if (filter->points.size() < 400){ 33 | odometry = odometry * predict; 34 | accept = true; 35 | return odometry; 36 | } 37 | std::vector indices; 38 | pcl::removeNaNFromPointCloud(*filter, *filter, indices); 39 | if (local_map.empty()) 40 | {// first frame 41 | odometry = Eigen::Matrix4f::Identity(); 42 | nextGuess = Eigen::Matrix4f::Identity(); 43 | updateLocalMap(filter, Eigen::Matrix4f::Identity()); 44 | return odometry; 45 | } 46 | ndt.setInputSource(filter); 47 | 48 | Eigen::Matrix4f odom_predict = odometry * predict; 49 | // double dt = (nextGuess.block<3, 1>(0, 3) - odom_predict.block<3, 1>(0, 3)).norm(); 50 | // 51 | // auto angle0 = nextGuess.block<3, 3>(0, 0).eulerAngles(2, 1, 0); 52 | // auto angle1 = odom_predict.block<3, 3>(0, 0).eulerAngles(2, 1, 0); 53 | // double da = std::abs(angle0.z() - angle1.z()); 54 | // if (da > M_PI) 55 | // da = da - 2*M_PI; 56 | // else if (da < -M_PI) 57 | // da = da += 2*M_PI; 58 | // std::cout<<" t0: "< 3){ 69 | nextGuess.block<3,3>(0,0) = odom_predict.block<3,3>(0,0); 70 | ndt.align(*align, nextGuess); 71 | accept = false; 72 | } 73 | Eigen::Matrix4f delta = odometry.inverse() * ndt.getFinalTransformation(); 74 | nextGuess = odometry * delta; 75 | odometry = ndt.getFinalTransformation(); 76 | 77 | auto e2 = std::chrono::steady_clock::now(); 78 | 79 | 80 | Eigen::Matrix4f distance = local_map_pose.back().inverse() * odometry; 81 | float kf_distance; 82 | node_->get_parameter("odom/kf_distance", kf_distance); 83 | 84 | if (fabs(distance(0, 3)) + fabs(distance(1, 3)) > kf_distance) 85 | {// new key frame 86 | updateLocalMap(filter, odometry); 87 | std::cout<<" add new key frame !"; 88 | } 89 | auto e3 = std::chrono::steady_clock::now(); 90 | 91 | //std::cout<<" total cost: "<(e3 - s).count()<<" ms align cost: "<(e2 - e1).count()<<" ms "; 92 | 93 | // std::cout<<" x: "<::Ptr &point) 101 | { 102 | auto s = std::chrono::steady_clock::now(); 103 | float frame_resolution = 1; 104 | node_->get_parameter("ndt/frame_resolution", frame_resolution); 105 | 106 | auto filter = downSample(point, frame_resolution); 107 | std::vector indices; 108 | pcl::removeNaNFromPointCloud(*filter, *filter, indices); 109 | if (local_map.empty()) 110 | {// first frame 111 | odometry = Eigen::Matrix4f::Identity(); 112 | nextGuess = Eigen::Matrix4f::Identity(); 113 | updateLocalMap(filter, Eigen::Matrix4f::Identity()); 114 | return odometry; 115 | } 116 | ndt.setInputSource(filter); 117 | auto e1 = std::chrono::steady_clock::now(); 118 | ndt.align(*align, nextGuess); 119 | auto e2 = std::chrono::steady_clock::now(); 120 | 121 | 122 | Eigen::Matrix4f delta = odometry.inverse() * ndt.getFinalTransformation(); 123 | odometry = ndt.getFinalTransformation(); 124 | nextGuess = odometry * delta; 125 | 126 | Eigen::Matrix4f distance = local_map_pose.back().inverse() * odometry; 127 | float kf_distance; 128 | node_->get_parameter("odom/kf_distance", kf_distance); 129 | 130 | if (fabs(distance(0, 3)) + fabs(distance(1, 3)) > kf_distance) 131 | {// new key frame 132 | updateLocalMap(filter, odometry); 133 | std::cout<<" add new key frame !"; 134 | } 135 | auto e3 = std::chrono::steady_clock::now(); 136 | 137 | //std::cout<<" total cost: "<(e3 - s).count()<<" ms align cost: "<(e2 - e1).count()<<" ms "; 138 | 139 | // std::cout<<" x: "<::Ptr laserOdometry::downSample(const pcl::PointCloud::Ptr& cloud, float resolution) 148 | { 149 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud); 150 | 151 | pcl::ApproximateVoxelGrid voxelGrid; 152 | voxelGrid.setLeafSize(resolution, resolution, resolution); 153 | voxelGrid.setInputCloud(cloud); 154 | voxelGrid.filter(*filtered); 155 | return filtered; 156 | } 157 | 158 | void laserOdometry::updateLocalMap(const pcl::PointCloud::Ptr& kf, const Eigen::Matrix4f& pose) 159 | { 160 | size_t local_map_size; 161 | node_->get_parameter("odom/local_map_size", local_map_size); 162 | local_map.push_back(kf); 163 | local_map_pose.push_back(pose); 164 | while (local_map.size() > local_map_size) 165 | { 166 | local_map.pop_front(); 167 | local_map_pose.pop_front(); 168 | } 169 | local_map_cloud.reset(new pcl::PointCloud()); 170 | pcl::PointCloud transformed; 171 | 172 | for (size_t i = 0;i < local_map.size(); ++i) 173 | { 174 | pcl::transformPointCloud(*local_map.at(i), transformed, local_map_pose.at(i)); 175 | *local_map_cloud += transformed; 176 | //std::cout<<" add key frame "<get_parameter("ndt/local_map_resolution", local_map_resolution); 186 | auto filter = downSample(local_map_cloud, local_map_resolution); 187 | ndt.setInputTarget(local_map_cloud); 188 | } 189 | } 190 | -------------------------------------------------------------------------------- /src/localization/UKF.cpp: -------------------------------------------------------------------------------- 1 | #include "localization/UKF.h" 2 | 3 | #include 4 | 5 | namespace laser_localization 6 | { 7 | filter::filter(rclcpp::Node *node): 8 | node_(node) 9 | { 10 | node_->get_parameter("filter/k1", k1_); 11 | pos_.x() = 0; 12 | pos_.y() = 0; 13 | pos_.z() = 0; 14 | } 15 | 16 | Eigen::Vector3f filter::predict(const Eigen::Matrix4f& update) 17 | { 18 | Eigen::Matrix4f p = Eigen::Matrix4f::Identity(); 19 | p.block<3,1>(0,3) = pos_; 20 | p.block<3,3>(0,0) = angular_.toRotationMatrix(); 21 | p = p * update; 22 | pos_ = p.block<3,1>(0,3); 23 | angular_ = Eigen::Quaternionf(p.block<3,3>(0,0)); 24 | } 25 | 26 | Eigen::Vector3f filter::update(const Eigen::Vector3f& p, const Eigen::Quaternionf& a) 27 | { 28 | pos_ = pos_*k1_ + p * (1 - k1_); 29 | angular_ = angular_.coeffs() * k1_ + a.coeffs() * (1 - k1_); 30 | angular_ = angular_.normalized(); 31 | } 32 | 33 | void filter::set_pos(const Eigen::Vector3f& p, const Eigen::Quaternionf& a) 34 | { 35 | pos_ = p; 36 | angular_ = a; 37 | } 38 | 39 | Eigen::Matrix4f filter::get_pos() 40 | { 41 | Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); 42 | T.block<3, 1>(0, 3) = pos_; 43 | T.block<3, 3>(0, 0) = angular_.toRotationMatrix(); 44 | return T; 45 | } 46 | } -------------------------------------------------------------------------------- /src/localization/localization.cpp: -------------------------------------------------------------------------------- 1 | #include "localization/localization.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "localization/tools.h" 10 | 11 | namespace laser_localization 12 | { 13 | localization::localization(rclcpp::Node *node): 14 | estimate_(filter(node)), node_(node) 15 | { 16 | Zero.rotation.w= 1; 17 | Zero.rotation.x= 0; 18 | Zero.rotation.y= 0; 19 | Zero.rotation.z= 0; 20 | Zero.translation.x = 0; 21 | Zero.translation.y = 0; 22 | Zero.translation.z = 0; 23 | pos_ = Zero; 24 | 25 | // init 26 | map_points_ = boost::make_shared>(); 27 | std::string global_map; 28 | node_->get_parameter("global_map", global_map); 29 | pcl::io::loadPCDFile(global_map, *map_points_); 30 | 31 | float global_resolution, global_view_resolution; 32 | node_->get_parameter("global_resolution", global_resolution); 33 | node_->get_parameter("global_view_resolution", global_view_resolution); 34 | 35 | map_points_ = downSample(map_points_, global_resolution); 36 | map_points_view_ = downSample(map_points_, global_view_resolution); 37 | 38 | ndt_ = create_registration(); 39 | ndt_->setInputTarget(map_points_); 40 | 41 | aligned_ = pcl::PointCloud::Ptr(new pcl::PointCloud()); 42 | local_map_points_ = pcl::PointCloud::Ptr(new pcl::PointCloud()); 43 | } 44 | 45 | void localization::update_local_map(const Eigen::Matrix4f& pose) 46 | { 47 | static bool init = false; 48 | double distance; 49 | node_->get_parameter("local_map_size", distance); 50 | Eigen::Vector2f delta = (pose.block<2, 1>(0, 3) - local_map_pose_.block<2, 1>(0, 3)); 51 | if (delta.norm() < distance / 3 && init) 52 | return; 53 | local_map_pose_ = pose; 54 | local_map_points_->points.clear(); 55 | local_map_points_->points.reserve(map_points_->points.size()); 56 | for (const auto& pt:map_points_->points){ 57 | if (std::abs(pt.x - pose(0, 3)) + std::abs(pt.y - pose(1, 3)) < distance){ 58 | local_map_points_->points.emplace_back(pt); 59 | } 60 | } 61 | ndt_ = create_registration(); 62 | ndt_->setInputTarget(map_points_); 63 | init = true; 64 | } 65 | 66 | // odom 67 | void localization::update_pos(const geometry_msgs::msg::Twist::SharedPtr& cmd, float dt) 68 | { 69 | static float yaw = 0; 70 | pos_.translation.x = (pos_.translation.x + cmd->linear.x * cos(yaw) * dt - cmd->linear.y * sin(yaw) * dt); 71 | pos_.translation.y = (pos_.translation.y + cmd->linear.x * sin(yaw) * dt + cmd->linear.y * cos(yaw) * dt); 72 | yaw += (float)cmd->angular.z * dt; 73 | Eigen::Quaternionf q(Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ())); 74 | pos_.rotation.x = q.x(); 75 | pos_.rotation.y = q.y(); 76 | pos_.rotation.z = q.z(); 77 | pos_.rotation.w = q.w(); 78 | } 79 | 80 | void localization::predict(const Eigen::Matrix4f& update) 81 | { 82 | estimate_.predict(update); 83 | } 84 | 85 | bool localization::correct(pcl::PointCloud::Ptr cloud, int times) 86 | { 87 | std::cout<<"start correcting !!!"<get_parameter("global_frame_resolution", resolution); 91 | cloud = downSample(cloud, resolution); 92 | if (cloud->points.size() < 400) 93 | return true; 94 | // 1. predict laser pos 95 | Eigen::Matrix4f predict_pos = estimate_.get_pos(); 96 | // 2. laser match 97 | Eigen::Matrix4f laser_pos = ndt_match(std::move(cloud), predict_pos, times); 98 | // 3. base_link pos 99 | Eigen::Matrix4f base_pos = laser_pos; 100 | Eigen::Quaternionf q(base_pos.block<3,3>(0,0)); 101 | // 4. update filter, get estimate pos 102 | if (ndt_->getFitnessScore() < 5){ 103 | estimate_.update(base_pos.block<3, 1>(0, 3), q); 104 | // 5. calculate map -->odom pos 105 | auto filter_pos = estimate_.get_pos(); 106 | Eigen::Matrix4f map_odom = (filter_pos * inv_odom_base_); 107 | Eigen::Quaternionf q1(map_odom.block<3,3>(0,0)); 108 | pos_.translation.x = map_odom(0, 3); 109 | pos_.translation.y = map_odom(1, 3); 110 | pos_.translation.z = map_odom(2, 3); 111 | pos_.rotation.x = q1.x(); 112 | pos_.rotation.y = q1.y(); 113 | pos_.rotation.z = q1.z(); 114 | pos_.rotation.w = q1.w(); 115 | std::cout<<"end correcting !!!"<getFitnessScore()<(0, 0); 130 | Eigen::Quaternionf q(R); 131 | estimate_.set_pos(laser_base.block<3, 1>(0 ,3), q); 132 | 133 | auto filter_pos = estimate_.get_pos(); 134 | 135 | Eigen::Matrix4f map_odom = (filter_pos * inv_odom_base_); 136 | Eigen::Quaternionf q1(map_odom.block<3,3>(0,0)); 137 | pos_.translation.x = map_odom(0, 3); 138 | pos_.translation.y = map_odom(1, 3); 139 | pos_.translation.z = map_odom(2, 3); 140 | pos_.rotation.x = q1.x(); 141 | pos_.rotation.y = q1.y(); 142 | pos_.rotation.z = q1.z(); 143 | pos_.rotation.w = q1.w(); 144 | } 145 | 146 | 147 | Eigen::Matrix4f localization::ndt_match(pcl::PointCloud::ConstPtr cloud, Eigen::Matrix4f gauss, int times) 148 | { 149 | ndt_->setMaximumIterations(times); 150 | ndt_->setInputSource(cloud); 151 | ndt_->align(*aligned_, gauss); 152 | if (ndt_->getFitnessScore() > 3){ 153 | std::cout<<" aligned score "<getFitnessScore()<<" re matching "<setMaximumIterations(times*2); 155 | ndt_->align(*aligned_, gauss); 156 | } 157 | // pcl::visualization::PCLVisualizer viewer("pcd viewer"); 158 | // pcl::visualization::PointCloudColorHandlerCustom c1(aligned_, 0, 255, 0); 159 | // pcl::visualization::PointCloudColorHandlerCustom c2(map_points_, 255, 0, 0); 160 | // viewer.addPointCloud(map_points_, c2, "map"); 161 | // viewer.addPointCloud(aligned_, c1, "aligned"); 162 | // viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "aligned"); 163 | // viewer.spin(); 164 | return ndt_->getFinalTransformation(); 165 | } 166 | 167 | pcl::Registration::Ptr localization::create_registration() 168 | { 169 | float resolution, step_size, epsilon; 170 | node_->get_parameter("localization/ndt_resolution", resolution); 171 | node_->get_parameter("localization/ndt_step_size", step_size); 172 | node_->get_parameter("localization/ndt_epsilon", epsilon); 173 | 174 | pcl::NormalDistributionsTransform::Ptr ndt(new pcl::NormalDistributionsTransform()); 175 | ndt->setResolution(resolution); 176 | ndt->setStepSize(step_size); 177 | ndt->setTransformationEpsilon(epsilon); 178 | return ndt; 179 | } 180 | } 181 | 182 | -------------------------------------------------------------------------------- /src/localization/tools.cpp: -------------------------------------------------------------------------------- 1 | #include "localization/tools.h" 2 | 3 | bool read_position(const std::string& file, Eigen::Matrix4f& base) 4 | { 5 | struct stat buffer{}; 6 | if(stat (file.c_str(), &buffer) == 0) 7 | { 8 | int size = boost::filesystem::file_size(file); 9 | if(boost::filesystem::file_size(file) > 80) 10 | { 11 | std::ifstream readFile(file); 12 | for(int i = 0;i < 16; i++) 13 | { 14 | std::string s; 15 | getline(readFile, s, ','); 16 | base(i) = atof(s.c_str()); 17 | } 18 | return true; 19 | } 20 | else 21 | { 22 | return false; 23 | } 24 | } 25 | else 26 | { 27 | auto cmd = "touch " + file; 28 | system(cmd.c_str()); 29 | return false; 30 | } 31 | } 32 | 33 | void write_position(const std::string& file,const Eigen::Matrix4f& position) 34 | { 35 | std::ofstream out; 36 | out.open(file.c_str()); 37 | for(int i = 0;i < 16;i ++) 38 | { 39 | out<::Ptr downSample(pcl::PointCloud::ConstPtr cloud, double resolution) 46 | { 47 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud); 48 | pcl::ApproximateVoxelGrid voxelGrid; 49 | voxelGrid.setLeafSize(resolution, resolution, resolution); 50 | voxelGrid.setInputCloud(cloud); 51 | voxelGrid.filter(*filtered); 52 | return filtered; 53 | } 54 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "localization/localization.h" 2 | #include "global_localization.h" 3 | #include 4 | #include 5 | #include 6 | #include "rclcpp/rclcpp.hpp" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "eigen3/Eigen/Core" 15 | #include "unistd.h" 16 | #include "thread" 17 | #include "fstream" 18 | #include "laser_odometry.h" 19 | #include "localization/tools.h" 20 | #include "std_srvs/srv/set_bool.hpp" 21 | 22 | 23 | using namespace std::chrono_literals; 24 | 25 | class Publisher : public rclcpp::Node 26 | { 27 | using PointT = pcl::PointXYZI; 28 | public: 29 | Publisher() 30 | :Node("my_localization") 31 | { 32 | // params 33 | this->declare_parameter("bbs/max_range", 15.0); 34 | this->declare_parameter("bbs/min_tx", -50.0); 35 | this->declare_parameter("bbs/max_tx", 50.0); 36 | this->declare_parameter("bbs/min_ty", -50.0); 37 | this->declare_parameter("bbs/max_ty", 50.0); 38 | this->declare_parameter("bbs/min_theta", -3.14); 39 | this->declare_parameter("bbs/max_theta", 3.14); 40 | this->declare_parameter("bbs/map_min_z", -0.2); 41 | this->declare_parameter("bbs/map_max_z", 1.2); 42 | this->declare_parameter("bbs/map_width", 512); 43 | this->declare_parameter("bbs/map_height", 512); 44 | this->declare_parameter("bbs/map_resolution", 0.5); 45 | this->declare_parameter("bbs/max_points_pre_cell", 5); 46 | this->declare_parameter("bbs/map_pyramid_level", 6); 47 | this->declare_parameter("bbs/scan_min_z", -0.2); 48 | this->declare_parameter("bbs/scan_max_z", 1.2); 49 | this->declare_parameter("bbs/map_filter_resolution", 0.5); 50 | this->declare_parameter("bbs/scan_filter_resolution", 0.5); 51 | this->declare_parameter("global_map_width", 100.0); 52 | this->declare_parameter("global_map_height", 100.0); 53 | 54 | this->declare_parameter("ndt/resolution", 0.8); 55 | this->declare_parameter("ndt/step_size", 0.2); 56 | this->declare_parameter("ndt/epsilon", 0.01); 57 | this->declare_parameter("ndt/max_iterations", 30.0); 58 | this->declare_parameter("ndt/frame_resolution", 0.5); 59 | this->declare_parameter("odom/kf_distance", 2.0); 60 | this->declare_parameter("odom/local_map_size", 10); 61 | this->declare_parameter("ndt/local_map_resolution", 0.5); 62 | 63 | this->declare_parameter("localization/mode", 2); 64 | this->declare_parameter("global_map", "../map/kitti.pcd"); 65 | this->declare_parameter("odom_frame", "odom"); 66 | this->declare_parameter("base_link_frame", "base_link"); 67 | this->declare_parameter("laser_frame", "os_sensor"); 68 | this->declare_parameter("laser_topic", "/points"); 69 | this->declare_parameter("pose_save", "pos.txt"); 70 | this->declare_parameter("correct_count", 5); 71 | this->declare_parameter("global_resolution", 1.3); 72 | this->declare_parameter("global_frame_resolution", 1); 73 | this->declare_parameter("global_view_resolution", 3); 74 | this->declare_parameter("local_map_size", 100); 75 | 76 | this->declare_parameter("localization/ndt_resolution", 1); 77 | this->declare_parameter("localization/ndt_step_size", 0.1); 78 | this->declare_parameter("localization/ndt_epsilon", 0.01); 79 | this->declare_parameter("initial_pose_save", "/home/sa/code/robot_ws/src/inspection_3d_robot/laser_localization/map/init_pos.txt"); 80 | 81 | localization_ = (new laser_localization::localization(this)); 82 | global_localization_ = (new global_localization::GlobalLocalizationBBS(this)); 83 | odometry = new laserOdometry(this); 84 | 85 | // localization mode 86 | this->get_parameter("localization/mode", location_mode_); 87 | this->get_parameter("odom_frame", odom_frame); 88 | this->get_parameter("base_link_frame", base_link_frame); 89 | this->get_parameter("laser_frame", laser_frame); 90 | std::string map_path; 91 | this->get_parameter("global_map", map_path); 92 | this->get_parameter("correct_count", correct_count); 93 | 94 | // global localization 95 | pcl::PointCloud::Ptr map(new pcl::PointCloud); 96 | if (pcl::io::loadPCDFile(map_path, *map) == -1) 97 | { 98 | PCL_ERROR("Couldn't read file test_pcd.pcd \n"); 99 | } 100 | global_localization_->set_global_map(map); 101 | 102 | using std::placeholders::_1; 103 | tf_broadcaster_ = std::make_shared(*this); 104 | buffer_ = std::make_shared(this->get_clock()); 105 | tf_listener_ = std::make_shared(*buffer_); 106 | 107 | // ros2 service call /set_init_pose std_srvs/srv/SetBool "{data: true}" 108 | initial_service_ = this->create_service( 109 | "set_init_pose", 110 | [this](const std::shared_ptr request, 111 | std::shared_ptr response){ 112 | set_initial_position(); 113 | response->success = true; 114 | response->message = "set initial position !!!"; 115 | }); 116 | 117 | // ros2 service call /relocalization std_srvs/srv/SetBool "{data: true}" 118 | relocalization_service_ = this->create_service( 119 | "relocalization", 120 | [this](const std::shared_ptr request, 121 | std::shared_ptr response){ 122 | manual_relocalization = true; 123 | std::string save_txt; 124 | this->get_parameter("initial_pose_save", save_txt); 125 | // wait for new frames 126 | while (correct_frames.empty()){ 127 | std::cout<<"wait frames "<data && global_localization_->globalLocalization(frame, save_txt, T)){ 142 | localization_->set_inv_odom_base(frame_odom.inverse()); 143 | localization_->init_localization(T); 144 | std::cout<<"global localization success !!!"<message = "relocalization position success!!!"; 147 | }else { 148 | std::cout<<" re localization again "<globalLocalization(frame, T)){ 150 | localization_state = 2; 151 | response->message = "relocalization position failed!!!"; 152 | }else{ 153 | localization_->set_inv_odom_base(frame_odom.inverse()); 154 | localization_->init_localization(T); 155 | std::cout<<"global localization success !!!"<message = "relocalization position success!!!"; 157 | localization_state = 1; 158 | } 159 | } 160 | localization_->update_local_map(T); 161 | manual_relocalization = false; 162 | }); 163 | 164 | if(location_mode_ == 3){// fake localization 165 | subscription_ = this->create_subscription( 166 | "cmd_vel", 10, std::bind(&Publisher::cmd_callback, this, _1)); 167 | }else if (location_mode_ == 1 || location_mode_ == 2){ 168 | int try_cnt = 0; 169 | while (rclcpp::ok()){ 170 | try{ 171 | auto pos = buffer_->lookupTransform( 172 | base_link_frame, 173 | laser_frame, tf2::TimePointZero); 174 | laser_base_ = transform2Matrix(pos); 175 | break; 176 | } 177 | catch (tf2::TransformException &ex){ 178 | RCLCPP_INFO(this->get_logger(),"Could not transform %s", 179 | ex.what()); 180 | } 181 | sleep(1); 182 | try_cnt ++; 183 | if (try_cnt > 10) { 184 | laser_base_ = Eigen::Matrix4f::Identity(); 185 | break; 186 | } 187 | } 188 | std::string topic; 189 | this->get_parameter("laser_topic", topic); 190 | laser_sub_ = this->create_subscription( 191 | topic, rclcpp::SensorDataQoS(), std::bind(&Publisher::laser_callback, this, _1)); 192 | 193 | if (location_mode_ == 2){ 194 | while (rclcpp::ok()) 195 | { 196 | try{ 197 | auto pos = buffer_->lookupTransform( 198 | odom_frame, 199 | base_link_frame, 200 | tf2::TimePointZero); 201 | last_trans_ = transform2Matrix(pos); 202 | break; 203 | } 204 | catch (tf2::TransformException &ex){ 205 | RCLCPP_INFO(this->get_logger(),"Could not transform %s", 206 | ex.what()); 207 | } 208 | sleep(1); 209 | odom_ready = true; 210 | } 211 | } 212 | } 213 | map_pub_ = this->create_publisher("/map", 10); 214 | aligned_pub_ = this->create_publisher("/aligned", 10); 215 | 216 | // 5 hz 217 | timer_ = this->create_wall_timer(100ms, [this] { timer_callback();}); 218 | // start thread 219 | t1 = std::make_shared(&Publisher::laser_thread, this); 220 | } 221 | private: 222 | 223 | void laser_thread() 224 | { 225 | Eigen::Matrix4f last_pos = Eigen::Matrix4f::Identity(); 226 | std::string save_txt; 227 | this->get_parameter("pose_save", save_txt); 228 | while (true){ 229 | // if (!laser_ready) 230 | // continue; 231 | // if (location_mode_ == 2 && !odom_ready) 232 | // continue; 233 | if(!correct_frames.empty() && !manual_relocalization){ 234 | lock_.lock(); 235 | auto frame = correct_frames.front(); 236 | Eigen::Matrix4f frame_odom = correct_frames_pose.front(); 237 | Eigen::Matrix4f odom; 238 | if (location_mode_ == 2){odom = odom_pose.front();} 239 | lock_.unlock(); 240 | 241 | if (localization_state == 0){ 242 | std::cout<<"start global localization "<globalLocalization(frame, save_txt, T)){ 246 | if (location_mode_ == 2){ 247 | localization_->set_inv_odom_base(odom.inverse()); 248 | }else { 249 | localization_->set_inv_odom_base(frame_odom.inverse()); 250 | } 251 | localization_->init_localization(T); 252 | last_pos = frame_odom; 253 | std::cout<<"global localization success !!!"<globalLocalization(frame, T)){ 258 | localization_state = 2; 259 | }else{ 260 | if (location_mode_ == 2){ 261 | localization_->set_inv_odom_base(odom.inverse()); 262 | }else { 263 | localization_->set_inv_odom_base(frame_odom.inverse()); 264 | } 265 | localization_->init_localization(T); 266 | last_pos = frame_odom; 267 | std::cout<<"global localization success !!!"<update_local_map(T); 272 | } 273 | if (localization_state == 1){ 274 | 275 | localization_->update_local_map(localization_->get_estimate_pos()); 276 | auto s = std::chrono::system_clock::now(); 277 | // predict 278 | Eigen::Matrix4f delta = last_pos.inverse() * frame_odom; 279 | localization_->predict(delta); 280 | last_pos = frame_odom; 281 | // set odom 282 | if (location_mode_ == 2){ 283 | localization_->set_inv_odom_base(odom.inverse()); 284 | }else { 285 | localization_->set_inv_odom_base(frame_odom.inverse()); 286 | } 287 | // correct 288 | if (!localization_->correct(frame, 30)){ 289 | localization_state = 2; 290 | } 291 | auto e = std::chrono::system_clock::now(); 292 | std::cout<<"cost "<(e-s).count()<<" ms"<get_estimate_pos(); 296 | Eigen::Matrix4f T; 297 | if (global_localization_->globalLocalization(frame, predict, T)){ 298 | if (location_mode_ == 2){ 299 | localization_->set_inv_odom_base(odom.inverse()); 300 | }else { 301 | localization_->set_inv_odom_base(frame_odom.inverse()); 302 | } 303 | localization_->init_localization(T); 304 | last_pos = frame_odom; 305 | std::cout<<"global localization success !!!"<globalLocalization(frame, T)){ 310 | localization_state = 2; 311 | }else{ 312 | if (location_mode_ == 2){ 313 | localization_->set_inv_odom_base(odom.inverse()); 314 | }else { 315 | localization_->set_inv_odom_base(frame_odom.inverse()); 316 | } 317 | localization_->init_localization(T); 318 | last_pos = frame_odom; 319 | std::cout<<"global localization success !!!"<get_estimate_pos()); 325 | lock_.lock(); 326 | correct_frames_pose.pop(); 327 | correct_frames.pop(); 328 | if (location_mode_ == 2){odom_pose.pop();} 329 | lock_.unlock(); 330 | } 331 | else 332 | { 333 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 334 | } 335 | } 336 | } 337 | 338 | 339 | void timer_callback() 340 | { 341 | static int cnt = 0; 342 | if(cnt%100 == 0) 343 | {// publish map 344 | sensor_msgs::msg::PointCloud2 map; 345 | pcl::toROSMsg(*localization_->get_view_map(), map); 346 | map.header.stamp = this->get_clock()->now(); 347 | map.header.frame_id = "map"; 348 | map_pub_->publish(map); 349 | } 350 | if(cnt%3 == 0 && localization_->get_aligned() != nullptr) 351 | { 352 | sensor_msgs::msg::PointCloud2 map; 353 | if(localization_->get_aligned().get()->points.size() == localization_->get_aligned().get()->width * localization_->get_aligned().get()->height) 354 | { 355 | pcl::toROSMsg(*localization_->get_aligned(), map); 356 | map.header.stamp = this->get_clock()->now(); 357 | map.header.frame_id = "map"; 358 | aligned_pub_->publish(map); 359 | } 360 | } 361 | 362 | cnt ++; 363 | } 364 | 365 | 366 | void publish_pos(geometry_msgs::msg::Transform pos) 367 | { 368 | geometry_msgs::msg::TransformStamped t; 369 | t.header.stamp = this->get_clock()->now(); 370 | t.header.frame_id = "map"; 371 | t.child_frame_id = "odom"; 372 | t.transform = pos; 373 | if (localization_state != 1){ 374 | t.transform.translation.z = 4; 375 | } 376 | // Send the transformation 377 | tf_broadcaster_->sendTransform(t); 378 | } 379 | 380 | void cmd_callback(const geometry_msgs::msg::Twist::SharedPtr msg) 381 | { 382 | static auto last_time = std::chrono::steady_clock::now(); 383 | auto now = std::chrono::steady_clock::now(); 384 | float dt = (float)std::chrono::duration_cast(now - last_time).count(); 385 | static auto Zero = localization_->get_pos(); 386 | localization_->update_pos(msg, dt / 1000000.f); 387 | last_time = now; 388 | 389 | // 发布定位信息 390 | { 391 | geometry_msgs::msg::TransformStamped t; 392 | t.header.stamp = this->get_clock()->now(); 393 | t.header.frame_id = "odom"; 394 | t.child_frame_id = "base_link"; 395 | t.transform = localization_->get_pos(); 396 | // Send the transformation 397 | tf_broadcaster_->sendTransform(t); 398 | } 399 | { 400 | geometry_msgs::msg::TransformStamped t; 401 | t.header.stamp = this->get_clock()->now(); 402 | t.header.frame_id = "map"; 403 | t.child_frame_id = "odom"; 404 | t.transform = Zero; 405 | // Send the transformation 406 | tf_broadcaster_->sendTransform(t); 407 | } 408 | } 409 | 410 | Eigen::Matrix4f transform2Matrix(const geometry_msgs::msg::TransformStamped& trans) 411 | { 412 | Eigen::Matrix4f M = Eigen::Matrix4f::Identity(); 413 | Eigen::Quaternionf q; 414 | 415 | M(0, 3) = (float)trans.transform.translation.x; 416 | M(1, 3) = (float)trans.transform.translation.y; 417 | M(2, 3) = (float)trans.transform.translation.z; 418 | q.x() = (float)trans.transform.rotation.x; 419 | q.y() = (float)trans.transform.rotation.y; 420 | q.z() = (float)trans.transform.rotation.z; 421 | q.w() = (float)trans.transform.rotation.w; 422 | M.block<3,3>(0,0) = q.toRotationMatrix(); 423 | 424 | return M; 425 | } 426 | 427 | 428 | // laser odom 429 | void laser_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 430 | { 431 | if (!laser_ready) 432 | laser_ready = true; 433 | // std::cout<<"get laser points "<::Ptr pcl_cloud(new pcl::PointCloud); 438 | // to pcl points 439 | pcl::fromROSMsg(*msg, *pcl_cloud); 440 | if(pcl_cloud->empty()) { 441 | std::cerr<<"cloud is empty"<points.size()<addFrame(pcl_cloud); 454 | odom = odom * laser_base_; 455 | // 发布定位信息 456 | geometry_msgs::msg::TransformStamped t; 457 | t.header.stamp = this->get_clock()->now(); 458 | t.header.frame_id = odom_frame; 459 | t.child_frame_id = base_link_frame; 460 | geometry_msgs::msg::Transform transform; 461 | Eigen::Quaternionf q1(odom.block<3,3>(0,0)); 462 | transform.translation.x = odom(0, 3); 463 | transform.translation.y = odom(1, 3); 464 | transform.translation.z = odom(2, 3); 465 | transform.rotation.x = q1.x(); 466 | transform.rotation.y = q1.y(); 467 | transform.rotation.z = q1.z(); 468 | transform.rotation.w = q1.w(); 469 | t.transform = transform; 470 | // Send the transformation 471 | tf_broadcaster_->sendTransform(t); 472 | }else if (location_mode_ == 2){ 473 | // with odometry 474 | // 1. get last pos --> current pos 475 | geometry_msgs::msg::TransformStamped trans; 476 | while(!buffer_->canTransform( 477 | odom_frame, 478 | base_link_frame, tf2::TimePointZero));//tf2::TimePoint(std::chrono::nanoseconds(msg->header.stamp.nanosec) + std::chrono::seconds(msg->header.stamp.sec)))); 479 | try{ 480 | trans = buffer_->lookupTransform( 481 | odom_frame, 482 | base_link_frame, tf2::TimePointZero);//tf2::TimePoint(std::chrono::nanoseconds(msg->header.stamp.nanosec) + std::chrono::seconds(msg->header.stamp.sec))); 483 | current_pos = transform2Matrix(trans); 484 | } 485 | catch (tf2::TransformException &ex){ 486 | RCLCPP_INFO(this->get_logger(),"Could not transform %s", 487 | ex.what()); 488 | } 489 | bool accept = false; 490 | // std::cout<<"current odom pose: "<addFrame(pcl_cloud, delta_pos, accept); 493 | // std::cout<<"delta_pos "< correct_count && correct_frames.size() < 2){ 498 | lock_.lock(); 499 | correct_frames.push(pcl_cloud); 500 | correct_frames_pose.push(odom); 501 | if (location_mode_ == 2){ 502 | odom_pose.emplace(current_pos); 503 | } 504 | lock_.unlock(); 505 | cnt = 0; 506 | } 507 | cnt ++; 508 | } 509 | auto e = std::chrono::system_clock::now(); 510 | //std::cout<<"frame time "<<(float)std::chrono::duration_cast(e - s).count()/1000.f<<" s"< odom 513 | publish_pos(localization_->get_pos()); 514 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 515 | } 516 | 517 | void set_initial_position(){ 518 | std::string save_txt; 519 | this->get_parameter("initial_pose_save", save_txt); 520 | write_position(save_txt, localization_->get_estimate_pos()); 521 | } 522 | 523 | std::string odom_frame = "odom"; 524 | std::string base_link_frame = "base_link"; 525 | std::string laser_frame = "os_sensor"; 526 | 527 | Eigen::Matrix4f last_trans_; 528 | Eigen::Matrix4f laser_base_; 529 | global_localization::GlobalLocalizationBBS* global_localization_; 530 | laserOdometry* odometry; 531 | rclcpp::TimerBase::SharedPtr timer_; 532 | std::shared_ptr tf_broadcaster_; 533 | std::shared_ptr tf_listener_; 534 | std::shared_ptr buffer_; 535 | // 订阅cmd,用于测试 536 | rclcpp::Subscription::SharedPtr subscription_; 537 | // 订阅激光雷达数据 538 | rclcpp::Subscription::SharedPtr laser_sub_; 539 | 540 | // 订阅里程计数据 541 | rclcpp::Subscription::SharedPtr odom_sub_; 542 | // 获取激光雷达与里程计的相对位置关系 543 | rclcpp::Publisher::SharedPtr map_pub_; 544 | rclcpp::Publisher::SharedPtr aligned_pub_; 545 | laser_localization::localization* localization_; 546 | 547 | std::queue::Ptr> correct_frames; 548 | std::queue correct_frames_pose; 549 | std::queue odom_pose; 550 | std::shared_ptr t1; 551 | std::mutex lock_; 552 | // 用于调试模式，利用cmd_vel指令推测当前位置，用于测试路径规划算法 553 | int location_mode_ = 1; 554 | 555 | // 0 wait initial 556 | // 1 normal 557 | // 2 error 558 | int localization_state = 0; 559 | int correct_count = 10; 560 | 561 | bool manual_relocalization = false; 562 | 563 | // service 564 | rclcpp::Service::SharedPtr initial_service_; 565 | rclcpp::Service::SharedPtr relocalization_service_; 566 | 567 | bool odom_ready = false; 568 | bool laser_ready = false; 569 | }; 570 | 571 | // read msg then publish to ros2 572 | int main(int argc, char * argv[]) 573 | { 574 | rclcpp::init(argc, argv); 575 | auto node = std::make_shared(); 576 | rclcpp::spin(node); 577 | rclcpp::shutdown(); 578 | return 0; 579 | } 580 | 581 | 582 | 583 | 584 | 585 | -------------------------------------------------------------------------------- /test/read_map.cpp: -------------------------------------------------------------------------------- 1 | #include //标准输入输出流 2 | #include //PCL的PCD格式文件的输入输出头文件 3 | #include //PCL的ply格式文件的输入输出头文件 4 | #include //PCL对各种格式的点的支持头文件 5 | #include 6 | #include "unistd.h" 7 | 8 | int main(int argc, char** argv) 9 | { 10 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); // 创建点云（指针） 11 | 12 | if (pcl::io::loadPCDFile("../map/map-old.pcd", *cloud) == -1) //* 读入PCD格式的文件，如果文件不存在，返回-1 13 | { 14 | PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //文件不存在时，返回错误，终止程序。 15 | return (-1); 16 | } 17 | std::cout << "Loaded " 18 | << cloud->width * cloud->height 19 | << " data points from test_file.pcd with the following fields: " 20 | << std::endl; 21 | // for (auto & point : cloud->points) //显示所有的点 22 | // std::cout << " " << point.x 23 | // << " " << point.y 24 | // << " " << point.z << std::endl; 25 | pcl::visualization::CloudViewer viewer("pcd viewer"); 26 | viewer.showCloud(cloud); 27 | sleep(-1); 28 | return (0); 29 | } 30 | -------------------------------------------------------------------------------- /test/test_error.cpp: -------------------------------------------------------------------------------- 1 | #include "error_estimater/error_estimater.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "rclcpp/rclcpp.hpp" 8 | 9 | using namespace std::chrono_literals; 10 | class Publisher : public rclcpp::Node 11 | { 12 | public: 13 | Publisher(): Node("test_laser"), error_(new match_error::MatchingCostEvaluaterFlann()) 14 | { 15 | pcl::PointCloud::Ptr map(new pcl::PointCloud); 16 | if (pcl::io::loadPCDFile("../map/kitti.pcd", *map) == -1) 17 | { 18 | PCL_ERROR("Couldn't read file test_pcd.pcd \n"); 19 | } 20 | map = downSample(map, 0.3); 21 | std::string path = "/media/lyc/软件/1-10/05/velodyne/"; 22 | std::vector file_name = getKittiData(path); 23 | 24 | for (int i = 200;i < 800;i += 40){ 25 | 26 | auto frame = readKittiBinData(file_name.at(i)); 27 | frame = downSample(frame, 0.3); 28 | 29 | Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); 30 | 31 | error_->set_target(map, 0.3); 32 | auto s = std::chrono::system_clock::now(); 33 | double error = error_->calc_matching_error(*frame, T, nullptr); 34 | auto e = std::chrono::system_clock::now(); 35 | std::cout<<"cost : "<(e-s).count()<<" ms"<::Ptr downSample(pcl::PointCloud::Ptr cloud, double resolution) 44 | { 45 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud); 46 | pcl::ApproximateVoxelGrid voxelGrid; 47 | voxelGrid.setLeafSize(resolution, resolution, resolution); 48 | voxelGrid.setInputCloud(cloud); 49 | voxelGrid.filter(*filtered); 50 | return filtered; 51 | } 52 | std::vector getKittiData(const std::string &path) 53 | { 54 | std::vector names; 55 | int num = getFileNum(path); 56 | 57 | for (int i = 0;i < num;i ++) 58 | { 59 | std::stringstream ss; 60 | ss << setw(6) << setfill('0') << i; 61 | names.emplace_back(path + ss.str() + ".bin"); 62 | } 63 | return names; 64 | } 65 | pcl::PointCloud::Ptr readKittiBinData(std::string &in_file) 66 | { 67 | std::fstream input(in_file.c_str(), ios::in | ios::binary); 68 | if (!input.good()) 69 | { 70 | std::cerr<<"Could not read file: "<< in_file << std::endl; 71 | return nullptr; 72 | } 73 | input.seekg(0, ios::beg); 74 | pcl::PointCloud::Ptr points(new pcl::PointCloud); 75 | int i; 76 | for (i=0; input.good() && !input.eof(); i++) 77 | { 78 | pcl::PointXYZI point; 79 | input.read((char*) &point.x, 3*sizeof(float)); 80 | float intensity = 0; 81 | input.read((char*) &intensity, sizeof(float)); 82 | points->push_back(point); 83 | } 84 | input.close(); 85 | return points; 86 | } 87 | int getFileNum(const std::string &path) 88 | { //需要用到头文件 89 | int fileNum = 0; 90 | DIR *pDir; 91 | struct dirent *ptr; 92 | if (!(pDir = opendir(path.c_str()))) 93 | return fileNum; 94 | while ((ptr = readdir(pDir)) != nullptr) 95 | { 96 | if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) 97 | fileNum++; 98 | } 99 | closedir(pDir); 100 | return fileNum; 101 | } 102 | }; 103 | 104 | int main(int argc, char * argv[]) 105 | { 106 | rclcpp::init(argc, argv); 107 | auto node = std::make_shared(); 108 | rclcpp::spin(node); 109 | rclcpp::shutdown(); 110 | return 0; 111 | } 112 | 113 | -------------------------------------------------------------------------------- /test/test_global_localization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "global_localization.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "localization/tools.h" 10 | 11 | using namespace std::chrono_literals; 12 | class Publisher : public rclcpp::Node 13 | { 14 | public: 15 | Publisher(): Node("test_laser"), global_localization_(new global_localization::GlobalLocalizationBBS(this)) 16 | { 17 | this->declare_parameter("bbs/max_range", 15.0); 18 | this->declare_parameter("bbs/min_tx", -50.0); 19 | this->declare_parameter("bbs/max_tx", 50.0); 20 | this->declare_parameter("bbs/min_ty", -50.0); 21 | this->declare_parameter("bbs/max_ty", 50.0); 22 | this->declare_parameter("bbs/min_theta", -3.14); 23 | this->declare_parameter("bbs/max_theta", 3.14); 24 | this->declare_parameter("bbs/map_min_z", 0.4); 25 | this->declare_parameter("bbs/map_max_z", 1.2); 26 | this->declare_parameter("bbs/map_width", 512); 27 | this->declare_parameter("bbs/map_height", 512); 28 | this->declare_parameter("bbs/map_resolution", 0.5); 29 | this->declare_parameter("bbs/max_points_pre_cell", 5); 30 | this->declare_parameter("bbs/map_pyramid_level", 6); 31 | this->declare_parameter("bbs/scan_min_z", 0.4); 32 | this->declare_parameter("bbs/scan_max_z", 1.2); 33 | this->declare_parameter("bbs/map_filter_resolution", 1); 34 | this->declare_parameter("bbs/scan_filter_resolution", 1); 35 | this->declare_parameter("global_map_width", 400.0); 36 | this->declare_parameter("global_map_height", 400.0); 37 | 38 | pcl::PointCloud::Ptr map(new pcl::PointCloud); 39 | if (pcl::io::loadPCDFile("../map/kitti.pcd", *map) == -1) 40 | { 41 | PCL_ERROR("Couldn't read file test_pcd.pcd \n"); 42 | } 43 | map = downSample(map, 1); 44 | global_localization_->set_global_map(map); 45 | 46 | std::string path = "/media/lyc/软件/1-10/05/velodyne/"; 47 | std::vector file_name = getKittiData(path); 48 | pcl::visualization::PCLVisualizer viewer_v("demo"); 49 | viewer_v.setBackgroundColor(1.0, 1.0, 1.0); 50 | map = downSample(map, 10); 51 | pcl::visualization::PointCloudColorHandlerCustom view_h(map, 0, 255, 0); 52 | viewer_v.addPointCloud(map, view_h, "map"); 53 | Eigen::Matrix4f predict = Eigen::Matrix4f::Identity(); 54 | 55 | for (int i = 100;i < file_name.size() / 2; i += 10) 56 | { 57 | auto frame = readKittiBinData(file_name.at(i)); 58 | Eigen::Matrix4f T; 59 | 60 | global_localization_->globalLocalization(frame, predict, T); 61 | predict = T; 62 | std::cout<<"T "< view_f(frame, 0, 0, 255); 66 | viewer_v.addPointCloud(frame, view_f, "frame"); 67 | }else { 68 | pcl::visualization::PointCloudColorHandlerCustom view_f(frame, 0, 0, 255); 69 | viewer_v.updatePointCloud(frame, view_f, "frame"); 70 | } 71 | 72 | viewer_v.spin(); 73 | } 74 | } 75 | 76 | private: 77 | global_localization::GlobalLocalizationBBS* global_localization_; 78 | 79 | 80 | std::vector getKittiData(const std::string &path) 81 | { 82 | std::vector names; 83 | int num = getFileNum(path); 84 | 85 | for (int i = 0;i < num;i ++) 86 | { 87 | std::stringstream ss; 88 | ss << setw(6) << setfill('0') << i; 89 | names.emplace_back(path + ss.str() + ".bin"); 90 | } 91 | return names; 92 | } 93 | pcl::PointCloud::Ptr readKittiBinData(std::string &in_file) 94 | { 95 | std::fstream input(in_file.c_str(), ios::in | ios::binary); 96 | if (!input.good()) 97 | { 98 | std::cerr<<"Could not read file: "<< in_file << std::endl; 99 | return nullptr; 100 | } 101 | input.seekg(0, ios::beg); 102 | pcl::PointCloud::Ptr points(new pcl::PointCloud); 103 | int i; 104 | for (i=0; input.good() && !input.eof(); i++) 105 | { 106 | pcl::PointXYZI point; 107 | input.read((char*) &point.x, 3*sizeof(float)); 108 | float intensity = 0; 109 | input.read((char*) &intensity, sizeof(float)); 110 | points->push_back(point); 111 | } 112 | input.close(); 113 | return points; 114 | } 115 | int getFileNum(const std::string &path) 116 | { //需要用到头文件 117 | int fileNum = 0; 118 | DIR *pDir; 119 | struct dirent *ptr; 120 | if (!(pDir = opendir(path.c_str()))) 121 | return fileNum; 122 | while ((ptr = readdir(pDir)) != nullptr) 123 | { 124 | if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) 125 | fileNum++; 126 | } 127 | closedir(pDir); 128 | return fileNum; 129 | } 130 | }; 131 | 132 | int main(int argc, char * argv[]) 133 | { 134 | rclcpp::init(argc, argv); 135 | auto node = std::make_shared(); 136 | rclcpp::spin(node); 137 | rclcpp::shutdown(); 138 | return 0; 139 | } 140 | 141 | -------------------------------------------------------------------------------- /test/test_kitti.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "laser_odometry.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "sensor_msgs/msg/point_cloud2.hpp" 11 | #include 12 | #include "geometry_msgs/msg/transform_stamped.hpp" 13 | #include 14 | 15 | using namespace std::chrono_literals; 16 | class Publisher : public rclcpp::Node 17 | { 18 | public: 19 | Publisher() 20 | : Node("test_laser") 21 | { 22 | this->declare_parameter("kitti_path", "/media/lyc/软件/1-10/05/velodyne/"); 23 | 24 | this->get_parameter("kitti_path", path); 25 | file_name = getKittiData(path); 26 | tf_broadcaster_ = std::make_shared(*this); 27 | publisher_ = this->create_publisher("points", rclcpp::SensorDataQoS()); 28 | timer_ = this->create_wall_timer( 29 | 200ms, std::bind(&Publisher::timer_callback, this)); 30 | } 31 | 32 | std::vector getKittiData(const std::string &path) 33 | { 34 | std::vector names; 35 | int num = getFileNum(path); 36 | 37 | for (int i = 0;i < num;i ++) 38 | { 39 | std::stringstream ss; 40 | ss << setw(6) << setfill('0') << i; 41 | names.emplace_back(path + ss.str() + ".bin"); 42 | } 43 | return names; 44 | } 45 | pcl::PointCloud::Ptr readKittiBinData(std::string &in_file) 46 | { 47 | std::fstream input(in_file.c_str(), ios::in | ios::binary); 48 | if (!input.good()) 49 | { 50 | std::cerr<<"Could not read file: "<< in_file << std::endl; 51 | return nullptr; 52 | } 53 | input.seekg(0, ios::beg); 54 | pcl::PointCloud::Ptr points(new pcl::PointCloud); 55 | int i; 56 | for (i=0; input.good() && !input.eof(); i++) 57 | { 58 | pcl::PointXYZI point; 59 | input.read((char*) &point.x, 3*sizeof(float)); 60 | input.read((char*) &point.intensity, sizeof(float)); 61 | points->push_back(point); 62 | } 63 | input.close(); 64 | return points; 65 | } 66 | 67 | std::string path; 68 | std::vector file_name; 69 | private: 70 | void timer_callback() 71 | { 72 | static size_t publish_frame_cnt = cnt; 73 | if (publish_frame_cnt < file_name.size()){ 74 | geometry_msgs::msg::TransformStamped t; 75 | t.header.stamp = this->get_clock()->now(); 76 | t.header.frame_id = "base_link"; 77 | t.child_frame_id = "laser"; 78 | geometry_msgs::msg::Transform transform; 79 | Eigen::Quaternionf q1(Eigen::Matrix3f::Identity()); 80 | transform.translation.x = 0; 81 | transform.translation.y = 0; 82 | transform.translation.z = 0; 83 | transform.rotation.x = q1.x(); 84 | transform.rotation.y = q1.y(); 85 | transform.rotation.z = q1.z(); 86 | transform.rotation.w = q1.w(); 87 | t.transform = transform; 88 | // Send the transformation 89 | tf_broadcaster_->sendTransform(t); 90 | 91 | auto points = readKittiBinData(file_name.at(publish_frame_cnt)); 92 | sensor_msgs::msg::PointCloud2 msg; 93 | pcl::toROSMsg(*points, msg); 94 | msg.header.frame_id = "laser"; 95 | publisher_->publish(msg); 96 | if (play) 97 | publish_frame_cnt ++; 98 | // std::cout<<" publish_frame_cnt "<::SharedPtr publisher_; 103 | std::shared_ptr tf_broadcaster_; 104 | int cnt = 0; 105 | bool play = true; 106 | int getFileNum(const std::string &path) 107 | { //需要用到头文件 108 | int fileNum = 0; 109 | DIR *pDir; 110 | struct dirent *ptr; 111 | if (!(pDir = opendir(path.c_str()))) 112 | return fileNum; 113 | while ((ptr = readdir(pDir)) != nullptr) 114 | { 115 | if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) 116 | fileNum++; 117 | } 118 | closedir(pDir); 119 | return fileNum; 120 | } 121 | }; 122 | 123 | 124 | int main(int argc, char * argv[]) 125 | { 126 | rclcpp::init(argc, argv); 127 | auto node = std::make_shared(); 128 | rclcpp::spin(node); 129 | rclcpp::shutdown(); 130 | return 0; 131 | } 132 | 133 | 134 | 135 | 136 | 137 | -------------------------------------------------------------------------------- /test/test_laser_odom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "laser_odometry.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std::chrono_literals; 13 | class Publisher : public rclcpp::Node 14 | { 15 | public: 16 | Publisher() 17 | : Node("test_laser") 18 | { 19 | this->declare_parameter("ndt/resolution", 1.0); 20 | this->declare_parameter("ndt/step_size", 0.1); 21 | this->declare_parameter("ndt/epsilon", 0.01); 22 | this->declare_parameter("ndt/max_iterations", 30.0); 23 | this->declare_parameter("ndt/frame_resolution", 2.0); 24 | this->declare_parameter("odom/kf_distance", 3.0); 25 | this->declare_parameter("odom/local_map_size", 10); 26 | this->declare_parameter("ndt/local_map_resolution", 2.0); 27 | 28 | odometry = new laserOdometry(this); 29 | timer_ = this->create_wall_timer( 30 | 500ms, std::bind(&Publisher::timer_callback, this)); 31 | } 32 | 33 | pcl::PointCloud::Ptr getLocalMap(){return odometry->getLocalMap();} 34 | std::vector getKittiData(const std::string &path) 35 | { 36 | std::vector names; 37 | int num = getFileNum(path); 38 | 39 | for (int i = 0;i < num;i ++) 40 | { 41 | std::stringstream ss; 42 | ss << setw(6) << setfill('0') << i; 43 | names.emplace_back(path + ss.str() + ".bin"); 44 | } 45 | return names; 46 | } 47 | pcl::PointCloud::Ptr readKittiBinData(std::string &in_file) 48 | { 49 | std::fstream input(in_file.c_str(), ios::in | ios::binary); 50 | if (!input.good()) 51 | { 52 | std::cerr<<"Could not read file: "<< in_file << std::endl; 53 | return nullptr; 54 | } 55 | input.seekg(0, ios::beg); 56 | pcl::PointCloud::Ptr points(new pcl::PointCloud); 57 | int i; 58 | for (i=0; input.good() && !input.eof(); i++) 59 | { 60 | pcl::PointXYZI point; 61 | input.read((char*) &point.x, 3*sizeof(float)); 62 | input.read((char*) &point.intensity, sizeof(float)); 63 | points->push_back(point); 64 | } 65 | input.close(); 66 | return points; 67 | } 68 | laserOdometry *odometry; 69 | private: 70 | void timer_callback() 71 | { 72 | } 73 | rclcpp::TimerBase::SharedPtr timer_; 74 | int getFileNum(const std::string &path) 75 | { //需要用到头文件 76 | int fileNum = 0; 77 | DIR *pDir; 78 | struct dirent *ptr; 79 | if (!(pDir = opendir(path.c_str()))) 80 | return fileNum; 81 | while ((ptr = readdir(pDir)) != nullptr) 82 | { 83 | if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) 84 | fileNum++; 85 | } 86 | closedir(pDir); 87 | return fileNum; 88 | } 89 | 90 | }; 91 | 92 | boost::mutex updateModelMutex; 93 | boost::shared_ptr simpleVis (const pcl::PointCloud::ConstPtr& cloud) 94 | { 95 | boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); 96 | // viewer->setCameraPosition(0, 0, 0, 0, 0, 0, 0, 0, -1); 97 | viewer->setBackgroundColor(1.0,1.0,1.0); 98 | /* viewer->addPointCloud (cloud, "sample cloud"); 99 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); 100 | viewer->initCameraParameters ();*/ 101 | pcl::visualization::PointCloudColorHandlerCustom view_h(cloud, 0, 255, 0); 102 | viewer->addPointCloud(cloud, view_h, "show"); 103 | viewer->addPointCloud(cloud, view_h, "map"); 104 | return (viewer); 105 | } 106 | 107 | int main(int argc, char * argv[]) 108 | { 109 | rclcpp::init(argc, argv); 110 | auto node = std::make_shared(); 111 | 112 | std::string path = "/media/lyc/软件/1-10/05/velodyne/"; 113 | std::vector file_name = node->getKittiData(path); 114 | pcl::PointCloud::Ptr show(new pcl::PointCloud); 115 | Eigen::Matrix4f last_pose = Eigen::Matrix4f::Identity(); 116 | auto viewer = simpleVis(show); 117 | 118 | for (auto & file : file_name) 119 | { 120 | if (!rclcpp::ok()) 121 | break; 122 | auto points = node->readKittiBinData(file); 123 | // process 124 | Eigen::Matrix4f pose = node->odometry->addFrame(points); 125 | 126 | // visualization 127 | viewer->addLine(pcl::PointXYZ(last_pose(0,3), last_pose(1,3), last_pose(2,3)), pcl::PointXYZ(pose(0,3), pose(1,3), pose(2,3)), file); 128 | last_pose = pose; 129 | pcl::transformPointCloud(*points, *show, pose); 130 | 131 | pcl::visualization::PointCloudColorHandlerGenericField view_h(show, "intensity"); 132 | viewer->updatePointCloud(show, view_h, "show"); 133 | 134 | pcl::visualization::PointCloudColorHandlerGenericField view_h2(node->getLocalMap(), "intensity"); 135 | viewer->updatePointCloud(node->getLocalMap(), view_h2, "map"); 136 | viewer->spinOnce(); 137 | // boost::this_thread::sleep (boost::posix_time::milliseconds (50)); 138 | rclcpp::spin_some(node); 139 | } 140 | viewer->spin(); 141 | rclcpp::shutdown(); 142 | return 0; 143 | } 144 | -------------------------------------------------------------------------------- /test/test_odom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "rclcpp/rclcpp.hpp" 6 | #include "std_msgs/msg/string.hpp" 7 | #include 8 | #include "tf2_ros/buffer.h" 9 | #include 10 | #include 11 | #include 12 | #include "pcl/io/pcd_io.h" 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std::chrono_literals; 18 | 19 | class MinimalSubscriber : public rclcpp::Node 20 | { 21 | public: 22 | MinimalSubscriber() 23 | : Node("minimal_subscriber") 24 | { 25 | publisher_ = this->create_publisher( 26 | "/points",10); 27 | tf_broadcaster_ = std::make_shared(*this); 28 | // 10Hz 29 | timer_ = this->create_wall_timer( 30 | 100ms,std::bind(&MinimalSubscriber::timer_callback,this)); 31 | // 100Hz 32 | timer2_ = this->create_wall_timer( 33 | 10ms,std::bind(&MinimalSubscriber::timer2_callback,this)); 34 | } 35 | 36 | private: 37 | void timer_callback() 38 | { 39 | sensor_msgs::msg::PointCloud2 data; 40 | pcl::PointCloud points; 41 | pcl::io::loadPCDFile( 42 | "/home/lyc/Inspection_3d_ws/src/inspection_3d_robot/localization/map/frame.pcd", 43 | points); 44 | pcl::toROSMsg(points, data); 45 | data.header.stamp = this->get_clock()->now(); 46 | data.header.frame_id = "os_sensor"; 47 | publisher_->publish(data); 48 | } 49 | 50 | void timer2_callback() 51 | { 52 | geometry_msgs::msg::Point pos; 53 | // test 54 | static float i = 0; 55 | i += 0.0001; 56 | pos.x = sin(i) * 3; 57 | pos.y = (cos(i)-1) * 3; 58 | pos.z = M_PI*0.15;//sin(i) * 3; 59 | // pos.x = 1.2;//sin(i) * 3; 60 | // pos.y = 0;//(cos(i)-1) * 3; 61 | // pos.z = M_PI*0.15;//sin(i) * 3; 62 | publish_pos(pos); 63 | } 64 | 65 | void publish_pos(geometry_msgs::msg::Point pos) 66 | { 67 | { 68 | rclcpp::Time now = this->get_clock()->now(); 69 | geometry_msgs::msg::TransformStamped t; 70 | 71 | t.header.stamp = now; 72 | t.header.frame_id = "odom"; 73 | t.child_frame_id = "base_link"; 74 | t.transform.translation.x = pos.x; 75 | t.transform.translation.y = pos.y; 76 | t.transform.translation.z = 0.0; 77 | 78 | tf2::Quaternion q; 79 | q.setRPY(0, 0, pos.z); 80 | t.transform.rotation.x = q.x(); 81 | t.transform.rotation.y = q.y(); 82 | t.transform.rotation.z = q.z(); 83 | t.transform.rotation.w = q.w(); 84 | // Send the transformation 85 | tf_broadcaster_->sendTransform(t); 86 | } 87 | { 88 | rclcpp::Time now = this->get_clock()->now(); 89 | geometry_msgs::msg::TransformStamped t; 90 | 91 | t.header.stamp = now; 92 | t.header.frame_id = "base_link"; 93 | t.child_frame_id = "os_sensor"; 94 | t.transform.translation.x = 0.5; 95 | t.transform.translation.y = 0; 96 | t.transform.translation.z = 0.0; 97 | 98 | tf2::Quaternion q; 99 | q.setRPY(0, 0, 0); 100 | t.transform.rotation.x = q.x(); 101 | t.transform.rotation.y = q.y(); 102 | t.transform.rotation.z = q.z(); 103 | t.transform.rotation.w = q.w(); 104 | // Send the transformation 105 | tf_broadcaster_->sendTransform(t); 106 | } 107 | } 108 | rclcpp::TimerBase::SharedPtr timer_; 109 | rclcpp::TimerBase::SharedPtr timer2_; 110 | rclcpp::Subscription::SharedPtr subscription_; 111 | rclcpp::Publisher::SharedPtr publisher_; 112 | std::shared_ptr tf_broadcaster_; 113 | }; 114 | 115 | int main(int argc, char * argv[]) 116 | { 117 | rclcpp::init(argc, argv); 118 | rclcpp::spin(std::make_shared()); 119 | rclcpp::shutdown(); 120 | return 0; 121 | } 122 | 123 | 124 | --------------------------------------------------------------------------------