├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── m2dgr_params.yaml └── nclt_params.yaml ├── figs ├── Cover.jpg ├── m2dgr_street_01.png └── m2dgr_street_02.png ├── include ├── block_localization │ ├── ndt_est.hpp │ ├── pose_estimator.hpp │ └── pose_system.hpp ├── kkl │ └── alg │ │ └── unscented_kalman_filter.hpp └── utility.h ├── launch ├── run_m2dgr.launch └── run_nclt.launch ├── msg └── cloud_info.msg ├── nodelet_plugins.xml ├── package.xml ├── rviz └── block_localization.rviz ├── src ├── block_localization_nodelet.cpp ├── globalmap_server_nodelet.cpp └── points_preprocessing_nodelet.cpp └── srv └── queryMap.srv /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(block_localization) 3 | 4 | # -mavx causes a lot of errors!! 5 | if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") 6 | if("$ENV{ROS_DISTRO}" STRGREATER "melodic") 7 | add_definitions(-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 8 | set(CMAKE_CXX_FLAGS "-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 9 | else() 10 | add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 11 | set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 12 | endif() 13 | else() 14 | add_definitions(-std=c++17) 15 | set(CMAKE_CXX_FLAGS "-std=c++17") 16 | endif() 17 | 18 | # pcl 1.7 causes a segfault when it is built with debug mode 19 | set(CMAKE_BUILD_TYPE "RELEASE") 20 | 21 | find_package(catkin REQUIRED COMPONENTS 22 | nodelet 23 | roscpp 24 | rospy 25 | tf 26 | map_server 27 | actionlib 28 | # pcl libraries 29 | pcl_ros 30 | # external libraries 31 | ndt_omp 32 | hdl_global_localization 33 | # msgs 34 | std_msgs 35 | sensor_msgs 36 | geometry_msgs 37 | nav_msgs 38 | message_generation 39 | ) 40 | 41 | find_package(PCL REQUIRED) 42 | find_package(GTSAM REQUIRED QUIET) 43 | find_package(Boost REQUIRED) 44 | 45 | add_message_files( 46 | DIRECTORY msg 47 | FILES 48 | cloud_info.msg 49 | ) 50 | 51 | add_service_files( 52 | DIRECTORY srv 53 | FILES 54 | queryMap.srv 55 | ) 56 | 57 | generate_messages( 58 | DEPENDENCIES 59 | std_msgs 60 | geometry_msgs 61 | nav_msgs 62 | sensor_msgs 63 | ) 64 | 65 | include_directories(${PCL_INCLUDE_DIRS}) 66 | link_directories(${PCL_LIBRARY_DIRS}) 67 | add_definitions(${PCL_DEFINITIONS}) 68 | 69 | pkg_check_modules(NEW_YAMLCPP yaml-cpp>=0.5) 70 | if(NEW_YAMLCPP_FOUND) 71 | add_definitions(-DHAVE_NEW_YAMLCPP) 72 | endif(NEW_YAMLCPP_FOUND) 73 | pkg_check_modules(YAML REQUIRED yaml-cpp) 74 | message(STATUS "YAML=${YAML_INCLUDE_DIRS} ${YAML_LIBRARIES}") 75 | 76 | message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) 77 | message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) 78 | message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) 79 | 80 | find_package(OpenMP) 81 | if (OPENMP_FOUND) 82 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 83 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 84 | endif() 85 | 86 | ################################### 87 | ## catkin specific configuration ## 88 | ################################### 89 | catkin_package( 90 | INCLUDE_DIRS include 91 | DEPENDS PCL GTSAM 92 | DEPENDS YAML 93 | 94 | CATKIN_DEPENDS 95 | roscpp 96 | rospy 97 | actionlib 98 | tf 99 | std_msgs 100 | nav_msgs 101 | geometry_msgs 102 | sensor_msgs 103 | message_runtime 104 | message_generation 105 | ) 106 | 107 | ########### 108 | ## Build ## 109 | ########### 110 | include_directories(include) 111 | include_directories( 112 | ${GTSAM_INCLUDE_DIR} 113 | ${catkin_INCLUDE_DIRS} 114 | ${YAML_INCLUDE_DIRS} 115 | ) 116 | 117 | # nodelets 118 | add_library(block_localization_nodelet src/block_localization_nodelet.cpp) 119 | target_link_libraries(block_localization_nodelet 120 | ${catkin_LIBRARIES} 121 | ${PCL_LIBRARIES} 122 | yaml-cpp 123 | gtsam 124 | tbb 125 | ) 126 | 127 | add_library(globalmap_server_nodelet src/globalmap_server_nodelet.cpp) 128 | target_link_libraries(globalmap_server_nodelet 129 | ${catkin_LIBRARIES} 130 | ${PCL_LIBRARIES} 131 | yaml-cpp 132 | gtsam 133 | tbb 134 | ) 135 | 136 | add_library(points_preprocessing_nodelet src/points_preprocessing_nodelet.cpp) 137 | target_link_libraries(points_preprocessing_nodelet 138 | ${catkin_LIBRARIES} 139 | ${PCL_LIBRARIES} 140 | yaml-cpp 141 | gtsam 142 | tbb 143 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Yixiao Feng 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

BM-Loc

3 |
4 | 🎬Video 5 |   •   6 | 🛠️Installation 7 |   •   8 | 📑Paper 9 |
10 |
11 |
12 | 13 | ![BMLoc_cover](figs/Cover.jpg) 14 | 15 | In this work, we propose a Block Map (BM) generation and maintenance method and the corresponding BM-based localization system. The main contributions are as follows: 16 | 17 | - A BM-based localization system in a large-scale environment is proposed for the first time. 18 | - A BM generation method and corresponding switching strategy is proposed which maintains the spatial continuity of adjacent BMs. 19 | - A factor graph-based optimization method with the dynamic sliding window based on BMs is proposed to achieve accurate and reliable state estimation. 20 | - We achieve the best performance on publicly available large-scale datasets [NCLT](https://robots.engin.umich.edu/nclt/) and [M2DGR](https://github.com/SJTU-ViSYS/M2DGR). 21 | 22 | 23 | *** 24 | ## Installation 25 | ### 1. Prerequisites 26 | #### 1.1 Ubuntu and ROS 27 | Ubuntu $\geq$ 18.04 28 | #### 1.2 ROS Package 29 | - [ndt_omp](https://github.com/koide3/ndt_omp) 30 | - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) 31 | - [hdl_global_localization](https://github.com/koide3/hdl_global_localization) (Can be replaced by our promoted global localization method; will release soon) 32 | 33 | ### 2. Build 34 | ```bash 35 | cd /src 36 | git clone https://github.com/koide3/ndt_omp 37 | git clone https://github.com/SMRT-AIST/fast_gicp --recursive 38 | git clone https://github.com/koide3/hdl_global_localization 39 | git clone https://github.com/YixFeng/Block-Map-Based-Localization 40 | 41 | cd .. 42 | catkin_make 43 | source devel/setup.bash 44 | ``` 45 | 46 | ## Run 47 | We provide some [Block Maps](https://drive.google.com/file/d/1Z2K56jTkMOouZhM4c9JhPvqyDxGFiXSY/view?usp=drive_link) (Google Drive) to make your tests easy. 48 | 49 | - **M2DGR street_01** 50 | 51 | ![m2dgr_street_01](figs/m2dgr_street_01.png) 52 | 53 | - **M2DGR street_02** 54 | 55 | ![m2dgr_street_02](figs/m2dgr_street_02.png) 56 | 57 | ### 1. NCLT 58 | Download NCLT from [https://robots.engin.umich.edu/nclt/](https://robots.engin.umich.edu/nclt/) 59 | ```bash 60 | roslaunch block_localization run_nclt.launch 61 | ``` 62 | 63 | ### 2. M2DGR 64 | Download M2DGR from [https://github.com/SJTU-ViSYS/M2DGR](https://github.com/SJTU-ViSYS/M2DGR) 65 | ```bash 66 | roslaunch block_localization run_m2dgr.launch 67 | ``` 68 | *Remarks:* 69 | Since BM-Loc is a map-based localization method, you need to provide the directory where maps are stored. Edit the parameter `globalmap_dir` in `config/*.yaml` files. 70 | 71 | 72 | ## Citation 73 | If you use any of this code, please cite our [paper](https://arxiv.org/pdf/2404.18192). 74 | 75 | ```bibtex 76 | @article{feng2024block, 77 | title={Block-Map-Based Localization in Large-Scale Environment}, 78 | author={Feng, Yixiao and Jiang, Zhou and Shi, Yongliang and Feng, Yunlong and Chen, Xiangyu and Zhao, Hao and Zhou, Guyue}, 79 | journal={arXiv preprint arXiv:2404.18192}, 80 | year={2024} 81 | } 82 | ``` 83 | 84 | ## Acknowledgements 85 | Thanks for the open-source projects [hdl_localization](https://github.com/koide3/hdl_localization), [hdl_global_localization](https://github.com/koide3/hdl_localization) and [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM). 86 | 87 | ## Star History 88 | 89 | [![Star History Chart](https://api.star-history.com/svg?repos=YixFeng/Block-Map-Based-Localization&type=Date)](https://www.star-history.com/#YixFeng/Block-Map-Based-Localization&Date) 90 | -------------------------------------------------------------------------------- /config/m2dgr_params.yaml: -------------------------------------------------------------------------------- 1 | block_localization: 2 | odom_child_frame_id: "velodyne" 3 | 4 | # lidar settings 5 | lidarTopic: "/velodyne_points" 6 | have_ring_time_channel: false # if "XYZI", false; if "XYZIRT", true 7 | lidarMinRange: 2.0 8 | lidarMaxRange: 80.0 9 | 10 | # imu settings 11 | imuTopic: "/handsfree/imu" 12 | imuAccNoise: 0.01 13 | imuGyrNoise: 0.01 14 | gravity: 9.80416 15 | imuFrequency: 150 16 | 17 | # sensors extrinsics T_lb 18 | extrinsicTrans: [-0.29, 0, -0.15] 19 | extrinsicRot: [1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | extrinsicRPY: [1, 0, 0, 23 | 0, 1, 0, 24 | 0, 0, 1] 25 | 26 | L2gt_t: [ 0.16, 0, -0.15 ] 27 | L2gt_R: [ 1, 0, 0, 28 | 0, 1, 0, 29 | 0, 0, 1 ] 30 | 31 | # system settings 32 | cool_time_duration: 2.00 # during "cool_time", imu inputs are ignored 33 | key_interval: 0.1 # select key frames according to time interval 34 | mapqry_interval: 1.0 # query time to change BM 35 | 36 | # ndt settings 37 | ndt_neighbor_search_method: "DIRECT1" 38 | ndt_resolution: 2.0 39 | ndt_epsilon: 0.01 # maximum allowable translation squared difference between two consecutive transformations 40 | penalty_thres: 0.6 41 | penalty_weight: 1.0 42 | 43 | # gtsam settings 44 | odomLinearNoise: 0.08 45 | odomAngularNoise: 0.05 46 | active_factors: 120 47 | preserve_factors: 10 48 | 49 | # initialpose settings 50 | # if "specify_init_pose" is true, pose estimator will be initialized with the following params. Otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" 51 | specify_init_pose: true 52 | init_pos_x: 0.0 53 | init_pos_y: 0.0 54 | init_pos_z: 0.0 55 | init_ori_w: 1.0 56 | init_ori_x: 0.0 57 | init_ori_y: 0.0 58 | init_ori_z: 0.0 59 | 60 | 61 | globalmap_server: 62 | globalmap_dir: "/home/yixfeng/Block-SLAM/map/M2DGR/street_01/" # directory address of your BMs; end with "/" 63 | downsample_resolution: 0.2 64 | 65 | -------------------------------------------------------------------------------- /config/nclt_params.yaml: -------------------------------------------------------------------------------- 1 | block_localization: 2 | odom_child_frame_id: "velodyne" 3 | 4 | # lidar settings 5 | lidarTopic: "/points_raw" 6 | have_ring_time_channel: false # if "XYZI", false; if "XYZIRT", true 7 | lidarMinRange: 2.0 8 | lidarMaxRange: 90.0 9 | 10 | # imu settings 11 | imuTopic: "/imu_raw" 12 | imuAccNoise: 0.01 13 | imuGyrNoise: 0.01 14 | gravity: 9.80416 15 | imuFrequency: 100 16 | 17 | # sensors extrinsics T_lb body to lidar 18 | extrinsicTrans: [ 0, 0, 0.28 ] 19 | 20 | extrinsicRot: [1, 0, 0, 21 | 0, 1, 0, 22 | 0, 0, 1] 23 | extrinsicRPY: [1, 0, 0, 24 | 0, 1, 0, 25 | 0, 0, 1] 26 | 27 | # L2gt_t: [0.00679684, 0.01542909, 0.95686191] 28 | 29 | # L2gt_R: [ -0.0122693, -0.9999205, -0.0028972, 30 | # -0.9998251, 0.0123089, -0.0140843, 31 | # 0.0141188, 0.0027239, -0.9998966 ] 32 | L2gt_t: [ 0.002, 0.004, 0.957] 33 | L2gt_R: [ -0.0122693, -0.9998251, 0.0141188, 34 | -0.9999205, 0.0123089, 0.0027239, 35 | -0.0028972, -0.0140843, -0.9998966 ] 36 | 37 | # system settings 38 | cool_time_duration: 2.00 # during "cool_time", imu inputs are ignored 39 | key_interval: 0.1 # select key frames according to time interval 40 | mapqry_interval: 1.0 # query time to change BM 41 | 42 | # ndt settings 43 | ndt_neighbor_search_method: "DIRECT1" 44 | ndt_resolution: 2.0 45 | ndt_epsilon: 0.01 # maximum allowable translation squared difference between two consecutive transformations 46 | penalty_thres: 0.6 47 | penalty_weight: 1.0 48 | 49 | # gtsam settings 50 | odomLinearNoise: 0.08 51 | odomAngularNoise: 0.05 52 | active_factors: 20 53 | preserve_factors: 5 54 | 55 | # initialpose settings 56 | # if "specify_init_pose" is true, pose estimator will be initialized with the following params. Otherwise, you need to input an initial pose with "2D Pose Estimate" on rviz" 57 | specify_init_pose: true 58 | init_pos_x: 0.0 59 | init_pos_y: 0.0 60 | init_pos_z: 0.0 61 | init_ori_w: 1 62 | init_ori_x: 0.0 63 | init_ori_y: 0.0 64 | init_ori_z: 0.0 65 | 66 | 67 | globalmap_server: 68 | globalmap_dir: "/home/yixfeng/Block-SLAM/map/nclt/20120122/" # directory address of your BMs; end with "/" 69 | downsample_resolution: 0.2 70 | 71 | -------------------------------------------------------------------------------- /figs/Cover.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YixFeng/Block-Map-Based-Localization/bd99dba86c9bafb28fb5501a7f8a876db5da922d/figs/Cover.jpg -------------------------------------------------------------------------------- /figs/m2dgr_street_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YixFeng/Block-Map-Based-Localization/bd99dba86c9bafb28fb5501a7f8a876db5da922d/figs/m2dgr_street_01.png -------------------------------------------------------------------------------- /figs/m2dgr_street_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YixFeng/Block-Map-Based-Localization/bd99dba86c9bafb28fb5501a7f8a876db5da922d/figs/m2dgr_street_02.png -------------------------------------------------------------------------------- /include/block_localization/ndt_est.hpp: -------------------------------------------------------------------------------- 1 | #ifndef NDT_EST_HPP 2 | #define NDT_EST_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace block_localization { 16 | 17 | /** 18 | * @brief scan matching-based pose estimator 19 | */ 20 | class NdtEstimator { 21 | public: 22 | using PointT = pcl::PointXYZI; 23 | 24 | 25 | NdtEstimator(pcl::Registration::Ptr& registration, const ros::Time& stamp, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration = 1.0) 26 | : init_stamp(stamp), 27 | registration(registration), 28 | cool_time_duration(cool_time_duration) 29 | { 30 | process_noise = Eigen::MatrixXf::Identity(16, 16); 31 | process_noise.middleRows(0, 3) *= 1.0; 32 | process_noise.middleRows(3, 3) *= 1.0; 33 | process_noise.middleRows(6, 4) *= 0.5; 34 | process_noise.middleRows(10, 3) *= 1e-6; 35 | process_noise.middleRows(13, 3) *= 1e-6; 36 | 37 | Eigen::MatrixXf measurement_noise = Eigen::MatrixXf::Identity(7, 7); 38 | measurement_noise.middleRows(0, 3) *= 0.01; 39 | measurement_noise.middleRows(3, 4) *= 0.001; 40 | 41 | Eigen::VectorXf mean(16); 42 | mean.middleRows(0, 3) = pos; 43 | mean.middleRows(3, 3).setZero(); 44 | mean.middleRows(6, 4) = Eigen::Vector4f(quat.w(), quat.x(), quat.y(), quat.z()); 45 | mean.middleRows(10, 3).setZero(); 46 | mean.middleRows(13, 3).setZero(); 47 | 48 | Eigen::MatrixXf cov = Eigen::MatrixXf::Identity(16, 16) * 0.01; 49 | 50 | PoseSystem system; 51 | ukf.reset(new kkl::alg::UnscentedKalmanFilterX(system, 16, 6, 7, process_noise, measurement_noise, mean, cov)); 52 | } 53 | 54 | void predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) { 55 | if((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) { 56 | prev_stamp = stamp; 57 | return; 58 | } 59 | 60 | double dt = (stamp - prev_stamp).toSec(); 61 | prev_stamp = stamp; 62 | 63 | ukf->setProcessNoiseCov(process_noise * dt); 64 | ukf->system.dt = dt; 65 | 66 | Eigen::VectorXf control(6); 67 | control.head<3>() = acc; 68 | control.tail<3>() = gyro; 69 | 70 | ukf->predict(control); 71 | } 72 | 73 | /** 74 | * @brief correct 75 | * @param cloud input cloud 76 | * @return cloud aligned to the globalmap 77 | */ 78 | pcl::PointCloud::Ptr correct(const pcl::PointCloud::ConstPtr& cloud) { 79 | Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity(); 80 | init_guess.block<3, 3>(0, 0) = quat().toRotationMatrix(); 81 | init_guess.block<3, 1>(0, 3) = pos(); 82 | 83 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 84 | registration->setInputSource(cloud); 85 | registration->align(*aligned, init_guess); 86 | 87 | Eigen::Matrix4f trans = registration->getFinalTransformation(); 88 | Eigen::Vector3f p = trans.block<3, 1>(0, 3); 89 | Eigen::Quaternionf q(trans.block<3, 3>(0, 0)); 90 | 91 | if(quat().coeffs().dot(q.coeffs()) < 0.0f) { 92 | q.coeffs() *= -1.0f; 93 | } 94 | 95 | Eigen::VectorXf observation(7); 96 | observation.middleRows(0, 3) = p; 97 | observation.middleRows(3, 4) = Eigen::Vector4f(q.w(), q.x(), q.y(), q.z()); 98 | 99 | ukf->correct(observation); 100 | return aligned; 101 | } 102 | 103 | /* getters */ 104 | Eigen::Vector3f pos() const { 105 | return Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]); 106 | } 107 | 108 | Eigen::Vector3f vel() const { 109 | return Eigen::Vector3f(ukf->mean[3], ukf->mean[4], ukf->mean[5]); 110 | } 111 | 112 | Eigen::Quaternionf quat() const { 113 | return Eigen::Quaternionf(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]).normalized(); 114 | } 115 | 116 | Eigen::Matrix4f matrix() const { 117 | Eigen::Matrix4f m = Eigen::Matrix4f::Identity(); 118 | m.block<3, 3>(0, 0) = quat().toRotationMatrix(); 119 | m.block<3, 1>(0, 3) = pos(); 120 | return m; 121 | } 122 | 123 | private: 124 | ros::Time init_stamp; // when the estimator was initialized 125 | ros::Time prev_stamp; // when the estimator was updated last time 126 | double cool_time_duration; // 127 | 128 | Eigen::MatrixXf process_noise; 129 | std::unique_ptr> ukf; 130 | 131 | pcl::Registration::Ptr registration; 132 | }; 133 | 134 | } 135 | 136 | #endif // POSE_ESTIMATOR_HPP 137 | -------------------------------------------------------------------------------- /include/block_localization/pose_estimator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSE_ESTIMATOR_HPP 2 | #define POSE_ESTIMATOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | namespace block_localization { 28 | 29 | /** 30 | * @brief scan matching-based pose estimator 31 | */ 32 | class PoseEstimator { 33 | public: 34 | Eigen::Matrix4d trans_delta; 35 | Eigen::Matrix4d trans; 36 | Eigen::Matrix4d trans_pre=Eigen::Matrix4d::Identity(); 37 | 38 | 39 | 40 | typedef Eigen::Matrix VectorXt; 41 | using PointT = pcl::PointXYZI; 42 | 43 | /** 44 | * @brief constructor 45 | * @param registration registration method 46 | * @param stamp timestamp 47 | * @param pos initial position 48 | * @param quat initial orientation 49 | * @param cool_time_duration during "cool time", prediction is not performed 50 | */ 51 | PoseEstimator(pcl::Registration::Ptr& registration, const ros::Time& stamp, const Eigen::Vector3d& pos, const Eigen::Quaterniond& quat_data, double cool_time_duration = 1.0) 52 | : init_stamp(stamp), 53 | registration(registration), 54 | cool_time_duration(cool_time_duration) 55 | { 56 | process_noise = Eigen::MatrixXd::Identity(16, 16); 57 | process_noise.middleRows(0, 3) *= 1.0;// middleRows(a,b): from row a to row (a+b-1) 58 | process_noise.middleRows(3, 3) *= 1.0; 59 | process_noise.middleRows(6, 4) *= 0.5; 60 | process_noise.middleRows(10, 3) *= 1e-6; 61 | process_noise.middleRows(13, 3) *= 1e-6; 62 | 63 | Eigen::MatrixXd measurement_noise = Eigen::MatrixXd::Identity(7, 7); 64 | measurement_noise.middleRows(0, 3) *= 0.01; 65 | measurement_noise.middleRows(3, 4) *= 0.001; 66 | 67 | Eigen::VectorXd mean(16); 68 | mean.middleRows(0, 3) = pos; 69 | mean.middleRows(3, 3).setZero(); 70 | mean.middleRows(6, 4) = Eigen::Vector4d(quat_data.w(), quat_data.x(), quat_data.y(), quat_data.z()); 71 | mean.middleRows(10, 3).setZero(); 72 | mean.middleRows(13, 3).setZero(); 73 | state=mean; 74 | std::cout<<"init_state:"<system=system; 91 | double dt = (stamp - prev_stamp).toSec(); 92 | prev_stamp = stamp; 93 | Eigen::VectorXd control(6); 94 | control.head<3>() = acc; 95 | control.tail<3>() = gyro; 96 | Eigen::VectorXd pre_state=state; 97 | std::cout<<"pre_state:"<state.middleRows(6,4) = Eigen::Vector4d(stateQuat.w(),stateQuat.x(),stateQuat.y(),stateQuat.z()); 108 | this->state.middleRows(0,3) = Eigen::Vector3d(latestState.position().x(),latestState.position().y(),latestState.position().z()); 109 | this->state.middleRows(3,3) = Eigen::Vector3d(latestState.velocity().x(),latestState.velocity().y(),latestState.velocity().z()); 110 | this->state.middleRows(10,3)= Eigen::Vector3d(latestBias.accelerometer().x(),latestBias.accelerometer().y(),latestBias.accelerometer().z()); 111 | this->state.middleRows(13,3)= Eigen::Vector3d(latestBias.gyroscope().x(),latestBias.gyroscope().y(),latestBias.gyroscope().z()); 112 | 113 | trans_pre.block<3, 3>(0, 0) = this->quat().toRotationMatrix(); 114 | trans_pre.block<3, 1>(0, 3) = this->pos(); 115 | } 116 | /** 117 | * @brief correct 118 | * @param cloud input cloud 119 | * @return cloud aligned to the globalmap 120 | */ 121 | pcl::PointCloud::Ptr correct(const pcl::PointCloud::ConstPtr& cloud) { 122 | 123 | Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity(); 124 | init_guess.block<3, 3>(0, 0) = this->quat().cast().toRotationMatrix(); 125 | init_guess.block<3, 1>(0, 3) = this->pos().cast(); 126 | 127 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 128 | registration->setInputSource(cloud); 129 | registration->align(*aligned, init_guess); 130 | 131 | 132 | Eigen::Matrix4f trans_temp; 133 | trans_temp = registration->getFinalTransformation(); 134 | trans=trans_temp.cast(); 135 | // std::cout<<"trans:"<state.middleRows(0,3)=trans.block<3, 1>(0, 3); 138 | Eigen::Quaterniond update_quat = Eigen::Quaterniond(trans.block<3, 3>(0, 0)).normalized(); 139 | this->state.middleRows(6,4)=Eigen::Vector4d(update_quat.w(), update_quat.x(), update_quat.y(), update_quat.z()); 140 | 141 | trans_delta=getDeltaTrans(trans_pre,trans); 142 | Eigen::Vector3d p_print = trans_delta.block<3, 1>(0, 3);//used to test incremental odometry 143 | 144 | 145 | // Eigen::Vector3f p = trans.block<3, 1>(0, 3); 146 | // Eigen::Quaternionf q(trans.block<3, 3>(0, 0)); 147 | // 148 | // if(quat().coeffs().dot(q.coeffs()) < 0.0f) { 149 | // q.coeffs() *= -1.0f; 150 | // } 151 | 152 | return aligned; 153 | } 154 | 155 | /* getters */ 156 | // Eigen::Vector3f pos() const { 157 | // return Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]); 158 | // } 159 | // Eigen::Vector3d pos() const { 160 | // Eigen::Vector3d p_tre = trans_pre.block<3, 1>(0, 3); 161 | // return Eigen::Vector3d(p_tre(0), p_tre(1), p_tre(2)); 162 | // } 163 | Eigen::Vector3d pos() const { 164 | return Eigen::Vector3d(state(0),state(1),state(2)); 165 | } 166 | 167 | // Eigen::Vector3f vel() const { 168 | // return Eigen::Vector3f(ukf->mean[3], ukf->mean[4], ukf->mean[5]); 169 | // } 170 | 171 | // Eigen::Quaternionf quat() const { 172 | // return Eigen::Quaternionf(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]).normalized(); 173 | // } 174 | // Eigen::Quaterniond quat() const { 175 | // return Eigen::Quaterniond(trans_pre.block<3, 3>(0, 0)).normalized(); 176 | // } 177 | Eigen::Quaterniond quat() const { 178 | return Eigen::Quaterniond(state(6),state(7),state(8),state(9)).normalized(); 179 | } 180 | 181 | // Eigen::Matrix4f matrix() const { 182 | // Eigen::Matrix4f m = Eigen::Matrix4f::Identity(); 183 | // m.block<3, 3>(0, 0) = quat().toRotationMatrix(); 184 | // m.block<3, 1>(0, 3) = pos(); 185 | // return m; 186 | // } 187 | 188 | Eigen::Matrix4d getDeltaTrans(Eigen::Matrix4d trans_pre,Eigen::Matrix4d trans_cur){ 189 | Eigen::Vector3d p_tre = trans_pre.block<3, 1>(0, 3); 190 | Eigen::Quaterniond q_pre(trans_pre.block<3, 3>(0, 0)); 191 | Eigen::Vector3d p_cur = trans_cur.block<3, 1>(0, 3); 192 | Eigen::Quaterniond q_cur(trans_cur.block<3, 3>(0, 0)); 193 | Eigen::Matrix4d delta=Eigen::Matrix4d::Identity(); 194 | delta.block<3,1>(0,3)=p_cur-p_tre; 195 | delta.block<3, 3>(0, 0)=(q_cur*q_pre.inverse()).toRotationMatrix(); 196 | return delta; 197 | } 198 | 199 | private: 200 | Eigen::VectorXd state; 201 | ros::Time init_stamp; // when the estimator was initialized 202 | ros::Time prev_stamp; // when the estimator was updated last time 203 | double cool_time_duration; 204 | // PoseSystem system; 205 | 206 | Eigen::MatrixXd process_noise; 207 | // std::unique_ptr> ukf; 208 | pcl::Registration::Ptr registration; 209 | 210 | }; 211 | 212 | } 213 | 214 | #endif // POSE_ESTIMATOR_HPP 215 | -------------------------------------------------------------------------------- /include/block_localization/pose_system.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSE_SYSTEM_HPP 2 | #define POSE_SYSTEM_HPP 3 | 4 | #include 5 | 6 | namespace block_localization { 7 | 8 | /** 9 | * @brief Definition of system to be estimated by ukf 10 | * @note state = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, acc_bias_x, acc_bias_y, acc_bias_z, gyro_bias_x, gyro_bias_y, gyro_bias_z] 11 | */ 12 | class PoseSystem { 13 | public: 14 | typedef float T; 15 | typedef Eigen::Matrix Vector3t; 16 | typedef Eigen::Matrix Matrix4t; 17 | typedef Eigen::Matrix VectorXt; 18 | typedef Eigen::Quaternion Quaterniont; 19 | public: 20 | PoseSystem() { 21 | dt = 0.01; 22 | } 23 | 24 | // system equation 25 | VectorXt f(const VectorXt& state, const VectorXt& control) const { 26 | VectorXt next_state(16); 27 | 28 | Vector3t pt = state.middleRows(0, 3); 29 | Vector3t vt = state.middleRows(3, 3); 30 | Quaterniont qt(state[6], state[7], state[8], state[9]); 31 | qt.normalize(); 32 | std::cout<<"control function state:"< 10 | #include 11 | 12 | namespace kkl { 13 | namespace alg { 14 | 15 | /** 16 | * @brief Unscented Kalman Filter class 17 | * @param T scaler type 18 | * @param System system class to be estimated 19 | */ 20 | template 21 | class UnscentedKalmanFilterX { 22 | typedef Eigen::Matrix VectorXt; 23 | typedef Eigen::Matrix MatrixXt; 24 | public: 25 | /** 26 | * @brief constructor 27 | * @param system system to be estimated 28 | * @param state_dim state vector dimension 29 | * @param input_dim input vector dimension 30 | * @param measurement_dim measurement vector dimension 31 | * @param process_noise process noise covariance (state_dim x state_dim) 32 | * @param measurement_noise measurement noise covariance (measurement_dim x measuremend_dim) 33 | * @param mean initial mean 34 | * @param cov initial covariance 35 | */ 36 | UnscentedKalmanFilterX(const System& system, int state_dim, int input_dim, int measurement_dim, const MatrixXt& process_noise, const MatrixXt& measurement_noise, const VectorXt& mean, const MatrixXt& cov) 37 | : state_dim(state_dim), 38 | input_dim(input_dim), 39 | measurement_dim(measurement_dim), 40 | N(state_dim), 41 | M(input_dim), 42 | K(measurement_dim), 43 | S(2 * state_dim + 1), 44 | mean(mean), 45 | cov(cov), 46 | system(system), 47 | process_noise(process_noise), 48 | measurement_noise(measurement_noise), 49 | lambda(1), 50 | normal_dist(0.0, 1.0) 51 | { 52 | weights.resize(S, 1); 53 | sigma_points.resize(S, N); 54 | ext_weights.resize(2 * (N + K) + 1, 1); 55 | ext_sigma_points.resize(2 * (N + K) + 1, N + K); 56 | expected_measurements.resize(2 * (N + K) + 1, K); 57 | 58 | // initialize weights for unscented filter 59 | weights[0] = lambda / (N + lambda); 60 | for (int i = 1; i < 2 * N + 1; i++) { 61 | weights[i] = 1 / (2 * (N + lambda)); 62 | } 63 | 64 | // weights for extended state space which includes error variances 65 | ext_weights[0] = lambda / (N + K + lambda); 66 | for (int i = 1; i < 2 * (N + K) + 1; i++) { 67 | ext_weights[i] = 1 / (2 * (N + K + lambda)); 68 | } 69 | } 70 | 71 | /** 72 | * @brief predict 73 | * @param control input vector 74 | */ 75 | void predict(const VectorXt& control) { 76 | // calculate sigma points 77 | ensurePositiveFinite(cov); 78 | computeSigmaPoints(mean, cov, sigma_points); 79 | for (int i = 0; i < S; i++) { 80 | sigma_points.row(i) = system.f(sigma_points.row(i), control); 81 | } 82 | 83 | const auto& R = process_noise; 84 | 85 | // unscented transform 86 | VectorXt mean_pred(mean.size()); 87 | MatrixXt cov_pred(cov.rows(), cov.cols()); 88 | 89 | mean_pred.setZero(); 90 | cov_pred.setZero(); 91 | for (int i = 0; i < S; i++) { 92 | mean_pred += weights[i] * sigma_points.row(i); 93 | } 94 | for (int i = 0; i < S; i++) { 95 | VectorXt diff = sigma_points.row(i).transpose() - mean_pred; 96 | cov_pred += weights[i] * diff * diff.transpose(); 97 | } 98 | cov_pred += R; 99 | 100 | mean = mean_pred; 101 | cov = cov_pred; 102 | } 103 | 104 | /** 105 | * @brief correct 106 | * @param measurement measurement vector 107 | */ 108 | void correct(const VectorXt& measurement) { 109 | // create extended state space which includes error variances 110 | VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1); 111 | MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K); 112 | ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean); 113 | ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov); 114 | ext_cov_pred.bottomRightCorner(K, K) = measurement_noise; 115 | 116 | ensurePositiveFinite(ext_cov_pred); 117 | computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points); 118 | 119 | // unscented transform 120 | expected_measurements.setZero(); 121 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 122 | expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1)); 123 | expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(K, 1)); 124 | } 125 | 126 | VectorXt expected_measurement_mean = VectorXt::Zero(K); 127 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 128 | expected_measurement_mean += ext_weights[i] * expected_measurements.row(i); 129 | } 130 | MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K); 131 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 132 | VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean; 133 | expected_measurement_cov += ext_weights[i] * diff * diff.transpose(); 134 | } 135 | 136 | // calculated transformed covariance 137 | MatrixXt sigma = MatrixXt::Zero(N + K, K); 138 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 139 | auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred); 140 | auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean); 141 | sigma += ext_weights[i] * (diffA * diffB.transpose()); 142 | } 143 | 144 | kalman_gain = sigma * expected_measurement_cov.inverse(); 145 | const auto& K = kalman_gain; 146 | 147 | VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean); 148 | MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpose(); 149 | 150 | mean = ext_mean.topLeftCorner(N, 1); 151 | cov = ext_cov.topLeftCorner(N, N); 152 | } 153 | 154 | /* getter */ 155 | const VectorXt& getMean() const { return mean; } 156 | const MatrixXt& getCov() const { return cov; } 157 | const MatrixXt& getSigmaPoints() const { return sigma_points; } 158 | 159 | System& getSystem() { return system; } 160 | const System& getSystem() const { return system; } 161 | const MatrixXt& getProcessNoiseCov() const { return process_noise; } 162 | const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; } 163 | 164 | const MatrixXt& getKalmanGain() const { return kalman_gain; } 165 | 166 | /* setter */ 167 | UnscentedKalmanFilterX& setMean(const VectorXt& m) { mean = m; return *this; } 168 | UnscentedKalmanFilterX& setCov(const MatrixXt& s) { cov = s; return *this; } 169 | 170 | UnscentedKalmanFilterX& setProcessNoiseCov(const MatrixXt& p) { process_noise = p; return *this; } 171 | UnscentedKalmanFilterX& setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m; return *this; } 172 | 173 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 174 | private: 175 | const int state_dim; 176 | const int input_dim; 177 | const int measurement_dim; 178 | 179 | const int N; 180 | const int M; 181 | const int K; 182 | const int S; 183 | 184 | public: 185 | VectorXt mean; 186 | MatrixXt cov; 187 | 188 | System system; 189 | MatrixXt process_noise; // 190 | MatrixXt measurement_noise; // 191 | 192 | T lambda; 193 | VectorXt weights; 194 | 195 | MatrixXt sigma_points; 196 | 197 | VectorXt ext_weights; 198 | MatrixXt ext_sigma_points; 199 | MatrixXt expected_measurements; 200 | 201 | private: 202 | /** 203 | * @brief compute sigma points 204 | * @param mean mean 205 | * @param cov covariance 206 | * @param sigma_points calculated sigma points 207 | */ 208 | void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) { 209 | const int n = mean.size(); 210 | assert(cov.rows() == n && cov.cols() == n); 211 | 212 | Eigen::LLT llt; 213 | llt.compute((n + lambda) * cov); 214 | MatrixXt l = llt.matrixL(); 215 | 216 | sigma_points.row(0) = mean; 217 | for (int i = 0; i < n; i++) { 218 | sigma_points.row(1 + i * 2) = mean + l.col(i); 219 | sigma_points.row(1 + i * 2 + 1) = mean - l.col(i); 220 | } 221 | } 222 | 223 | /** 224 | * @brief make covariance matrix positive finite 225 | * @param cov covariance matrix 226 | */ 227 | void ensurePositiveFinite(MatrixXt& cov) { 228 | return; 229 | const double eps = 1e-9; 230 | 231 | Eigen::EigenSolver solver(cov); 232 | MatrixXt D = solver.pseudoEigenvalueMatrix(); 233 | MatrixXt V = solver.pseudoEigenvectors(); 234 | for (int i = 0; i < D.rows(); i++) { 235 | if (D(i, i) < eps) { 236 | D(i, i) = eps; 237 | } 238 | } 239 | 240 | cov = V * D * V.inverse(); 241 | } 242 | 243 | public: 244 | MatrixXt kalman_gain; 245 | 246 | std::mt19937 mt; 247 | std::normal_distribution normal_dist; 248 | }; 249 | 250 | } 251 | } 252 | 253 | 254 | #endif 255 | -------------------------------------------------------------------------------- /include/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _UTILITY_H_ 3 | #define _UTILITY_H_ 4 | #define PCL_NO_PRECOMPILE 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | using namespace std; 52 | 53 | struct VelodynePointXYZIRT { 54 | PCL_ADD_POINT4D 55 | PCL_ADD_INTENSITY; 56 | uint16_t ring; 57 | float time; 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 59 | } EIGEN_ALIGN16; 60 | 61 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT, 62 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) 63 | (uint16_t, ring, ring)(float, time, time) 64 | ) 65 | 66 | using PointT = pcl::PointXYZI; 67 | using PointIRT = VelodynePointXYZIRT; 68 | 69 | class ParamServer 70 | { 71 | public: 72 | ros::NodeHandle nh; 73 | ros::NodeHandle private_nh; 74 | 75 | string odom_child_frame_id; 76 | 77 | // BlockMap(BM) settings 78 | double downsample_resolution; 79 | string globalmap_dir; 80 | 81 | // Gridmap settings 82 | string yaml_path_; 83 | string pgm_path_; 84 | 85 | // Lidar configuation 86 | string lidarTopic; 87 | bool have_ring_time_channel; 88 | float lidarMinRange; 89 | float lidarMaxRange; 90 | string ndt_neighbor_search_method; 91 | double ndt_resolution; 92 | double ndt_epsilon; 93 | 94 | // IMU configuation 95 | string imuTopic; 96 | double imuAccNoise, imuGyrNoise; 97 | double gravity; 98 | int imuFrequency; 99 | vector extRotV; 100 | vector L2GtRotV; 101 | vector extRPYV; 102 | vector extTransV; 103 | vector L2GtTransV; 104 | Eigen::Matrix3d extRot; 105 | Eigen::Matrix3d extRPY; 106 | 107 | Eigen::Matrix3d L2GtRot; 108 | Eigen::Vector3d extTrans; 109 | Eigen::Vector3d L2GtTrans; 110 | Eigen::Quaterniond extQRPY; 111 | 112 | // System settings 113 | float key_interval, mapqry_interval; 114 | 115 | // Optimization settings 116 | int active_factors; 117 | int preserve_factors; 118 | double odomLinearNoise, odomAngularNoise; 119 | double penalty_thres; 120 | double penalty_weight; 121 | 122 | ParamServer() { 123 | nh.param("/block_localization/odom_child_frame_id", odom_child_frame_id, "velodyne"); 124 | nh.param("/globalmap_server/globalmap_dir", globalmap_dir, ""); 125 | nh.param("/globalmap_server/downsample_resolution", downsample_resolution, 0.1); 126 | nh.param("/globalmap_server/yaml_path", yaml_path_, ""); 127 | nh.param("/globalmap_server/pgm_path", pgm_path_, ""); 128 | 129 | nh.param("/block_localization/lidarTopic", lidarTopic, "/points_raw"); 130 | nh.param("/block_localization/have_ring_time_channel", have_ring_time_channel, true); 131 | nh.param("/block_localization/lidarMinRange", lidarMinRange, 0.2); 132 | nh.param("/block_localization/lidarMaxRange", lidarMaxRange, 100.0); 133 | nh.param("/block_localization/ndt_neighbor_search_method", ndt_neighbor_search_method, "DIRECT7"); 134 | nh.param("/block_localization/ndt_resolution", ndt_resolution, 1.0); 135 | nh.param("/block_localization/ndt_epsilon", ndt_epsilon, 0.01); 136 | 137 | nh.param("/block_localization/imuTopic", imuTopic, "/imu_raw"); 138 | nh.param("/block_localization/imuAccNoise", imuAccNoise, 0.01); 139 | nh.param("/block_localization/imuGyrNoise", imuGyrNoise, 0.001); 140 | nh.param("/block_localization/gravity", gravity, 9.8); 141 | nh.param("/block_localization/imuFrequency", imuFrequency, 100); 142 | nh.param>("/block_localization/extrinsicRot", extRotV, vector()); 143 | nh.param>("/block_localization/extrinsicRPY", extRPYV, vector()); 144 | nh.param>("/block_localization/extrinsicTrans", extTransV, vector()); 145 | nh.param>("/block_localization/L2gt_t", L2GtTransV, vector()); 146 | nh.param>("/block_localization/L2gt_R", L2GtRotV, vector()); 147 | extRot = Eigen::Map>(extRotV.data(), 3, 3); 148 | extRPY = Eigen::Map>(extRPYV.data(), 3, 3); 149 | extTrans = Eigen::Map>(extTransV.data(), 3, 1); 150 | extQRPY = Eigen::Quaterniond(extRPY).inverse(); 151 | 152 | L2GtTrans = Eigen::Map>(L2GtTransV.data(), 3, 1); 153 | L2GtRot = Eigen::Map>(L2GtRotV.data(), 3, 3); 154 | 155 | nh.param("/block_localization/key_interval", key_interval, 0.1); 156 | nh.param("/block_localization/mapqry_interval", mapqry_interval, 1.0); 157 | 158 | nh.param("/block_localization/active_factors", active_factors, 120); 159 | nh.param("/block_localization/preserve_factors", preserve_factors, 10); 160 | nh.param("/block_localization/odomLinearNoise", odomLinearNoise, 0.01); 161 | nh.param("/block_localization/odomAngularNoise", odomAngularNoise, 0.01); 162 | nh.param("/block_localization/penalty_thres", penalty_thres, 1.0); 163 | nh.param("/block_localization/penalty_weight", penalty_weight, 1.0); 164 | } 165 | 166 | 167 | sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in) 168 | { 169 | sensor_msgs::Imu imu_out = imu_in; 170 | // rotate acceleration 171 | Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); 172 | acc = extRot * acc; 173 | imu_out.linear_acceleration.x = acc.x(); 174 | imu_out.linear_acceleration.y = acc.y(); 175 | imu_out.linear_acceleration.z = acc.z(); 176 | // rotate gyroscope 177 | Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); 178 | gyr = extRot * gyr; 179 | imu_out.angular_velocity.x = gyr.x(); 180 | imu_out.angular_velocity.y = gyr.y(); 181 | imu_out.angular_velocity.z = gyr.z(); 182 | // rotate roll pitch yaw 183 | Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z); 184 | Eigen::Quaterniond q_final = q_from * extQRPY; 185 | imu_out.orientation.x = q_final.x(); 186 | imu_out.orientation.y = q_final.y(); 187 | imu_out.orientation.z = q_final.z(); 188 | imu_out.orientation.w = q_final.w(); 189 | 190 | if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1) 191 | { 192 | ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!"); 193 | ros::shutdown(); 194 | } 195 | 196 | return imu_out; 197 | } 198 | }; 199 | 200 | 201 | template 202 | sensor_msgs::PointCloud2 publishCloud(const ros::Publisher& thisPub, const T& thisCloud, ros::Time thisStamp, std::string thisFrame) 203 | { 204 | sensor_msgs::PointCloud2 tempCloud; 205 | pcl::toROSMsg(*thisCloud, tempCloud); 206 | tempCloud.header.stamp = thisStamp; 207 | tempCloud.header.frame_id = thisFrame; 208 | if (thisPub.getNumSubscribers() != 0) 209 | thisPub.publish(tempCloud); 210 | return tempCloud; 211 | } 212 | 213 | 214 | template 215 | double getROSTime(T msg) { 216 | return msg->header.stamp.toSec(); 217 | } 218 | 219 | 220 | template 221 | void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z) 222 | { 223 | *angular_x = thisImuMsg->angular_velocity.x; 224 | *angular_y = thisImuMsg->angular_velocity.y; 225 | *angular_z = thisImuMsg->angular_velocity.z; 226 | } 227 | 228 | 229 | template 230 | void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z) 231 | { 232 | *acc_x = thisImuMsg->linear_acceleration.x; 233 | *acc_y = thisImuMsg->linear_acceleration.y; 234 | *acc_z = thisImuMsg->linear_acceleration.z; 235 | } 236 | 237 | 238 | template 239 | void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw) { 240 | double imuRoll, imuPitch, imuYaw; 241 | tf::Quaternion orientation; 242 | tf::quaternionMsgToTF(thisImuMsg->orientation, orientation); 243 | tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); 244 | 245 | *rosRoll = imuRoll; 246 | *rosPitch = imuPitch; 247 | *rosYaw = imuYaw; 248 | } 249 | 250 | 251 | float pointDistance(PointT p) 252 | { 253 | return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); 254 | } 255 | 256 | #endif -------------------------------------------------------------------------------- /launch/run_m2dgr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/run_nclt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /msg/cloud_info.msg: -------------------------------------------------------------------------------- 1 | # Cloud Info 2 | Header header 3 | 4 | float32 startOrientation 5 | float32 endOrientation 6 | float32 orientationDiff 7 | 8 | int64 imuAvailable 9 | int64 odomAvailable 10 | 11 | float32 imuRollInit 12 | float32 imuPitchInit 13 | float32 imuYawInit 14 | 15 | # Initial guess for next ndt 16 | float32 initialGuessX 17 | float32 initialGuessY 18 | float32 initialGuessZ 19 | float32 initialGuessRoll 20 | float32 initialGuessPitch 21 | float32 initialGuessYaw 22 | 23 | # Point cloud messages 24 | sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | block_localization 4 | 0.0.0 5 | The Block-Map-Based Localization Package 6 | 7 | Yixiao Feng 8 | 9 | BSD-3-Clause license 10 | 11 | catkin 12 | 13 | roscpp 14 | roscpp 15 | rospy 16 | rospy 17 | 18 | nodelet 19 | nodelet 20 | 21 | tf 22 | tf 23 | 24 | pcl_ros 25 | pcl_ros 26 | 27 | std_msgs 28 | std_msgs 29 | sensor_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | geometry_msgs 33 | nav_msgs 34 | nav_msgs 35 | map_server 36 | map_server 37 | actionlib 38 | actionlib 39 | 40 | message_generation 41 | message_generation 42 | message_runtime 43 | message_runtime 44 | 45 | ndt_omp 46 | ndt_omp 47 | GTSAM 48 | GTSAM 49 | 50 | yaml-cpp 51 | yaml-cpp 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /rviz/block_localization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 125 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 1021 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: GlobalMap 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Class: rviz/Group 36 | Displays: 37 | - Alpha: 1 38 | Autocompute Intensity Bounds: true 39 | Autocompute Value Bounds: 40 | Max Value: 25.970352172851562 41 | Min Value: -133.18020629882812 42 | Value: true 43 | Axis: X 44 | Channel Name: intensity 45 | Class: rviz/PointCloud2 46 | Color: 255; 255; 255 47 | Color Transformer: Intensity 48 | Decay Time: 0 49 | Enabled: true 50 | Invert Rainbow: false 51 | Max Color: 255; 255; 255 52 | Min Color: 0; 0; 0 53 | Name: CurrPoint 54 | Position Transformer: XYZ 55 | Queue Size: 10 56 | Selectable: true 57 | Size (Pixels): 3 58 | Size (m): 0.15000000596046448 59 | Style: Flat Squares 60 | Topic: /cloud_deskewed 61 | Unreliable: false 62 | Use Fixed Frame: true 63 | Use rainbow: true 64 | Value: true 65 | - Class: rviz/TF 66 | Enabled: true 67 | Filter (blacklist): "" 68 | Filter (whitelist): "" 69 | Frame Timeout: 50 70 | Frames: 71 | All Enabled: true 72 | Marker Alpha: 1 73 | Marker Scale: 10 74 | Name: TF 75 | Show Arrows: false 76 | Show Axes: true 77 | Show Names: true 78 | Tree: 79 | {} 80 | Update Interval: 0 81 | Value: true 82 | - Alpha: 1 83 | Buffer Length: 1 84 | Class: rviz/Path 85 | Color: 0; 255; 127 86 | Enabled: true 87 | Head Diameter: 0.30000001192092896 88 | Head Length: 0.20000000298023224 89 | Length: 0.30000001192092896 90 | Line Style: Billboards 91 | Line Width: 0.6000000238418579 92 | Name: Path 93 | Offset: 94 | X: 0 95 | Y: 0 96 | Z: 0 97 | Pose Color: 255; 85; 255 98 | Pose Style: None 99 | Queue Size: 10 100 | Radius: 0.029999999329447746 101 | Shaft Diameter: 0.10000000149011612 102 | Shaft Length: 0.10000000149011612 103 | Topic: /path 104 | Unreliable: false 105 | Value: true 106 | Enabled: true 107 | Name: Localization 108 | - Class: rviz/Group 109 | Displays: 110 | - Alpha: 1 111 | Autocompute Intensity Bounds: true 112 | Autocompute Value Bounds: 113 | Max Value: 41.27330017089844 114 | Min Value: -11.287060737609863 115 | Value: true 116 | Axis: Z 117 | Channel Name: z 118 | Class: rviz/PointCloud2 119 | Color: 255; 255; 255 120 | Color Transformer: Intensity 121 | Decay Time: 0 122 | Enabled: true 123 | Invert Rainbow: false 124 | Max Color: 255; 255; 255 125 | Min Color: 245; 121; 0 126 | Name: GlobalMap 127 | Position Transformer: XYZ 128 | Queue Size: 10 129 | Selectable: true 130 | Size (Pixels): 1 131 | Size (m): 0.05000000074505806 132 | Style: Flat Squares 133 | Topic: /globalmap 134 | Unreliable: false 135 | Use Fixed Frame: true 136 | Use rainbow: true 137 | Value: true 138 | Enabled: true 139 | Name: Map 140 | Enabled: true 141 | Global Options: 142 | Background Color: 0; 0; 0 143 | Default Light: true 144 | Fixed Frame: globalmap_link 145 | Frame Rate: 30 146 | Name: root 147 | Tools: 148 | - Class: rviz/Interact 149 | Hide Inactive Objects: true 150 | - Class: rviz/MoveCamera 151 | - Class: rviz/Select 152 | - Class: rviz/FocusCamera 153 | - Class: rviz/Measure 154 | - Class: rviz/SetInitialPose 155 | Theta std deviation: 0.2617993950843811 156 | Topic: /initialpose 157 | X std deviation: 0.5 158 | Y std deviation: 0.5 159 | - Class: rviz/SetGoal 160 | Topic: /move_base_simple/goal 161 | - Class: rviz/PublishPoint 162 | Single click: true 163 | Topic: /clicked_point 164 | Value: true 165 | Views: 166 | Current: 167 | Class: rviz/XYOrbit 168 | Distance: 132.33316040039062 169 | Enable Stereo Rendering: 170 | Stereo Eye Separation: 0.05999999865889549 171 | Stereo Focal Distance: 1 172 | Swap Stereo Eyes: false 173 | Value: false 174 | Field of View: 0.7853981852531433 175 | Focal Point: 176 | X: 22.95223617553711 177 | Y: 11.352641105651855 178 | Z: -0.03150531277060509 179 | Focal Shape Fixed Size: true 180 | Focal Shape Size: 0.05000000074505806 181 | Invert Z Axis: false 182 | Name: Current View 183 | Near Clip Distance: 0.009999999776482582 184 | Pitch: 1.5497963428497314 185 | Target Frame: velodyne 186 | Yaw: 1.8272769451141357 187 | Saved: ~ 188 | Window Geometry: 189 | Displays: 190 | collapsed: true 191 | Height: 1384 192 | Hide Left Dock: true 193 | Hide Right Dock: true 194 | QMainWindow State: 000000ff00000000fd00000004000000000000025e000004b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000004b7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000098b00000051fc0100000002fb0000000800540069006d006501000000000000098b000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000098b000004b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 195 | Selection: 196 | collapsed: false 197 | Time: 198 | collapsed: false 199 | Tool Properties: 200 | collapsed: false 201 | Views: 202 | collapsed: true 203 | Width: 2443 204 | X: 134 205 | Y: 76 206 | -------------------------------------------------------------------------------- /src/block_localization_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | #include "block_localization/pose_estimator.hpp" 3 | #include "block_localization/queryMap.h" 4 | #include "block_localization/cloud_info.h" 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace gtsam; 26 | using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 27 | using symbol_shorthand::V; // Vel (xdot,ydot,zdot) 28 | using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) 29 | 30 | 31 | namespace block_localization { 32 | 33 | class BlockLocalizationNodelet : public ParamServer, public nodelet::Nodelet 34 | { 35 | public: 36 | BlockLocalizationNodelet() { 37 | } 38 | virtual ~BlockLocalizationNodelet() { 39 | } 40 | 41 | 42 | void onInit() override { 43 | nh = getNodeHandle(); 44 | 45 | imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z())); // T_imu_lidar 46 | lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z())); // T_lidar_imu 47 | // lidar2gt = gtsam::Pose3(gtsam::Rot3(L2GtRot), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z())); // T_lidar_gt 48 | lidar2gt = gtsam::Pose3(gtsam::Rot3(L2GtRot), gtsam::Point3(L2GtTrans.x(), L2GtTrans.y(), L2GtTrans.z())); 49 | 50 | imu_sub = nh.subscribe(imuTopic, 2000, &BlockLocalizationNodelet::imuHandler, this, ros::TransportHints().tcpNoDelay()); 51 | points_sub = nh.subscribe("/cloud_info", 5, &BlockLocalizationNodelet::blocMainProcess, this, ros::TransportHints().tcpNoDelay()); 52 | globalmap_sub = nh.subscribe("/globalmap", 1, &BlockLocalizationNodelet::globalmapCB, this, ros::TransportHints().tcpNoDelay()); 53 | initialpose_sub = nh.subscribe("/initialpose", 8, &BlockLocalizationNodelet::initialposeCB, this, ros::TransportHints().tcpNoDelay()); 54 | 55 | pose_pub = nh.advertise("/global_odom", 2000); 56 | path_pub = nh.advertise("/path", 1); 57 | imuOdom_pub = nh.advertise("/imu_incremental", 2000); 58 | 59 | mapQuery_client = nh.serviceClient("/mapQuery", this); 60 | 61 | initializeParams(); 62 | } 63 | 64 | private: 65 | void initializeParams() { 66 | boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); 67 | voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 68 | downsample_filter = voxelgrid; 69 | 70 | pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); 71 | pclomp::GeneralizedIterativeClosestPoint::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint()); 72 | 73 | ndt->setTransformationEpsilon(ndt_epsilon); 74 | ndt->setResolution(ndt_resolution); 75 | if(ndt_neighbor_search_method == "DIRECT1") { 76 | NODELET_INFO("search_method DIRECT1 is selected"); 77 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); 78 | registration = ndt; 79 | } else if(ndt_neighbor_search_method == "DIRECT7") { 80 | NODELET_INFO("search_method DIRECT7 is selected"); 81 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); 82 | registration = ndt; 83 | 84 | } else if(ndt_neighbor_search_method == "GICP_OMP"){ 85 | NODELET_INFO("search_method GICP_OMP is selected"); 86 | registration = gicp; 87 | } 88 | else { 89 | if(ndt_neighbor_search_method == "KDTREE") { 90 | NODELET_INFO("search_method KDTREE is selected"); 91 | } else { 92 | NODELET_WARN("invalid search method was given"); 93 | NODELET_WARN("default method is selected (KDTREE)"); 94 | } 95 | ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); 96 | registration = ndt; 97 | } 98 | 99 | 100 | // initialize pose estimator 101 | if(private_nh.param("/block_localization/specify_init_pose", true)) 102 | { 103 | NODELET_INFO("Initialize pose estimator with specified parameters."); 104 | pose_estimator.reset(new block_localization::PoseEstimator(registration, 105 | ros::Time::now(), 106 | Eigen::Vector3d(private_nh.param("/block_localization/init_pos_x", 0.0), private_nh.param("/block_localization/init_pos_y", 0.0), private_nh.param("/block_localization/init_pos_z", 0.0)), 107 | Eigen::Quaterniond(private_nh.param("/block_localization/init_ori_w", 1.0), private_nh.param("/block_localization/init_ori_x", 0.0), 108 | private_nh.param("/block_localization/init_ori_y", 0.0), private_nh.param("/block_localization/init_ori_z", 0.0)), 109 | private_nh.param("/block_localization/cool_time_duration", 0.5) 110 | )); 111 | } 112 | 113 | // GTSAM settings 114 | boost::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(gravity); 115 | p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // acc white noise in continuous 116 | p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2); // gyro white noise in continuous 117 | p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); // error committed in integrating position from velocities 118 | gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // assume zero initial bias 119 | priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m 120 | priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e2); // m/s 121 | priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good 122 | correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1); // meter 123 | noiseModelBetweenBias = (gtsam::Vector(6) << 3.99395e-03, 3.99395e-03, 3.99395e-03, 1.56363e-03, 1.56363e-03, 1.56363e-03).finished(); 124 | imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 125 | imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); 126 | 127 | // poses output file 128 | foutC = ofstream(globalmap_dir + "poses.txt", ios::ate); 129 | foutC.setf(ios::fixed, ios::floatfield); 130 | 131 | key_count = 0; 132 | time_count = 0; 133 | time_sum = 0; 134 | systemInitialized = false; 135 | doneFirstOpt = false; 136 | } 137 | 138 | private: 139 | void imuHandler(const sensor_msgs::ImuConstPtr& imu_msg) { 140 | std::lock_guard lock(imu_data_mutex); 141 | 142 | sensor_msgs::Imu thisImu = imuConverter(*imu_msg); 143 | imuQueOpt.push_back(thisImu); 144 | imuQueImu.push_back(thisImu); 145 | 146 | if (!doneFirstOpt) 147 | return; 148 | 149 | double imuTime = getROSTime(&thisImu); 150 | double dt = (lastImuT_imu < 0) ? (1.0 / imuFrequency) : (imuTime - lastImuT_imu); 151 | if (dt < 1e-16) dt = 1e-16; 152 | 153 | lastImuT_imu = imuTime; 154 | 155 | // integrate this single imu message 156 | const auto& acc = thisImu.linear_acceleration; 157 | const auto& gyro = thisImu.angular_velocity; 158 | imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(acc.x, acc.y, acc.z), gtsam::Vector3(gyro.x, gyro.y, gyro.z), dt); 159 | 160 | // predict odometry 161 | gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 162 | 163 | // publish odometry 164 | nav_msgs::Odometry odom; 165 | odom.header.stamp = thisImu.header.stamp; 166 | odom.header.frame_id = "globalmap_link"; 167 | odom.child_frame_id = "odom_imu"; 168 | 169 | // transform imu pose to lidar 170 | gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position()); 171 | gtsam::Pose3 odomPose = imuPose.compose(imu2Lidar); 172 | 173 | odom.pose.pose.position.x = odomPose.translation().x(); 174 | odom.pose.pose.position.y = odomPose.translation().y(); 175 | odom.pose.pose.position.z = odomPose.translation().z(); 176 | odom.pose.pose.orientation.x = odomPose.rotation().toQuaternion().x(); 177 | odom.pose.pose.orientation.y = odomPose.rotation().toQuaternion().y(); 178 | odom.pose.pose.orientation.z = odomPose.rotation().toQuaternion().z(); 179 | odom.pose.pose.orientation.w = odomPose.rotation().toQuaternion().w(); 180 | 181 | odom.twist.twist.linear.x = currentState.velocity().x(); 182 | odom.twist.twist.linear.y = currentState.velocity().y(); 183 | odom.twist.twist.linear.z = currentState.velocity().z(); 184 | odom.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x(); 185 | odom.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y(); 186 | odom.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z(); 187 | imuOdom_pub.publish(odom); 188 | } 189 | 190 | 191 | void blocMainProcess(const block_localization::cloud_infoConstPtr& cloudinfo_msg) { 192 | sensor_msgs::PointCloud2 points_msg = cloudinfo_msg->cloud_deskewed; 193 | points_curr_time = getROSTime(&points_msg); 194 | 195 | if(firstkey) { 196 | // if (!first_query) 197 | // { 198 | // return; 199 | // } 200 | // first_query = false; 201 | points_pre_time = getROSTime(&points_msg); 202 | mapqry_pre_time = getROSTime(&points_msg); 203 | 204 | // NODELET_INFO("Global localization query..."); 205 | 206 | // if(queryGlobalLocalization(&points_msg)) { 207 | // NODELET_INFO("Global localization succeed!"); 208 | // } 209 | firstkey = false; 210 | } 211 | 212 | double time_interval = points_curr_time - points_pre_time; 213 | 214 | if(time_interval < key_interval) return; // select key frames according to time interval 215 | 216 | std::lock_guard estimator_lock(pose_estimator_mutex); 217 | 218 | if(!pose_estimator) { 219 | NODELET_ERROR("Waiting for initial pose input!"); 220 | return; 221 | } 222 | 223 | if(!globalmap) { 224 | NODELET_ERROR("Globalmap has not been received!"); 225 | return; 226 | } 227 | 228 | pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); 229 | pcl::fromROSMsg(points_msg, *pcl_cloud); 230 | 231 | if(pcl_cloud->empty()) { 232 | NODELET_ERROR("Cloud is empty!"); 233 | return; 234 | } 235 | 236 | // transform pointcloud into odom_child_frame_id 237 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 238 | if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) { 239 | NODELET_ERROR("Point cloud cannot be transformed into target frame!"); 240 | return; 241 | } 242 | auto filtered = downsample(cloud); 243 | 244 | // 0. initialize system 245 | if(!systemInitialized) { 246 | 247 | resetOptimization(); 248 | 249 | // pop old IMU message 250 | while (!imuQueOpt.empty()) { 251 | if (getROSTime(&imuQueOpt.front()) < points_curr_time) { 252 | lastImuT_opt = getROSTime(&imuQueOpt.front()); 253 | imuQueOpt.pop_front(); 254 | } 255 | else 256 | break; 257 | } 258 | 259 | // initial pose 260 | prevPose_ = prevPose_.compose(lidar2Imu); 261 | newgraph.add(PriorFactor(X(0), prevPose_, priorPoseNoise)); 262 | // query initial BM 263 | queryMapSwitch(prevPose_); 264 | // initial velocity 265 | prevVel_ = gtsam::Vector3(0, 0, 0); 266 | gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise); 267 | newgraph.add(priorVel); 268 | // initial bias 269 | prevBias_ = gtsam::imuBias::ConstantBias(); 270 | gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise); 271 | newgraph.add(priorBias); 272 | // add values 273 | initialEstimate.insert(X(0), prevPose_); 274 | initialEstimate.insert(V(0), prevVel_); 275 | initialEstimate.insert(B(0), prevBias_); 276 | 277 | optimizer->update(newgraph, initialEstimate); 278 | 279 | newgraph.resize(0); 280 | initialEstimate.clear(); 281 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); 282 | systemInitialized = true; 283 | key_count = 1; 284 | time_count = 1; 285 | return; 286 | } 287 | 288 | if (!prev_centroid.isApprox(curr_centroid, 1e-5)) { 289 | NODELET_INFO("Reset optimization."); 290 | // update registration target 291 | registration->setInputTarget(globalmap); 292 | // get updated noise before reset 293 | gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(X(key_count-1))); 294 | gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(V(key_count-1))); 295 | gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer->marginalCovariance(B(key_count-1))); 296 | // reset graph 297 | resetOptimization(); 298 | // add pose 299 | gtsam::PriorFactor priorPose(X(0), prevPose_, updatedPoseNoise); 300 | newgraph.add(priorPose); 301 | // add velocity 302 | gtsam::PriorFactor priorVel(V(0), prevVel_, updatedVelNoise); 303 | newgraph.add(priorVel); 304 | // add bias 305 | gtsam::PriorFactor priorBias(B(0), prevBias_, updatedBiasNoise); 306 | newgraph.add(priorBias); 307 | // add values 308 | initialEstimate.insert(X(0), prevPose_); 309 | initialEstimate.insert(V(0), prevVel_); 310 | initialEstimate.insert(B(0), prevBias_); 311 | // optimize once 312 | optimizer->update(newgraph, initialEstimate); 313 | newgraph.resize(0); 314 | initialEstimate.clear(); 315 | 316 | key_count = 1; 317 | prev_globalmap = globalmap; 318 | prev_centroid = curr_centroid; 319 | } 320 | 321 | // 1. ndt registration, imu integration and optimization 322 | gtsam::Pose3 poseFrom = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()), 323 | gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2))); 324 | // std::cout<<"prior value: "<correct(filtered); 327 | gtsam::Pose3 poseTo_beforeExt = gtsam::Pose3(gtsam::Rot3::Quaternion(pose_estimator->quat().w(),pose_estimator->quat().x(),pose_estimator->quat().y(),pose_estimator->quat().z()), 328 | gtsam::Point3(pose_estimator->pos()(0),pose_estimator->pos()(1),pose_estimator->pos()(2))); 329 | // std::cout<<"update value:"< lock(imu_data_mutex); 336 | while (!imuQueOpt.empty()) { 337 | sensor_msgs::Imu *thisImu = &imuQueOpt.front(); 338 | double imuTime = getROSTime(thisImu); 339 | if (imuTime < points_curr_time) { 340 | double dt = (lastImuT_opt < 0) ? (1 / imuFrequency) : (imuTime - lastImuT_opt); 341 | if (dt < 1e-16) dt = 1e-16; 342 | const auto& acc = thisImu->linear_acceleration; 343 | const auto& gyro = thisImu->angular_velocity; 344 | imuIntegratorOpt_->integrateMeasurement(gtsam::Vector3(acc.x, acc.y, acc.z), gtsam::Vector3(gyro.x, gyro.y, gyro.z), dt); 345 | 346 | lastImuT_opt = imuTime; 347 | imu_count++; 348 | sum_ang_vel += gyro.z; 349 | imuQueOpt.pop_front(); 350 | } else 351 | break; 352 | } 353 | average_ang_vel = sum_ang_vel / imu_count; 354 | // NODELET_INFO_STREAM("Average angular velocity on z-axis between two lidar frames: " << average_ang_vel << "[rad/s]"); 355 | 356 | gtsam::Pose3 poseTo = poseTo_beforeExt.compose(lidar2Imu); 357 | // add penalty for the effect of the large angular velocity on z-axis in ndt 358 | if(fabs(average_ang_vel) > penalty_thres) 359 | correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, penalty_weight * fabs(average_ang_vel / penalty_thres)); 360 | else 361 | correctionNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1); // meter 362 | // add pose factor 363 | gtsam::PriorFactor pose_factor(X(key_count), poseTo, correctionNoise); 364 | newgraph.add(pose_factor); 365 | 366 | // add imu factor 367 | const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast(*imuIntegratorOpt_); 368 | gtsam::ImuFactor imu_factor(X(key_count - 1), V(key_count - 1), X(key_count), V(key_count), B(key_count - 1), preint_imu); 369 | newgraph.add(imu_factor); 370 | // add imu bias 371 | newgraph.add(gtsam::BetweenFactor(B(key_count - 1), B(key_count), gtsam::imuBias::ConstantBias(), 372 | gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias))); 373 | gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); 374 | 375 | // incremental constraint 376 | if ((lidarPose.translation().z() - lidarPose_temp.translation().z())< -0.02) 377 | { 378 | gtsam::Quaternion quat(lidarPose.rotation().toQuaternion()); 379 | quat.normalized(); 380 | lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(lidarPose.translation().x(), lidarPose.translation().y(),lidarPose_temp.translation().z()-0.02)); 381 | } 382 | else if ((lidarPose.translation().z() - lidarPose_temp.translation().z()) > 0.02) 383 | { 384 | gtsam::Quaternion quat(lidarPose.rotation().toQuaternion()); 385 | quat.normalized(); 386 | lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(lidarPose.translation().x(), lidarPose.translation().y(),lidarPose_temp.translation().z()+0.02)); 387 | } 388 | 389 | if ((lidarPose.rotation().roll() - lidarPose_temp.rotation().roll())< -0.02) 390 | { 391 | 392 | gtsam::Rot3 newRotation = gtsam::Rot3::RzRyRx(lidarPose.rotation().roll() - 0.02, lidarPose.rotation().pitch(), lidarPose.rotation().yaw()); 393 | gtsam::Quaternion quat(newRotation.toQuaternion()); 394 | quat.normalized(); 395 | lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(lidarPose.translation().x(), lidarPose.translation().y(),lidarPose_temp.translation().z())); 396 | } 397 | else if ((lidarPose.rotation().roll() - lidarPose_temp.rotation().roll())> 0.02) 398 | { 399 | 400 | gtsam::Rot3 newRotation = gtsam::Rot3::RzRyRx(lidarPose.rotation().roll() + 0.02, lidarPose.rotation().pitch(), lidarPose.rotation().yaw()); 401 | gtsam::Quaternion quat(newRotation.toQuaternion()); 402 | quat.normalized(); 403 | lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(lidarPose.translation().x(), lidarPose.translation().y(),lidarPose_temp.translation().z())); 404 | } 405 | if ((lidarPose.rotation().pitch() - lidarPose_temp.rotation().pitch())< -0.02) 406 | { 407 | 408 | gtsam::Rot3 newRotation = gtsam::Rot3::RzRyRx(lidarPose.rotation().roll() , lidarPose.rotation().pitch()- 0.02, lidarPose.rotation().yaw()); 409 | gtsam::Quaternion quat(newRotation.toQuaternion()); 410 | quat.normalized(); 411 | lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(lidarPose.translation().x(), lidarPose.translation().y(),lidarPose_temp.translation().z())); 412 | } 413 | else if ((lidarPose.rotation().pitch() - lidarPose_temp.rotation().pitch())> 0.02) 414 | { 415 | 416 | gtsam::Rot3 newRotation = gtsam::Rot3::RzRyRx(lidarPose.rotation().roll() , lidarPose.rotation().pitch()+ 0.02, lidarPose.rotation().yaw()); 417 | gtsam::Quaternion quat(newRotation.toQuaternion()); 418 | quat.normalized(); 419 | lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(quat.w(),quat.x(),quat.y(),quat.z()),gtsam::Point3(lidarPose.translation().x(), lidarPose.translation().y(),lidarPose_temp.translation().z())); 420 | } 421 | // std::cout << "propState_.pose().z(): " << propState_.pose().z() << std::endl; 422 | 423 | noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << odomLinearNoise, odomLinearNoise, odomLinearNoise, odomAngularNoise, odomAngularNoise, odomAngularNoise).finished()); 424 | newgraph.add(BetweenFactor(X(key_count - 1), X(key_count), poseFrom.between(poseTo), odometryNoise)); 425 | 426 | // insert predicted values 427 | initialEstimate.insert(X(key_count), poseTo); 428 | initialEstimate.insert(V(key_count), propState_.v()); 429 | // Eigen::Vector3d z_zero(propState_.v()(0), propState_.v()(1), 0); // test in indoor,limit the z velocity to 0 430 | // initialEstimate.insert(V(key_count), z_zero); //test in indoor,limit the z velocity to 0 431 | initialEstimate.insert(B(key_count), prevBias_); 432 | 433 | // optimize 434 | optimizer->update(newgraph, initialEstimate); 435 | newgraph.resize(0); 436 | initialEstimate.clear(); 437 | 438 | isamCurrentEstimate = optimizer->calculateEstimate(); 439 | prevPose_ = isamCurrentEstimate.at(X(key_count)); 440 | prevVel_ = isamCurrentEstimate.at(V(key_count)); 441 | prevState_ = gtsam::NavState(prevPose_, prevVel_); 442 | prevBias_ = isamCurrentEstimate.at(B(key_count)); 443 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); 444 | 445 | pose_estimator->update(prevState_, prevBias_); 446 | 447 | // transform pose Twb * Tbl = Twl 448 | lidarPose = prevPose_.compose(imu2Lidar); 449 | lidarPose_temp=gtsam::Pose3(lidarPose); 450 | 451 | // 2. after optimization, re-propagate imu odometry preintegration 452 | prevStateOdom = prevState_; 453 | prevBiasOdom = prevBias_; 454 | double lastImuQT = -1; 455 | while (!imuQueImu.empty() && getROSTime(&imuQueImu.front()) < points_curr_time) { 456 | lastImuQT = getROSTime(&imuQueImu.front()); 457 | imuQueImu.pop_front(); 458 | } 459 | // repropagate 460 | if (!imuQueImu.empty()) { 461 | // reset bias use the newly optimized bias 462 | imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom); 463 | // integrate imu message from the beginning of this optimization 464 | for (int i = 0; i < (int)imuQueImu.size(); ++i) { 465 | sensor_msgs::Imu *thisImu = &imuQueImu[i]; 466 | double imuTime = getROSTime(thisImu); 467 | double dt = (lastImuQT < 0) ? (1.0 / imuFrequency) : (imuTime - lastImuQT); 468 | if (dt < 1e-16) dt = 1e-16; 469 | const auto& acc = thisImu->linear_acceleration; 470 | const auto& gyro = thisImu->angular_velocity; 471 | imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(acc.x, acc.y, acc.z), gtsam::Vector3(gyro.x, gyro.y, gyro.z), dt); 472 | lastImuQT = imuTime; 473 | } 474 | } 475 | 476 | // publish odometry and path 477 | publishGlobalOdomPath(points_msg.header.stamp, lidarPose); 478 | 479 | // save trajectory following TUM format 480 | 481 | // Twgt=Twl*Tlgt 482 | gt_pose=lidar2gt.compose(lidarPose); 483 | saveTUMTraj(points_curr_time, gt_pose); 484 | 485 | points_pre_time = points_curr_time; 486 | ++key_count; 487 | ++time_count; 488 | 489 | // the core step: check conditions for querying a new block map 490 | queryMapSwitch(lidarPose); 491 | 492 | doneFirstOpt = true; 493 | } 494 | 495 | 496 | bool queryGlobalLocalization(const sensor_msgs::PointCloud2ConstPtr& cloud_in) { 497 | NODELET_INFO("Querying global location."); 498 | ros::ServiceClient query_client = nh.serviceClient("/hdl_global_localization/query"); 499 | hdl_global_localization::QueryGlobalLocalization qry; 500 | qry.request.cloud = *cloud_in; 501 | qry.request.max_num_candidates = 1; 502 | query_client.call(qry); 503 | NODELET_INFO("Globallocalization queried!"); 504 | 505 | std::lock_guard lock(pose_estimator_mutex); 506 | const auto &p = qry.response.poses[0].position; 507 | const auto &q = qry.response.poses[0].orientation; 508 | pose_estimator.reset( 509 | new block_localization::PoseEstimator( 510 | registration, 511 | ros::Time::now(), 512 | Eigen::Vector3d(p.x, p.y, p.z), 513 | Eigen::Quaterniond(q.w, q.x, q.y, q.z), 514 | private_nh.param("/block_localization/cool_time_duration", 0.5)) 515 | ); 516 | if(!pose_estimator){ 517 | NODELET_INFO("Fail to get initial pose from global localizer!"); 518 | return false; 519 | } 520 | 521 | return true; 522 | } 523 | 524 | 525 | void globalmapCB(const sensor_msgs::PointCloud2ConstPtr& points_msg) { 526 | // NODELET_INFO("globalmap received!"); 527 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 528 | pcl::fromROSMsg(*points_msg, *cloud); 529 | if(!map_init) { 530 | globalmap = cloud; 531 | prev_globalmap = globalmap; 532 | registration->setInputTarget(globalmap); 533 | pcl::compute3DCentroid(*globalmap, curr_centroid); 534 | prev_centroid = curr_centroid; 535 | map_init = true; 536 | } else { 537 | globalmap = cloud; 538 | pcl::compute3DCentroid(*globalmap, curr_centroid); 539 | } 540 | } 541 | 542 | 543 | void initialposeCB(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) { 544 | NODELET_INFO("Initial pose received!"); 545 | std::lock_guard lock(pose_estimator_mutex); 546 | const auto& p = pose_msg->pose.pose.position; 547 | const auto& q = pose_msg->pose.pose.orientation; 548 | pose_estimator.reset( 549 | new block_localization::PoseEstimator( 550 | registration, 551 | ros::Time::now(), 552 | Eigen::Vector3d(p.x, p.y, p.z), 553 | Eigen::Quaterniond(q.w, q.x, q.y, q.z), 554 | private_nh.param("/block_localization/cool_time_duration", 0.5)) 555 | ); 556 | } 557 | 558 | 559 | pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { 560 | if(!downsample_filter) { 561 | return cloud; 562 | } 563 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 564 | downsample_filter->setInputCloud(cloud); 565 | downsample_filter->filter(*filtered); 566 | filtered->header = cloud->header; 567 | return filtered; 568 | } 569 | 570 | 571 | void publishGlobalOdomPath(ros::Time timestamp, gtsam::Pose3 pose) { 572 | Eigen::Quaterniond quat(pose.rotation().toQuaternion().w(), pose.rotation().toQuaternion().x(), pose.rotation().toQuaternion().y(), pose.rotation().toQuaternion().z()); 573 | quat.normalize(); 574 | geometry_msgs::Quaternion odom_quat; 575 | odom_quat.w = quat.w(); 576 | odom_quat.x = quat.x(); 577 | odom_quat.y = quat.y(); 578 | odom_quat.z = quat.z(); 579 | 580 | // broadcast transform over tf 581 | geometry_msgs::TransformStamped odom_trans; 582 | odom_trans.header.stamp = timestamp; 583 | odom_trans.header.frame_id = "globalmap_link"; 584 | odom_trans.child_frame_id = odom_child_frame_id; 585 | odom_trans.transform.translation.x = pose.x(); 586 | odom_trans.transform.translation.y = pose.y(); 587 | odom_trans.transform.translation.z = pose.z(); 588 | odom_trans.transform.rotation = odom_quat; 589 | pose_broadcaster.sendTransform(odom_trans); 590 | 591 | // publish odometry 592 | nav_msgs::Odometry laserOdometryROS; 593 | laserOdometryROS.header.stamp = timestamp; 594 | laserOdometryROS.header.frame_id = "globalmap_link"; 595 | laserOdometryROS.child_frame_id = "odom_bloc"; 596 | laserOdometryROS.pose.pose.position.x = pose.translation().x(); 597 | laserOdometryROS.pose.pose.position.y = pose.translation().y(); 598 | laserOdometryROS.pose.pose.position.z = pose.translation().z(); 599 | laserOdometryROS.pose.pose.orientation = odom_quat; 600 | pose_pub.publish(laserOdometryROS); 601 | 602 | geometry_msgs::PoseStamped laserPose; 603 | laserPose.header = laserOdometryROS.header; 604 | laserPose.pose = laserOdometryROS.pose.pose; 605 | odomPath.header.stamp = laserOdometryROS.header.stamp; 606 | odomPath.poses.push_back(laserPose); 607 | odomPath.header.frame_id = "globalmap_link"; 608 | path_pub.publish(odomPath); 609 | } 610 | 611 | 612 | void saveTUMTraj(double timestamp, gtsam::Pose3 pose) { 613 | ofstream foutC(globalmap_dir + "poses.txt", ios::app); 614 | foutC.setf(ios::fixed, ios::floatfield); 615 | foutC.precision(6); 616 | foutC << timestamp << " "; 617 | foutC.precision(6); 618 | 619 | Eigen::Quaterniond quat(pose.rotation().toQuaternion().w(), pose.rotation().toQuaternion().x(), pose.rotation().toQuaternion().y(), pose.rotation().toQuaternion().z()); 620 | quat.normalize(); 621 | auto trans = pose.translation(); 622 | foutC << trans.x() << " " << trans.y() << " " << trans.z() << " " << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << endl; 623 | } 624 | 625 | 626 | void queryMapSwitch(gtsam::Pose3 pose) { 627 | double time_elapsed = points_curr_time - mapqry_pre_time; 628 | 629 | if((time_elapsed < mapqry_interval) && systemInitialized) 630 | return; 631 | 632 | mapqry_pre_time = points_curr_time; 633 | 634 | ros::service::waitForService("/mapQuery"); 635 | block_localization::queryMap qry; 636 | qry.request.position.x = pose.x(); 637 | qry.request.position.y = pose.y(); 638 | qry.request.position.z = pose.z(); 639 | mapQuery_client.call(qry); 640 | } 641 | 642 | 643 | void resetOptimization() { 644 | // std::cout << "resetOptimization" << std::endl; 645 | gtsam::ISAM2Params optParameters; 646 | optParameters.relinearizeThreshold = 0.1; 647 | optParameters.relinearizeSkip = 1; 648 | optimizer = new gtsam::ISAM2(optParameters); 649 | gtsam::NonlinearFactorGraph newGraphFactors; 650 | newgraph = newGraphFactors; 651 | gtsam::Values NewGraphValues; 652 | initialEstimate = NewGraphValues; 653 | } 654 | 655 | 656 | private: 657 | // imu 658 | std::mutex imu_data_mutex; 659 | std::deque imuQueImu; 660 | std::deque imuQueOpt; 661 | double lastImuT_opt = -1; 662 | double lastImuT_imu = -1; 663 | 664 | // publishers and subscribers 665 | ros::Publisher pose_pub; 666 | ros::Publisher path_pub; 667 | ros::Publisher imuOdom_pub; 668 | nav_msgs::Path odomPath; 669 | ros::Subscriber imu_sub; 670 | ros::Subscriber points_sub; 671 | ros::Subscriber globalmap_sub; 672 | ros::Subscriber initialpose_sub; 673 | 674 | // sensors extrinsics 675 | gtsam::Pose3 imu2Lidar; 676 | gtsam::Pose3 lidar2Imu; 677 | gtsam::Pose3 lidar2gt; 678 | 679 | // system settings 680 | double points_pre_time, points_curr_time, mapqry_pre_time; 681 | 682 | // tf and frame_ids 683 | tf::TransformBroadcaster pose_broadcaster; 684 | tf::TransformListener tf_listener; 685 | 686 | // globalmap and registration method 687 | bool map_init = false; 688 | ros::ServiceClient mapQuery_client; 689 | pcl::PointCloud::Ptr prev_globalmap; 690 | pcl::PointCloud::Ptr globalmap; 691 | gtsam::Vector4 prev_centroid; 692 | gtsam::Vector4 curr_centroid; 693 | pcl::Filter::Ptr downsample_filter; 694 | pcl::Registration::Ptr registration; 695 | 696 | // pose estimator 697 | std::mutex pose_estimator_mutex; 698 | std::unique_ptr pose_estimator; 699 | 700 | // add penalty factor for the large ang vel 701 | int imu_count; 702 | double sum_ang_vel; 703 | double average_ang_vel; 704 | 705 | // processing time buffer 706 | double time_sum; 707 | double time_avg; 708 | 709 | // poses.txt 710 | ofstream foutC; 711 | 712 | 713 | public: 714 | bool first_query{true}; 715 | bool firstkey{true}; 716 | 717 | // GTSAM settings 718 | NonlinearFactorGraph newgraph; 719 | gtsam::ISAM2 *optimizer; 720 | gtsam::Values initialEstimate; 721 | gtsam::Values isamCurrentEstimate; 722 | gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_; 723 | gtsam::PreintegratedImuMeasurements *imuIntegratorImu_; 724 | bool systemInitialized; 725 | bool doneFirstOpt; 726 | int key_count; 727 | int time_count; 728 | 729 | gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; 730 | gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; 731 | gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; 732 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; 733 | gtsam::Vector noiseModelBetweenBias; 734 | 735 | gtsam::Pose3 prevPose_; 736 | gtsam::Vector3 prevVel_; 737 | gtsam::NavState prevState_; 738 | gtsam::imuBias::ConstantBias prevBias_; 739 | gtsam::NavState prevStateOdom; 740 | gtsam::imuBias::ConstantBias prevBiasOdom; 741 | gtsam::Pose3 lidarPose; 742 | gtsam::Pose3 lidarPose_temp; 743 | gtsam::Pose3 gt_pose; 744 | }; 745 | 746 | } 747 | 748 | 749 | PLUGINLIB_EXPORT_CLASS(block_localization::BlockLocalizationNodelet, nodelet::Nodelet) -------------------------------------------------------------------------------- /src/globalmap_server_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | #include "block_localization/queryMap.h" 3 | 4 | #include 5 | #include 6 | 7 | #ifdef HAVE_NEW_YAMLCPP 8 | template 9 | void operator >> (const YAML::Node& node, T& i) 10 | { 11 | i = node.as(); 12 | } 13 | #endif 14 | 15 | 16 | namespace block_localization { 17 | 18 | class GlobalmapServerNodelet : public ParamServer, public nodelet::Nodelet 19 | { 20 | public: 21 | GlobalmapServerNodelet() { 22 | } 23 | virtual ~GlobalmapServerNodelet() { 24 | } 25 | 26 | void onInit() override { 27 | nh = getNodeHandle(); 28 | 29 | loadAllMap(); 30 | 31 | loadMapCentroids(); 32 | 33 | // load initial BM (using the pose from global localization) 34 | globalmap = *loadMapFromIdx(0); 35 | // service for changing BM 36 | mapQueryServer = nh.advertiseService("/mapQuery", &GlobalmapServerNodelet::mapQueryCB, this); 37 | // publish globalmap with "latched" publisher 38 | globalmap_pub = nh.advertise("/globalmap", 5, true); 39 | globalmap_pub_timer = nh.createWallTimer(ros::WallDuration(mapqry_interval), &GlobalmapServerNodelet::pubMapOnce, this, false, true); 40 | } 41 | 42 | private: 43 | void pubMapOnce(const ros::WallTimerEvent& event) { 44 | sensor_msgs::PointCloud2 ros_cloud; 45 | if (!globalmap.empty()) { 46 | pcl::toROSMsg(globalmap, ros_cloud); 47 | globalmap_pub.publish(ros_cloud); 48 | } 49 | } 50 | 51 | 52 | bool mapQueryCB(block_localization::queryMap::Request &req, 53 | block_localization::queryMap::Response &res) { 54 | PointT searchPoint; 55 | searchPoint.x = req.position.x; 56 | searchPoint.y = req.position.y; 57 | searchPoint.z = req.position.z; 58 | // NODELET_INFO("K-nearest neighbor search at (%f, %f, %f).", searchPoint.x, searchPoint.y, searchPoint.z); 59 | 60 | // find nearest block map 61 | pointIdxNKNSearch.clear(); 62 | pointNKNSquareDistance.clear(); 63 | int k_nearest = centroid_kdtree.nearestKSearch(searchPoint, 2, pointIdxNKNSearch, pointNKNSquareDistance); 64 | if (k_nearest == 2) { 65 | globalmap.clear(); 66 | globalmap = (*loadMapFromIdx(pointIdxNKNSearch[0])) + (*loadMapFromIdx(pointIdxNKNSearch[1])); 67 | } else if (k_nearest == 1) { 68 | globalmap.clear(); 69 | globalmap = *loadMapFromIdx(pointIdxNKNSearch[0]); 70 | } // else BM doesn't need to be changed 71 | 72 | res.success = true; 73 | return true; 74 | } 75 | 76 | 77 | void loadAllMap() { 78 | auto t1 = ros::WallTime::now(); 79 | boost::format load_format("%03d.pcd"); 80 | 81 | for (int map_id = 0; ; map_id++) 82 | { 83 | pcl::PointCloud::Ptr tmp_cloud(new pcl::PointCloud()); 84 | std::string map_name = globalmap_dir + (load_format % map_id).str(); 85 | 86 | if (pcl::io::loadPCDFile(map_name, *tmp_cloud) == -1) { 87 | map_id -= 1; 88 | map_name = globalmap_dir + (load_format % map_id).str(); 89 | NODELET_WARN("The last map is: %s", map_name.c_str()); 90 | break; 91 | } 92 | 93 | tmp_cloud->header.frame_id = "globalmap_link"; 94 | 95 | // downsampling 96 | boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); 97 | voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 98 | voxelgrid->setInputCloud(tmp_cloud); 99 | pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud()); 100 | voxelgrid->filter(*filtered_cloud); 101 | tmp_cloud = filtered_cloud; 102 | 103 | globalmap_vec.push_back(tmp_cloud); 104 | } 105 | auto t2 = ros::WallTime::now(); 106 | NODELET_INFO("Globalmap server has already loaded %ld maps. Time cost: %f [msec]", globalmap_vec.size(), (t2 - t1).toSec() * 1000.0); 107 | } 108 | 109 | 110 | inline pcl::PointCloud::Ptr loadMapFromIdx(int map_idx) { 111 | // NODELET_INFO("Globalmap NO.%03d is already loaded!", map_idx); 112 | return globalmap_vec[map_idx]; 113 | } 114 | 115 | 116 | void loadMapCentroids() { 117 | std::string centroid_filename = globalmap_dir + "CentroidCloud.pcd"; 118 | 119 | centroid_cloud.reset(new pcl::PointCloud()); 120 | if (pcl::io::loadPCDFile(centroid_filename, *centroid_cloud) == -1) { 121 | NODELET_ERROR("Fail to load the centroid cloud! Please check your source!"); 122 | ros::shutdown(); 123 | } 124 | 125 | NODELET_INFO("Already loaded %ld centroids of block maps.", centroid_cloud->points.size()); 126 | 127 | if (centroid_cloud->empty()) { 128 | NODELET_ERROR("Fail to build a KD-Tree! Please check your centroid pointcloud!"); 129 | ros::shutdown(); 130 | } 131 | 132 | // build KD-Tree 133 | centroid_kdtree.setInputCloud(centroid_cloud); 134 | } 135 | 136 | 137 | void pubGridmap() { 138 | YAML::Node doc = YAML::LoadFile(yaml_path_); 139 | try 140 | { 141 | doc["resolution"] >> res_; 142 | doc["origin"][0] >> origin_[0]; 143 | doc["origin"][1] >> origin_[1]; 144 | doc["origin"][2] >> origin_[2]; 145 | doc["negate"] >> negate_; 146 | doc["occupied_thresh"] >> occ_th_; 147 | doc["free_thresh"] >> free_th_; 148 | } 149 | catch(YAML::InvalidScalar) 150 | { 151 | ROS_ERROR("The .yaml does not contain tags required or they are invalid."); 152 | ros::shutdown(); 153 | } 154 | mode_ = TRINARY; 155 | 156 | std::cout << " map name: " << map_name_ 157 | << "\n resolution: " << res_ 158 | << "\n origin: " << origin_[0] << ", " << origin_[1] << ", " << origin_[2] 159 | << "\n negate: " << negate_ 160 | << "\n occupied thresh: " << occ_th_ 161 | << "\n free thresh: " << free_th_ << std::endl; 162 | map_server::loadMapFromFile(&map_resp_, pgm_path_.c_str(), res_, negate_, occ_th_, free_th_, origin_, mode_); 163 | map_resp_.map.header.frame_id = "gridmap_link"; 164 | map_publisher_ = nh.advertise ("/gridmap", 1, true); 165 | map_publisher_.publish(map_resp_.map); 166 | } 167 | 168 | 169 | private: 170 | // map settings 171 | pcl::PointCloud globalmap; 172 | std::vector::Ptr> globalmap_vec; 173 | 174 | ros::ServiceServer mapQueryServer; 175 | ros::Publisher globalmap_pub; 176 | ros::WallTimer globalmap_pub_timer; 177 | 178 | // block map centroids 179 | pcl::PointCloud::Ptr centroid_cloud; 180 | 181 | // KD-Tree for block maps 182 | pcl::KdTreeFLANN centroid_kdtree; 183 | std::vector pointIdxNKNSearch; 184 | std::vector pointNKNSquareDistance; 185 | 186 | MapMode mode_; 187 | std::string map_name_; 188 | double res_; 189 | double origin_[3]; 190 | int negate_; 191 | double occ_th_, free_th_; 192 | nav_msgs::GetMap::Response map_resp_; 193 | ros::Publisher map_publisher_; 194 | }; 195 | 196 | } 197 | 198 | 199 | PLUGINLIB_EXPORT_CLASS(block_localization::GlobalmapServerNodelet, nodelet::Nodelet) -------------------------------------------------------------------------------- /src/points_preprocessing_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | #include "block_localization/cloud_info.h" 3 | 4 | const int queueLength = 2000; 5 | 6 | namespace block_localization { 7 | 8 | class PointsPreprocessingNodelet : public ParamServer, public nodelet::Nodelet 9 | { 10 | private: 11 | std::mutex imu_mtx; 12 | std::mutex odom_mtx; 13 | 14 | ros::Subscriber subImu; 15 | std::deque imuQueue; 16 | 17 | ros::Subscriber subOdom; 18 | std::deque odomQueue; 19 | 20 | ros::Subscriber subLidarCloud; 21 | ros::Publisher pubLidarCloud; 22 | ros::Publisher pubLidarCloudInfo; 23 | 24 | std::deque cloudQueue; 25 | sensor_msgs::PointCloud2 currentCloudMsg; 26 | pcl::PointCloud::Ptr lidarCloudIn; 27 | pcl::PointCloud::Ptr deskewedCloud; 28 | 29 | double *imuTime = new double[queueLength]; 30 | double *imuRotX = new double[queueLength]; 31 | double *imuRotY = new double[queueLength]; 32 | double *imuRotZ = new double[queueLength]; 33 | 34 | block_localization::cloud_info cloudInfo; 35 | double timeScanCur; 36 | double timeScanEnd; 37 | std_msgs::Header cloudHeader; 38 | 39 | int deskewFlag; 40 | 41 | bool odomDeskewFlag; 42 | float odomIncreX; 43 | float odomIncreY; 44 | float odomIncreZ; 45 | 46 | int imuPointerCur; 47 | bool firstPointFlag; 48 | Eigen::Affine3f transStartInverse; 49 | 50 | 51 | public: 52 | PointsPreprocessingNodelet() : deskewFlag(0) { 53 | } 54 | virtual ~PointsPreprocessingNodelet() { 55 | }; 56 | 57 | void onInit() override { 58 | nh = getNodeHandle(); 59 | 60 | subImu = nh.subscribe(imuTopic, 2000, &PointsPreprocessingNodelet::imuHandler, this, ros::TransportHints().tcpNoDelay()); 61 | subOdom = nh.subscribe("/imu_incremental", 2000, &PointsPreprocessingNodelet::odometryHandler, this, ros::TransportHints().tcpNoDelay()); 62 | subLidarCloud = nh.subscribe(lidarTopic, 5, &PointsPreprocessingNodelet::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 63 | 64 | pubLidarCloud = nh.advertise("/cloud_deskewed", 1); 65 | pubLidarCloudInfo = nh.advertise("/cloud_info", 1); 66 | 67 | allocateMemory(); 68 | 69 | pcl::console::setVerbosityLevel(pcl::console::L_ERROR); 70 | } 71 | 72 | 73 | void allocateMemory() 74 | { 75 | lidarCloudIn.reset(new pcl::PointCloud()); 76 | deskewedCloud.reset(new pcl::PointCloud()); 77 | 78 | resetParameters(); 79 | } 80 | 81 | 82 | void resetParameters() 83 | { 84 | lidarCloudIn->clear(); 85 | deskewedCloud->clear(); 86 | 87 | imuPointerCur = 0; 88 | firstPointFlag = true; 89 | odomDeskewFlag = false; 90 | 91 | for (int i = 0; i < queueLength; ++i) 92 | { 93 | imuTime[i] = 0; 94 | imuRotX[i] = 0; 95 | imuRotY[i] = 0; 96 | imuRotZ[i] = 0; 97 | } 98 | } 99 | 100 | 101 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg) { 102 | sensor_msgs::Imu thisImu = imuConverter(*imuMsg); 103 | 104 | std::lock_guard lock(imu_mtx); 105 | imuQueue.push_back(thisImu); 106 | } 107 | 108 | 109 | void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) { 110 | std::lock_guard lock(odom_mtx); 111 | odomQueue.push_back(*odomMsg); 112 | } 113 | 114 | 115 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& cloudMsg) { 116 | if (!cachePointCloud(cloudMsg)) 117 | return; 118 | 119 | if (!deskewInfo()) 120 | return; 121 | 122 | projectPointCloud(); 123 | 124 | publishClouds(); 125 | 126 | resetParameters(); 127 | } 128 | 129 | 130 | bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg) { 131 | cloudQueue.push_back(*cloudMsg); 132 | if (cloudQueue.size() <= 2) 133 | return false; 134 | 135 | currentCloudMsg = std::move(cloudQueue.front()); 136 | cloudQueue.pop_front(); 137 | pcl::moveFromROSMsg(currentCloudMsg, *lidarCloudIn); 138 | 139 | // for no ring and time channel pointcloud 140 | if (!have_ring_time_channel) 141 | { 142 | bool halfPassed = false; 143 | int cloudNum = lidarCloudIn->points.size(); 144 | 145 | cloudInfo.startOrientation = -atan2(lidarCloudIn->points[0].y, lidarCloudIn->points[0].x); 146 | cloudInfo.endOrientation = -atan2(lidarCloudIn->points[lidarCloudIn->points.size() - 1].y, lidarCloudIn->points[lidarCloudIn->points.size() - 1].x) + 2 * M_PI; 147 | if (cloudInfo.endOrientation - cloudInfo.startOrientation > 3 * M_PI) { 148 | cloudInfo.endOrientation -= 2*M_PI; 149 | } 150 | else if (cloudInfo.endOrientation - cloudInfo.startOrientation < M_PI) { 151 | cloudInfo.endOrientation += 2 * M_PI; 152 | } 153 | cloudInfo.orientationDiff = cloudInfo.endOrientation - cloudInfo.startOrientation; 154 | PointT point; 155 | for (int i = 0; i < cloudNum; ++i) 156 | { 157 | point.x = lidarCloudIn->points[i].x; 158 | point.y = lidarCloudIn->points[i].y; 159 | point.z = lidarCloudIn->points[i].z; 160 | float ori = -atan2(point.y, point.x); 161 | if (!halfPassed) { 162 | if (ori < cloudInfo.startOrientation - M_PI / 2) { 163 | ori += 2 * M_PI; 164 | } else if (ori > cloudInfo.startOrientation + M_PI * 3 / 2) { 165 | ori -= 2 * M_PI; 166 | } 167 | if (ori - cloudInfo.startOrientation > M_PI) { 168 | halfPassed = true; 169 | } 170 | } else { 171 | ori += 2 * M_PI; 172 | if (ori < cloudInfo.endOrientation - M_PI * 3 / 2) { 173 | ori += 2 * M_PI; 174 | } else if (ori > cloudInfo.endOrientation + M_PI / 2) { 175 | ori -= 2 * M_PI; 176 | } 177 | } 178 | float relTime = (ori - cloudInfo.startOrientation) / cloudInfo.orientationDiff; 179 | 180 | lidarCloudIn->points[i].time = 0.1 * relTime; 181 | } 182 | deskewFlag = 1; 183 | } 184 | else 185 | { 186 | // check ring channel 187 | static int ringFlag = 0; 188 | if (ringFlag == 0) { 189 | ringFlag = -1; 190 | for (auto &field : currentCloudMsg.fields) { 191 | if (field.name == "ring") { 192 | ringFlag = 1; 193 | break; 194 | } 195 | } 196 | if (ringFlag == -1) { 197 | NODELET_ERROR("Point cloud ring channel not available, please configure your point cloud data!"); 198 | ros::shutdown(); 199 | } 200 | } 201 | 202 | // check point time 203 | if (deskewFlag == 0) 204 | { 205 | deskewFlag = -1; 206 | for (auto &field : currentCloudMsg.fields) 207 | { 208 | if (field.name == "time" || field.name == "t") 209 | { 210 | deskewFlag = 1; 211 | break; 212 | } 213 | } 214 | if (deskewFlag == -1) 215 | NODELET_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!"); 216 | } 217 | } 218 | cloudHeader = currentCloudMsg.header; 219 | timeScanCur = cloudHeader.stamp.toSec(); 220 | timeScanEnd = timeScanCur + lidarCloudIn->points.back().time; 221 | 222 | return true; 223 | } 224 | 225 | 226 | bool deskewInfo() { 227 | std::lock_guard lock1(imu_mtx); 228 | std::lock_guard lock2(odom_mtx); 229 | 230 | // make sure Imu data available for the scan 231 | if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd) 232 | { 233 | NODELET_DEBUG("Waiting for IMU data ..."); 234 | return false; 235 | } 236 | 237 | imuDeskewInfo(); 238 | 239 | odomDeskewInfo(); 240 | 241 | return true; 242 | } 243 | 244 | 245 | void imuDeskewInfo() { 246 | cloudInfo.imuAvailable = false; 247 | 248 | // pop out imu before last cloud 249 | while (!imuQueue.empty()) 250 | { 251 | if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01) 252 | imuQueue.pop_front(); 253 | else 254 | break; 255 | } 256 | 257 | if (imuQueue.empty()) 258 | return; 259 | 260 | imuPointerCur = 0; 261 | 262 | for (auto &thisImuMsg : imuQueue) { 263 | double currentImuTime = thisImuMsg.header.stamp.toSec(); 264 | 265 | // get roll, pitch, and yaw estimation for this scan 266 | if (currentImuTime <= timeScanCur) 267 | imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit); 268 | 269 | if (currentImuTime > timeScanEnd + 0.01) 270 | break; 271 | 272 | if (imuPointerCur == 0) { 273 | imuRotX[0] = 0; 274 | imuRotY[0] = 0; 275 | imuRotZ[0] = 0; 276 | imuTime[0] = currentImuTime; 277 | ++imuPointerCur; 278 | continue; 279 | } 280 | 281 | // get angular velocity 282 | double angular_x, angular_y, angular_z; 283 | imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z); 284 | 285 | // integrate rotation 286 | double timeDiff = currentImuTime - imuTime[imuPointerCur-1]; 287 | imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff; 288 | imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff; 289 | imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff; 290 | imuTime[imuPointerCur] = currentImuTime; 291 | ++imuPointerCur; 292 | } 293 | 294 | --imuPointerCur; 295 | 296 | if (imuPointerCur <= 0) 297 | return; 298 | 299 | cloudInfo.imuAvailable = true; 300 | } 301 | 302 | 303 | void odomDeskewInfo() { 304 | cloudInfo.odomAvailable = false; 305 | 306 | while (!odomQueue.empty()) 307 | { 308 | if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01) 309 | odomQueue.pop_front(); 310 | else 311 | break; 312 | } 313 | 314 | if (odomQueue.empty()) 315 | return; 316 | 317 | if (odomQueue.front().header.stamp.toSec() > timeScanCur) 318 | return; 319 | 320 | // get start odometry at the beinning of the scan 321 | nav_msgs::Odometry startOdomMsg; 322 | 323 | for (int i = 0; i < (int)odomQueue.size(); ++i) 324 | { 325 | startOdomMsg = odomQueue[i]; 326 | 327 | if (getROSTime(&startOdomMsg) < timeScanCur) 328 | continue; 329 | else 330 | break; 331 | } 332 | 333 | tf::Quaternion orientation; 334 | tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation); 335 | 336 | double roll, pitch, yaw; 337 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 338 | 339 | // Initial guess 340 | cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x; 341 | cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y; 342 | cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z; 343 | cloudInfo.initialGuessRoll = roll; 344 | cloudInfo.initialGuessPitch = pitch; 345 | cloudInfo.initialGuessYaw = yaw; 346 | 347 | cloudInfo.odomAvailable = true; 348 | 349 | // get end odometry at the end of the scan 350 | odomDeskewFlag = false; 351 | 352 | if (odomQueue.back().header.stamp.toSec() < timeScanEnd) 353 | return; 354 | 355 | nav_msgs::Odometry endOdomMsg; 356 | 357 | for (int i = 0; i < (int)odomQueue.size(); ++i) 358 | { 359 | endOdomMsg = odomQueue[i]; 360 | 361 | if (getROSTime(&endOdomMsg) < timeScanEnd) 362 | continue; 363 | else 364 | break; 365 | } 366 | 367 | if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0]))) 368 | return; 369 | 370 | Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw); 371 | 372 | tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation); 373 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 374 | Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw); 375 | 376 | Eigen::Affine3f transBt = transBegin.inverse() * transEnd; 377 | 378 | float rollIncre, pitchIncre, yawIncre; 379 | pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre); 380 | 381 | odomDeskewFlag = true; 382 | } 383 | 384 | 385 | void projectPointCloud() { 386 | int cloudSize = lidarCloudIn->points.size(); 387 | 388 | for (int i = 0; i < cloudSize; ++i) { 389 | PointT thisPoint; 390 | thisPoint.x = lidarCloudIn->points[i].x; 391 | thisPoint.y = lidarCloudIn->points[i].y; 392 | thisPoint.z = lidarCloudIn->points[i].z; 393 | thisPoint.intensity = lidarCloudIn->points[i].intensity; 394 | 395 | float range = pointDistance(thisPoint); 396 | if (range < lidarMinRange || range > lidarMaxRange) 397 | continue; 398 | 399 | thisPoint = deskewPoint(&thisPoint, lidarCloudIn->points[i].time); 400 | deskewedCloud->push_back(thisPoint); 401 | } 402 | } 403 | 404 | 405 | void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur) { 406 | *rotXCur = 0; *rotYCur = 0; *rotZCur = 0; 407 | 408 | int imuPointerFront = 0; 409 | while (imuPointerFront < imuPointerCur) 410 | { 411 | if (pointTime < imuTime[imuPointerFront]) 412 | break; 413 | ++imuPointerFront; 414 | } 415 | 416 | if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0) 417 | { 418 | *rotXCur = imuRotX[imuPointerFront]; 419 | *rotYCur = imuRotY[imuPointerFront]; 420 | *rotZCur = imuRotZ[imuPointerFront]; 421 | } else { 422 | int imuPointerBack = imuPointerFront - 1; 423 | double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 424 | double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 425 | *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack; 426 | *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack; 427 | *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack; 428 | } 429 | } 430 | 431 | 432 | void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur) 433 | { 434 | *posXCur = 0; *posYCur = 0; *posZCur = 0; 435 | 436 | // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented. 437 | 438 | if (cloudInfo.odomAvailable == false || odomDeskewFlag == false) 439 | return; 440 | 441 | float ratio = relTime / (timeScanEnd - timeScanCur); 442 | 443 | *posXCur = ratio * odomIncreX; 444 | *posYCur = ratio * odomIncreY; 445 | *posZCur = ratio * odomIncreZ; 446 | } 447 | 448 | 449 | PointT deskewPoint(PointT *point, double relTime) { 450 | if (deskewFlag == -1 || cloudInfo.imuAvailable == false) 451 | return *point; 452 | 453 | double pointTime = timeScanCur + relTime; 454 | 455 | float rotXCur, rotYCur, rotZCur; 456 | findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur); 457 | 458 | float posXCur, posYCur, posZCur; 459 | findPosition(relTime, &posXCur, &posYCur, &posZCur); 460 | 461 | if (firstPointFlag == true) 462 | { 463 | transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse(); 464 | firstPointFlag = false; 465 | } 466 | 467 | // transform points to start 468 | Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur); 469 | Eigen::Affine3f transBt = transStartInverse * transFinal; 470 | 471 | PointT newPoint; 472 | newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3); 473 | newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3); 474 | newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3); 475 | newPoint.intensity = point->intensity; 476 | 477 | return newPoint; 478 | } 479 | 480 | 481 | void publishClouds() { 482 | cloudInfo.header = cloudHeader; 483 | cloudInfo.cloud_deskewed = publishCloud(pubLidarCloud, deskewedCloud, cloudHeader.stamp, cloudHeader.frame_id); 484 | pubLidarCloudInfo.publish(cloudInfo); 485 | } 486 | }; 487 | 488 | } 489 | 490 | PLUGINLIB_EXPORT_CLASS(block_localization::PointsPreprocessingNodelet, nodelet::Nodelet) -------------------------------------------------------------------------------- /srv/queryMap.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point position 2 | --- 3 | bool success --------------------------------------------------------------------------------