├── CMakeLists.txt ├── README.md ├── cmake ├── PCL.cmake ├── YAML.cmake ├── ceres.cmake ├── g2o.cmake ├── geographic.cmake ├── global_defination.cmake ├── glog.cmake ├── protobuf.cmake └── sophus.cmake ├── config ├── filtering │ └── kitti_filtering.yaml ├── mapping │ ├── back_end.yaml │ ├── front_end.yaml │ ├── lio_back_end.yaml │ ├── loop_closing.yaml │ └── viewer.yaml ├── matching │ ├── matching.yaml │ └── sliding_window.yaml └── scan_context │ ├── key_frames.proto │ ├── ring_keys.proto │ └── scan_contexts.proto ├── include └── lidar_localization │ ├── data_pretreat │ └── data_pretreat_flow.hpp │ ├── filtering │ ├── kitti_filtering.hpp │ └── kitti_filtering_flow.hpp │ ├── global_defination │ └── global_defination.h.in │ ├── mapping │ ├── back_end │ │ ├── lio_back_end.hpp │ │ └── lio_back_end_flow.hpp │ ├── front_end │ │ ├── front_end.hpp │ │ └── front_end_flow.hpp │ ├── loop_closing │ │ ├── loop_closing.hpp │ │ └── loop_closing_flow.hpp │ └── viewer │ │ ├── viewer.hpp │ │ └── viewer_flow.hpp │ ├── matching │ ├── back_end │ │ ├── sliding_window.hpp │ │ └── sliding_window_flow.hpp │ └── front_end │ │ ├── matching.hpp │ │ └── matching_flow.hpp │ ├── models │ ├── cloud_filter │ │ ├── box_filter.hpp │ │ ├── cloud_filter_interface.hpp │ │ ├── no_filter.hpp │ │ └── voxel_filter.hpp │ ├── graph_optimizer │ │ ├── g2o │ │ │ ├── edge │ │ │ │ ├── edge_prvag_imu_pre_integration.hpp │ │ │ │ ├── edge_prvag_odo_pre_integration.hpp │ │ │ │ ├── edge_prvag_prior_pos.hpp │ │ │ │ ├── edge_prvag_relative_pose.hpp │ │ │ │ ├── edge_se3_priorquat.hpp │ │ │ │ └── edge_se3_priorxyz.hpp │ │ │ ├── g2o_graph_optimizer.hpp │ │ │ └── vertex │ │ │ │ └── vertex_prvag.hpp │ │ └── interface_graph_optimizer.hpp │ ├── kalman_filter │ │ ├── error_state_kalman_filter.hpp │ │ └── kalman_filter.hpp │ ├── pre_integrator │ │ ├── imu_pre_integrator.hpp │ │ ├── odo_pre_integrator.hpp │ │ └── pre_integrator.hpp │ ├── registration │ │ ├── ndt_registration.hpp │ │ └── registration_interface.hpp │ ├── scan_adjust │ │ └── distortion_adjust.hpp │ ├── scan_context_manager │ │ ├── kdtree_vector_of_vectors_adaptor.hpp │ │ ├── key_frames.pb.h │ │ ├── nanoflann.hpp │ │ ├── ring_keys.pb.h │ │ ├── scan_context_manager.hpp │ │ └── scan_contexts.pb.h │ └── sliding_window │ │ ├── ceres_sliding_window.hpp │ │ ├── factors │ │ ├── factor_prvag_imu_pre_integration.hpp │ │ ├── factor_prvag_map_matching_pose.hpp │ │ ├── factor_prvag_marginalization.hpp │ │ └── factor_prvag_relative_pose.hpp │ │ ├── params │ │ └── param_prvag.hpp │ │ └── utils │ │ └── utils.hpp │ ├── publisher │ ├── cloud_publisher.hpp │ ├── imu_publisher.hpp │ ├── key_frame_publisher.hpp │ ├── key_frames_publisher.hpp │ ├── lidar_measurement_publisher.hpp │ ├── loop_pose_publisher.hpp │ ├── odometry_publisher.hpp │ ├── pos_vel_mag_publisher.hpp │ ├── pos_vel_publisher.hpp │ └── tf_broadcaster.hpp │ ├── sensor_data │ ├── cloud_data.hpp │ ├── gnss_data.hpp │ ├── imu_data.hpp │ ├── key_frame.hpp │ ├── lidar_measurement_data.hpp │ ├── loop_pose.hpp │ ├── pos_vel_data.hpp │ ├── pose_data.hpp │ └── velocity_data.hpp │ ├── subscriber │ ├── cloud_subscriber.hpp │ ├── gnss_subscriber.hpp │ ├── imu_subscriber.hpp │ ├── key_frame_subscriber.hpp │ ├── key_frames_subscriber.hpp │ ├── lidar_measurement_subscriber.hpp │ ├── loop_pose_subscriber.hpp │ ├── odometry_subscriber.hpp │ ├── pos_vel_subscriber.hpp │ └── velocity_subscriber.hpp │ ├── tf_listener │ └── tf_listener.hpp │ └── tools │ ├── CSVWriter.hpp │ ├── file_manager.hpp │ ├── print_info.hpp │ └── tic_toc.hpp ├── launch ├── kitti_localization.launch ├── lio_localization.launch └── lio_mapping.launch ├── msg ├── EKFStd.msg ├── ESKFStd.msg ├── IMUGNSSMeasurement.msg ├── LidarMeasurement.msg └── PosVel.msg ├── package.xml ├── rviz ├── filtering.rviz ├── lio_localization.rviz └── mapping.rviz ├── scripts └── kitti2bag.py ├── slam_data └── .gitignore ├── src ├── apps │ ├── data_pretreat_node.cpp │ ├── front_end_node.cpp │ ├── kitti_filtering_node.cpp │ ├── lio_back_end_node.cpp │ ├── lio_matching_node.cpp │ ├── loop_closing_node.cpp │ ├── sliding_window_node.cpp │ └── viewer_node.cpp ├── data_pretreat │ └── data_pretreat_flow.cpp ├── filtering │ ├── kitti_filtering.cpp │ └── kitti_filtering_flow.cpp ├── mapping │ ├── back_end │ │ ├── lio_back_end.cpp │ │ └── lio_back_end_flow.cpp │ ├── front_end │ │ ├── front_end.cpp │ │ └── front_end_flow.cpp │ ├── loop_closing │ │ ├── loop_closing.cpp │ │ └── loop_closing_flow.cpp │ └── viewer │ │ ├── viewer.cpp │ │ └── viewer_flow.cpp ├── matching │ ├── back_end │ │ ├── sliding_window.cpp │ │ └── sliding_window_flow.cpp │ └── front_end │ │ ├── matching.cpp │ │ └── matching_flow.cpp ├── models │ ├── cloud_filter │ │ ├── box_filter.cpp │ │ ├── no_filter.cpp │ │ └── voxel_filter.cpp │ ├── graph_optimizer │ │ ├── g2o │ │ │ └── g2o_graph_optimizer.cpp │ │ └── interface_graph_optimizer.cpp │ ├── kalman_filter │ │ ├── error_state_kalman_filter.cpp │ │ └── kalman_filter.cpp │ ├── pre_integrator │ │ ├── imu_pre_integrator.cpp │ │ └── odo_pre_integrator.cpp │ ├── registration │ │ └── ndt_registration.cpp │ ├── scan_adjust │ │ └── distortion_adjust.cpp │ ├── scan_context_manager │ │ ├── key_frames.pb.cpp │ │ ├── ring_keys.pb.cpp │ │ ├── scan_context_manager.cpp │ │ └── scan_contexts.pb.cpp │ └── sliding_window │ │ └── ceres_sliding_window.cpp ├── publisher │ ├── cloud_publisher.cpp │ ├── imu_publisher.cpp │ ├── key_frame_publisher.cpp │ ├── key_frames_publisher.cpp │ ├── lidar_measurement_publisher.cpp │ ├── loop_pose_publisher.cpp │ ├── odometry_publisher.cpp │ ├── pos_vel_publisher.cpp │ └── tf_broadcaster.cpp ├── sensor_data │ ├── gnss_data.cpp │ ├── imu_data.cpp │ ├── key_frame.cpp │ ├── loop_pose.cpp │ ├── pose_data.cpp │ └── velocity_data.cpp ├── subscriber │ ├── cloud_subscriber.cpp │ ├── gnss_subscriber.cpp │ ├── imu_subscriber.cpp │ ├── key_frame_subscriber.cpp │ ├── key_frames_subscriber.cpp │ ├── lidar_measurement_subscriber.cpp │ ├── loop_pose_subscriber.cpp │ ├── odometry_subscriber.cpp │ ├── pos_vel_subscriber.cpp │ └── velocity_subscriber.cpp ├── tf_listener │ └── tf_lisener.cpp └── tools │ ├── file_manager.cpp │ └── print_info.cpp └── srv ├── optimizeMap.srv ├── saveMap.srv ├── saveOdometry.srv └── saveScanContext.srv /README.md: -------------------------------------------------------------------------------- 1 | # multi_sensor_loclization_and_mapping 2 | It is the course work of [localization and mapping based on mutiple sensor fusion](https://www.shenlanxueyuan.com/course/558). Most of the codes are written by the tutors [Qian Ren] and [Yao Ge]. 3 | 4 | It consists of : 5 | 6 | 7 | - a mapping pipeline based on graph-based optimization, which utilze RTK, IMU, NDT-based Lidar odometry, and loop closure; 8 | - a localization pipeline based on ESKF, which utlized IMU, NDT-based map matching, motion model and velocity; 9 | - a localization pipeline based on graph optimization, which utlized IMU, NDT-based map matching, motion model and velocity. 10 | 11 | ## Dependencies 12 | 13 | 1. ROS 14 | 2. Eigen 15 | 3. PCL 16 | 4. g2o 17 | 5. ceres solver 18 | 6. Sophus 19 | 7. Protobuf 20 | 8. yaml 21 | 9. Geographic 22 | 10. Glog 23 | 24 | I installed the same version as the third parties in [SLAMbook](https://github.com/gaoxiang12/slambook). 25 | 26 | ## Build 27 | 28 | `catkin_make` 29 | 30 | ## Run 31 | 32 | ### Mapping 33 | 34 | Run mapping pipeline to build a global map for the environment. 35 | 36 | `roslaunch lidar_localization lio_mapping.launch` 37 | 38 | ### ESKF localization 39 | 40 | Run localization pipeline based on ESKF using the built map 41 | 42 | `roslaunch lidar_localization kitti_localization.launch` 43 | 44 | ### Graph-based localization 45 | Run localization pipeline based on graph optimization using the built map 46 | 47 | `roslaunch lidar_localization lio_localization.launch` 48 | -------------------------------------------------------------------------------- /cmake/PCL.cmake: -------------------------------------------------------------------------------- 1 | find_package(PCL 1.7 REQUIRED) 2 | list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") 3 | 4 | include_directories(${PCL_INCLUDE_DIRS}) 5 | list(APPEND ALL_TARGET_LIBRARIES ${PCL_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/YAML.cmake: -------------------------------------------------------------------------------- 1 | find_package(PkgConfig REQUIRED) 2 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 3 | include_directories(${YAML_CPP_INCLUDEDIR}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${YAML_CPP_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/ceres.cmake: -------------------------------------------------------------------------------- 1 | find_package (Ceres REQUIRED) 2 | 3 | include_directories(${CERES_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${CERES_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/geographic.cmake: -------------------------------------------------------------------------------- 1 | find_package (GeographicLib REQUIRED) 2 | 3 | include_directories(${GeographicLib_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${GeographicLib_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/global_defination.cmake: -------------------------------------------------------------------------------- 1 | set(WORK_SPACE_PATH ${PROJECT_SOURCE_DIR}) 2 | configure_file ( 3 | ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h.in 4 | ${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h) 5 | include_directories(${PROJECT_BINARY_DIR}/include) -------------------------------------------------------------------------------- /cmake/glog.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | 3 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 4 | 5 | if(WIN32) 6 | find_path(GLOG_INCLUDE_DIR glog/logging.h 7 | PATHS ${GLOG_ROOT_DIR}/src/windows) 8 | else() 9 | find_path(GLOG_INCLUDE_DIR glog/logging.h 10 | PATHS ${GLOG_ROOT_DIR}) 11 | endif() 12 | 13 | if(MSVC) 14 | find_library(GLOG_LIBRARY_RELEASE libglog_static 15 | PATHS ${GLOG_ROOT_DIR} 16 | PATH_SUFFIXES Release) 17 | 18 | find_library(GLOG_LIBRARY_DEBUG libglog_static 19 | PATHS ${GLOG_ROOT_DIR} 20 | PATH_SUFFIXES Debug) 21 | 22 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 23 | else() 24 | find_library(GLOG_LIBRARY glog 25 | PATHS ${GLOG_ROOT_DIR} 26 | PATH_SUFFIXES lib lib64) 27 | endif() 28 | 29 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 30 | 31 | if(GLOG_FOUND) 32 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 33 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 34 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 35 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 36 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 37 | endif() 38 | 39 | include_directories(${GLOG_INCLUDE_DIRS}) 40 | list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES}) 41 | -------------------------------------------------------------------------------- /cmake/protobuf.cmake: -------------------------------------------------------------------------------- 1 | find_package (Protobuf REQUIRED) 2 | 3 | include_directories(${Protobuf_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${Protobuf_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/sophus.cmake: -------------------------------------------------------------------------------- 1 | find_package (Sophus REQUIRED) 2 | 3 | include_directories(${Sophus_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES Sophus::Sophus) -------------------------------------------------------------------------------- /config/filtering/kitti_filtering.yaml: -------------------------------------------------------------------------------- 1 | # 全局地图 2 | map_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data/map/filtered_map.pcd 3 | global_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 4 | 5 | # 局部地图 6 | # 局部地图从全局地图切割得到，此处box_filter_size是切割区间 7 | # 参数顺序是min_x, max_x, min_y, max_y, min_z, max_z 8 | box_filter_size: [-150.0, 150.0, -150.0, 150.0, -150.0, 150.0] 9 | local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 10 | 11 | # 当前帧 12 | # no_filter指不对点云滤波，在匹配中，理论上点云越稠密，精度越高，但是速度也越慢 13 | # 所以提供这种不滤波的模式做为对比，以方便使用者去体会精度和效率随稠密度的变化关系 14 | current_scan_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter 15 | 16 | # loop closure for localization initialization/re-initialization: 17 | loop_closure_method: scan_context # 选择回环检测方法, 目前支持scan_context 18 | scan_context_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data/scan_context 19 | 20 | # 匹配 21 | registration_method: NDT # 选择点云匹配方法，目前支持：NDT 22 | 23 | # select fusion method for IMU-GNSS-Odo-Mag, available methods are: 24 | # 1. error_state_kalman_filter 25 | fusion_method: error_state_kalman_filter 26 | # select fusion strategy for IMU-GNSS-Odo-Mag, available methods are: 27 | # 1. pose_velocity 28 | fusion_strategy: pose_velocity 29 | 30 | # 各配置选项对应参数 31 | ## a. point cloud filtering: 32 | voxel_filter: 33 | global_map: 34 | leaf_size: [0.9, 0.9, 0.9] 35 | local_map: 36 | leaf_size: [0.5, 0.5, 0.5] 37 | current_scan: 38 | leaf_size: [1.5, 1.5, 1.5] 39 | ## b. scan context: 40 | scan_context: 41 | # a. ROI definition: 42 | max_radius: 80.0 43 | max_theta: 360.0 44 | # b. resolution: 45 | num_rings: 20 46 | num_sectors: 60 47 | # c. ring key indexing interval: 48 | indexing_interval: 1 49 | # d. min. key frame sequence distance: 50 | min_key_frame_seq_distance: 100 51 | # e. num. of nearest-neighbor candidates to check: 52 | num_candidates: 5 53 | # f. sector key fast alignment search ratio: 54 | # avoid brute-force match using sector key 55 | fast_alignment_search_ratio: 0.1 56 | # g. scan context distance threshold for proposal generation: 57 | # 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold 58 | # if not, recommend 0.1-0.15 59 | scan_context_distance_thresh: 0.15 60 | ## c. frontend matching 61 | NDT: 62 | res : 1.0 63 | step_size : 0.1 64 | trans_eps : 0.01 65 | max_iter : 30 66 | ## d. Kalman filter for IMU-lidar-GNSS fusion: 67 | ## d.1. Error-State Kalman filter for IMU-GNSS-Odo fusion: 68 | error_state_kalman_filter: 69 | earth: 70 | # gravity can be calculated from https://www.sensorsone.com/local-gravity-calculator/ using latitude and height: 71 | gravity_magnitude: 9.80943 72 | # rotation speed, rad/s: 73 | rotation_speed: 7.292115e-5 74 | # latitude: 75 | latitude: 48.9827703173 76 | covariance: 77 | prior: 78 | pos: 1.0e-6 79 | vel: 1.0e-6 80 | ori: 1.0e-6 81 | epsilon: 1.0e-6 82 | delta: 1.0e-6 83 | process: 84 | gyro: 1.0e-4 85 | accel: 2.5e-3 86 | bias_accel: 2.5e-3 87 | bias_gyro: 1.0e-4 88 | measurement: 89 | pose: 90 | pos: 1.0e-4 91 | ori: 1.0e-4 92 | pos: 1.0e-4 93 | vel: 2.5e-3 94 | motion_constraint: 95 | activated: true 96 | w_b_thresh: 0.13 -------------------------------------------------------------------------------- /config/mapping/back_end.yaml: -------------------------------------------------------------------------------- 1 | data_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data # 数据存放路径 2 | 3 | # 关键帧 4 | key_frame_distance: 2.0 # 关键帧距离 5 | 6 | # select backend optimizer, available optimizers are 7 | # 1. g2o 8 | # 2. TO-BE-ADDED gtsam 9 | graph_optimizer_type: g2o 10 | 11 | # config measurement used: 12 | # a. GNSS 13 | use_gnss: true 14 | # b. loop closure 15 | use_loop_close: true 16 | # c. IMU pre-integration 17 | use_imu: true 18 | 19 | optimize_step_with_key_frame: 10000 # 没有其他信息时，每隔 step 发送一次优化的位姿 20 | optimize_step_with_gnss: 950 # 每累计 step 个 gnss 观测时，优化一次 21 | optimize_step_with_loop: 100 # 每累计 step 个闭环约束时优化一次 22 | 23 | g2o_param: 24 | odom_edge_noise: [0.5, 0.5, 0.5, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch 25 | close_loop_noise: [0.3, 0.3, 0.3, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch 26 | gnss_noise: [2.0, 2.0, 2.0] # 噪声：x y z -------------------------------------------------------------------------------- /config/mapping/front_end.yaml: -------------------------------------------------------------------------------- 1 | # 匹配 2 | registration_method: NDT # 选择点云匹配方法，目前支持：NDT 3 | 4 | 5 | # 当前帧 6 | # no_filter指不对点云滤波，在匹配中，理论上点云越稠密，精度越高，但是速度也越慢 7 | # 所以提供这种不滤波的模式做为对比，以方便使用者去体会精度和效率随稠密度的变化关系 8 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter 9 | 10 | # 局部地图 11 | key_frame_distance: 2.0 # 关键帧距离 12 | local_frame_num: 20 13 | local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 14 | 15 | 16 | # 各配置选项对应参数 17 | ## 匹配相关参数 18 | NDT: 19 | res : 1.0 20 | step_size : 0.1 21 | trans_eps : 0.01 22 | max_iter : 30 23 | ## 滤波相关参数 24 | voxel_filter: 25 | local_map: 26 | leaf_size: [0.6, 0.6, 0.6] 27 | frame: 28 | leaf_size: [1.3, 1.3, 1.3] -------------------------------------------------------------------------------- /config/mapping/lio_back_end.yaml: -------------------------------------------------------------------------------- 1 | data_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data # 数据存放路径 2 | 3 | # 关键帧 4 | key_frame_distance: 2.0 # 关键帧距离 5 | 6 | # select backend optimizer, available optimizers are 7 | # 1. g2o 8 | graph_optimizer_type: g2o 9 | 10 | # config measurement used: 11 | # a. GNSS 12 | use_gnss: true 13 | # b. loop closure 14 | use_loop_close: true 15 | # c. IMU pre-integration 16 | use_imu_pre_integration: true 17 | # c. odo pre-integration 18 | use_odo_pre_integration: false 19 | 20 | optimization_step_size: 21 | key_frame: 50 # optimize per key frames 22 | loop_closure: 10 # optimize per loop closure detections 23 | 24 | g2o_param: 25 | odom_edge_noise: [2.5e-1, 2.5e-1, 2.5e-1, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch 26 | close_loop_noise: [2.5e-1, 2.5e-1, 2.5e-1, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch 27 | gnss_noise: [1.0, 1.0, 4.0] # 噪声：x y z 28 | 29 | imu_pre_integration: 30 | earth: 31 | # gravity can be calculated from https://www.sensorsone.com/local-gravity-calculator/ using latitude and height: 32 | gravity_magnitude: 9.80943 33 | covariance: 34 | measurement: 35 | accel: 2.5e-3 36 | gyro: 1.0e-4 37 | random_walk: 38 | accel: 1.0e-8 39 | gyro: 1.0e-8 40 | 41 | odo_pre_integration: 42 | covariance: 43 | measurement: 44 | v: 2.5e-3 45 | w: 1.0e-2 -------------------------------------------------------------------------------- /config/mapping/loop_closing.yaml: -------------------------------------------------------------------------------- 1 | data_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data # 数据存放路径 2 | 3 | registration_method: NDT # 选择点云匹配方法，目前支持：NDT 4 | loop_closure_method: scan_context # 选择回环检测方法, 目前支持scan_context 5 | 6 | # 匹配时为了精度更高，应该选用scan-to-map的方式 7 | # map是以历史帧为中心，往前后时刻各选取extend_frame_num个关键帧，放在一起拼接成的 8 | extend_frame_num: 5 9 | 10 | loop_step: 5 # 防止检测过于频繁，每隔loop_step个关键帧检测一次闭环 11 | diff_num: 100 12 | detect_area: 10.0 # 检测区域，只有两帧距离小于这个值，才做闭环匹配 13 | fitness_score_limit: 0.2 # 匹配误差小于这个值才认为是有效的 14 | 15 | # 之所以要提供no_filter（即不滤波）模式，是因为闭环检测对计算时间要求没那么高，而点云越稠密，精度就越高，所以滤波与否都有道理 16 | map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 17 | scan_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter 18 | 19 | # 各配置选项对应参数 20 | 21 | ## 滤波相关参数 22 | voxel_filter: 23 | map: 24 | leaf_size: [0.3, 0.3, 0.3] 25 | scan: 26 | leaf_size: [0.3, 0.3, 0.3] 27 | ## 匹配相关参数 28 | NDT: 29 | res : 1.0 30 | step_size : 0.1 31 | trans_eps : 0.01 32 | max_iter : 30 33 | ## ScanContext params: 34 | scan_context: 35 | # a. ROI definition: 36 | max_radius: 80.0 37 | max_theta: 360.0 38 | # b. resolution: 39 | num_rings: 20 40 | num_sectors: 60 41 | # c. ring key indexing interval: 42 | indexing_interval: 1 43 | # d. min. key frame sequence distance: 44 | min_key_frame_seq_distance: 100 45 | # e. num. of nearest-neighbor candidates to check: 46 | num_candidates: 5 47 | # f. sector key fast alignment search ratio: 48 | # avoid brute-force match using sector key 49 | fast_alignment_search_ratio: 0.1 50 | # g. scan context distance threshold for proposal generation: 51 | # 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold 52 | # if not, recommend 0.1-0.15 53 | scan_context_distance_thresh: 0.20 -------------------------------------------------------------------------------- /config/mapping/viewer.yaml: -------------------------------------------------------------------------------- 1 | data_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data # 数据存放路径 2 | 3 | # 全局地图 4 | global_map_filter: voxel_filter # 选择全局地图点云滤波方法，目前支持：voxel_filter 5 | 6 | # 局部地图 7 | local_frame_num: 20 8 | local_map_filter: voxel_filter # 选择滑窗小地图点云滤波方法，目前支持：voxel_filter 9 | 10 | # 当前帧 11 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter 12 | 13 | # 各配置选项对应参数 14 | ## 滤波相关参数 15 | voxel_filter: 16 | global_map: 17 | leaf_size: [0.5, 0.5, 0.5] 18 | local_map: 19 | leaf_size: [0.5, 0.5, 0.5] 20 | frame: 21 | leaf_size: [0.5, 0.5, 0.5] -------------------------------------------------------------------------------- /config/matching/matching.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # data storage: 3 | # 4 | # a. global map for relative pose estimation using lidar frontend: 5 | map_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data/map/filtered_map.pcd 6 | # b. scan context index for map matching: 7 | scan_context_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data/scan_context 8 | 9 | # 10 | # point cloud map & measurement processors 11 | # 12 | # a. global map filter: 13 | global_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 14 | 15 | # b.1. local map segmenter: 16 | box_filter_size: [-150.0, 150.0, -150.0, 150.0, -150.0, 150.0] # 参数顺序是min_x, max_x, min_y, max_y, min_z, max_z 17 | # b.2. local map filtering: 18 | local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 19 | 20 | # c. current frame filter 21 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter 22 | 23 | # 24 | # relative pose estimation using lidar frontend 25 | # 26 | registration_method: NDT # 选择点云匹配方法，目前支持：NDT 27 | 28 | # 29 | # map matching using scan context 30 | # 31 | map_matching_method: scan_context # available map matching methods: scan_context 32 | 33 | 34 | # 各配置选项对应参数 35 | ## 滤波相关参数 36 | voxel_filter: 37 | global_map: 38 | leaf_size: [0.9, 0.9, 0.9] 39 | local_map: 40 | leaf_size: [0.5, 0.5, 0.5] 41 | frame: 42 | leaf_size: [1.5, 1.5, 1.5] 43 | 44 | ## 匹配相关参数 45 | NDT: 46 | res : 1.0 47 | step_size : 0.1 48 | trans_eps : 0.01 49 | max_iter : 30 50 | 51 | ## ScanContext params: 52 | scan_context: 53 | # a. ROI definition: 54 | max_radius: 80.0 55 | max_theta: 360.0 56 | # b. resolution: 57 | num_rings: 20 58 | num_sectors: 60 59 | # c. ring key indexing interval: 60 | indexing_interval: 1 61 | # d. min. key frame sequence distance: 62 | min_key_frame_seq_distance: 100 63 | # e. num. of nearest-neighbor candidates to check: 64 | num_candidates: 5 65 | # f. sector key fast alignment search ratio: 66 | # avoid brute-force match using sector key 67 | fast_alignment_search_ratio: 0.1 68 | # g. scan context distance threshold for proposal generation: 69 | # 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold 70 | # if not, recommend 0.1-0.15 71 | scan_context_distance_thresh: 0.15 -------------------------------------------------------------------------------- /config/matching/sliding_window.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # data output path: 3 | # 4 | data_path: /home/rick/cat_ws/src/multi_sensor_localization_and_mapping/slam_data # 数据存放路径 5 | 6 | # 7 | # key frame detection 8 | # 9 | key_frame: 10 | # max. distance between two key frames: 11 | max_distance: 0.25 12 | # max. time interval between two key frames: 13 | max_interval: 0.10 14 | 15 | # 16 | # sliding window size: 17 | # 18 | sliding_window_size: 30 19 | 20 | # 21 | # select measurements: 22 | # 23 | measurements: 24 | map_matching: true 25 | imu_pre_integration: true 26 | 27 | # 28 | # measurement configs: 29 | # 30 | lidar_odometry: 31 | noise: [2.5e-1, 2.5e-1, 2.5e-1, 0.001, 0.001, 0.001] # x y z yaw roll pitch 32 | 33 | map_matching: 34 | noise: [2.5e-1, 2.5e-1, 2.5e-1, 0.001, 0.001, 0.001] # x y z yaw roll pitch 35 | 36 | gnss_position: 37 | noise: [1.0, 1.0, 4.0] # x y z 38 | 39 | imu_pre_integration: 40 | earth: 41 | # gravity can be calculated from https://www.sensorsone.com/local-gravity-calculator/ using latitude and height: 42 | gravity_magnitude: 9.80943 43 | covariance: 44 | measurement: 45 | accel: 2.5e-4 46 | gyro: 1.0e-5 47 | random_walk: 48 | accel: 1.0e-4 49 | gyro: 1.0e-4 -------------------------------------------------------------------------------- /config/scan_context/key_frames.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package scan_context_io; 4 | 5 | message Trans { 6 | required float x = 1; 7 | required float y = 2; 8 | required float z = 3; 9 | } 10 | 11 | message Quat { 12 | required float w = 1; 13 | required float x = 2; 14 | required float y = 3; 15 | required float z = 4; 16 | } 17 | 18 | message KeyFrame { 19 | required Quat q = 1; 20 | required Trans t = 2; 21 | } 22 | 23 | message KeyFrames { 24 | repeated KeyFrame data = 1; 25 | } -------------------------------------------------------------------------------- /config/scan_context/ring_keys.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package scan_context_io; 4 | 5 | message RingKey { 6 | repeated float data = 1; 7 | } 8 | 9 | message RingKeys { 10 | repeated RingKey data = 1; 11 | } -------------------------------------------------------------------------------- /config/scan_context/scan_contexts.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package scan_context_io; 4 | 5 | message ScanContext { 6 | repeated float data = 1; 7 | } 8 | 9 | message ScanContexts { 10 | required int32 num_rings = 1; 11 | required int32 num_sectors = 2; 12 | 13 | repeated ScanContext data = 3; 14 | } -------------------------------------------------------------------------------- /include/lidar_localization/data_pretreat/data_pretreat_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 数据预处理模块，包括时间同步、点云去畸变等 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-10 08:31:22 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_ 8 | 9 | #include 10 | // subscriber 11 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 12 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 13 | #include "lidar_localization/subscriber/velocity_subscriber.hpp" 14 | #include "lidar_localization/subscriber/gnss_subscriber.hpp" 15 | #include "lidar_localization/tf_listener/tf_listener.hpp" 16 | // publisher 17 | // a. synced lidar measurement 18 | #include "lidar_localization/publisher/cloud_publisher.hpp" 19 | // b. synced IMU measurement 20 | #include "lidar_localization/publisher/imu_publisher.hpp" 21 | // c. synced GNSS-odo measurement: 22 | #include "lidar_localization/publisher/pos_vel_publisher.hpp" 23 | // d. synced reference trajectory: 24 | #include "lidar_localization/publisher/odometry_publisher.hpp" 25 | 26 | // models 27 | #include "lidar_localization/models/scan_adjust/distortion_adjust.hpp" 28 | 29 | namespace lidar_localization { 30 | class DataPretreatFlow { 31 | public: 32 | DataPretreatFlow(ros::NodeHandle& nh, std::string cloud_topic); 33 | 34 | bool Run(); 35 | 36 | private: 37 | bool ReadData(); 38 | bool InitCalibration(); 39 | bool InitGNSS(); 40 | bool HasData(); 41 | bool ValidData(); 42 | bool TransformData(); 43 | bool PublishData(); 44 | 45 | private: 46 | // subscriber 47 | std::shared_ptr imu_sub_ptr_; 48 | std::shared_ptr velocity_sub_ptr_; 49 | std::shared_ptr cloud_sub_ptr_; 50 | std::shared_ptr gnss_sub_ptr_; 51 | std::shared_ptr lidar_to_imu_ptr_; 52 | // publisher 53 | std::shared_ptr cloud_pub_ptr_; 54 | std::shared_ptr imu_pub_ptr_; 55 | std::shared_ptr pos_vel_pub_ptr_; 56 | std::shared_ptr gnss_pub_ptr_; 57 | // models 58 | std::shared_ptr distortion_adjust_ptr_; 59 | 60 | Eigen::Matrix4f lidar_to_imu_ = Eigen::Matrix4f::Identity(); 61 | 62 | std::deque cloud_data_buff_; 63 | std::deque imu_data_buff_; 64 | std::deque velocity_data_buff_; 65 | std::deque gnss_data_buff_; 66 | 67 | CloudData current_cloud_data_; 68 | IMUData current_imu_data_; 69 | VelocityData current_velocity_data_; 70 | GNSSData current_gnss_data_; 71 | 72 | PosVelData pos_vel_; 73 | Eigen::Matrix4f gnss_pose_ = Eigen::Matrix4f::Identity(); 74 | }; 75 | } 76 | 77 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/global_defination/global_defination.h.in: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_GLOBAL_DEFINATION_H_IN_ 7 | #define LIDAR_LOCALIZATION_GLOBAL_DEFINATION_H_IN_ 8 | 9 | #include 10 | 11 | namespace lidar_localization { 12 | const std::string WORK_SPACE_PATH = "@WORK_SPACE_PATH@"; 13 | } 14 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/mapping/back_end/lio_back_end_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO mapping backend workflow, interface 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_BACK_END_LIO_BACK_END_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_BACK_END_LIO_BACK_END_FLOW_HPP_ 8 | 9 | #include 10 | 11 | // 12 | // subscribers: 13 | // 14 | // a. lidar scan, key frame measurement: 15 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 16 | // b. lidar odometry & GNSS position: 17 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 18 | // c. loop closure detection: 19 | #include "lidar_localization/subscriber/loop_pose_subscriber.hpp" 20 | // d. IMU measurement, for pre-integration: 21 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 22 | // e. odometer measurement, for pre-integration: 23 | #include "lidar_localization/subscriber/velocity_subscriber.hpp" 24 | 25 | #include "lidar_localization/publisher/odometry_publisher.hpp" 26 | #include "lidar_localization/publisher/cloud_publisher.hpp" 27 | #include "lidar_localization/publisher/key_frame_publisher.hpp" 28 | #include "lidar_localization/publisher/key_frames_publisher.hpp" 29 | 30 | #include "lidar_localization/mapping/back_end/lio_back_end.hpp" 31 | 32 | namespace lidar_localization { 33 | 34 | class LIOBackEndFlow { 35 | public: 36 | LIOBackEndFlow(ros::NodeHandle& nh, std::string cloud_topic, std::string odom_topic); 37 | 38 | bool Run(); 39 | bool ForceOptimize(); 40 | bool SaveOptimizedOdometry(); 41 | 42 | private: 43 | bool ReadData(); 44 | bool InsertLoopClosurePose(); 45 | bool HasData(); 46 | bool ValidData(); 47 | bool UpdateIMUPreIntegration(void); 48 | bool UpdateOdoPreIntegration(void); 49 | bool UpdateBackEnd(); 50 | bool PublishData(); 51 | 52 | private: 53 | // 54 | // subscribers: 55 | // 56 | // a. lidar scan, key frame measurement: 57 | std::shared_ptr cloud_sub_ptr_; 58 | std::deque cloud_data_buff_; 59 | // b. lidar odometry: 60 | std::shared_ptr laser_odom_sub_ptr_; 61 | std::deque laser_odom_data_buff_; 62 | // c. GNSS position: 63 | std::shared_ptr gnss_pose_sub_ptr_; 64 | std::deque gnss_pose_data_buff_; 65 | // d. loop closure detection: 66 | std::shared_ptr loop_pose_sub_ptr_; 67 | std::deque loop_pose_data_buff_; 68 | // e. IMU measurement, for pre-integration: 69 | std::shared_ptr imu_raw_sub_ptr_; 70 | std::deque imu_raw_data_buff_; 71 | std::shared_ptr imu_synced_sub_ptr_; 72 | std::deque imu_synced_data_buff_; 73 | // f. odometer measurement, for pre-integration: 74 | std::shared_ptr velocity_sub_ptr_; 75 | std::deque velocity_data_buff_; 76 | 77 | std::shared_ptr transformed_odom_pub_ptr_; 78 | std::shared_ptr key_scan_pub_ptr_; 79 | std::shared_ptr key_frame_pub_ptr_; 80 | std::shared_ptr key_gnss_pub_ptr_; 81 | std::shared_ptr key_frames_pub_ptr_; 82 | std::shared_ptr back_end_ptr_; 83 | 84 | CloudData current_cloud_data_; 85 | PoseData current_laser_odom_data_; 86 | PoseData current_gnss_pose_data_; 87 | IMUData current_imu_data_; 88 | }; 89 | 90 | } // namespace lidar_localization 91 | 92 | #endif // LIDAR_LOCALIZATION_MAPPING_BACK_END_LIO_BACK_END_FLOW_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/mapping/front_end/front_end.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 前端里程计算法 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-04 18:52:45 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "lidar_localization/sensor_data/cloud_data.hpp" 14 | #include "lidar_localization/models/registration/registration_interface.hpp" 15 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 16 | 17 | namespace lidar_localization { 18 | class FrontEnd { 19 | public: 20 | struct Frame { 21 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 22 | CloudData cloud_data; 23 | }; 24 | 25 | public: 26 | FrontEnd(); 27 | 28 | bool Update(const CloudData& cloud_data, Eigen::Matrix4f& cloud_pose); 29 | bool SetInitPose(const Eigen::Matrix4f& init_pose); 30 | 31 | private: 32 | bool InitWithConfig(); 33 | bool InitParam(const YAML::Node& config_node); 34 | bool InitRegistration(std::shared_ptr& registration_ptr, const YAML::Node& config_node); 35 | bool InitFilter(std::string filter_user, std::shared_ptr& filter_ptr, const YAML::Node& config_node); 36 | bool UpdateWithNewFrame(const Frame& new_key_frame); 37 | 38 | private: 39 | std::string data_path_ = ""; 40 | 41 | // scan filter: 42 | std::shared_ptr frame_filter_ptr_; 43 | // local map filter: 44 | std::shared_ptr local_map_filter_ptr_; 45 | // point cloud registrator: 46 | std::shared_ptr registration_ptr_; 47 | 48 | std::deque local_map_frames_; 49 | 50 | CloudData::CLOUD_PTR local_map_ptr_; 51 | Frame current_frame_; 52 | 53 | Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity(); 54 | 55 | float key_frame_distance_ = 2.0; 56 | int local_frame_num_ = 20; 57 | }; 58 | } 59 | 60 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/mapping/front_end/front_end_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: front end 任务管理， 放在类里使代码更清晰 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-10 08:31:22 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_FLOW_HPP_ 8 | 9 | #include 10 | 11 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 12 | #include "lidar_localization/publisher/odometry_publisher.hpp" 13 | #include "lidar_localization/mapping/front_end/front_end.hpp" 14 | 15 | namespace lidar_localization { 16 | class FrontEndFlow { 17 | public: 18 | FrontEndFlow(ros::NodeHandle& nh, std::string cloud_topic, std::string odom_topic); 19 | 20 | bool Run(); 21 | 22 | private: 23 | bool ReadData(); 24 | bool HasData(); 25 | bool ValidData(); 26 | bool UpdateLaserOdometry(); 27 | bool PublishData(); 28 | 29 | private: 30 | std::shared_ptr cloud_sub_ptr_; 31 | std::shared_ptr laser_odom_pub_ptr_; 32 | std::shared_ptr front_end_ptr_; 33 | 34 | std::deque cloud_data_buff_; 35 | 36 | CloudData current_cloud_data_; 37 | 38 | Eigen::Matrix4f laser_odometry_ = Eigen::Matrix4f::Identity(); 39 | }; 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /include/lidar_localization/mapping/loop_closing/loop_closing.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 闭环检测算法 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-04 18:52:45 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "lidar_localization/sensor_data/key_frame.hpp" 15 | #include "lidar_localization/sensor_data/loop_pose.hpp" 16 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 17 | #include "lidar_localization/models/scan_context_manager/scan_context_manager.hpp" 18 | #include "lidar_localization/models/registration/registration_interface.hpp" 19 | 20 | 21 | namespace lidar_localization { 22 | class LoopClosing { 23 | public: 24 | LoopClosing(); 25 | 26 | bool Update( 27 | const CloudData &key_scan, 28 | const KeyFrame &key_frame, 29 | const KeyFrame &key_gnss 30 | ); 31 | 32 | bool HasNewLoopPose(); 33 | LoopPose& GetCurrentLoopPose(); 34 | 35 | bool Save(void); 36 | 37 | private: 38 | bool InitWithConfig(); 39 | bool InitParam(const YAML::Node& config_node); 40 | bool InitDataPath(const YAML::Node& config_node); 41 | bool InitFilter(std::string filter_user, std::shared_ptr& filter_ptr, const YAML::Node& config_node); 42 | bool InitLoopClosure(const YAML::Node& config_node); 43 | bool InitRegistration(std::shared_ptr& registration_ptr, const YAML::Node& config_node); 44 | 45 | bool DetectNearestKeyFrame( 46 | int& key_frame_index, 47 | float& yaw_change_in_rad 48 | ); 49 | bool CloudRegistration( 50 | const int key_frame_index, 51 | const float yaw_change_in_rad 52 | ); 53 | bool JointMap( 54 | const int key_frame_index, const float yaw_change_in_rad, 55 | CloudData::CLOUD_PTR& map_cloud_ptr, Eigen::Matrix4f& map_pose 56 | ); 57 | bool JointScan(CloudData::CLOUD_PTR& scan_cloud_ptr, Eigen::Matrix4f& scan_pose); 58 | bool Registration(CloudData::CLOUD_PTR& map_cloud_ptr, 59 | CloudData::CLOUD_PTR& scan_cloud_ptr, 60 | Eigen::Matrix4f& scan_pose, 61 | Eigen::Matrix4f& result_pose); 62 | 63 | private: 64 | std::string key_frames_path_ = ""; 65 | std::string scan_context_path_ = ""; 66 | 67 | std::string loop_closure_method_ = ""; 68 | 69 | int extend_frame_num_ = 3; 70 | int loop_step_ = 10; 71 | int diff_num_ = 100; 72 | float detect_area_ = 10.0; 73 | float fitness_score_limit_ = 2.0; 74 | 75 | std::shared_ptr scan_filter_ptr_; 76 | std::shared_ptr map_filter_ptr_; 77 | std::shared_ptr scan_context_manager_ptr_; 78 | std::shared_ptr registration_ptr_; 79 | 80 | std::deque all_key_frames_; 81 | std::deque all_key_gnss_; 82 | 83 | LoopPose current_loop_pose_; 84 | bool has_new_loop_pose_ = false; 85 | }; 86 | } 87 | 88 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/mapping/loop_closing/loop_closing_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-29 03:32:14 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_FLOW_HPP_ 8 | 9 | #include 10 | #include 11 | // subscriber 12 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 13 | #include "lidar_localization/subscriber/key_frame_subscriber.hpp" 14 | // publisher 15 | #include "lidar_localization/publisher/loop_pose_publisher.hpp" 16 | // loop closing 17 | #include "lidar_localization/mapping/loop_closing/loop_closing.hpp" 18 | 19 | namespace lidar_localization { 20 | class LoopClosingFlow { 21 | public: 22 | LoopClosingFlow(ros::NodeHandle& nh); 23 | 24 | bool Run(); 25 | bool Save(); 26 | 27 | private: 28 | bool ReadData(); 29 | bool HasData(); 30 | bool ValidData(); 31 | bool PublishData(); 32 | 33 | private: 34 | // subscriber 35 | std::shared_ptr key_scan_sub_ptr_; 36 | std::shared_ptr key_frame_sub_ptr_; 37 | std::shared_ptr key_gnss_sub_ptr_; 38 | // publisher 39 | std::shared_ptr loop_pose_pub_ptr_; 40 | // loop closing 41 | std::shared_ptr loop_closing_ptr_; 42 | 43 | std::deque key_scan_buff_; 44 | std::deque key_frame_buff_; 45 | std::deque key_gnss_buff_; 46 | 47 | CloudData current_key_scan_; 48 | KeyFrame current_key_frame_; 49 | KeyFrame current_key_gnss_; 50 | }; 51 | } 52 | 53 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/mapping/viewer/viewer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 实时显示，主要是点云 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-29 03:19:45 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_VIEWER_VIEWER_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_VIEWER_VIEWER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "lidar_localization/sensor_data/cloud_data.hpp" 14 | #include "lidar_localization/sensor_data/key_frame.hpp" 15 | #include "lidar_localization/sensor_data/pose_data.hpp" 16 | #include "lidar_localization/models/cloud_filter/voxel_filter.hpp" 17 | 18 | namespace lidar_localization { 19 | class Viewer { 20 | public: 21 | Viewer(); 22 | 23 | bool UpdateWithOptimizedKeyFrames(std::deque& optimized_key_frames); 24 | bool UpdateWithNewKeyFrame(std::deque& new_key_frames, 25 | PoseData transformed_data, 26 | CloudData cloud_data); 27 | 28 | bool SaveMap(); 29 | Eigen::Matrix4f& GetCurrentPose(); 30 | CloudData::CLOUD_PTR& GetCurrentScan(); 31 | bool GetLocalMap(CloudData::CLOUD_PTR& local_map_ptr); 32 | bool GetGlobalMap(CloudData::CLOUD_PTR& local_map_ptr); 33 | bool HasNewLocalMap(); 34 | bool HasNewGlobalMap(); 35 | 36 | private: 37 | bool InitWithConfig(); 38 | bool InitParam(const YAML::Node& config_node); 39 | bool InitDataPath(const YAML::Node& config_node); 40 | bool InitFilter(std::string filter_user, 41 | std::shared_ptr& filter_ptr, 42 | const YAML::Node& config_node); 43 | 44 | bool OptimizeKeyFrames(); 45 | bool JointGlobalMap(CloudData::CLOUD_PTR& global_map_ptr); 46 | bool JointLocalMap(CloudData::CLOUD_PTR& local_map_ptr); 47 | bool JointCloudMap(const std::deque& key_frames, 48 | CloudData::CLOUD_PTR& map_cloud_ptr); 49 | 50 | private: 51 | std::string data_path_ = ""; 52 | int local_frame_num_ = 20; 53 | 54 | std::string key_frames_path_ = ""; 55 | std::string map_path_ = ""; 56 | 57 | std::shared_ptr frame_filter_ptr_; 58 | std::shared_ptr local_map_filter_ptr_; 59 | std::shared_ptr global_map_filter_ptr_; 60 | 61 | Eigen::Matrix4f pose_to_optimize_ = Eigen::Matrix4f::Identity(); 62 | PoseData optimized_odom_; 63 | CloudData optimized_cloud_; 64 | std::deque optimized_key_frames_; 65 | std::deque all_key_frames_; 66 | 67 | bool has_new_global_map_ = false; 68 | bool has_new_local_map_ = false; 69 | }; 70 | } 71 | 72 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/mapping/viewer/viewer_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-29 03:32:14 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MAPPING_VIEWER_VIEWER_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MAPPING_VIEWER_VIEWER_FLOW_HPP_ 8 | 9 | #include 10 | #include 11 | // subscriber 12 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 13 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 14 | #include "lidar_localization/subscriber/key_frame_subscriber.hpp" 15 | #include "lidar_localization/subscriber/key_frames_subscriber.hpp" 16 | // publisher 17 | #include "lidar_localization/publisher/odometry_publisher.hpp" 18 | #include "lidar_localization/publisher/cloud_publisher.hpp" 19 | // viewer 20 | #include "lidar_localization/mapping/viewer/viewer.hpp" 21 | 22 | namespace lidar_localization { 23 | class ViewerFlow { 24 | public: 25 | ViewerFlow(ros::NodeHandle& nh, std::string cloud_topic); 26 | 27 | bool Run(); 28 | bool SaveMap(); 29 | 30 | private: 31 | bool ReadData(); 32 | bool HasData(); 33 | bool ValidData(); 34 | bool PublishGlobalData(); 35 | bool PublishLocalData(); 36 | 37 | private: 38 | // subscriber 39 | std::shared_ptr cloud_sub_ptr_; 40 | std::shared_ptr transformed_odom_sub_ptr_; 41 | std::shared_ptr key_frame_sub_ptr_; 42 | std::shared_ptr optimized_key_frames_sub_ptr_; 43 | // publisher 44 | std::shared_ptr optimized_odom_pub_ptr_; 45 | std::shared_ptr current_scan_pub_ptr_; 46 | std::shared_ptr global_map_pub_ptr_; 47 | std::shared_ptr local_map_pub_ptr_; 48 | // viewer 49 | std::shared_ptr viewer_ptr_; 50 | 51 | std::deque cloud_data_buff_; 52 | std::deque transformed_odom_buff_; 53 | std::deque key_frame_buff_; 54 | std::deque optimized_key_frames_; 55 | std::deque all_key_frames_; 56 | 57 | CloudData current_cloud_data_; 58 | PoseData current_transformed_odom_; 59 | }; 60 | } 61 | 62 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/matching/back_end/sliding_window.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO localization backend, interface 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MATCHING_BACK_END_SLIDING_WINDOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MATCHING_BACK_END_SLIDING_WINDOW_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "lidar_localization/sensor_data/pose_data.hpp" 15 | #include "lidar_localization/sensor_data/imu_data.hpp" 16 | 17 | #include "lidar_localization/sensor_data/key_frame.hpp" 18 | 19 | #include "lidar_localization/models/pre_integrator/imu_pre_integrator.hpp" 20 | 21 | #include "lidar_localization/models/sliding_window/ceres_sliding_window.hpp" 22 | 23 | 24 | namespace lidar_localization { 25 | 26 | class SlidingWindow { 27 | public: 28 | SlidingWindow(); 29 | 30 | bool UpdateIMUPreIntegration(const IMUData &imu_data); 31 | bool Update( 32 | const PoseData &laser_odom, 33 | const PoseData &map_matching_odom, 34 | const IMUData &imu_data, 35 | const PoseData& gnss_pose 36 | ); 37 | 38 | bool HasNewKeyFrame(); 39 | bool HasNewOptimized(); 40 | 41 | void GetLatestKeyFrame(KeyFrame& key_frame); 42 | void GetLatestKeyGNSS(KeyFrame& key_frame); 43 | void GetLatestOptimizedOdometry(KeyFrame& key_frame); 44 | void GetOptimizedKeyFrames(std::deque& key_frames_deque); 45 | bool SaveOptimizedTrajectory(); 46 | 47 | private: 48 | bool InitWithConfig(); 49 | 50 | bool InitDataPath(const YAML::Node& config_node); 51 | bool InitKeyFrameSelection(const YAML::Node& config_node); 52 | bool InitSlidingWindow(const YAML::Node& config_node); 53 | bool InitIMUPreIntegrator(const YAML::Node& config_node); 54 | 55 | void ResetParam(); 56 | bool SavePose(std::ofstream& ofs, const Eigen::Matrix4f& pose); 57 | 58 | bool MaybeNewKeyFrame( 59 | const PoseData& laser_odom, 60 | const PoseData &map_matching_odom, 61 | const IMUData &imu_data, 62 | const PoseData& gnss_pose 63 | ); 64 | 65 | bool Update(void); 66 | bool MaybeOptimized(); 67 | 68 | private: 69 | std::string trajectory_path_ = ""; 70 | 71 | bool has_new_key_frame_ = false; 72 | bool has_new_optimized_ = false; 73 | 74 | KeyFrame current_key_frame_; 75 | PoseData current_map_matching_pose_; 76 | KeyFrame current_key_gnss_; 77 | 78 | // key frame buffer: 79 | struct { 80 | std::deque lidar; 81 | std::deque optimized; 82 | std::deque reference; 83 | } key_frames_; 84 | 85 | // pre-integrator: 86 | std::shared_ptr imu_pre_integrator_ptr_; 87 | IMUPreIntegrator::IMUPreIntegration imu_pre_integration_; 88 | 89 | // key frame config: 90 | struct { 91 | float max_distance; 92 | float max_interval; 93 | } key_frame_config_; 94 | 95 | // optimizer: 96 | std::shared_ptr sliding_window_ptr_; 97 | 98 | // measurement config: 99 | struct MeasurementConfig { 100 | struct { 101 | bool map_matching = false; 102 | bool imu_pre_integration = false; 103 | } source; 104 | 105 | struct { 106 | Eigen::VectorXd lidar_odometry; 107 | Eigen::VectorXd map_matching; 108 | Eigen::VectorXd gnss_position; 109 | } noise; 110 | }; 111 | MeasurementConfig measurement_config_; 112 | }; 113 | 114 | } // namespace lidar_localization 115 | 116 | #endif // LIDAR_LOCALIZATION_MATCHING_BACK_END_SLIDING_WINDOW_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/matching/back_end/sliding_window_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO localization backend workflow, interface 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MATCHING_BACK_END_SLIDING_WINDOW_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MATCHING_BACK_END_SLIDING_WINDOW_FLOW_HPP_ 8 | 9 | #include 10 | 11 | // 12 | // subscribers: 13 | // 14 | // a. lidar odometry, map matching pose & GNSS reference position: 15 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 16 | // b. IMU measurement, for pre-integration: 17 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 18 | 19 | #include "lidar_localization/publisher/odometry_publisher.hpp" 20 | #include "lidar_localization/publisher/cloud_publisher.hpp" 21 | #include "lidar_localization/publisher/key_frame_publisher.hpp" 22 | #include "lidar_localization/publisher/key_frames_publisher.hpp" 23 | #include "lidar_localization/publisher/tf_broadcaster.hpp" 24 | 25 | #include "lidar_localization/matching/back_end/sliding_window.hpp" 26 | 27 | 28 | namespace lidar_localization { 29 | 30 | class SlidingWindowFlow { 31 | public: 32 | SlidingWindowFlow(ros::NodeHandle& nh); 33 | 34 | bool Run(); 35 | bool SaveOptimizedTrajectory(); 36 | 37 | private: 38 | bool ReadData(); 39 | bool HasData(); 40 | bool ValidData(); 41 | bool UpdateIMUPreIntegration(void); 42 | bool UpdateBackEnd(); 43 | bool PublishData(); 44 | 45 | private: 46 | // 47 | // subscribers: 48 | // 49 | // a. lidar odometry: 50 | std::shared_ptr laser_odom_sub_ptr_; 51 | std::deque laser_odom_data_buff_; 52 | // b. map matching odometry: 53 | std::shared_ptr map_matching_odom_sub_ptr_; 54 | std::deque map_matching_odom_data_buff_; 55 | // c. IMU measurement, for pre-integration: 56 | std::shared_ptr imu_raw_sub_ptr_; 57 | std::deque imu_raw_data_buff_; 58 | std::shared_ptr imu_synced_sub_ptr_; 59 | std::deque imu_synced_data_buff_; 60 | // d. GNSS position: 61 | std::shared_ptr gnss_pose_sub_ptr_; 62 | std::deque gnss_pose_data_buff_; 63 | 64 | // 65 | // publishers: 66 | // 67 | std::shared_ptr key_frame_pub_ptr_; 68 | std::shared_ptr key_gnss_pub_ptr_; 69 | std::shared_ptr optimized_odom_pub_ptr_; 70 | std::shared_ptr optimized_trajectory_pub_ptr_; 71 | std::shared_ptr laser_tf_pub_ptr_; 72 | 73 | // 74 | // backend: 75 | // 76 | std::shared_ptr sliding_window_ptr_; 77 | 78 | // 79 | // synced data: 80 | // 81 | PoseData current_laser_odom_data_; 82 | PoseData current_map_matching_odom_data_; 83 | IMUData current_imu_data_; 84 | PoseData current_gnss_pose_data_; 85 | }; 86 | 87 | } // namespace lidar_localization 88 | 89 | #endif // LIDAR_LOCALIZATION_MATCHING_BACK_END_SLIDING_WINDOW_FLOW_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/matching/front_end/matching.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: lidar localization frontend, interface 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-04 18:52:45 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_HPP_ 7 | #define LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "lidar_localization/sensor_data/cloud_data.hpp" 14 | #include "lidar_localization/sensor_data/pose_data.hpp" 15 | 16 | #include "lidar_localization/models/scan_context_manager/scan_context_manager.hpp" 17 | #include "lidar_localization/models/registration/registration_interface.hpp" 18 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 19 | #include "lidar_localization/models/cloud_filter/box_filter.hpp" 20 | 21 | namespace lidar_localization { 22 | 23 | class Matching { 24 | public: 25 | Matching(); 26 | 27 | bool HasInited(void); 28 | bool HasNewGlobalMap(void); 29 | bool HasNewLocalMap(void); 30 | 31 | Eigen::Matrix4f GetInitPose(void); 32 | CloudData::CLOUD_PTR& GetGlobalMap(void); 33 | CloudData::CLOUD_PTR& GetLocalMap(void); 34 | CloudData::CLOUD_PTR& GetCurrentScan(void); 35 | 36 | bool Update( 37 | const CloudData& cloud_data, 38 | Eigen::Matrix4f& laser_pose, Eigen::Matrix4f &map_matching_pose 39 | ); 40 | 41 | bool SetGNSSPose(const Eigen::Matrix4f& init_pose); 42 | bool SetScanContextPose(const CloudData& init_scan); 43 | 44 | private: 45 | bool InitWithConfig(); 46 | // 47 | // point cloud map & measurement processors: 48 | // 49 | bool InitFilter( 50 | const YAML::Node &config_node, std::string filter_user, 51 | std::shared_ptr& filter_ptr 52 | ); 53 | bool InitLocalMapSegmenter(const YAML::Node& config_node); 54 | bool InitPointCloudProcessors(const YAML::Node& config_node); 55 | // 56 | // global map: 57 | // 58 | bool InitGlobalMap(const YAML::Node& config_node); 59 | // 60 | // lidar frontend for relative pose estimation: 61 | // 62 | bool InitRegistration(const YAML::Node& config_node, std::shared_ptr& registration_ptr); 63 | // 64 | // map matcher: 65 | // 66 | bool InitScanContextManager(const YAML::Node& config_node); 67 | 68 | bool SetInitPose(const Eigen::Matrix4f& init_pose); 69 | bool ResetLocalMap(float x, float y, float z); 70 | 71 | private: 72 | std::shared_ptr global_map_filter_ptr_; 73 | 74 | std::shared_ptr local_map_segmenter_ptr_; 75 | std::shared_ptr local_map_filter_ptr_; 76 | 77 | std::shared_ptr frame_filter_ptr_; 78 | 79 | CloudData::CLOUD_PTR global_map_ptr_; 80 | CloudData::CLOUD_PTR local_map_ptr_; 81 | CloudData::CLOUD_PTR current_scan_ptr_; 82 | 83 | std::shared_ptr registration_ptr_; 84 | 85 | std::shared_ptr scan_context_manager_ptr_; 86 | 87 | Eigen::Matrix4f current_pose_ = Eigen::Matrix4f::Identity(); 88 | 89 | Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity(); 90 | Eigen::Matrix4f current_gnss_pose_ = Eigen::Matrix4f::Identity(); 91 | 92 | bool has_inited_ = false; 93 | bool has_new_global_map_ = false; 94 | bool has_new_local_map_ = false; 95 | }; 96 | 97 | } // namespace lidar_localization 98 | 99 | #endif // LIDAR_LOCALIZATION_MATCHING_FRONTEND_MATCHING_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/matching/front_end/matching_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO localization frontend workflow, interface 3 | * @Author: Ge Yao 4 | * @Date: 2021-01-02 10:47:23 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_FLOW_HPP_ 8 | 9 | #include 10 | 11 | // subscriber 12 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 13 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 14 | 15 | // publisher 16 | #include "lidar_localization/publisher/cloud_publisher.hpp" 17 | #include "lidar_localization/publisher/odometry_publisher.hpp" 18 | #include "lidar_localization/publisher/tf_broadcaster.hpp" 19 | 20 | // matching 21 | #include "lidar_localization/matching/front_end/matching.hpp" 22 | 23 | namespace lidar_localization { 24 | 25 | class MatchingFlow { 26 | public: 27 | MatchingFlow(ros::NodeHandle& nh); 28 | 29 | bool Run(); 30 | 31 | private: 32 | bool ReadData(); 33 | bool HasData(); 34 | bool ValidData(); 35 | bool UpdateMatching(); 36 | bool PublishData(); 37 | 38 | private: 39 | // 40 | // subscribers: 41 | // 42 | std::shared_ptr cloud_sub_ptr_; 43 | std::shared_ptr gnss_sub_ptr_; 44 | 45 | // 46 | // publishers: 47 | // 48 | std::shared_ptr global_map_pub_ptr_; 49 | std::shared_ptr local_map_pub_ptr_; 50 | std::shared_ptr current_scan_pub_ptr_; 51 | 52 | std::shared_ptr laser_odom_pub_ptr_; 53 | std::shared_ptr map_matching_odom_pub_ptr_; 54 | 55 | // matching 56 | std::shared_ptr matching_ptr_; 57 | 58 | std::deque cloud_data_buff_; 59 | std::deque gnss_data_buff_; 60 | 61 | CloudData current_cloud_data_; 62 | PoseData current_gnss_data_; 63 | 64 | Eigen::Matrix4f laser_odometry_ = Eigen::Matrix4f::Identity(); 65 | Eigen::Matrix4f map_matching_odometry_ = Eigen::Matrix4f::Identity(); 66 | }; 67 | 68 | } // namespace lidar_localization 69 | 70 | #endif // LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_FLOW_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/models/cloud_filter/box_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 从点云中截取一个立方体部分 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-04 20:09:37 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_BOX_FILTER_HPP_ 8 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_BOX_FILTER_HPP_ 9 | 10 | #include 11 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 12 | 13 | namespace lidar_localization { 14 | class BoxFilter: public CloudFilterInterface { 15 | public: 16 | BoxFilter(YAML::Node node); 17 | BoxFilter() = default; 18 | 19 | bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override; 20 | 21 | void SetSize(std::vector size); 22 | void SetOrigin(std::vector origin); 23 | std::vector GetEdge(); 24 | 25 | private: 26 | void CalculateEdge(); 27 | 28 | private: 29 | pcl::CropBox pcl_box_filter_; 30 | 31 | std::vector origin_; 32 | std::vector size_; 33 | std::vector edge_; 34 | }; 35 | } 36 | 37 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/cloud_filter/cloud_filter_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云滤波模块的接口 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:29:50 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/sensor_data/cloud_data.hpp" 11 | 12 | namespace lidar_localization { 13 | class CloudFilterInterface { 14 | public: 15 | virtual ~CloudFilterInterface() = default; 16 | 17 | virtual bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) = 0; 18 | }; 19 | } 20 | 21 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/cloud_filter/no_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 不滤波 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:37:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_NO_FILTER_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_NO_FILTER_HPP_ 8 | 9 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 10 | 11 | namespace lidar_localization { 12 | class NoFilter: public CloudFilterInterface { 13 | public: 14 | NoFilter(); 15 | 16 | bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override; 17 | }; 18 | } 19 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/cloud_filter/voxel_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel filter 模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:37:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 11 | 12 | namespace lidar_localization { 13 | class VoxelFilter: public CloudFilterInterface { 14 | public: 15 | VoxelFilter(const YAML::Node& node); 16 | VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z); 17 | 18 | bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override; 19 | 20 | private: 21 | bool SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z); 22 | 23 | private: 24 | pcl::VoxelGrid voxel_filter_; 25 | }; 26 | } 27 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/graph_optimizer/g2o/edge/edge_prvag_odo_pre_integration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: g2o edge for LIO odo pre-integration measurement 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_ODO_PRE_INTEGRATION_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_ODO_PRE_INTEGRATION_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp" 20 | 21 | #include 22 | 23 | #include "glog/logging.h" 24 | 25 | namespace g2o { 26 | 27 | namespace EdgePRVAGOdoPreIntegration { 28 | 29 | static const int DIM = 6; 30 | 31 | typedef Eigen::Matrix Measurement; 32 | 33 | class EdgePRVAGOdoPreIntegration : public g2o::BaseBinaryEdge { 34 | public: 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | 37 | static const int INDEX_P = 0; 38 | static const int INDEX_R = 3; 39 | 40 | EdgePRVAGOdoPreIntegration() 41 | : g2o::BaseBinaryEdge() { 42 | } 43 | 44 | virtual void computeError() override { 45 | g2o::VertexPRVAG* v0 = dynamic_cast(_vertices[0]); 46 | g2o::VertexPRVAG* v1 = dynamic_cast(_vertices[1]); 47 | 48 | const Eigen::Vector3d &pos_i = v0->estimate().pos; 49 | const Sophus::SO3d &ori_i = v0->estimate().ori; 50 | 51 | const Eigen::Vector3d &pos_j = v1->estimate().pos; 52 | const Sophus::SO3d &ori_j = v1->estimate().ori; 53 | 54 | const Eigen::Vector3d &alpha_ij = _measurement.block<3, 1>(INDEX_P, 0); 55 | const Eigen::Vector3d &theta_ij = _measurement.block<3, 1>(INDEX_R, 0); 56 | 57 | _error.block<3, 1>(INDEX_P, 0) = ori_i.inverse() * (pos_j - pos_i) - alpha_ij; 58 | _error.block<3, 1>(INDEX_R, 0) = (Sophus::SO3d::exp(theta_ij).inverse()*ori_i.inverse()*ori_j).log(); 59 | } 60 | 61 | virtual void setMeasurement(const Measurement& m) override { 62 | _measurement = m; 63 | } 64 | 65 | virtual bool read(std::istream& is) override { 66 | Measurement m; 67 | is >> m(INDEX_P + 0) >> m(INDEX_P + 1) >> m(INDEX_P + 2) 68 | >> m(INDEX_R + 0) >> m(INDEX_R + 1) >> m(INDEX_R + 2); 69 | 70 | setMeasurement(m); 71 | 72 | for (int i = 0; i < information().rows(); ++i) { 73 | for (int j = i; j < information().cols(); ++j) { 74 | is >> information()(i, j); 75 | // update cross-diagonal element: 76 | if (i != j) { 77 | information()(j, i) = information()(i, j); 78 | } 79 | } 80 | } 81 | 82 | return true; 83 | } 84 | 85 | virtual bool write(std::ostream& os) const override { 86 | Measurement m = _measurement; 87 | 88 | os << m(INDEX_P + 0) << " " < 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp" 18 | 19 | #include 20 | 21 | namespace g2o { 22 | 23 | class EdgePRVAGPriorPos : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPRVAG> { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | EdgePRVAGPriorPos() 28 | : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPRVAG>() { 29 | } 30 | 31 | virtual void computeError() override { 32 | const g2o::VertexPRVAG* v = static_cast(_vertices[0]); 33 | 34 | const Eigen::Vector3d &estimate = v->estimate().pos; 35 | 36 | _error = estimate - _measurement; 37 | } 38 | 39 | virtual void setMeasurement(const Eigen::Vector3d& m) override { 40 | _measurement = m; 41 | } 42 | 43 | virtual bool read(std::istream& is) override { 44 | Eigen::Vector3d v; 45 | 46 | is >> v(0) >> v(1) >> v(2); 47 | 48 | setMeasurement(v); 49 | 50 | for (int i = 0; i < information().rows(); ++i) { 51 | for (int j = i; j < information().cols(); ++j) { 52 | is >> information()(i, j); 53 | // update cross-diagonal element: 54 | if (i != j) { 55 | information()(j, i) = information()(i, j); 56 | } 57 | } 58 | } 59 | 60 | return true; 61 | } 62 | 63 | virtual bool write(std::ostream& os) const override { 64 | Eigen::Vector3d v = _measurement; 65 | 66 | os << v(0) << " " << v(1) << " " << v(2) << " "; 67 | 68 | for (int i = 0; i < information().rows(); ++i) 69 | for (int j = i; j < information().cols(); ++j) 70 | os << " " << information()(i, j); 71 | 72 | return os.good(); 73 | } 74 | }; 75 | 76 | } // namespace g2o 77 | 78 | #endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_PRIOR_POS_HPP_ 79 | -------------------------------------------------------------------------------- /include/lidar_localization/models/graph_optimizer/g2o/edge/edge_prvag_relative_pose.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: g2o edge for LIO lidar frontend relative pose measurement 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_RELATIVE_POSE_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_RELATIVE_POSE_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp" 20 | 21 | #include 22 | 23 | typedef Eigen::Matrix Vector6d; 24 | 25 | namespace g2o { 26 | 27 | class EdgePRVAGRelativePose : public g2o::BaseBinaryEdge<6, Vector6d, g2o::VertexPRVAG, g2o::VertexPRVAG> { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | 31 | static const int INDEX_P = 0; 32 | static const int INDEX_R = 3; 33 | 34 | EdgePRVAGRelativePose() 35 | : g2o::BaseBinaryEdge<6, Vector6d, g2o::VertexPRVAG, g2o::VertexPRVAG>() { 36 | } 37 | 38 | virtual void computeError() override { 39 | const g2o::VertexPRVAG* v0 = static_cast(_vertices[0]); 40 | const g2o::VertexPRVAG* v1 = static_cast(_vertices[1]); 41 | 42 | const Eigen::Vector3d &pos_i = v0->estimate().pos; 43 | const Sophus::SO3d &ori_i = v0->estimate().ori; 44 | 45 | const Eigen::Vector3d &pos_j = v1->estimate().pos; 46 | const Sophus::SO3d &ori_j = v1->estimate().ori; 47 | 48 | const Eigen::Vector3d &pos_ij = _measurement.block<3, 1>(INDEX_P, 0); 49 | const Eigen::Vector3d &ori_ij = _measurement.block<3, 1>(INDEX_R, 0); 50 | 51 | _error.block(INDEX_P, 0, 3, 1) = ori_i.inverse() * (pos_j - pos_i) - pos_ij; 52 | _error.block(INDEX_R, 0, 3, 1) = (Sophus::SO3d::exp(ori_ij).inverse()*ori_i.inverse()*ori_j).log(); 53 | } 54 | 55 | virtual void setMeasurement(const Vector6d& m) override { 56 | _measurement = m; 57 | } 58 | 59 | virtual bool read(std::istream& is) override { 60 | Vector6d v; 61 | is >> v(INDEX_P + 0) >> v(INDEX_P + 1) >> v(INDEX_P + 2) 62 | >> v(INDEX_R + 0) >> v(INDEX_R + 1) >> v(INDEX_R + 2); 63 | 64 | setMeasurement(v); 65 | 66 | for (int i = 0; i < information().rows(); ++i) { 67 | for (int j = i; j < information().cols(); ++j) { 68 | is >> information()(i, j); 69 | // update cross-diagonal element: 70 | if (i != j) { 71 | information()(j, i) = information()(i, j); 72 | } 73 | } 74 | } 75 | 76 | return true; 77 | } 78 | 79 | virtual bool write(std::ostream& os) const override { 80 | Vector6d v = _measurement; 81 | 82 | os << v(INDEX_P + 0) << " " < 10 | #include 11 | 12 | namespace g2o { 13 | class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | EdgeSE3PriorQuat() 17 | :g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() { 18 | } 19 | 20 | void computeError() override { 21 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 22 | 23 | Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear()); 24 | if(estimate.w() < 0) { 25 | estimate.coeffs() = -estimate.coeffs(); 26 | } 27 | _error = estimate.vec() - _measurement.vec(); 28 | } 29 | 30 | void setMeasurement(const Eigen::Quaterniond& m) override { 31 | _measurement = m; 32 | if(m.w() < 0.0) { 33 | _measurement.coeffs() = -m.coeffs(); 34 | } 35 | } 36 | 37 | virtual bool read(std::istream& is) override { 38 | Eigen::Quaterniond q; 39 | is >> q.w() >> q.x() >> q.y() >> q.z(); 40 | setMeasurement(q); 41 | for (int i = 0; i < information().rows(); ++i) 42 | for (int j = i; j < information().cols(); ++j) { 43 | is >> information()(i, j); 44 | if (i != j) 45 | information()(j, i) = information()(i, j); 46 | } 47 | return true; 48 | } 49 | 50 | virtual bool write(std::ostream& os) const override { 51 | Eigen::Quaterniond q = _measurement; 52 | os << q.w() << " " << q.x() << " " << q.y() << " " << q.z(); 53 | for (int i = 0; i < information().rows(); ++i) 54 | for (int j = i; j < information().cols(); ++j) 55 | os << " " << information()(i, j); 56 | return os.good(); 57 | } 58 | }; 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /include/lidar_localization/models/graph_optimizer/g2o/edge/edge_se3_priorxyz.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: GNSS 坐标做观测时使用的先验边 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-01 18:05:35 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORXYZ_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORXYZ_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | namespace g2o { 13 | class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | EdgeSE3PriorXYZ() 17 | :g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() { 18 | } 19 | 20 | void computeError() override { 21 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 22 | 23 | Eigen::Vector3d estimate = v1->estimate().translation(); 24 | _error = estimate - _measurement; 25 | } 26 | 27 | void setMeasurement(const Eigen::Vector3d& m) override { 28 | _measurement = m; 29 | } 30 | 31 | virtual bool read(std::istream& is) override { 32 | Eigen::Vector3d v; 33 | is >> v(0) >> v(1) >> v(2); 34 | 35 | setMeasurement(Eigen::Vector3d(v)); 36 | 37 | for (int i = 0; i < information().rows(); ++i) { 38 | for (int j = i; j < information().cols(); ++j) { 39 | is >> information()(i, j); 40 | // update cross-diagonal element: 41 | if (i != j) { 42 | information()(j, i) = information()(i, j); 43 | } 44 | } 45 | } 46 | 47 | return true; 48 | } 49 | 50 | virtual bool write(std::ostream& os) const override { 51 | Eigen::Vector3d v = _measurement; 52 | os << v(0) << " " << v(1) << " " << v(2) << " "; 53 | for (int i = 0; i < information().rows(); ++i) 54 | for (int j = i; j < information().cols(); ++j) 55 | os << " " << information()(i, j); 56 | return os.good(); 57 | } 58 | }; 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /include/lidar_localization/models/graph_optimizer/interface_graph_optimizer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-01 18:35:19 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_ 8 | #define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_ 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | // data input: 16 | #include "lidar_localization/sensor_data/key_frame.hpp" 17 | 18 | #include "lidar_localization/models/pre_integrator/imu_pre_integrator.hpp" 19 | #include "lidar_localization/models/pre_integrator/odo_pre_integrator.hpp" 20 | 21 | 22 | namespace lidar_localization { 23 | 24 | class InterfaceGraphOptimizer { 25 | public: 26 | virtual ~InterfaceGraphOptimizer() {} 27 | 28 | // 优化 29 | virtual bool Optimize() = 0; 30 | 31 | // 输入、输出数据 32 | virtual int GetNodeNum() = 0; 33 | // 添加节点、边、鲁棒核 34 | virtual void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size) = 0; 35 | 36 | // SE3: 37 | virtual bool GetOptimizedPose(std::deque& optimized_pose) = 0; 38 | virtual void AddSe3Node( 39 | const Eigen::Isometry3d &pose, const bool need_fix 40 | ) = 0; 41 | virtual void AddSe3Edge( 42 | const int vertex_index1, const int vertex_index2, 43 | const Eigen::Isometry3d &relative_pose, const Eigen::VectorXd &noise 44 | ) = 0; 45 | virtual void AddSe3PriorXYZEdge( 46 | const int se3_vertex_index, 47 | const Eigen::Vector3d &xyz, const Eigen::VectorXd &noise 48 | ) = 0; 49 | virtual void AddSe3PriorQuaternionEdge( 50 | const int se3_vertex_index, 51 | const Eigen::Quaterniond &quat, const Eigen::VectorXd &noise 52 | ) = 0; 53 | 54 | // LIO state: 55 | virtual bool GetOptimizedKeyFrame(std::deque &optimized_key_frames) = 0; 56 | /** 57 | * @brief add vertex for LIO key frame 58 | * @param lio_key_frame, LIO key frame with (pos, ori, vel, b_a and b_g) 59 | * @param need_fix, shall the vertex be fixed to eliminate trajectory estimation ambiguity 60 | * @return true if success false otherwise 61 | */ 62 | virtual void AddPRVAGNode( 63 | const KeyFrame &lio_key_frame, const bool need_fix 64 | ) = 0; 65 | virtual void AddPRVAGRelativePoseEdge( 66 | const int vertex_index_i, const int vertex_index_j, 67 | const Eigen::Matrix4d &relative_pose, const Eigen::VectorXd &noise 68 | ) = 0; 69 | virtual void AddPRVAGPriorPosEdge( 70 | const int vertex_index, 71 | const Eigen::Vector3d &pos, const Eigen::Vector3d &noise 72 | ) = 0; 73 | virtual void AddPRVAGIMUPreIntegrationEdge( 74 | const int vertex_index_i, const int vertex_index_j, 75 | const IMUPreIntegrator::IMUPreIntegration &imu_pre_integration 76 | ) = 0; 77 | virtual void AddPRVAGOdoPreIntegrationEdge( 78 | const int vertex_index_i, const int vertex_index_j, 79 | const OdoPreIntegrator::OdoPreIntegration &odo_pre_integration 80 | ) = 0; 81 | 82 | void SetMaxIterationsNum(int max_iterations_num); 83 | 84 | protected: 85 | int max_iterations_num_ = 512; 86 | }; 87 | 88 | } // namespace lidar_localization 89 | 90 | #endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_INTERFACE_GRAPH_OPTIMIZER_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/models/pre_integrator/pre_integrator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: pre-integrator interface 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_MODELS_PRE_INTEGRATOR_PRE_INTEGRATOR_HPP_ 8 | #define LIDAR_LOCALIZATION_MODELS_PRE_INTEGRATOR_PRE_INTEGRATOR_HPP_ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace lidar_localization { 17 | 18 | class PreIntegrator { 19 | public: 20 | /** 21 | * @brief whether the pre-integrator is inited: 22 | * @return true if inited false otherwise 23 | */ 24 | double IsInited(void) const { return is_inited_; } 25 | 26 | /** 27 | * @brief get pre-integrator time 28 | * @return pre-integrator time as double 29 | */ 30 | double GetTime(void) const { return time_; } 31 | 32 | protected: 33 | PreIntegrator() {} 34 | 35 | // init: 36 | bool is_inited_ = false; 37 | 38 | // time: 39 | double time_; 40 | }; 41 | 42 | } // namespace lidar_localization 43 | 44 | #endif // LIDAR_LOCALIZATION_MODELS_PRE_INTEGRATOR_PRE_INTEGRATOR_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/models/registration/ndt_registration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: NDT 匹配模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-08 21:46:57 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/models/registration/registration_interface.hpp" 11 | 12 | namespace lidar_localization { 13 | class NDTRegistration: public RegistrationInterface { 14 | public: 15 | NDTRegistration(const YAML::Node& node); 16 | NDTRegistration(float res, float step_size, float trans_eps, int max_iter); 17 | 18 | bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override; 19 | bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 20 | const Eigen::Matrix4f& predict_pose, 21 | CloudData::CLOUD_PTR& result_cloud_ptr, 22 | Eigen::Matrix4f& result_pose) override; 23 | float GetFitnessScore() override; 24 | 25 | private: 26 | bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter); 27 | 28 | private: 29 | pcl::NormalDistributionsTransform::Ptr ndt_ptr_; 30 | }; 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/registration/registration_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云匹配模块的基类 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-08 21:25:11 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 8 | 9 | #include 10 | #include 11 | #include "lidar_localization/sensor_data/cloud_data.hpp" 12 | 13 | namespace lidar_localization { 14 | class RegistrationInterface { 15 | public: 16 | virtual ~RegistrationInterface() = default; 17 | 18 | virtual bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) = 0; 19 | virtual bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 20 | const Eigen::Matrix4f& predict_pose, 21 | CloudData::CLOUD_PTR& result_cloud_ptr, 22 | Eigen::Matrix4f& result_pose) = 0; 23 | virtual float GetFitnessScore() = 0; 24 | }; 25 | } 26 | 27 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/scan_adjust/distortion_adjust.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云畸变补偿 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-25 14:38:12 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_ 8 | #define LIDAR_LOCALIZATION_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_ 9 | 10 | #include 11 | #include 12 | #include "glog/logging.h" 13 | 14 | #include "lidar_localization/models/scan_adjust/distortion_adjust.hpp" 15 | #include "lidar_localization/sensor_data/velocity_data.hpp" 16 | #include "lidar_localization/sensor_data/cloud_data.hpp" 17 | 18 | namespace lidar_localization { 19 | class DistortionAdjust { 20 | public: 21 | void SetMotionInfo(float scan_period, VelocityData velocity_data); 22 | bool AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr); 23 | 24 | private: 25 | inline Eigen::Matrix3f UpdateMatrix(float real_time); 26 | 27 | private: 28 | float scan_period_; 29 | Eigen::Vector3f velocity_; 30 | Eigen::Vector3f angular_rate_; 31 | }; 32 | } // namespace lidar_slam 33 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/models/sliding_window/factors/factor_prvag_map_matching_pose.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: ceres residual block for map matching pose measurement 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MAP_MATCHING_POSE_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MAP_MATCHING_POSE_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include "glog/logging.h" 18 | 19 | namespace sliding_window { 20 | 21 | class FactorPRVAGMapMatchingPose : public ceres::SizedCostFunction<6, 15> { 22 | public: 23 | static const int INDEX_P = 0; 24 | static const int INDEX_R = 3; 25 | 26 | FactorPRVAGMapMatchingPose(void) {}; 27 | 28 | void SetMeasurement(const Eigen::VectorXd &m) { 29 | m_ = m; 30 | } 31 | 32 | void SetInformation(const Eigen::MatrixXd &I) { 33 | I_ = I; 34 | } 35 | 36 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { 37 | // 38 | // parse parameters: 39 | // 40 | // pose 41 | Eigen::Map pos(¶meters[0][INDEX_P]); 42 | Eigen::Map log_ori(¶meters[0][INDEX_R]); 43 | const Sophus::SO3d ori = Sophus::SO3d::exp(log_ori); 44 | 45 | // 46 | // parse measurement: 47 | // 48 | const Eigen::Vector3d &pos_prior = m_.block<3, 1>(INDEX_P, 0); 49 | const Eigen::Vector3d &log_ori_prior = m_.block<3, 1>(INDEX_R, 0); 50 | const Sophus::SO3d ori_prior = Sophus::SO3d::exp(log_ori_prior); 51 | 52 | // 53 | // TODO: get square root of information matrix: 54 | // 55 | Eigen::Matrix sqrt_info = Eigen::LLT>( 56 | I_ 57 | ).matrixL().transpose(); 58 | // 59 | // TODO: compute residual: 60 | // 61 | Eigen::Map> residual(residuals); 62 | 63 | residual.block(INDEX_P, 0, 3, 1) = pos - pos_prior; 64 | residual.block(INDEX_R, 0, 3, 1) = (ori*ori_prior.inverse()).log(); 65 | 66 | // 67 | // TODO: compute jacobians: 68 | // 69 | if ( jacobians ) { 70 | if ( jacobians[0] ) { 71 | // implement jacobian computing: 72 | Eigen::Map > jacobian_prior( jacobians[0] ); 73 | jacobian_prior.setZero(); 74 | 75 | jacobian_prior.block<3, 3>(INDEX_P, INDEX_P) = Eigen::Matrix3d::Identity(); 76 | jacobian_prior.block<3, 3>(INDEX_R, INDEX_R) = JacobianRInv(residual.block(INDEX_R, 0, 3, 1))*ori_prior.matrix(); 77 | } 78 | } 79 | 80 | // 81 | // TODO: correct residual by square root of information matrix: 82 | // 83 | residual = sqrt_info * residual; 84 | 85 | return true; 86 | } 87 | 88 | private: 89 | static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) { 90 | Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity(); 91 | 92 | double theta = w.norm(); 93 | 94 | if ( theta > 1e-5 ) { 95 | Eigen::Vector3d k = w.normalized(); 96 | Eigen::Matrix3d K = Sophus::SO3d::hat(k); 97 | 98 | J_r_inv = J_r_inv 99 | + 0.5 * K 100 | + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K; 101 | } 102 | 103 | return J_r_inv; 104 | } 105 | 106 | Eigen::VectorXd m_; 107 | Eigen::MatrixXd I_; 108 | }; 109 | 110 | } // namespace sliding_window 111 | 112 | #endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MAP_MATCHING_POSE_HPP_ 113 | -------------------------------------------------------------------------------- /include/lidar_localization/models/sliding_window/params/param_prvag.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: ceres parameter block for LIO extended pose 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_PARAM_PRVAG_HPP_ 8 | #define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_PARAM_PRVAG_HPP_ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace sliding_window { 19 | 20 | class ParamPRVAG : public ceres::LocalParameterization { 21 | public: 22 | static const int INDEX_P = 0; 23 | static const int INDEX_R = 3; 24 | static const int INDEX_V = 6; 25 | static const int INDEX_A = 9; 26 | static const int INDEX_G = 12; 27 | 28 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const { 29 | Eigen::Map pos(x + INDEX_P); 30 | Eigen::Map ori(x + INDEX_R); 31 | Eigen::Map vel(x + INDEX_V); 32 | Eigen::Map b_a(x + INDEX_A); 33 | Eigen::Map b_g(x + INDEX_G); 34 | 35 | Eigen::Map d_pos(delta + INDEX_P); 36 | Eigen::Map d_ori(delta + INDEX_R); 37 | Eigen::Map d_vel(delta + INDEX_V); 38 | Eigen::Map d_b_a(delta + INDEX_A); 39 | Eigen::Map d_b_g(delta + INDEX_G); 40 | 41 | Eigen::Map pos_plus(x_plus_delta + INDEX_P); 42 | Eigen::Map ori_plus(x_plus_delta + INDEX_R); 43 | Eigen::Map vel_plus(x_plus_delta + INDEX_V); 44 | Eigen::Map b_a_plus(x_plus_delta + INDEX_A); 45 | Eigen::Map b_g_plus(x_plus_delta + INDEX_G); 46 | 47 | pos_plus = pos + d_pos; 48 | // 49 | // TODO: evaluate performance penalty of applying exp-exp-log transform for each update 50 | // 51 | ori_plus = (Sophus::SO3d::exp(ori) * Sophus::SO3d::exp(d_ori)).log(); 52 | vel_plus = vel + d_vel; 53 | b_a_plus = b_a + d_b_a; 54 | b_g_plus = b_g + b_g_plus; 55 | 56 | return true; 57 | } 58 | 59 | virtual bool ComputeJacobian(const double *x, double *jacobian) const { 60 | Eigen::Map> J(jacobian); 61 | J.setIdentity(); 62 | 63 | return true; 64 | } 65 | 66 | virtual int GlobalSize() const { return 15; }; 67 | 68 | virtual int LocalSize() const { return 15; }; 69 | }; 70 | 71 | } // namespace sliding_window 72 | 73 | #endif //LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_PARAM_PRVAG_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/models/sliding_window/utils/utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: utils for ceres residual block analytic Jacobians 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_UTILS_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_UTILS_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace sliding_window { 16 | 17 | Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) { 18 | Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity(); 19 | 20 | double theta = w.norm(); 21 | 22 | if ( theta > 1e-5 ) { 23 | Eigen::Vector3d k = w.normalized(); 24 | Eigen::Matrix3d K = Sophus::SO3d::hat(k); 25 | 26 | J_r_inv = J_r_inv 27 | + 0.5 * K 28 | + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K; 29 | } 30 | 31 | return J_r_inv; 32 | } 33 | 34 | } // namespace sliding_window 35 | 36 | #endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_UTILS_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/publisher/cloud_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 在ros中发布点云 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 8 | #define LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "lidar_localization/sensor_data/cloud_data.hpp" 16 | 17 | namespace lidar_localization { 18 | class CloudPublisher { 19 | public: 20 | CloudPublisher(ros::NodeHandle& nh, 21 | std::string topic_name, 22 | std::string frame_id, 23 | size_t buff_size); 24 | CloudPublisher() = default; 25 | 26 | void Publish(CloudData::CLOUD_PTR& cloud_ptr_input, double time); 27 | void Publish(CloudData::CLOUD_PTR& cloud_ptr_input); 28 | 29 | bool HasSubscribers(); 30 | 31 | private: 32 | void PublishData(CloudData::CLOUD_PTR& cloud_ptr_input, ros::Time time); 33 | 34 | private: 35 | ros::NodeHandle nh_; 36 | ros::Publisher publisher_; 37 | std::string frame_id_; 38 | }; 39 | } 40 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/publisher/imu_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 在ros中发布IMU数据 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_IMU_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_IMU_PUBLISHER_HPP_ 8 | 9 | #include 10 | #include "sensor_msgs/Imu.h" 11 | #include "lidar_localization/sensor_data/imu_data.hpp" 12 | 13 | namespace lidar_localization { 14 | class IMUPublisher { 15 | public: 16 | IMUPublisher( 17 | ros::NodeHandle& nh, 18 | std::string topic_name, 19 | std::string frame_id, 20 | size_t buff_size 21 | ); 22 | IMUPublisher() = default; 23 | 24 | void Publish(const IMUData &imu_data, double time); 25 | void Publish(const IMUData &imu_data); 26 | 27 | bool HasSubscribers(void); 28 | 29 | private: 30 | void PublishData(const IMUData &imu_data, ros::Time time); 31 | 32 | ros::NodeHandle nh_; 33 | ros::Publisher publisher_; 34 | std::string frame_id_; 35 | 36 | sensor_msgs::Imu imu_; 37 | }; 38 | } 39 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/publisher/key_frame_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 单个 key frame 信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:05:47 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_KEY_FRAME_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_KEY_FRAME_PUBLISHER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "lidar_localization/sensor_data/key_frame.hpp" 14 | 15 | namespace lidar_localization { 16 | class KeyFramePublisher { 17 | public: 18 | KeyFramePublisher(ros::NodeHandle& nh, 19 | std::string topic_name, 20 | std::string frame_id, 21 | int buff_size); 22 | KeyFramePublisher() = default; 23 | 24 | void Publish(KeyFrame& key_frame); 25 | 26 | bool HasSubscribers(); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Publisher publisher_; 31 | std::string frame_id_ = ""; 32 | }; 33 | } 34 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/publisher/key_frames_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: key frames 信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:05:47 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_KEY_FRAMES_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_KEY_FRAMES_PUBLISHER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "lidar_localization/sensor_data/key_frame.hpp" 16 | 17 | namespace lidar_localization { 18 | class KeyFramesPublisher { 19 | public: 20 | KeyFramesPublisher(ros::NodeHandle& nh, 21 | std::string topic_name, 22 | std::string frame_id, 23 | int buff_size); 24 | KeyFramesPublisher() = default; 25 | 26 | void Publish(const std::deque& key_frames); 27 | 28 | bool HasSubscribers(); 29 | 30 | private: 31 | ros::NodeHandle nh_; 32 | ros::Publisher publisher_; 33 | std::string frame_id_ = ""; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/publisher/lidar_measurement_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: Publish synced Lidar-IMU-GNSS measurement 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-12 15:14:07 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_LIDAR_MEASUREMENT_PUBLISHER_HPP_ 8 | #define LIDAR_LOCALIZATION_PUBLISHER_LIDAR_MEASUREMENT_PUBLISHER_HPP_ 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "lidar_localization/sensor_data/cloud_data.hpp" 23 | #include "lidar_localization/sensor_data/imu_data.hpp" 24 | #include "lidar_localization/sensor_data/velocity_data.hpp" 25 | 26 | namespace lidar_localization { 27 | 28 | class LidarMeasurementPublisher { 29 | public: 30 | LidarMeasurementPublisher( 31 | ros::NodeHandle& nh, 32 | std::string topic_name, 33 | std::string lidar_frame_id, 34 | std::string imu_frame_id, 35 | std::string gnss_odometry_pos_frame_id, 36 | std::string gnss_odometry_vel_frame_id, 37 | size_t buff_size 38 | ); 39 | LidarMeasurementPublisher() = default; 40 | 41 | void Publish( 42 | double time, 43 | const CloudData::CLOUD_PTR& cloud_ptr_input, 44 | const IMUData &imu_data, 45 | const Eigen::Matrix4f& transform_matrix, 46 | const VelocityData &velocity_data 47 | ); 48 | void Publish( 49 | const CloudData::CLOUD_PTR& cloud_ptr_input, 50 | const IMUData &imu_data, 51 | const Eigen::Matrix4f& transform_matrix, 52 | const VelocityData &velocity_data 53 | ); 54 | 55 | bool HasSubscribers(); 56 | 57 | private: 58 | void SetPointCloud( 59 | const ros::Time &time, 60 | const CloudData::CLOUD_PTR &cloud_data_ptr, 61 | sensor_msgs::PointCloud2 &point_cloud 62 | ); 63 | void SetIMU( 64 | const ros::Time &time, 65 | const IMUData &imu_data, 66 | sensor_msgs::Imu &imu 67 | ); 68 | void SetGNSSOdometry( 69 | const ros::Time &time, 70 | const Eigen::Matrix4f& transform_matrix, 71 | const VelocityData &velocity_data, 72 | nav_msgs::Odometry &gnss_odometry 73 | ); 74 | void PublishData( 75 | const ros::Time &time, 76 | const CloudData::CLOUD_PTR& cloud_ptr_input, 77 | const IMUData &imu_data, 78 | const Eigen::Matrix4f& transform_matrix, 79 | const VelocityData &velocity_data 80 | ); 81 | 82 | private: 83 | ros::NodeHandle nh_; 84 | 85 | std::string lidar_frame_id_; 86 | std::string imu_frame_id_; 87 | std::string gnss_odometry_pos_frame_id_; 88 | std::string gnss_odometry_vel_frame_id_; 89 | 90 | LidarMeasurement lidar_measurement_; 91 | 92 | ros::Publisher publisher_; 93 | }; 94 | 95 | } // namespace lidar_localization 96 | 97 | #endif // LIDAR_LOCALIZATION_PUBLISHER_LIDAR_MEASUREMENT_PUBLISHER_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/publisher/loop_pose_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发送闭环检测的相对位姿 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:05:47 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_LOOP_POSE_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_LOOP_POSE_PUBLISHER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "lidar_localization/sensor_data/loop_pose.hpp" 14 | 15 | namespace lidar_localization { 16 | class LoopPosePublisher { 17 | public: 18 | LoopPosePublisher(ros::NodeHandle& nh, 19 | std::string topic_name, 20 | std::string frame_id, 21 | int buff_size); 22 | LoopPosePublisher() = default; 23 | 24 | void Publish(LoopPose& loop_pose); 25 | 26 | bool HasSubscribers(); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Publisher publisher_; 31 | std::string frame_id_ = ""; 32 | }; 33 | } 34 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/publisher/odometry_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: odometry 信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:05:47 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "lidar_localization/sensor_data/velocity_data.hpp" 16 | 17 | namespace lidar_localization { 18 | class OdometryPublisher { 19 | public: 20 | OdometryPublisher(ros::NodeHandle& nh, 21 | std::string topic_name, 22 | std::string base_frame_id, 23 | std::string child_frame_id, 24 | int buff_size); 25 | OdometryPublisher() = default; 26 | 27 | void Publish(const Eigen::Matrix4f& transform_matrix, double time); 28 | void Publish(const Eigen::Matrix4f& transform_matrix); 29 | void Publish(const Eigen::Matrix4f& transform_matrix, const VelocityData &velocity_data, double time); 30 | void Publish(const Eigen::Matrix4f& transform_matrix, const VelocityData &velocity_data); 31 | void Publish(const Eigen::Matrix4f& transform_matrix, const Eigen::Vector3f& vel, double time); 32 | void Publish(const Eigen::Matrix4f& transform_matrix, const Eigen::Vector3f& vel); 33 | 34 | bool HasSubscribers(); 35 | 36 | private: 37 | void PublishData( 38 | const Eigen::Matrix4f& transform_matrix, 39 | const VelocityData &velocity_data, 40 | ros::Time time 41 | ); 42 | 43 | private: 44 | ros::NodeHandle nh_; 45 | ros::Publisher publisher_; 46 | 47 | VelocityData velocity_data_; 48 | nav_msgs::Odometry odometry_; 49 | }; 50 | } 51 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/publisher/pos_vel_mag_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: synced PosVelMagData publisher 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-21 15:39:24 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_POS_VEL_MAG_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_POS_VEL_MAG_PUBLISHER_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include "lidar_localization/sensor_data/pos_vel_mag_data.hpp" 16 | 17 | #include "lidar_localization/PosVelMag.h" 18 | 19 | namespace lidar_localization { 20 | 21 | class PosVelMagPublisher { 22 | public: 23 | PosVelMagPublisher( 24 | ros::NodeHandle& nh, 25 | std::string topic_name, 26 | std::string base_frame_id, 27 | std::string child_frame_id, 28 | int buff_size 29 | ); 30 | PosVelMagPublisher() = default; 31 | 32 | void Publish(const PosVelMagData &pos_vel_mag_data, const double &time); 33 | void Publish(const PosVelMagData &pos_vel_mag_data); 34 | 35 | bool HasSubscribers(); 36 | 37 | private: 38 | void PublishData( 39 | const PosVelMagData &pos_vel_mag_data, 40 | ros::Time time 41 | ); 42 | 43 | private: 44 | ros::NodeHandle nh_; 45 | ros::Publisher publisher_; 46 | PosVelMag pos_vel_mag_msg_; 47 | }; 48 | 49 | } // namespace lidar_localization 50 | 51 | #endif // LIDAR_LOCALIZATION_PUBLISHER_POS_VEL_MAG_PUBLISHER_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/publisher/pos_vel_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: synced PosVelData publisher 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-21 15:39:24 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_POS_VEL_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_POS_VEL_PUBLISHER_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include "lidar_localization/sensor_data/pos_vel_data.hpp" 16 | 17 | #include "lidar_localization/PosVel.h" 18 | 19 | namespace lidar_localization { 20 | 21 | class PosVelPublisher { 22 | public: 23 | PosVelPublisher( 24 | ros::NodeHandle& nh, 25 | std::string topic_name, 26 | std::string base_frame_id, 27 | std::string child_frame_id, 28 | int buff_size 29 | ); 30 | PosVelPublisher() = default; 31 | 32 | void Publish(const PosVelData &pos_vel_data, const double &time); 33 | void Publish(const PosVelData &pos_vel_data); 34 | 35 | bool HasSubscribers(); 36 | 37 | private: 38 | void PublishData( 39 | const PosVelData &pos_vel_data, 40 | ros::Time time 41 | ); 42 | 43 | private: 44 | ros::NodeHandle nh_; 45 | ros::Publisher publisher_; 46 | PosVel pos_vel_msg_; 47 | }; 48 | 49 | } // namespace lidar_localization 50 | 51 | #endif // LIDAR_LOCALIZATION_PUBLISHER_POS_VEL_PUBLISHER_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/publisher/tf_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发布tf的类 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-05 15:23:26 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_TF_BROADCASTER_HPP_ 8 | #define LIDAR_LOCALIZATION_PUBLISHER_TF_BROADCASTER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace lidar_localization { 17 | class TFBroadCaster { 18 | public: 19 | TFBroadCaster(std::string frame_id, std::string child_frame_id); 20 | TFBroadCaster() = default; 21 | void SendTransform(Eigen::Matrix4f pose, double time); 22 | protected: 23 | tf::StampedTransform transform_; 24 | tf::TransformBroadcaster broadcaster_; 25 | }; 26 | } 27 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/cloud_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:17:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | namespace lidar_localization { 13 | class CloudData { 14 | public: 15 | using POINT = pcl::PointXYZ; 16 | using CLOUD = pcl::PointCloud; 17 | using CLOUD_PTR = CLOUD::Ptr; 18 | 19 | public: 20 | CloudData() 21 | :cloud_ptr(new CLOUD()) { 22 | } 23 | 24 | public: 25 | double time = 0.0; 26 | CLOUD_PTR cloud_ptr; 27 | }; 28 | } 29 | 30 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/gnss_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:25:13 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace lidar_localization { 14 | class GNSSData { 15 | public: 16 | double time = 0.0; 17 | double latitude = 0.0; 18 | double longitude = 0.0; 19 | double altitude = 0.0; 20 | double local_E = 0.0; 21 | double local_N = 0.0; 22 | double local_U = 0.0; 23 | int status = 0; 24 | int service = 0; 25 | 26 | static double origin_longitude; 27 | static double origin_latitude; 28 | static double origin_altitude; 29 | 30 | private: 31 | static GeographicLib::LocalCartesian geo_converter; 32 | static bool origin_position_inited; 33 | 34 | public: 35 | void InitOriginPosition(); 36 | void UpdateXYZ(); 37 | 38 | static void Reverse( 39 | const double &local_E, const double &local_N, const double &local_U, 40 | double &lat, double &lon, double &alt 41 | ); 42 | 43 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 44 | }; 45 | } 46 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/imu_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:27:40 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace lidar_localization { 14 | class IMUData { 15 | public: 16 | class Orientation { 17 | public: 18 | double x = 0.0; 19 | double y = 0.0; 20 | double z = 0.0; 21 | double w = 0.0; 22 | 23 | public: 24 | void Normlize() { 25 | double norm = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0) + pow(w, 2.0)); 26 | x /= norm; 27 | y /= norm; 28 | z /= norm; 29 | w /= norm; 30 | } 31 | }; 32 | 33 | struct LinearAcceleration { 34 | double x = 0.0; 35 | double y = 0.0; 36 | double z = 0.0; 37 | }; 38 | 39 | struct AngularVelocity { 40 | double x = 0.0; 41 | double y = 0.0; 42 | double z = 0.0; 43 | }; 44 | 45 | struct AccelBias { 46 | double x = 0.0; 47 | double y = 0.0; 48 | double z = 0.0; 49 | }; 50 | 51 | struct GyroBias { 52 | double x = 0.0; 53 | double y = 0.0; 54 | double z = 0.0; 55 | }; 56 | 57 | double time = 0.0; 58 | 59 | Orientation orientation; 60 | 61 | LinearAcceleration linear_acceleration; 62 | AngularVelocity angular_velocity; 63 | 64 | AccelBias accel_bias; 65 | GyroBias gyro_bias; 66 | 67 | public: 68 | // 把四元数转换成旋转矩阵送出去 69 | Eigen::Matrix3f GetOrientationMatrix() const; 70 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 71 | }; 72 | } 73 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/key_frame.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO key frame 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-28 19:13:26 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_ 8 | 9 | #include 10 | 11 | #include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp" 12 | 13 | namespace lidar_localization { 14 | 15 | struct KeyFrame { 16 | public: 17 | static const int INDEX_P = 0; 18 | static const int INDEX_R = 3; 19 | static const int INDEX_V = 6; 20 | static const int INDEX_A = 9; 21 | static const int INDEX_G = 12; 22 | 23 | double time = 0.0; 24 | 25 | // key frame ID: 26 | unsigned int index = 0; 27 | 28 | // a. position & orientation: 29 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 30 | // b. velocity: 31 | struct { 32 | Eigen::Vector3f v = Eigen::Vector3f::Zero(); 33 | Eigen::Vector3f w = Eigen::Vector3f::Zero(); 34 | } vel; 35 | // c. bias: 36 | struct { 37 | // c.1. accelerometer: 38 | Eigen::Vector3f accel = Eigen::Vector3f::Zero(); 39 | // c.2. gyroscope: 40 | Eigen::Vector3f gyro = Eigen::Vector3f::Zero(); 41 | } bias; 42 | 43 | KeyFrame() {} 44 | 45 | explicit KeyFrame(const int vertex_id, const g2o::PRVAG &prvag) { 46 | // set time: 47 | time = prvag.time; 48 | // set seq. ID: 49 | index = vertex_id; 50 | // set state: 51 | pose.block<3, 1>(0, 3) = prvag.pos.cast(); 52 | pose.block<3, 3>(0, 0) = prvag.ori.matrix().cast(); 53 | vel.v = prvag.vel.cast(); 54 | bias.accel = prvag.b_a.cast(); 55 | bias.gyro = prvag.b_g.cast(); 56 | } 57 | 58 | explicit KeyFrame(const int param_index, const double &T, const double *prvag) { 59 | // set time: 60 | time = T; 61 | // set seq. ID: 62 | index = param_index; 63 | // set state: 64 | Eigen::Map pos(prvag + INDEX_P); 65 | Eigen::Map log_ori(prvag + INDEX_R); 66 | Eigen::Map v(prvag + INDEX_V); 67 | Eigen::Map b_a(prvag + INDEX_A); 68 | Eigen::Map b_g(prvag + INDEX_G); 69 | 70 | pose.block<3, 1>(0, 3) = pos.cast(); 71 | pose.block<3, 3>(0, 0) = Sophus::SO3d::exp(log_ori).matrix().cast(); 72 | 73 | vel.v = v.cast(); 74 | 75 | bias.accel = b_a.cast(); 76 | bias.gyro = b_g.cast(); 77 | } 78 | 79 | Eigen::Quaternionf GetQuaternion() const; 80 | Eigen::Vector3f GetTranslation() const; 81 | }; 82 | 83 | } 84 | 85 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/lidar_measurement_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:17:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_LIDAR_MEASUREMENT_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_LIDAR_MEASUREMENT_DATA_HPP_ 8 | 9 | #include "lidar_localization/sensor_data/cloud_data.hpp" 10 | #include "lidar_localization/sensor_data/imu_data.hpp" 11 | #include "lidar_localization/sensor_data/pose_data.hpp" 12 | 13 | namespace lidar_localization { 14 | 15 | class LidarMeasurementData { 16 | public: 17 | double time = 0.0; 18 | 19 | CloudData point_cloud; 20 | IMUData imu; 21 | PoseData gnss_odometry; 22 | }; 23 | 24 | } // namespace lidar_localization 25 | 26 | #endif // LIDAR_LOCALIZATION_SENSOR_DATA_LIDAR_MEASUREMENT_DATA_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/loop_pose.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 关键帧之间的相对位姿，用于闭环检测 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-28 19:13:26 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_LOOP_POSE_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_LOOP_POSE_HPP_ 8 | 9 | #include 10 | 11 | namespace lidar_localization { 12 | class LoopPose { 13 | public: 14 | double time = 0.0; 15 | unsigned int index0 = 0; 16 | unsigned int index1 = 0; 17 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 18 | 19 | public: 20 | Eigen::Quaternionf GetQuaternion(); 21 | }; 22 | } 23 | 24 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/pos_vel_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: synced GNSS-odo measurements as PosVelData 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-21 15:39:24 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_POS_VEL_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_POS_VEL_DATA_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace lidar_localization { 14 | 15 | class PosVelData { 16 | public: 17 | double time = 0.0; 18 | 19 | Eigen::Vector3f pos = Eigen::Vector3f::Zero(); 20 | Eigen::Vector3f vel = Eigen::Vector3f::Zero(); 21 | }; 22 | 23 | } // namespace lidar_localization 24 | 25 | #endif // LIDAR_LOCALIZATION_SENSOR_DATA_POS_VEL_DATA_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/pose_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 存放处理后的IMU姿态以及GNSS位置 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-27 23:10:56 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_ 8 | 9 | #include 10 | 11 | #include "lidar_localization/sensor_data/velocity_data.hpp" 12 | 13 | namespace lidar_localization { 14 | 15 | class PoseData { 16 | public: 17 | double time = 0.0; 18 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 19 | 20 | struct { 21 | Eigen::Vector3f v = Eigen::Vector3f::Zero(); 22 | Eigen::Vector3f w = Eigen::Vector3f::Zero(); 23 | } vel; 24 | 25 | public: 26 | Eigen::Quaternionf GetQuaternion(); 27 | 28 | void GetVelocityData(VelocityData &velocity_data) const; 29 | }; 30 | 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/sensor_data/velocity_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: velocity 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:27:40 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | namespace lidar_localization { 13 | class VelocityData { 14 | public: 15 | struct LinearVelocity { 16 | double x = 0.0; 17 | double y = 0.0; 18 | double z = 0.0; 19 | }; 20 | 21 | struct AngularVelocity { 22 | double x = 0.0; 23 | double y = 0.0; 24 | double z = 0.0; 25 | }; 26 | 27 | double time = 0.0; 28 | LinearVelocity linear_velocity; 29 | AngularVelocity angular_velocity; 30 | 31 | public: 32 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 33 | void TransformCoordinate(Eigen::Matrix4f transform_matrix); 34 | void NED2ENU(void); 35 | }; 36 | } 37 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/cloud_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 8 | #define LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "lidar_localization/sensor_data/cloud_data.hpp" 21 | 22 | namespace lidar_localization { 23 | class CloudSubscriber { 24 | public: 25 | CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | CloudSubscriber() = default; 27 | void ParseData(std::deque& deque_cloud_data); 28 | 29 | private: 30 | void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_cloud_data_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } 40 | 41 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/gnss_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-03-31 12:58:10 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include "sensor_msgs/NavSatFix.h" 15 | 16 | #include "lidar_localization/sensor_data/gnss_data.hpp" 17 | 18 | namespace lidar_localization { 19 | class GNSSSubscriber { 20 | public: 21 | GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 22 | GNSSSubscriber() = default; 23 | void ParseData(std::deque& deque_gnss_data); 24 | 25 | private: 26 | void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Subscriber subscriber_; 31 | std::deque new_gnss_data_; 32 | 33 | std::mutex buff_mutex_; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/imu_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include "sensor_msgs/Imu.h" 15 | 16 | #include "lidar_localization/sensor_data/imu_data.hpp" 17 | 18 | namespace lidar_localization { 19 | class IMUSubscriber { 20 | public: 21 | IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 22 | IMUSubscriber() = default; 23 | void ParseData(std::deque& deque_imu_data); 24 | 25 | private: 26 | void msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Subscriber subscriber_; 31 | std::deque new_imu_data_; 32 | 33 | std::mutex buff_mutex_; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/key_frame_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_KEY_FRAME_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_KEY_FRAME_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "lidar_localization/sensor_data/key_frame.hpp" 17 | 18 | namespace lidar_localization { 19 | class KeyFrameSubscriber { 20 | public: 21 | KeyFrameSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 22 | KeyFrameSubscriber() = default; 23 | void ParseData(std::deque& key_frame_buff); 24 | 25 | private: 26 | void msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& key_frame_msg_ptr); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Subscriber subscriber_; 31 | std::deque new_key_frame_; 32 | 33 | std::mutex buff_mutex_; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/key_frames_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_KEY_FRAMES_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_KEY_FRAMES_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "lidar_localization/sensor_data/key_frame.hpp" 18 | 19 | namespace lidar_localization { 20 | class KeyFramesSubscriber { 21 | public: 22 | KeyFramesSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 23 | KeyFramesSubscriber() = default; 24 | void ParseData(std::deque& deque_key_frames); 25 | 26 | private: 27 | void msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr); 28 | 29 | private: 30 | ros::NodeHandle nh_; 31 | ros::Subscriber subscriber_; 32 | std::deque new_key_frames_; 33 | 34 | std::mutex buff_mutex_; 35 | }; 36 | } 37 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/lidar_measurement_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_LIDAR_MEASUREMENT_SUBSCRIBER_HPP_ 8 | #define LIDAR_LOCALIZATION_SUBSCRIBER_LIDAR_MEASUREMENT_SUBSCRIBER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include "lidar_localization/sensor_data/lidar_measurement_data.hpp" 27 | 28 | namespace lidar_localization { 29 | 30 | class LidarMeasurementSubscriber { 31 | public: 32 | LidarMeasurementSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 33 | LidarMeasurementSubscriber() = default; 34 | void ParseData(std::deque& deque_cloud_data); 35 | 36 | private: 37 | void ParseCloudData( 38 | const sensor_msgs::PointCloud2 &point_cloud_msg, 39 | CloudData &point_cloud_data 40 | ); 41 | void ParseIMUData( 42 | const sensor_msgs::Imu &imu_msg, 43 | IMUData &imu_data 44 | ); 45 | void ParsePoseData( 46 | const nav_msgs::Odometry &gnss_odometry_msg, 47 | PoseData &gnss_odometry_data 48 | ); 49 | void msg_callback(const LidarMeasurement::ConstPtr& synced_cloud_msg_ptr); 50 | 51 | private: 52 | ros::NodeHandle nh_; 53 | ros::Subscriber subscriber_; 54 | std::deque new_cloud_data_; 55 | 56 | std::mutex buff_mutex_; 57 | }; 58 | 59 | } // namespace lidar_localization 60 | 61 | #endif // LIDAR_LOCALIZATION_SUBSCRIBER_LIDAR_MEASUREMENT_SUBSCRIBER_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/loop_pose_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 闭环检测位姿 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_LOOP_POSE_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_LOOP_POSE_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "lidar_localization/sensor_data/loop_pose.hpp" 17 | 18 | namespace lidar_localization { 19 | class LoopPoseSubscriber { 20 | public: 21 | LoopPoseSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 22 | LoopPoseSubscriber() = default; 23 | void ParseData(std::deque& loop_pose_buff); 24 | 25 | private: 26 | void msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& loop_pose_msg_ptr); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Subscriber subscriber_; 31 | std::deque new_loop_pose_; 32 | 33 | std::mutex buff_mutex_; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/odometry_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅odometry数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "lidar_localization/sensor_data/pose_data.hpp" 17 | 18 | namespace lidar_localization { 19 | class OdometrySubscriber { 20 | public: 21 | OdometrySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 22 | OdometrySubscriber() = default; 23 | void ParseData(std::deque& deque_pose_data); 24 | 25 | private: 26 | void msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Subscriber subscriber_; 31 | std::deque new_pose_data_; 32 | 33 | std::mutex buff_mutex_; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/pos_vel_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: Subscribe to PosVel messages 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-12 15:14:07 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_POS_VEL_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_POS_VEL_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "lidar_localization/PosVel.h" 17 | #include "lidar_localization/sensor_data/pos_vel_data.hpp" 18 | 19 | namespace lidar_localization { 20 | 21 | class PosVelSubscriber { 22 | public: 23 | PosVelSubscriber( 24 | ros::NodeHandle& nh, 25 | std::string topic_name, 26 | size_t buff_size 27 | ); 28 | PosVelSubscriber() = default; 29 | void ParseData(std::deque& pos_vel_data_buff); 30 | 31 | private: 32 | void msg_callback(const PosVelConstPtr& pos_vel_msg_ptr); 33 | 34 | private: 35 | ros::NodeHandle nh_; 36 | ros::Subscriber subscriber_; 37 | std::deque new_pos_vel_data_; 38 | 39 | std::mutex buff_mutex_; 40 | }; 41 | 42 | } // namespace lidar_localization 43 | 44 | #endif // LIDAR_LOCALIZATION_SUBSCRIBER_POS_VEL_SUBSCRIBER_HPP_ -------------------------------------------------------------------------------- /include/lidar_localization/subscriber/velocity_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅velocity数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include "geometry_msgs/TwistStamped.h" 15 | 16 | #include "lidar_localization/sensor_data/velocity_data.hpp" 17 | 18 | namespace lidar_localization { 19 | class VelocitySubscriber { 20 | public: 21 | VelocitySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 22 | VelocitySubscriber() = default; 23 | void ParseData(std::deque& deque_velocity_data); 24 | 25 | private: 26 | void msg_callback(const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr); 27 | 28 | private: 29 | ros::NodeHandle nh_; 30 | ros::Subscriber subscriber_; 31 | std::deque new_velocity_data_; 32 | 33 | std::mutex buff_mutex_; 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/tf_listener/tf_listener.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: tf监听模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 16:01:21 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_TF_LISTENER_HPP_ 7 | #define LIDAR_LOCALIZATION_TF_LISTENER_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace lidar_localization { 16 | class TFListener { 17 | public: 18 | TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id); 19 | TFListener() = default; 20 | 21 | bool LookupData(Eigen::Matrix4f& transform_matrix); 22 | 23 | private: 24 | bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix); 25 | 26 | private: 27 | ros::NodeHandle nh_; 28 | tf::TransformListener listener_; 29 | std::string base_frame_id_; 30 | std::string child_frame_id_; 31 | }; 32 | } 33 | 34 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/tools/file_manager.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 读写文件管理 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-24 19:22:53 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_TOOLS_FILE_MANAGER_HPP_ 7 | #define LIDAR_LOCALIZATION_TOOLS_FILE_MANAGER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace lidar_localization { 14 | class FileManager{ 15 | public: 16 | static bool CreateFile(std::ofstream& ofs, std::string file_path); 17 | static bool InitDirectory(std::string directory_path, std::string use_for); 18 | static bool CreateDirectory(std::string directory_path, std::string use_for); 19 | static bool CreateDirectory(std::string directory_path); 20 | }; 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /include/lidar_localization/tools/print_info.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 打印信息 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-02 23:25:26 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_TOOLS_PRINT_INFO_HPP_ 7 | #define LIDAR_LOCALIZATION_TOOLS_PRINT_INFO_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include "pcl/common/eigen.h" 13 | 14 | namespace lidar_localization { 15 | class PrintInfo { 16 | public: 17 | static void PrintPose(std::string head, Eigen::Matrix4f pose); 18 | }; 19 | } 20 | #endif -------------------------------------------------------------------------------- /include/lidar_localization/tools/tic_toc.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 用来测试运行时间 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-01 18:12:03 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_TOOLS_TIC_TOC_HPP_ 8 | #define LIDAR_LOCALIZATION_TOOLS_TIC_TOC_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace lidar_localization { 15 | class TicToc { 16 | public: 17 | TicToc() { 18 | tic(); 19 | } 20 | 21 | void tic() { 22 | start = std::chrono::system_clock::now(); 23 | } 24 | 25 | double toc() { 26 | end = std::chrono::system_clock::now(); 27 | std::chrono::duration elapsed_seconds = end - start; 28 | start = std::chrono::system_clock::now(); 29 | return elapsed_seconds.count(); 30 | } 31 | 32 | private: 33 | std::chrono::time_point start, end; 34 | }; 35 | } 36 | #endif 37 | -------------------------------------------------------------------------------- /launch/kitti_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/lio_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/lio_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /msg/EKFStd.msg: -------------------------------------------------------------------------------- 1 | # time of ESKF estimation: 2 | Header header 3 | 4 | # a. position: 5 | float64 pos_x_std 6 | float64 pos_y_std 7 | float64 pos_z_std 8 | 9 | # b. velocity: 10 | float64 vel_x_std 11 | float64 vel_y_std 12 | float64 vel_z_std 13 | 14 | # c. orientation: 15 | float64 ori_w_std 16 | float64 ori_x_std 17 | float64 ori_y_std 18 | float64 ori_z_std 19 | 20 | # d. gyro. bias: 21 | float64 gyro_bias_x_std 22 | float64 gyro_bias_y_std 23 | float64 gyro_bias_z_std 24 | 25 | # e. accel. bias: 26 | float64 accel_bias_x_std 27 | float64 accel_bias_y_std 28 | float64 accel_bias_z_std -------------------------------------------------------------------------------- /msg/ESKFStd.msg: -------------------------------------------------------------------------------- 1 | # time of ESKF estimation: 2 | Header header 3 | 4 | # a. position: 5 | float64 delta_pos_x_std 6 | float64 delta_pos_y_std 7 | float64 delta_pos_z_std 8 | 9 | # b. velocity: 10 | float64 delta_vel_x_std 11 | float64 delta_vel_y_std 12 | float64 delta_vel_z_std 13 | 14 | # c. orientation: 15 | float64 delta_ori_x_std 16 | float64 delta_ori_y_std 17 | float64 delta_ori_z_std 18 | 19 | # d. gyro. bias: 20 | float64 gyro_bias_x_std 21 | float64 gyro_bias_y_std 22 | float64 gyro_bias_z_std 23 | 24 | # e. accel. bias: 25 | float64 accel_bias_x_std 26 | float64 accel_bias_y_std 27 | float64 accel_bias_z_std -------------------------------------------------------------------------------- /msg/IMUGNSSMeasurement.msg: -------------------------------------------------------------------------------- 1 | # time of lidar data acquisition, and the coordinate frame ID: 2 | Header header 3 | 4 | # synced IMU measurement: 5 | sensor_msgs/Imu imu 6 | 7 | # synced GNSS pose estimation: 8 | nav_msgs/Odometry gnss -------------------------------------------------------------------------------- /msg/LidarMeasurement.msg: -------------------------------------------------------------------------------- 1 | # time of lidar data acquisition, and the coordinate frame ID: 2 | Header header 3 | 4 | # lidar measurement: 5 | sensor_msgs/PointCloud2 point_cloud 6 | 7 | # synced IMU measurement: 8 | sensor_msgs/Imu imu 9 | 10 | # synced GNSS pose estimation: 11 | nav_msgs/Odometry gnss_odometry -------------------------------------------------------------------------------- /msg/PosVel.msg: -------------------------------------------------------------------------------- 1 | # timestamp of synced GNSS-odo measurement: 2 | Header header 3 | 4 | string child_frame_id 5 | 6 | # a. position: 7 | geometry_msgs/Point position 8 | 9 | # b. velocity: 10 | geometry_msgs/Vector3 velocity -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_localization 4 | 0.0.0 5 | The lidar_localization package 6 | 7 | 8 | 9 | 10 | rq 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | roscpp 55 | rospy 56 | roscpp 57 | rospy 58 | 59 | message_generation 60 | message_runtime 61 | 62 | std_msgs 63 | geometry_msgs 64 | sensor_msgs 65 | nav_msgs 66 | pcl_ros 67 | tf 68 | eigen_conversions 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /slam_data/.gitignore: -------------------------------------------------------------------------------- 1 | map 2 | scan_context 3 | key_frames 4 | trajectory -------------------------------------------------------------------------------- /src/apps/data_pretreat_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 数据预处理的node文件 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:56:27 5 | */ 6 | #include 7 | #include "glog/logging.h" 8 | 9 | #include "lidar_localization/global_defination/global_defination.h" 10 | #include "lidar_localization/data_pretreat/data_pretreat_flow.hpp" 11 | 12 | using namespace lidar_localization; 13 | 14 | int main(int argc, char *argv[]) { 15 | google::InitGoogleLogging(argv[0]); 16 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 17 | FLAGS_alsologtostderr = 1; 18 | 19 | ros::init(argc, argv, "data_pretreat_node"); 20 | ros::NodeHandle nh; 21 | 22 | std::string cloud_topic; 23 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 24 | 25 | // subscribe to 26 | // a. raw Velodyne measurement 27 | // b. raw GNSS/IMU measurement 28 | // publish 29 | // a. undistorted Velodyne measurement 30 | // b. lidar pose in map frame 31 | std::shared_ptr data_pretreat_flow_ptr = std::make_shared(nh, cloud_topic); 32 | 33 | // pre-process lidar point cloud at 100Hz: 34 | ros::Rate rate(100); 35 | while (ros::ok()) { 36 | ros::spinOnce(); 37 | 38 | data_pretreat_flow_ptr->Run(); 39 | 40 | rate.sleep(); 41 | } 42 | 43 | return 0; 44 | } -------------------------------------------------------------------------------- /src/apps/front_end_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 前端里程计的node文件 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:56:27 5 | */ 6 | #include 7 | #include "glog/logging.h" 8 | 9 | #include "lidar_localization/global_defination/global_defination.h" 10 | #include "lidar_localization/mapping/front_end/front_end_flow.hpp" 11 | 12 | using namespace lidar_localization; 13 | 14 | int main(int argc, char *argv[]) { 15 | google::InitGoogleLogging(argv[0]); 16 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 17 | FLAGS_alsologtostderr = 1; 18 | 19 | ros::init(argc, argv, "front_end_node"); 20 | ros::NodeHandle nh; 21 | 22 | std::string cloud_topic, odom_topic; 23 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 24 | nh.param("odom_topic", odom_topic, "/laser_odom"); 25 | 26 | // subscribe to 27 | // a. undistorted Velodyne measurement 28 | // publish 29 | // a. lidar odometry estimation 30 | std::shared_ptr front_end_flow_ptr = std::make_shared(nh, cloud_topic, odom_topic); 31 | 32 | ros::Rate rate(100); 33 | while (ros::ok()) { 34 | ros::spinOnce(); 35 | 36 | front_end_flow_ptr->Run(); 37 | 38 | rate.sleep(); 39 | } 40 | 41 | return 0; 42 | } -------------------------------------------------------------------------------- /src/apps/kitti_filtering_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: IMU-lidar fusion for localization 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-12 15:14:07 5 | */ 6 | #include 7 | 8 | #include "lidar_localization/global_defination/global_defination.h" 9 | 10 | #include "lidar_localization/filtering/kitti_filtering_flow.hpp" 11 | 12 | #include 13 | 14 | #include "glog/logging.h" 15 | 16 | using namespace lidar_localization; 17 | 18 | bool _need_save_odometry = false; 19 | 20 | bool SaveOdometryCB(saveOdometry::Request &request, saveOdometry::Response &response) { 21 | _need_save_odometry = true; 22 | response.succeed = true; 23 | return response.succeed; 24 | } 25 | 26 | 27 | int main(int argc, char *argv[]) { 28 | google::InitGoogleLogging(argv[0]); 29 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 30 | FLAGS_alsologtostderr = 1; 31 | 32 | ros::init(argc, argv, "kitti_filtering_node"); 33 | ros::NodeHandle nh; 34 | 35 | std::shared_ptr kitti_filtering_flow_ptr = std::make_shared(nh); 36 | ros::ServiceServer service = nh.advertiseService("save_odometry", SaveOdometryCB); 37 | 38 | ros::Rate rate(100); 39 | while (ros::ok()) { 40 | ros::spinOnce(); 41 | 42 | kitti_filtering_flow_ptr->Run(); 43 | 44 | // save odometry estimations for evo evaluation: 45 | if ( 46 | _need_save_odometry && 47 | kitti_filtering_flow_ptr->SaveOdometry() 48 | ) { 49 | _need_save_odometry = false; 50 | } 51 | 52 | rate.sleep(); 53 | } 54 | 55 | return 0; 56 | } -------------------------------------------------------------------------------- /src/apps/lio_back_end_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: LIO mapping backend ROS node 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-29 15:47:49 5 | */ 6 | #include 7 | #include "glog/logging.h" 8 | 9 | #include 10 | #include "lidar_localization/global_defination/global_defination.h" 11 | #include "lidar_localization/mapping/back_end/lio_back_end_flow.hpp" 12 | 13 | using namespace lidar_localization; 14 | 15 | bool _need_optimize_map = false; 16 | 17 | bool OptimizeMapCb( 18 | optimizeMap::Request &request, 19 | optimizeMap::Response &response 20 | ) { 21 | _need_optimize_map = true; 22 | response.succeed = true; 23 | return response.succeed; 24 | } 25 | 26 | int main(int argc, char *argv[]) { 27 | google::InitGoogleLogging(argv[0]); 28 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 29 | FLAGS_alsologtostderr = 1; 30 | 31 | ros::init(argc, argv, "lio_back_end_node"); 32 | ros::NodeHandle nh; 33 | 34 | std::string cloud_topic, odom_topic; 35 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 36 | nh.param("odom_topic", odom_topic, "/laser_odom"); 37 | 38 | // 39 | // subscribe to: 40 | // 41 | // a. undistorted Velodyne measurement: 42 | // b. lidar pose in map frame: 43 | // c. lidar odometry estimation: 44 | // d. loop close pose: 45 | // publish: 46 | // a. lidar odometry in map frame: 47 | // b. key frame pose and corresponding GNSS/IMU pose 48 | // c. optimized key frame sequence as trajectory 49 | std::shared_ptr lio_back_end_flow_ptr = std::make_shared( 50 | nh, cloud_topic, odom_topic 51 | ); 52 | ros::ServiceServer service = nh.advertiseService( 53 | "optimize_map", OptimizeMapCb 54 | ); 55 | 56 | ros::Rate rate(10); 57 | while (ros::ok()) { 58 | ros::spinOnce(); 59 | 60 | lio_back_end_flow_ptr->Run(); 61 | 62 | if (_need_optimize_map) { 63 | lio_back_end_flow_ptr->ForceOptimize(); 64 | lio_back_end_flow_ptr->SaveOptimizedOdometry(); 65 | 66 | _need_optimize_map = false; 67 | } 68 | 69 | rate.sleep(); 70 | } 71 | 72 | return EXIT_SUCCESS; 73 | } -------------------------------------------------------------------------------- /src/apps/lio_matching_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: frontend node for lio localization 3 | * @Author: Ge Yao 4 | * @Date: 2021-01-02 10:47:23 5 | */ 6 | #include 7 | #include "glog/logging.h" 8 | 9 | #include "lidar_localization/global_defination/global_defination.h" 10 | #include "lidar_localization/matching/front_end/matching_flow.hpp" 11 | 12 | using namespace lidar_localization; 13 | 14 | int main(int argc, char *argv[]) { 15 | google::InitGoogleLogging(argv[0]); 16 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 17 | FLAGS_alsologtostderr = 1; 18 | 19 | ros::init(argc, argv, "lio_matching_node"); 20 | ros::NodeHandle nh; 21 | 22 | // subscribe to: 23 | // a. 去畸变后的点云 24 | // b. 雷达在世界坐标系中的位姿 25 | 26 | // publish: 27 | // a. 相对位姿估计 28 | // b. 地图匹配的位姿 29 | // this provides input to sliding window backend 30 | std::shared_ptr matching_flow_ptr = std::make_shared(nh); 31 | 32 | ros::Rate rate(100); 33 | while (ros::ok()) { 34 | ros::spinOnce(); 35 | 36 | matching_flow_ptr->Run(); 37 | 38 | rate.sleep(); 39 | } 40 | 41 | return 0; 42 | } -------------------------------------------------------------------------------- /src/apps/loop_closing_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 闭环检测的node文件 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:56:27 5 | */ 6 | #include 7 | #include "glog/logging.h" 8 | 9 | #include "lidar_localization/global_defination/global_defination.h" 10 | #include "lidar_localization/mapping/loop_closing/loop_closing_flow.hpp" 11 | #include 12 | 13 | using namespace lidar_localization; 14 | 15 | bool save_scan_context = false; 16 | 17 | bool SaveScanContextCb(saveScanContext::Request &request, saveScanContext::Response &response) { 18 | save_scan_context = true; 19 | response.succeed = true; 20 | return response.succeed; 21 | } 22 | 23 | int main(int argc, char *argv[]) { 24 | google::InitGoogleLogging(argv[0]); 25 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 26 | FLAGS_alsologtostderr = 1; 27 | 28 | ros::init(argc, argv, "loop_closing_node"); 29 | ros::NodeHandle nh; 30 | 31 | // subscribe to: 32 | // a. key frame pose and corresponding GNSS/IMU pose from backend node 33 | // publish: 34 | // a. loop closure detection result for backend node: 35 | std::shared_ptr loop_closing_flow_ptr = std::make_shared(nh); 36 | 37 | // register service for scan context save: 38 | ros::ServiceServer service = nh.advertiseService("save_scan_context", SaveScanContextCb); 39 | 40 | ros::Rate rate(100); 41 | while (ros::ok()) { 42 | ros::spinOnce(); 43 | 44 | loop_closing_flow_ptr->Run(); 45 | 46 | if (save_scan_context) { 47 | save_scan_context = false; 48 | loop_closing_flow_ptr->Save(); 49 | } 50 | 51 | rate.sleep(); 52 | } 53 | 54 | return 0; 55 | } -------------------------------------------------------------------------------- /src/apps/sliding_window_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: backend node for lio localization 3 | * @Author: Ge Yao 4 | * @Date: 2021-01-02 10:47:23 5 | */ 6 | #include 7 | 8 | #include "lidar_localization/global_defination/global_defination.h" 9 | #include "lidar_localization/matching/back_end/sliding_window_flow.hpp" 10 | #include "lidar_localization/saveOdometry.h" 11 | 12 | #include "glog/logging.h" 13 | 14 | using namespace lidar_localization; 15 | 16 | bool save_odometry = false; 17 | 18 | bool SaveOdometryCb(saveOdometry::Request &request, saveOdometry::Response &response) { 19 | save_odometry = true; 20 | response.succeed = true; 21 | return response.succeed; 22 | } 23 | 24 | int main(int argc, char *argv[]) { 25 | google::InitGoogleLogging(argv[0]); 26 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 27 | FLAGS_alsologtostderr = 1; 28 | 29 | ros::init(argc, argv, "sliding_window_node"); 30 | ros::NodeHandle nh; 31 | 32 | // 33 | // subscribe to: 34 | // 35 | // a. lidar odometry 36 | // b. map matching odometry 37 | 38 | // 39 | // publish: 40 | // 41 | // a. optimized key frame sequence as trajectory 42 | std::shared_ptr sliding_window_flow_ptr = std::make_shared(nh); 43 | 44 | // register service for optimized trajectory save: 45 | ros::ServiceServer service = nh.advertiseService("save_odometry", SaveOdometryCb); 46 | 47 | ros::Rate rate(10); 48 | while (ros::ok()) { 49 | ros::spinOnce(); 50 | 51 | sliding_window_flow_ptr->Run(); 52 | 53 | if (save_odometry) { 54 | save_odometry = false; 55 | sliding_window_flow_ptr->SaveOptimizedTrajectory(); 56 | } 57 | 58 | rate.sleep(); 59 | } 60 | 61 | return EXIT_SUCCESS; 62 | } -------------------------------------------------------------------------------- /src/apps/viewer_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: viewer 的 node 文件 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:56:27 5 | */ 6 | #include 7 | #include "glog/logging.h" 8 | 9 | #include 10 | #include "lidar_localization/global_defination/global_defination.h" 11 | #include "lidar_localization/mapping/viewer/viewer_flow.hpp" 12 | 13 | using namespace lidar_localization; 14 | 15 | std::shared_ptr _viewer_flow_ptr; 16 | bool _need_save_map = false; 17 | 18 | bool save_map_callback(saveMap::Request &request, saveMap::Response &response) { 19 | _need_save_map = true; 20 | response.succeed = true; 21 | return response.succeed; 22 | } 23 | 24 | int main(int argc, char *argv[]) { 25 | google::InitGoogleLogging(argv[0]); 26 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 27 | FLAGS_alsologtostderr = 1; 28 | 29 | ros::init(argc, argv, "viewer_node"); 30 | ros::NodeHandle nh; 31 | 32 | std::string cloud_topic; 33 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 34 | std::shared_ptr _viewer_flow_ptr = std::make_shared(nh, cloud_topic); 35 | 36 | ros::ServiceServer service = nh.advertiseService("save_map", save_map_callback); 37 | 38 | ros::Rate rate(100); 39 | while (ros::ok()) { 40 | ros::spinOnce(); 41 | 42 | _viewer_flow_ptr->Run(); 43 | if (_need_save_map) { 44 | _need_save_map = false; 45 | _viewer_flow_ptr->SaveMap(); 46 | } 47 | 48 | rate.sleep(); 49 | } 50 | 51 | return 0; 52 | } -------------------------------------------------------------------------------- /src/mapping/front_end/front_end_flow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: front end 任务管理， 放在类里使代码更清晰 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-10 08:38:42 5 | */ 6 | #include "lidar_localization/mapping/front_end/front_end_flow.hpp" 7 | #include "glog/logging.h" 8 | #include "lidar_localization/global_defination/global_defination.h" 9 | 10 | namespace lidar_localization { 11 | FrontEndFlow::FrontEndFlow(ros::NodeHandle& nh, std::string cloud_topic, std::string odom_topic) { 12 | cloud_sub_ptr_ = std::make_shared(nh, cloud_topic, 100000); 13 | laser_odom_pub_ptr_ = std::make_shared(nh, odom_topic, "/map", "/lidar", 100); 14 | 15 | front_end_ptr_ = std::make_shared(); 16 | } 17 | 18 | bool FrontEndFlow::Run() { 19 | if (!ReadData()) 20 | return false; 21 | 22 | while(HasData()) { 23 | if (!ValidData()) 24 | continue; 25 | 26 | if (UpdateLaserOdometry()) { 27 | PublishData(); 28 | } 29 | } 30 | 31 | return true; 32 | } 33 | 34 | bool FrontEndFlow::ReadData() { 35 | cloud_sub_ptr_->ParseData(cloud_data_buff_); 36 | return true; 37 | } 38 | 39 | bool FrontEndFlow::HasData() { 40 | return cloud_data_buff_.size() > 0; 41 | } 42 | 43 | bool FrontEndFlow::ValidData() { 44 | current_cloud_data_ = cloud_data_buff_.front(); 45 | cloud_data_buff_.pop_front(); 46 | 47 | return true; 48 | } 49 | 50 | bool FrontEndFlow::UpdateLaserOdometry() { 51 | static bool odometry_inited = false; 52 | if (!odometry_inited) { 53 | odometry_inited = true; 54 | // init lidar odometry: 55 | front_end_ptr_->SetInitPose(Eigen::Matrix4f::Identity()); 56 | } 57 | 58 | // update lidar odometry using current undistorted measurement: 59 | return front_end_ptr_->Update(current_cloud_data_, laser_odometry_); 60 | } 61 | 62 | bool FrontEndFlow::PublishData() { 63 | laser_odom_pub_ptr_->Publish(laser_odometry_, current_cloud_data_.time); 64 | 65 | return true; 66 | } 67 | } -------------------------------------------------------------------------------- /src/mapping/loop_closing/loop_closing_flow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-10 08:38:42 5 | */ 6 | #include "lidar_localization/mapping/loop_closing/loop_closing_flow.hpp" 7 | #include "glog/logging.h" 8 | #include "lidar_localization/global_defination/global_defination.h" 9 | 10 | namespace lidar_localization { 11 | LoopClosingFlow::LoopClosingFlow(ros::NodeHandle& nh) { 12 | // subscriber: 13 | key_scan_sub_ptr_ = std::make_shared(nh, "/key_scan", 100000); 14 | key_frame_sub_ptr_ = std::make_shared(nh, "/key_frame", 100000); 15 | key_gnss_sub_ptr_ = std::make_shared(nh, "/key_gnss", 100000); 16 | // publisher 17 | loop_pose_pub_ptr_ = std::make_shared(nh, "/loop_pose", "/map", 100); 18 | // loop closing 19 | loop_closing_ptr_ = std::make_shared(); 20 | } 21 | 22 | bool LoopClosingFlow::Run() { 23 | if (!ReadData()) 24 | return false; 25 | 26 | while(HasData()) { 27 | if (!ValidData()) 28 | continue; 29 | 30 | loop_closing_ptr_->Update( 31 | current_key_scan_, current_key_frame_, current_key_gnss_ 32 | ); 33 | 34 | PublishData(); 35 | } 36 | 37 | return true; 38 | } 39 | 40 | bool LoopClosingFlow::Save(void) { 41 | return loop_closing_ptr_->Save(); 42 | } 43 | 44 | bool LoopClosingFlow::ReadData() { 45 | key_scan_sub_ptr_->ParseData(key_scan_buff_); 46 | key_frame_sub_ptr_->ParseData(key_frame_buff_); 47 | key_gnss_sub_ptr_->ParseData(key_gnss_buff_); 48 | 49 | return true; 50 | } 51 | 52 | bool LoopClosingFlow::HasData() { 53 | if ( 54 | key_scan_buff_.size() == 0 || 55 | key_frame_buff_.size() == 0 || 56 | key_gnss_buff_.size() == 0 57 | ) { 58 | return false; 59 | } 60 | 61 | return true; 62 | } 63 | 64 | bool LoopClosingFlow::ValidData() { 65 | current_key_scan_ = key_scan_buff_.front(); 66 | current_key_frame_ = key_frame_buff_.front(); 67 | current_key_gnss_ = key_gnss_buff_.front(); 68 | 69 | double diff_gnss_time = current_key_frame_.time - current_key_gnss_.time; 70 | 71 | if (diff_gnss_time < -0.05) { 72 | key_frame_buff_.pop_front(); 73 | return false; 74 | } 75 | 76 | if (diff_gnss_time > 0.05) { 77 | key_gnss_buff_.pop_front(); 78 | return false; 79 | } 80 | 81 | key_scan_buff_.pop_front(); 82 | key_frame_buff_.pop_front(); 83 | key_gnss_buff_.pop_front(); 84 | 85 | return true; 86 | } 87 | 88 | bool LoopClosingFlow::PublishData() { 89 | if (loop_closing_ptr_->HasNewLoopPose()) 90 | loop_pose_pub_ptr_->Publish(loop_closing_ptr_->GetCurrentLoopPose()); 91 | 92 | return true; 93 | } 94 | } -------------------------------------------------------------------------------- /src/mapping/viewer/viewer_flow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-10 08:38:42 5 | */ 6 | #include "lidar_localization/mapping/viewer/viewer_flow.hpp" 7 | #include "glog/logging.h" 8 | #include "lidar_localization/global_defination/global_defination.h" 9 | 10 | namespace lidar_localization { 11 | ViewerFlow::ViewerFlow(ros::NodeHandle& nh, std::string cloud_topic) { 12 | // subscriber 13 | cloud_sub_ptr_ = std::make_shared(nh, cloud_topic, 100000); 14 | key_frame_sub_ptr_ = std::make_shared(nh, "/key_frame", 100000); 15 | transformed_odom_sub_ptr_ = std::make_shared(nh, "/transformed_odom", 100000); 16 | optimized_key_frames_sub_ptr_ = std::make_shared(nh, "/optimized_key_frames", 100000); 17 | // publisher 18 | optimized_odom_pub_ptr_ = std::make_shared(nh, "/optimized_odom", "/map", "/lidar", 100); 19 | current_scan_pub_ptr_ = std::make_shared(nh, "/current_scan", "/map", 100); 20 | global_map_pub_ptr_ = std::make_shared(nh, "/global_map", "/map", 100); 21 | local_map_pub_ptr_ = std::make_shared(nh, "/local_map", "/map", 100); 22 | // viewer 23 | viewer_ptr_ = std::make_shared(); 24 | } 25 | 26 | bool ViewerFlow::Run() { 27 | if (!ReadData()) 28 | return false; 29 | 30 | while(HasData()) { 31 | if (ValidData()) { 32 | viewer_ptr_->UpdateWithNewKeyFrame(key_frame_buff_, current_transformed_odom_, current_cloud_data_); 33 | PublishLocalData(); 34 | } 35 | } 36 | 37 | if (optimized_key_frames_.size() > 0) { 38 | viewer_ptr_->UpdateWithOptimizedKeyFrames(optimized_key_frames_); 39 | PublishGlobalData(); 40 | } 41 | 42 | return true; 43 | } 44 | 45 | bool ViewerFlow::ReadData() { 46 | cloud_sub_ptr_->ParseData(cloud_data_buff_); 47 | transformed_odom_sub_ptr_->ParseData(transformed_odom_buff_); 48 | key_frame_sub_ptr_->ParseData(key_frame_buff_); 49 | optimized_key_frames_sub_ptr_->ParseData(optimized_key_frames_); 50 | 51 | return true; 52 | } 53 | 54 | bool ViewerFlow::HasData() { 55 | if (cloud_data_buff_.size() == 0) 56 | return false; 57 | if (transformed_odom_buff_.size() == 0) 58 | return false; 59 | 60 | return true; 61 | } 62 | 63 | bool ViewerFlow::ValidData() { 64 | current_cloud_data_ = cloud_data_buff_.front(); 65 | current_transformed_odom_ = transformed_odom_buff_.front(); 66 | 67 | double diff_odom_time = current_cloud_data_.time - current_transformed_odom_.time; 68 | 69 | if (diff_odom_time < -0.05) { 70 | cloud_data_buff_.pop_front(); 71 | return false; 72 | } 73 | 74 | if (diff_odom_time > 0.05) { 75 | transformed_odom_buff_.pop_front(); 76 | return false; 77 | } 78 | 79 | cloud_data_buff_.pop_front(); 80 | transformed_odom_buff_.pop_front(); 81 | 82 | return true; 83 | } 84 | 85 | bool ViewerFlow::PublishGlobalData() { 86 | if (viewer_ptr_->HasNewGlobalMap() && global_map_pub_ptr_->HasSubscribers()) { 87 | CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD()); 88 | viewer_ptr_->GetGlobalMap(cloud_ptr); 89 | global_map_pub_ptr_->Publish(cloud_ptr); 90 | } 91 | 92 | return true; 93 | } 94 | 95 | bool ViewerFlow::PublishLocalData() { 96 | optimized_odom_pub_ptr_->Publish(viewer_ptr_->GetCurrentPose()); 97 | current_scan_pub_ptr_->Publish(viewer_ptr_->GetCurrentScan()); 98 | 99 | if (viewer_ptr_->HasNewLocalMap() && local_map_pub_ptr_->HasSubscribers()) { 100 | CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD()); 101 | viewer_ptr_->GetLocalMap(cloud_ptr); 102 | local_map_pub_ptr_->Publish(cloud_ptr); 103 | } 104 | 105 | return true; 106 | } 107 | 108 | bool ViewerFlow::SaveMap() { 109 | return viewer_ptr_->SaveMap(); 110 | } 111 | } -------------------------------------------------------------------------------- /src/models/cloud_filter/box_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 从点云中截取一个立方体部分 3 | * @Author: Ren Qian 4 | * @Date: 2019-03-12 23:38:31 5 | */ 6 | #include 7 | #include 8 | #include "glog/logging.h" 9 | 10 | #include "lidar_localization/models/cloud_filter/box_filter.hpp" 11 | 12 | namespace lidar_localization { 13 | BoxFilter::BoxFilter(YAML::Node node) { 14 | size_.resize(6); 15 | edge_.resize(6); 16 | origin_.resize(3); 17 | 18 | for (size_t i = 0; i < size_.size(); i++) { 19 | size_.at(i) = node["box_filter_size"][i].as(); 20 | } 21 | SetSize(size_); 22 | } 23 | 24 | bool BoxFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, 25 | CloudData::CLOUD_PTR& output_cloud_ptr) { 26 | output_cloud_ptr->clear(); 27 | pcl_box_filter_.setMin(Eigen::Vector4f(edge_.at(0), edge_.at(2), edge_.at(4), 1.0e-6)); 28 | pcl_box_filter_.setMax(Eigen::Vector4f(edge_.at(1), edge_.at(3), edge_.at(5), 1.0e6)); 29 | pcl_box_filter_.setInputCloud(input_cloud_ptr); 30 | pcl_box_filter_.filter(*output_cloud_ptr); 31 | 32 | return true; 33 | } 34 | 35 | void BoxFilter::SetSize(std::vector size) { 36 | size_ = size; 37 | std::cout << "Box Filter params:" << std::endl 38 | << "min_x: " << size.at(0) << ", " 39 | << "max_x: " << size.at(1) << ", " 40 | << "min_y: " << size.at(2) << ", " 41 | << "max_y: " << size.at(3) << ", " 42 | << "min_z: " << size.at(4) << ", " 43 | << "max_z: " << size.at(5) 44 | << std::endl << std::endl; 45 | 46 | CalculateEdge(); 47 | } 48 | 49 | void BoxFilter::SetOrigin(std::vector origin) { 50 | origin_ = origin; 51 | CalculateEdge(); 52 | } 53 | 54 | void BoxFilter::CalculateEdge() { 55 | for (size_t i = 0; i < origin_.size(); ++i) { 56 | edge_.at(2 * i) = size_.at(2 * i) + origin_.at(i); 57 | edge_.at(2 * i + 1) = size_.at(2 * i + 1) + origin_.at(i); 58 | } 59 | } 60 | 61 | std::vector BoxFilter::GetEdge() { 62 | return edge_; 63 | } 64 | } // namespace lidar_slam -------------------------------------------------------------------------------- /src/models/cloud_filter/no_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 不滤波 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:53:20 5 | */ 6 | #include "lidar_localization/models/cloud_filter/no_filter.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization { 10 | NoFilter::NoFilter() { 11 | } 12 | 13 | bool NoFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) { 14 | filtered_cloud_ptr.reset(new CloudData::CLOUD(*input_cloud_ptr)); 15 | return true; 16 | } 17 | } -------------------------------------------------------------------------------- /src/models/cloud_filter/voxel_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel filter 模块实现 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:53:20 5 | */ 6 | #include "lidar_localization/models/cloud_filter/voxel_filter.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | 12 | VoxelFilter::VoxelFilter(const YAML::Node& node) { 13 | float leaf_size_x = node["leaf_size"][0].as(); 14 | float leaf_size_y = node["leaf_size"][1].as(); 15 | float leaf_size_z = node["leaf_size"][2].as(); 16 | 17 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 18 | } 19 | 20 | VoxelFilter::VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z) { 21 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 22 | } 23 | 24 | bool VoxelFilter::SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z) { 25 | voxel_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z); 26 | 27 | std::cout << "Voxel Filter params:" << std::endl 28 | << leaf_size_x << ", " 29 | << leaf_size_y << ", " 30 | << leaf_size_z 31 | << std::endl << std::endl; 32 | 33 | return true; 34 | } 35 | 36 | bool VoxelFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) { 37 | voxel_filter_.setInputCloud(input_cloud_ptr); 38 | voxel_filter_.filter(*filtered_cloud_ptr); 39 | 40 | return true; 41 | } 42 | } -------------------------------------------------------------------------------- /src/models/graph_optimizer/interface_graph_optimizer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-01 18:35:19 5 | */ 6 | 7 | #include "lidar_localization/models/graph_optimizer/interface_graph_optimizer.hpp" 8 | 9 | namespace lidar_localization { 10 | 11 | void InterfaceGraphOptimizer::SetMaxIterationsNum(int max_iterations_num) { 12 | max_iterations_num_ = max_iterations_num; 13 | } 14 | 15 | } -------------------------------------------------------------------------------- /src/models/kalman_filter/kalman_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: Kalman Filter utilities. 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-12 15:14:07 5 | */ 6 | 7 | #include "lidar_localization/models/kalman_filter/kalman_filter.hpp" 8 | 9 | // SVD for observability analysis: 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include "lidar_localization/tools/CSVWriter.hpp" 16 | 17 | #include "lidar_localization/global_defination/global_defination.h" 18 | 19 | #include "glog/logging.h" 20 | 21 | namespace lidar_localization { 22 | 23 | void KalmanFilter::AnalyzeQ( 24 | const int DIM_STATE, 25 | const double &time, const Eigen::MatrixXd &Q, const Eigen::VectorXd &Y, 26 | std::vector> &data 27 | ) { 28 | // perform SVD analysis, thin for rectangular matrix: 29 | Eigen::JacobiSVD svd(Q, Eigen::ComputeThinU | Eigen::ComputeThinV); 30 | 31 | // add record: 32 | std::vector record; 33 | 34 | // a. record timestamp: 35 | record.push_back(time); 36 | 37 | // b. record singular values: 38 | for (int i = 0; i < DIM_STATE; ++i) { 39 | record.push_back(svd.singularValues()(i, 0)); 40 | } 41 | 42 | // c. record degree of observability: 43 | Eigen::MatrixXd X = ( 44 | svd.matrixV() * 45 | svd.singularValues().asDiagonal().inverse() 46 | ) * ( 47 | svd.matrixU().transpose() * Y 48 | ).asDiagonal(); 49 | 50 | Eigen::VectorXd degree_of_observability = Eigen::VectorXd::Zero(DIM_STATE); 51 | for (int i = 0; i < DIM_STATE; ++i) { 52 | // find max. magnitude response in X: 53 | Eigen::MatrixXd::Index sv_index; 54 | X.col(i).cwiseAbs().maxCoeff(&sv_index); 55 | 56 | // associate with corresponding singular value: 57 | degree_of_observability(sv_index) = svd.singularValues()(i); 58 | } 59 | // normalize: 60 | degree_of_observability = 1.0 / svd.singularValues().maxCoeff() * degree_of_observability; 61 | 62 | for (int i = 0; i < DIM_STATE; ++i) { 63 | record.push_back(degree_of_observability(i)); 64 | } 65 | 66 | // add to data: 67 | data.push_back(record); 68 | } 69 | 70 | void KalmanFilter::WriteAsCSV( 71 | const int DIM_STATE, 72 | const std::vector> &data, 73 | const std::string filename 74 | ) { 75 | // init: 76 | CSVWriter csv(","); 77 | csv.enableAutoNewRow(1 + 2*DIM_STATE); 78 | 79 | // a. write header: 80 | csv << "T"; 81 | for (int i = 0; i < DIM_STATE; ++i) { 82 | csv << ("sv" + std::to_string(i + 1)); 83 | } 84 | for (int i = 0; i < DIM_STATE; ++i) { 85 | csv << ("doo" + std::to_string(i + 1)); 86 | } 87 | 88 | // b. write contents: 89 | for (const auto &record: data) { 90 | // cast timestamp to int: 91 | csv << static_cast(record.at(0)); 92 | 93 | for (size_t i = 1; i < record.size(); ++i) { 94 | csv << std::fabs(record.at(i)); 95 | } 96 | } 97 | 98 | // save to persistent storage: 99 | csv.writeToFile(filename); 100 | } 101 | 102 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/models/registration/ndt_registration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: NDT 匹配模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-08 21:46:45 5 | */ 6 | #include "lidar_localization/models/registration/ndt_registration.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | 12 | NDTRegistration::NDTRegistration(const YAML::Node& node) 13 | :ndt_ptr_(new pcl::NormalDistributionsTransform()) { 14 | 15 | float res = node["res"].as(); 16 | float step_size = node["step_size"].as(); 17 | float trans_eps = node["trans_eps"].as(); 18 | int max_iter = node["max_iter"].as(); 19 | 20 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 21 | } 22 | 23 | NDTRegistration::NDTRegistration(float res, float step_size, float trans_eps, int max_iter) 24 | :ndt_ptr_(new pcl::NormalDistributionsTransform()) { 25 | 26 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 27 | } 28 | 29 | bool NDTRegistration::SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter) { 30 | ndt_ptr_->setResolution(res); 31 | ndt_ptr_->setStepSize(step_size); 32 | ndt_ptr_->setTransformationEpsilon(trans_eps); 33 | ndt_ptr_->setMaximumIterations(max_iter); 34 | 35 | std::cout << "NDT params:" << std::endl 36 | << "res: " << res << ", " 37 | << "step_size: " << step_size << ", " 38 | << "trans_eps: " << trans_eps << ", " 39 | << "max_iter: " << max_iter 40 | << std::endl << std::endl; 41 | 42 | return true; 43 | } 44 | 45 | bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) { 46 | ndt_ptr_->setInputTarget(input_target); 47 | 48 | return true; 49 | } 50 | 51 | bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 52 | const Eigen::Matrix4f& predict_pose, 53 | CloudData::CLOUD_PTR& result_cloud_ptr, 54 | Eigen::Matrix4f& result_pose) { 55 | ndt_ptr_->setInputSource(input_source); 56 | ndt_ptr_->align(*result_cloud_ptr, predict_pose); 57 | result_pose = ndt_ptr_->getFinalTransformation(); 58 | 59 | return true; 60 | } 61 | 62 | float NDTRegistration::GetFitnessScore() { 63 | return ndt_ptr_->getFitnessScore(); 64 | } 65 | } -------------------------------------------------------------------------------- /src/models/scan_adjust/distortion_adjust.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云畸变补偿 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-25 14:39:00 5 | */ 6 | #include "lidar_localization/models/scan_adjust/distortion_adjust.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization { 10 | void DistortionAdjust::SetMotionInfo(float scan_period, VelocityData velocity_data) { 11 | scan_period_ = scan_period; 12 | velocity_ << velocity_data.linear_velocity.x, velocity_data.linear_velocity.y, velocity_data.linear_velocity.z; 13 | angular_rate_ << velocity_data.angular_velocity.x, velocity_data.angular_velocity.y, velocity_data.angular_velocity.z; 14 | } 15 | 16 | bool DistortionAdjust::AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr) { 17 | CloudData::CLOUD_PTR origin_cloud_ptr(new CloudData::CLOUD(*input_cloud_ptr)); 18 | output_cloud_ptr.reset(new CloudData::CLOUD()); 19 | 20 | float orientation_space = 2.0 * M_PI; 21 | float delete_space = 5.0 * M_PI / 180.0; 22 | float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x); 23 | 24 | Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ()); 25 | Eigen::Matrix3f rotate_matrix = t_V.matrix(); 26 | Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity(); 27 | transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse(); 28 | pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix); 29 | 30 | velocity_ = rotate_matrix * velocity_; 31 | angular_rate_ = rotate_matrix * angular_rate_; 32 | 33 | for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) { 34 | float orientation = atan2(origin_cloud_ptr->points[point_index].y, origin_cloud_ptr->points[point_index].x); 35 | if (orientation < 0.0) 36 | orientation += 2.0 * M_PI; 37 | 38 | if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space) 39 | continue; 40 | 41 | float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0; 42 | 43 | Eigen::Vector3f origin_point(origin_cloud_ptr->points[point_index].x, 44 | origin_cloud_ptr->points[point_index].y, 45 | origin_cloud_ptr->points[point_index].z); 46 | 47 | Eigen::Matrix3f current_matrix = UpdateMatrix(real_time); 48 | Eigen::Vector3f rotated_point = current_matrix * origin_point; 49 | Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time; 50 | CloudData::POINT point; 51 | point.x = adjusted_point(0); 52 | point.y = adjusted_point(1); 53 | point.z = adjusted_point(2); 54 | output_cloud_ptr->points.push_back(point); 55 | } 56 | 57 | pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse()); 58 | return true; 59 | } 60 | 61 | Eigen::Matrix3f DistortionAdjust::UpdateMatrix(float real_time) { 62 | Eigen::Vector3f angle = angular_rate_ * real_time; 63 | Eigen::AngleAxisf t_Vz(angle(2), Eigen::Vector3f::UnitZ()); 64 | Eigen::AngleAxisf t_Vy(angle(1), Eigen::Vector3f::UnitY()); 65 | Eigen::AngleAxisf t_Vx(angle(0), Eigen::Vector3f::UnitX()); 66 | Eigen::AngleAxisf t_V; 67 | t_V = t_Vz * t_Vy * t_Vx; 68 | return t_V.matrix(); 69 | } 70 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/publisher/cloud_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 通过ros发布点云 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #include "lidar_localization/publisher/cloud_publisher.hpp" 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | CloudPublisher::CloudPublisher(ros::NodeHandle& nh, 12 | std::string topic_name, 13 | std::string frame_id, 14 | size_t buff_size) 15 | :nh_(nh), frame_id_(frame_id) { 16 | publisher_ = nh_.advertise(topic_name, buff_size); 17 | } 18 | 19 | void CloudPublisher::Publish(CloudData::CLOUD_PTR& cloud_ptr_input, double time) { 20 | ros::Time ros_time(time); 21 | PublishData(cloud_ptr_input, ros_time); 22 | } 23 | 24 | void CloudPublisher::Publish(CloudData::CLOUD_PTR& cloud_ptr_input) { 25 | ros::Time time = ros::Time::now(); 26 | PublishData(cloud_ptr_input, time); 27 | } 28 | 29 | void CloudPublisher::PublishData(CloudData::CLOUD_PTR& cloud_ptr_input, ros::Time time) { 30 | sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2()); 31 | pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output); 32 | 33 | cloud_ptr_output->header.stamp = time; 34 | cloud_ptr_output->header.frame_id = frame_id_; 35 | publisher_.publish(*cloud_ptr_output); 36 | } 37 | 38 | bool CloudPublisher::HasSubscribers() { 39 | return publisher_.getNumSubscribers() != 0; 40 | } 41 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/publisher/imu_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 通过ros发布点云 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | #include "lidar_localization/publisher/imu_publisher.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization { 10 | 11 | IMUPublisher::IMUPublisher( 12 | ros::NodeHandle& nh, 13 | std::string topic_name, 14 | std::string frame_id, 15 | size_t buff_size 16 | ) 17 | :nh_(nh), frame_id_(frame_id) { 18 | publisher_ = nh_.advertise(topic_name, buff_size); 19 | 20 | imu_.header.frame_id = frame_id_; 21 | } 22 | 23 | void IMUPublisher::Publish(const IMUData &imu_data, double time) { 24 | ros::Time ros_time(time); 25 | PublishData(imu_data, ros_time); 26 | } 27 | 28 | void IMUPublisher::Publish(const IMUData &imu_data) { 29 | ros::Time time = ros::Time::now(); 30 | PublishData(imu_data, time); 31 | } 32 | 33 | void IMUPublisher::PublishData(const IMUData &imu_data, ros::Time time) { 34 | imu_.header.stamp = time; 35 | 36 | // set orientation: 37 | imu_.orientation.w = imu_data.orientation.w; 38 | imu_.orientation.x = imu_data.orientation.x; 39 | imu_.orientation.y = imu_data.orientation.y; 40 | imu_.orientation.z = imu_data.orientation.z; 41 | 42 | // set angular velocity: 43 | imu_.angular_velocity.x = imu_data.angular_velocity.x; 44 | imu_.angular_velocity.y = imu_data.angular_velocity.y; 45 | imu_.angular_velocity.z = imu_data.angular_velocity.z; 46 | 47 | // set linear acceleration: 48 | imu_.linear_acceleration.x = imu_data.linear_acceleration.x; 49 | imu_.linear_acceleration.y = imu_data.linear_acceleration.y; 50 | imu_.linear_acceleration.z = imu_data.linear_acceleration.z; 51 | 52 | publisher_.publish(imu_); 53 | } 54 | 55 | bool IMUPublisher::HasSubscribers(void) { 56 | return publisher_.getNumSubscribers() != 0; 57 | } 58 | 59 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/publisher/key_frame_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 单个 key frame 信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:11:44 5 | */ 6 | #include "lidar_localization/publisher/key_frame_publisher.hpp" 7 | 8 | #include 9 | 10 | namespace lidar_localization { 11 | KeyFramePublisher::KeyFramePublisher(ros::NodeHandle& nh, 12 | std::string topic_name, 13 | std::string frame_id, 14 | int buff_size) 15 | :nh_(nh), frame_id_(frame_id) { 16 | 17 | publisher_ = nh_.advertise(topic_name, buff_size); 18 | } 19 | 20 | void KeyFramePublisher::Publish(KeyFrame& key_frame) { 21 | geometry_msgs::PoseWithCovarianceStamped pose_stamped; 22 | 23 | ros::Time ros_time(key_frame.time); 24 | pose_stamped.header.stamp = ros_time; 25 | pose_stamped.header.frame_id = frame_id_; 26 | 27 | pose_stamped.pose.pose.position.x = key_frame.pose(0,3); 28 | pose_stamped.pose.pose.position.y = key_frame.pose(1,3); 29 | pose_stamped.pose.pose.position.z = key_frame.pose(2,3); 30 | 31 | Eigen::Quaternionf q = key_frame.GetQuaternion(); 32 | pose_stamped.pose.pose.orientation.x = q.x(); 33 | pose_stamped.pose.pose.orientation.y = q.y(); 34 | pose_stamped.pose.pose.orientation.z = q.z(); 35 | pose_stamped.pose.pose.orientation.w = q.w(); 36 | 37 | pose_stamped.pose.covariance[0] = (double)key_frame.index; 38 | 39 | publisher_.publish(pose_stamped); 40 | } 41 | 42 | bool KeyFramePublisher::HasSubscribers() { 43 | return publisher_.getNumSubscribers() != 0; 44 | } 45 | } -------------------------------------------------------------------------------- /src/publisher/key_frames_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: key frames 信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:11:44 5 | */ 6 | #include "lidar_localization/publisher/key_frames_publisher.hpp" 7 | 8 | #include 9 | 10 | namespace lidar_localization { 11 | KeyFramesPublisher::KeyFramesPublisher(ros::NodeHandle& nh, 12 | std::string topic_name, 13 | std::string frame_id, 14 | int buff_size) 15 | :nh_(nh), frame_id_(frame_id) { 16 | 17 | publisher_ = nh_.advertise(topic_name, buff_size); 18 | } 19 | 20 | void KeyFramesPublisher::Publish(const std::deque& key_frames) { 21 | nav_msgs::Path path; 22 | path.header.stamp = ros::Time::now(); 23 | path.header.frame_id = frame_id_; 24 | 25 | for (size_t i = 0; i < key_frames.size(); ++i) { 26 | KeyFrame key_frame = key_frames.at(i); 27 | 28 | geometry_msgs::PoseStamped pose_stamped; 29 | ros::Time ros_time(key_frame.time); 30 | pose_stamped.header.stamp = ros_time; 31 | pose_stamped.header.frame_id = frame_id_; 32 | 33 | pose_stamped.header.seq = key_frame.index; 34 | 35 | pose_stamped.pose.position.x = key_frame.pose(0,3); 36 | pose_stamped.pose.position.y = key_frame.pose(1,3); 37 | pose_stamped.pose.position.z = key_frame.pose(2,3); 38 | 39 | Eigen::Quaternionf q = key_frame.GetQuaternion(); 40 | pose_stamped.pose.orientation.x = q.x(); 41 | pose_stamped.pose.orientation.y = q.y(); 42 | pose_stamped.pose.orientation.z = q.z(); 43 | pose_stamped.pose.orientation.w = q.w(); 44 | 45 | path.poses.push_back(pose_stamped); 46 | } 47 | 48 | publisher_.publish(path); 49 | } 50 | 51 | bool KeyFramesPublisher::HasSubscribers() { 52 | return publisher_.getNumSubscribers() != 0; 53 | } 54 | } -------------------------------------------------------------------------------- /src/publisher/loop_pose_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发送闭环检测的相对位姿 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:11:44 5 | */ 6 | #include "lidar_localization/publisher/loop_pose_publisher.hpp" 7 | 8 | #include 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | LoopPosePublisher::LoopPosePublisher(ros::NodeHandle& nh, 13 | std::string topic_name, 14 | std::string frame_id, 15 | int buff_size) 16 | :nh_(nh), frame_id_(frame_id) { 17 | 18 | publisher_ = nh_.advertise(topic_name, buff_size); 19 | } 20 | 21 | void LoopPosePublisher::Publish(LoopPose& loop_pose) { 22 | geometry_msgs::PoseWithCovarianceStamped pose_stamped; 23 | 24 | ros::Time ros_time(loop_pose.time); 25 | pose_stamped.header.stamp = ros_time; 26 | pose_stamped.header.frame_id = frame_id_; 27 | 28 | pose_stamped.pose.pose.position.x = loop_pose.pose(0,3); 29 | pose_stamped.pose.pose.position.y = loop_pose.pose(1,3); 30 | pose_stamped.pose.pose.position.z = loop_pose.pose(2,3); 31 | 32 | Eigen::Quaternionf q = loop_pose.GetQuaternion(); 33 | pose_stamped.pose.pose.orientation.x = q.x(); 34 | pose_stamped.pose.pose.orientation.y = q.y(); 35 | pose_stamped.pose.pose.orientation.z = q.z(); 36 | pose_stamped.pose.pose.orientation.w = q.w(); 37 | 38 | pose_stamped.pose.covariance[0] = (double)loop_pose.index0; 39 | pose_stamped.pose.covariance[1] = (double)loop_pose.index1; 40 | 41 | publisher_.publish(pose_stamped); 42 | } 43 | 44 | bool LoopPosePublisher::HasSubscribers() { 45 | return publisher_.getNumSubscribers() != 0; 46 | } 47 | } -------------------------------------------------------------------------------- /src/publisher/odometry_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 里程计信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:11:44 5 | */ 6 | #include "lidar_localization/publisher/odometry_publisher.hpp" 7 | 8 | namespace lidar_localization { 9 | OdometryPublisher::OdometryPublisher(ros::NodeHandle& nh, 10 | std::string topic_name, 11 | std::string base_frame_id, 12 | std::string child_frame_id, 13 | int buff_size) 14 | :nh_(nh) { 15 | 16 | publisher_ = nh_.advertise(topic_name, buff_size); 17 | odometry_.header.frame_id = base_frame_id; 18 | odometry_.child_frame_id = child_frame_id; 19 | } 20 | 21 | void OdometryPublisher::Publish( 22 | const Eigen::Matrix4f& transform_matrix, 23 | double time 24 | ) { 25 | ros::Time ros_time(time); 26 | PublishData(transform_matrix, velocity_data_, ros_time); 27 | } 28 | 29 | void OdometryPublisher::Publish( 30 | const Eigen::Matrix4f& transform_matrix 31 | ) { 32 | PublishData(transform_matrix, velocity_data_, ros::Time::now()); 33 | } 34 | 35 | void OdometryPublisher::Publish( 36 | const Eigen::Matrix4f& transform_matrix, 37 | const VelocityData &velocity_data, 38 | double time 39 | ) { 40 | ros::Time ros_time(time); 41 | PublishData(transform_matrix, velocity_data, ros_time); 42 | } 43 | 44 | void OdometryPublisher::Publish( 45 | const Eigen::Matrix4f& transform_matrix, 46 | const VelocityData &velocity_data 47 | ) { 48 | PublishData(transform_matrix, velocity_data, ros::Time::now()); 49 | } 50 | 51 | void OdometryPublisher::Publish( 52 | const Eigen::Matrix4f& transform_matrix, 53 | const Eigen::Vector3f& vel, 54 | double time 55 | ) { 56 | ros::Time ros_time(time); 57 | 58 | velocity_data_.linear_velocity.x = vel.x(); 59 | velocity_data_.linear_velocity.y = vel.y(); 60 | velocity_data_.linear_velocity.z = vel.z(); 61 | 62 | PublishData(transform_matrix, velocity_data_, ros_time); 63 | 64 | velocity_data_.linear_velocity.x = velocity_data_.linear_velocity.y = velocity_data_.linear_velocity.z = 0.0; 65 | } 66 | 67 | void OdometryPublisher::Publish( 68 | const Eigen::Matrix4f& transform_matrix, 69 | const Eigen::Vector3f& vel 70 | ) { 71 | velocity_data_.linear_velocity.x = vel.x(); 72 | velocity_data_.linear_velocity.y = vel.y(); 73 | velocity_data_.linear_velocity.z = vel.z(); 74 | 75 | PublishData(transform_matrix, velocity_data_, ros::Time::now()); 76 | 77 | velocity_data_.linear_velocity.x = velocity_data_.linear_velocity.y = velocity_data_.linear_velocity.z = 0.0; 78 | } 79 | 80 | void OdometryPublisher::PublishData( 81 | const Eigen::Matrix4f& transform_matrix, 82 | const VelocityData &velocity_data, 83 | ros::Time time 84 | ) { 85 | odometry_.header.stamp = time; 86 | 87 | // set the pose 88 | odometry_.pose.pose.position.x = transform_matrix(0,3); 89 | odometry_.pose.pose.position.y = transform_matrix(1,3); 90 | odometry_.pose.pose.position.z = transform_matrix(2,3); 91 | 92 | Eigen::Quaternionf q; 93 | q = transform_matrix.block<3,3>(0,0); 94 | odometry_.pose.pose.orientation.x = q.x(); 95 | odometry_.pose.pose.orientation.y = q.y(); 96 | odometry_.pose.pose.orientation.z = q.z(); 97 | odometry_.pose.pose.orientation.w = q.w(); 98 | 99 | // set the twist: 100 | odometry_.twist.twist.linear.x = velocity_data.linear_velocity.x; 101 | odometry_.twist.twist.linear.y = velocity_data.linear_velocity.y; 102 | odometry_.twist.twist.linear.z = velocity_data.linear_velocity.z; 103 | 104 | odometry_.twist.twist.angular.x = velocity_data.angular_velocity.x; 105 | odometry_.twist.twist.angular.y = velocity_data.angular_velocity.y; 106 | odometry_.twist.twist.angular.z = velocity_data.angular_velocity.z; 107 | 108 | publisher_.publish(odometry_); 109 | } 110 | 111 | bool OdometryPublisher::HasSubscribers() { 112 | return publisher_.getNumSubscribers() != 0; 113 | } 114 | } -------------------------------------------------------------------------------- /src/publisher/pos_vel_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: synced PosVelData publisher 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-21 15:39:24 5 | */ 6 | #include "lidar_localization/publisher/pos_vel_publisher.hpp" 7 | 8 | namespace lidar_localization { 9 | 10 | PosVelPublisher::PosVelPublisher( 11 | ros::NodeHandle& nh, 12 | std::string topic_name, 13 | std::string base_frame_id, 14 | std::string child_frame_id, 15 | int buff_size 16 | ) 17 | :nh_(nh) { 18 | 19 | publisher_ = nh_.advertise(topic_name, buff_size); 20 | pos_vel_msg_.header.frame_id = base_frame_id; 21 | pos_vel_msg_.child_frame_id = child_frame_id; 22 | } 23 | 24 | void PosVelPublisher::Publish( 25 | const PosVelData &pos_vel_data, 26 | const double &time 27 | ) { 28 | ros::Time ros_time(time); 29 | PublishData(pos_vel_data, ros_time); 30 | } 31 | 32 | void PosVelPublisher::Publish( 33 | const PosVelData &pos_vel_data 34 | ) { 35 | PublishData(pos_vel_data, ros::Time::now()); 36 | } 37 | 38 | bool PosVelPublisher::HasSubscribers() { 39 | return publisher_.getNumSubscribers() != 0; 40 | } 41 | 42 | void PosVelPublisher::PublishData( 43 | const PosVelData &pos_vel_data, 44 | ros::Time time 45 | ) { 46 | pos_vel_msg_.header.stamp = time; 47 | 48 | // a. set position 49 | pos_vel_msg_.position.x = pos_vel_data.pos.x(); 50 | pos_vel_msg_.position.y = pos_vel_data.pos.y(); 51 | pos_vel_msg_.position.z = pos_vel_data.pos.z(); 52 | 53 | // b. set velocity: 54 | pos_vel_msg_.velocity.x = pos_vel_data.vel.x(); 55 | pos_vel_msg_.velocity.y = pos_vel_data.vel.y(); 56 | pos_vel_msg_.velocity.z = pos_vel_data.vel.z(); 57 | 58 | publisher_.publish(pos_vel_msg_); 59 | } 60 | 61 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/publisher/tf_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发布tf的类 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-05 15:23:26 5 | */ 6 | 7 | #include "lidar_localization/publisher/tf_broadcaster.hpp" 8 | 9 | namespace lidar_localization { 10 | TFBroadCaster::TFBroadCaster(std::string frame_id, std::string child_frame_id) { 11 | transform_.frame_id_ = frame_id; 12 | transform_.child_frame_id_ = child_frame_id; 13 | } 14 | 15 | void TFBroadCaster::SendTransform(Eigen::Matrix4f pose, double time) { 16 | Eigen::Quaternionf q(pose.block<3,3>(0,0)); 17 | ros::Time ros_time(time); 18 | transform_.stamp_ = ros_time; 19 | transform_.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w())); 20 | transform_.setOrigin(tf::Vector3(pose(0,3), pose(1,3), pose(2,3))); 21 | broadcaster_.sendTransform(transform_); 22 | } 23 | } -------------------------------------------------------------------------------- /src/sensor_data/gnss_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 20:42:23 5 | */ 6 | #include "lidar_localization/sensor_data/gnss_data.hpp" 7 | 8 | #include "glog/logging.h" 9 | #include 10 | 11 | //静态成员变量必须在类外初始化 12 | double lidar_localization::GNSSData::origin_latitude = 0.0; 13 | double lidar_localization::GNSSData::origin_longitude = 0.0; 14 | double lidar_localization::GNSSData::origin_altitude = 0.0; 15 | bool lidar_localization::GNSSData::origin_position_inited = false; 16 | GeographicLib::LocalCartesian lidar_localization::GNSSData::geo_converter; 17 | 18 | namespace lidar_localization { 19 | void GNSSData::InitOriginPosition() { 20 | geo_converter.Reset(latitude, longitude, altitude); 21 | 22 | origin_latitude = latitude; 23 | origin_longitude = longitude; 24 | origin_altitude = altitude; 25 | 26 | origin_position_inited = true; 27 | } 28 | 29 | void GNSSData::UpdateXYZ() { 30 | if (!origin_position_inited) { 31 | LOG(WARNING) << "WARNING: GeoConverter is NOT initialized."; 32 | } 33 | 34 | geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U); 35 | } 36 | 37 | void GNSSData::Reverse( 38 | const double &local_E, const double &local_N, const double &local_U, 39 | double &lat, double &lon, double &alt 40 | ) { 41 | if (!origin_position_inited) { 42 | LOG(WARNING) << "WARNING: GeoConverter is NOT initialized."; 43 | } 44 | 45 | geo_converter.Reverse(local_E, local_N, local_U, lat, lon, alt); 46 | } 47 | 48 | bool GNSSData::SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time) { 49 | // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置 50 | // 即找到与同步时间相邻的左右两个数据 51 | // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值 52 | while (UnsyncedData.size() >= 2) { 53 | if (UnsyncedData.front().time > sync_time) 54 | return false; 55 | if (UnsyncedData.at(1).time < sync_time) { 56 | UnsyncedData.pop_front(); 57 | continue; 58 | } 59 | if (sync_time - UnsyncedData.front().time > 0.2) { 60 | UnsyncedData.pop_front(); 61 | return false; 62 | } 63 | if (UnsyncedData.at(1).time - sync_time > 0.2) { 64 | UnsyncedData.pop_front(); 65 | return false; 66 | } 67 | break; 68 | } 69 | if (UnsyncedData.size() < 2) 70 | return false; 71 | 72 | GNSSData front_data = UnsyncedData.at(0); 73 | GNSSData back_data = UnsyncedData.at(1); 74 | GNSSData synced_data; 75 | 76 | double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 77 | double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time); 78 | synced_data.time = sync_time; 79 | synced_data.status = back_data.status; 80 | synced_data.longitude = front_data.longitude * front_scale + back_data.longitude * back_scale; 81 | synced_data.latitude = front_data.latitude * front_scale + back_data.latitude * back_scale; 82 | synced_data.altitude = front_data.altitude * front_scale + back_data.altitude * back_scale; 83 | synced_data.local_E = front_data.local_E * front_scale + back_data.local_E * back_scale; 84 | synced_data.local_N = front_data.local_N * front_scale + back_data.local_N * back_scale; 85 | synced_data.local_U = front_data.local_U * front_scale + back_data.local_U * back_scale; 86 | 87 | SyncedData.push_back(synced_data); 88 | 89 | return true; 90 | } 91 | 92 | } -------------------------------------------------------------------------------- /src/sensor_data/imu_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: imu data 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-23 22:20:41 5 | */ 6 | #include "lidar_localization/sensor_data/imu_data.hpp" 7 | 8 | #include 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | Eigen::Matrix3f IMUData::GetOrientationMatrix() const { 13 | Eigen::Quaterniond q(orientation.w, orientation.x, orientation.y, orientation.z); 14 | return q.matrix().cast(); 15 | } 16 | 17 | bool IMUData::SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time) { 18 | // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置 19 | // 即找到与同步时间相邻的左右两个数据 20 | // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值 21 | while (UnsyncedData.size() >= 2) { 22 | // UnsyncedData.front().time should be <= sync_time: 23 | if (UnsyncedData.front().time > sync_time) 24 | return false; 25 | // sync_time should be <= UnsyncedData.at(1).time: 26 | if (UnsyncedData.at(1).time < sync_time) { 27 | UnsyncedData.pop_front(); 28 | continue; 29 | } 30 | 31 | // sync_time - UnsyncedData.front().time should be <= 0.2: 32 | if (sync_time - UnsyncedData.front().time > 0.2) { 33 | UnsyncedData.pop_front(); 34 | return false; 35 | } 36 | // UnsyncedData.at(1).time - sync_time should be <= 0.2 37 | if (UnsyncedData.at(1).time - sync_time > 0.2) { 38 | UnsyncedData.pop_front(); 39 | return false; 40 | } 41 | break; 42 | } 43 | if (UnsyncedData.size() < 2) 44 | return false; 45 | 46 | IMUData front_data = UnsyncedData.at(0); 47 | IMUData back_data = UnsyncedData.at(1); 48 | IMUData synced_data; 49 | 50 | double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 51 | double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time); 52 | synced_data.time = sync_time; 53 | synced_data.linear_acceleration.x = front_data.linear_acceleration.x * front_scale + back_data.linear_acceleration.x * back_scale; 54 | synced_data.linear_acceleration.y = front_data.linear_acceleration.y * front_scale + back_data.linear_acceleration.y * back_scale; 55 | synced_data.linear_acceleration.z = front_data.linear_acceleration.z * front_scale + back_data.linear_acceleration.z * back_scale; 56 | synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale; 57 | synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale; 58 | synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale; 59 | // 四元数插值有线性插值和球面插值，球面插值更准确，但是两个四元数差别不大是，二者精度相当 60 | // 由于是对相邻两时刻姿态插值，姿态差比较小，所以可以用线性插值 61 | synced_data.orientation.x = front_data.orientation.x * front_scale + back_data.orientation.x * back_scale; 62 | synced_data.orientation.y = front_data.orientation.y * front_scale + back_data.orientation.y * back_scale; 63 | synced_data.orientation.z = front_data.orientation.z * front_scale + back_data.orientation.z * back_scale; 64 | synced_data.orientation.w = front_data.orientation.w * front_scale + back_data.orientation.w * back_scale; 65 | // 线性插值之后要归一化 66 | synced_data.orientation.Normlize(); 67 | 68 | SyncedData.push_back(synced_data); 69 | 70 | return true; 71 | } 72 | } -------------------------------------------------------------------------------- /src/sensor_data/key_frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-28 19:17:00 5 | */ 6 | #include "lidar_localization/sensor_data/key_frame.hpp" 7 | 8 | namespace lidar_localization { 9 | 10 | Eigen::Quaternionf KeyFrame::GetQuaternion() const { 11 | Eigen::Quaternionf q; 12 | q = pose.block<3,3>(0,0); 13 | 14 | return q; 15 | } 16 | 17 | Eigen::Vector3f KeyFrame::GetTranslation() const { 18 | Eigen::Vector3f t = pose.block<3,1>(0,3); 19 | 20 | return t; 21 | } 22 | 23 | } -------------------------------------------------------------------------------- /src/sensor_data/loop_pose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-28 19:17:00 5 | */ 6 | #include "lidar_localization/sensor_data/loop_pose.hpp" 7 | 8 | namespace lidar_localization { 9 | Eigen::Quaternionf LoopPose::GetQuaternion() { 10 | Eigen::Quaternionf q; 11 | q = pose.block<3,3>(0,0); 12 | 13 | return q; 14 | } 15 | } -------------------------------------------------------------------------------- /src/sensor_data/pose_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-28 18:50:16 5 | */ 6 | #include "lidar_localization/sensor_data/pose_data.hpp" 7 | 8 | namespace lidar_localization { 9 | 10 | Eigen::Quaternionf PoseData::GetQuaternion() { 11 | Eigen::Quaternionf q; 12 | q = pose.block<3,3>(0,0); 13 | return q; 14 | } 15 | 16 | void PoseData::GetVelocityData(VelocityData &velocity_data) const { 17 | velocity_data.time = time; 18 | 19 | velocity_data.linear_velocity.x = vel.v.x(); 20 | velocity_data.linear_velocity.y = vel.v.y(); 21 | velocity_data.linear_velocity.z = vel.v.z(); 22 | 23 | velocity_data.angular_velocity.x = vel.w.x(); 24 | velocity_data.angular_velocity.y = vel.w.y(); 25 | velocity_data.angular_velocity.z = vel.w.z(); 26 | } 27 | 28 | } -------------------------------------------------------------------------------- /src/subscriber/cloud_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 8 | 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 13 | :nh_(nh) { 14 | subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this); 15 | } 16 | 17 | void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) { 18 | buff_mutex_.lock(); 19 | 20 | // convert ROS PointCloud2 to pcl::PointCloud: 21 | CloudData cloud_data; 22 | cloud_data.time = cloud_msg_ptr->header.stamp.toSec(); 23 | pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr)); 24 | // add new message to buffer: 25 | new_cloud_data_.push_back(cloud_data); 26 | 27 | buff_mutex_.unlock(); 28 | } 29 | 30 | void CloudSubscriber::ParseData(std::deque& cloud_data_buff) { 31 | buff_mutex_.lock(); 32 | 33 | // pipe all available measurements to output buffer: 34 | if (new_cloud_data_.size() > 0) { 35 | cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end()); 36 | new_cloud_data_.clear(); 37 | } 38 | 39 | buff_mutex_.unlock(); 40 | } 41 | } // namespace data_input -------------------------------------------------------------------------------- /src/subscriber/gnss_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-03-31 13:10:51 5 | */ 6 | #include "lidar_localization/subscriber/gnss_subscriber.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 12 | :nh_(nh) { 13 | subscriber_ = nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this); 14 | } 15 | 16 | void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) { 17 | buff_mutex_.lock(); 18 | 19 | // convert ROS NavSatFix to GeographicLib compatible GNSS message: 20 | GNSSData gnss_data; 21 | gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec(); 22 | gnss_data.latitude = nav_sat_fix_ptr->latitude; 23 | gnss_data.longitude = nav_sat_fix_ptr->longitude; 24 | gnss_data.altitude = nav_sat_fix_ptr->altitude; 25 | gnss_data.status = nav_sat_fix_ptr->status.status; 26 | gnss_data.service = nav_sat_fix_ptr->status.service; 27 | // add new message to buffer: 28 | new_gnss_data_.push_back(gnss_data); 29 | 30 | buff_mutex_.unlock(); 31 | } 32 | 33 | void GNSSSubscriber::ParseData(std::deque& gnss_data_buff) { 34 | buff_mutex_.lock(); 35 | 36 | // pipe all available measurements to output buffer: 37 | if (new_gnss_data_.size() > 0) { 38 | gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end()); 39 | new_gnss_data_.clear(); 40 | } 41 | 42 | buff_mutex_.unlock(); 43 | } 44 | } -------------------------------------------------------------------------------- /src/subscriber/imu_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization{ 10 | IMUSubscriber::IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 11 | :nh_(nh) { 12 | subscriber_ = nh_.subscribe(topic_name, buff_size, &IMUSubscriber::msg_callback, this); 13 | } 14 | 15 | void IMUSubscriber::msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr) { 16 | buff_mutex_.lock(); 17 | 18 | // convert ROS IMU to GeographicLib compatible GNSS message: 19 | IMUData imu_data; 20 | imu_data.time = imu_msg_ptr->header.stamp.toSec(); 21 | 22 | imu_data.linear_acceleration.x = imu_msg_ptr->linear_acceleration.x; 23 | imu_data.linear_acceleration.y = imu_msg_ptr->linear_acceleration.y; 24 | imu_data.linear_acceleration.z = imu_msg_ptr->linear_acceleration.z; 25 | 26 | imu_data.angular_velocity.x = imu_msg_ptr->angular_velocity.x; 27 | imu_data.angular_velocity.y = imu_msg_ptr->angular_velocity.y; 28 | imu_data.angular_velocity.z = imu_msg_ptr->angular_velocity.z; 29 | 30 | imu_data.orientation.x = imu_msg_ptr->orientation.x; 31 | imu_data.orientation.y = imu_msg_ptr->orientation.y; 32 | imu_data.orientation.z = imu_msg_ptr->orientation.z; 33 | imu_data.orientation.w = imu_msg_ptr->orientation.w; 34 | 35 | // add new message to buffer: 36 | new_imu_data_.push_back(imu_data); 37 | 38 | buff_mutex_.unlock(); 39 | } 40 | 41 | void IMUSubscriber::ParseData(std::deque& imu_data_buff) { 42 | buff_mutex_.lock(); 43 | 44 | // pipe all available measurements to output buffer: 45 | if (new_imu_data_.size() > 0) { 46 | imu_data_buff.insert(imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end()); 47 | new_imu_data_.clear(); 48 | } 49 | 50 | buff_mutex_.unlock(); 51 | } 52 | } -------------------------------------------------------------------------------- /src/subscriber/key_frame_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/key_frame_subscriber.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization{ 10 | KeyFrameSubscriber::KeyFrameSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 11 | :nh_(nh) { 12 | subscriber_ = nh_.subscribe(topic_name, buff_size, &KeyFrameSubscriber::msg_callback, this); 13 | } 14 | 15 | void KeyFrameSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& key_frame_msg_ptr) { 16 | buff_mutex_.lock(); 17 | KeyFrame key_frame; 18 | key_frame.time = key_frame_msg_ptr->header.stamp.toSec(); 19 | key_frame.index = (unsigned int)key_frame_msg_ptr->pose.covariance[0]; 20 | 21 | key_frame.pose(0,3) = key_frame_msg_ptr->pose.pose.position.x; 22 | key_frame.pose(1,3) = key_frame_msg_ptr->pose.pose.position.y; 23 | key_frame.pose(2,3) = key_frame_msg_ptr->pose.pose.position.z; 24 | 25 | Eigen::Quaternionf q; 26 | q.x() = key_frame_msg_ptr->pose.pose.orientation.x; 27 | q.y() = key_frame_msg_ptr->pose.pose.orientation.y; 28 | q.z() = key_frame_msg_ptr->pose.pose.orientation.z; 29 | q.w() = key_frame_msg_ptr->pose.pose.orientation.w; 30 | key_frame.pose.block<3,3>(0,0) = q.matrix(); 31 | 32 | new_key_frame_.push_back(key_frame); 33 | buff_mutex_.unlock(); 34 | } 35 | 36 | void KeyFrameSubscriber::ParseData(std::deque& key_frame_buff) { 37 | buff_mutex_.lock(); 38 | if (new_key_frame_.size() > 0) { 39 | key_frame_buff.insert(key_frame_buff.end(), new_key_frame_.begin(), new_key_frame_.end()); 40 | new_key_frame_.clear(); 41 | } 42 | buff_mutex_.unlock(); 43 | } 44 | } -------------------------------------------------------------------------------- /src/subscriber/key_frames_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/key_frames_subscriber.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization{ 10 | KeyFramesSubscriber::KeyFramesSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 11 | :nh_(nh) { 12 | subscriber_ = nh_.subscribe(topic_name, buff_size, &KeyFramesSubscriber::msg_callback, this); 13 | } 14 | 15 | void KeyFramesSubscriber::msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr) { 16 | buff_mutex_.lock(); 17 | new_key_frames_.clear(); 18 | 19 | for (size_t i = 0; i < key_frames_msg_ptr->poses.size(); i++) { 20 | KeyFrame key_frame; 21 | key_frame.time = key_frames_msg_ptr->poses.at(i).header.stamp.toSec(); 22 | key_frame.index = (unsigned int)i; 23 | 24 | key_frame.pose(0,3) = key_frames_msg_ptr->poses.at(i).pose.position.x; 25 | key_frame.pose(1,3) = key_frames_msg_ptr->poses.at(i).pose.position.y; 26 | key_frame.pose(2,3) = key_frames_msg_ptr->poses.at(i).pose.position.z; 27 | 28 | Eigen::Quaternionf q; 29 | q.x() = key_frames_msg_ptr->poses.at(i).pose.orientation.x; 30 | q.y() = key_frames_msg_ptr->poses.at(i).pose.orientation.y; 31 | q.z() = key_frames_msg_ptr->poses.at(i).pose.orientation.z; 32 | q.w() = key_frames_msg_ptr->poses.at(i).pose.orientation.w; 33 | key_frame.pose.block<3,3>(0,0) = q.matrix(); 34 | 35 | new_key_frames_.push_back(key_frame); 36 | } 37 | buff_mutex_.unlock(); 38 | } 39 | 40 | void KeyFramesSubscriber::ParseData(std::deque& key_frames_buff) { 41 | buff_mutex_.lock(); 42 | if (new_key_frames_.size() > 0) { 43 | key_frames_buff = new_key_frames_; 44 | new_key_frames_.clear(); 45 | } 46 | buff_mutex_.unlock(); 47 | } 48 | } -------------------------------------------------------------------------------- /src/subscriber/lidar_measurement_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: Subscribe to & parse synced lidar measurement 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #include "lidar_localization/subscriber/lidar_measurement_subscriber.hpp" 8 | 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | 13 | LidarMeasurementSubscriber::LidarMeasurementSubscriber( 14 | ros::NodeHandle& nh, 15 | std::string topic_name, 16 | size_t buff_size 17 | ) 18 | :nh_(nh) { 19 | subscriber_ = nh_.subscribe(topic_name, buff_size, &LidarMeasurementSubscriber::msg_callback, this); 20 | } 21 | 22 | void LidarMeasurementSubscriber::ParseData( 23 | std::deque& cloud_data_buff 24 | ) { 25 | buff_mutex_.lock(); 26 | 27 | // pipe all available measurements to output buffer: 28 | if (new_cloud_data_.size() > 0) { 29 | cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end()); 30 | new_cloud_data_.clear(); 31 | } 32 | 33 | buff_mutex_.unlock(); 34 | } 35 | 36 | void LidarMeasurementSubscriber::ParseCloudData( 37 | const sensor_msgs::PointCloud2 &point_cloud_msg, 38 | CloudData &point_cloud_data 39 | ) { 40 | point_cloud_data.time = point_cloud_msg.header.stamp.toSec(); 41 | pcl::fromROSMsg(point_cloud_msg, *(point_cloud_data.cloud_ptr)); 42 | } 43 | 44 | void LidarMeasurementSubscriber::ParseIMUData( 45 | const sensor_msgs::Imu &imu_msg, 46 | IMUData &imu_data 47 | ) { 48 | imu_data.time = imu_msg.header.stamp.toSec(); 49 | 50 | imu_data.linear_acceleration.x = imu_msg.linear_acceleration.x; 51 | imu_data.linear_acceleration.y = imu_msg.linear_acceleration.y; 52 | imu_data.linear_acceleration.z = imu_msg.linear_acceleration.z; 53 | 54 | imu_data.angular_velocity.x = imu_msg.angular_velocity.x; 55 | imu_data.angular_velocity.y = imu_msg.angular_velocity.y; 56 | imu_data.angular_velocity.z = imu_msg.angular_velocity.z; 57 | 58 | imu_data.orientation.x = imu_msg.orientation.x; 59 | imu_data.orientation.y = imu_msg.orientation.y; 60 | imu_data.orientation.z = imu_msg.orientation.z; 61 | imu_data.orientation.w = imu_msg.orientation.w; 62 | } 63 | 64 | void LidarMeasurementSubscriber::ParsePoseData( 65 | const nav_msgs::Odometry &gnss_odometry_msg, 66 | PoseData &gnss_odometry_data 67 | ) { 68 | gnss_odometry_data.time = gnss_odometry_msg.header.stamp.toSec(); 69 | 70 | // set the position: 71 | gnss_odometry_data.pose(0,3) = gnss_odometry_msg.pose.pose.position.x; 72 | gnss_odometry_data.pose(1,3) = gnss_odometry_msg.pose.pose.position.y; 73 | gnss_odometry_data.pose(2,3) = gnss_odometry_msg.pose.pose.position.z; 74 | 75 | // set the orientation: 76 | Eigen::Quaternionf q; 77 | q.x() = gnss_odometry_msg.pose.pose.orientation.x; 78 | q.y() = gnss_odometry_msg.pose.pose.orientation.y; 79 | q.z() = gnss_odometry_msg.pose.pose.orientation.z; 80 | q.w() = gnss_odometry_msg.pose.pose.orientation.w; 81 | gnss_odometry_data.pose.block<3,3>(0,0) = q.matrix(); 82 | 83 | // set the linear velocity: 84 | gnss_odometry_data.vel.v.x() = gnss_odometry_msg.twist.twist.linear.x; 85 | gnss_odometry_data.vel.v.y() = gnss_odometry_msg.twist.twist.linear.y; 86 | gnss_odometry_data.vel.v.z() = gnss_odometry_msg.twist.twist.linear.z; 87 | } 88 | 89 | void LidarMeasurementSubscriber::msg_callback( 90 | const LidarMeasurement::ConstPtr& synced_cloud_msg_ptr 91 | ) { 92 | buff_mutex_.lock(); 93 | 94 | LidarMeasurementData synced_cloud_data; 95 | 96 | // parse header: 97 | synced_cloud_data.time = synced_cloud_msg_ptr->header.stamp.toSec(); 98 | 99 | // a. parse lidar measurement: 100 | ParseCloudData(synced_cloud_msg_ptr->point_cloud, synced_cloud_data.point_cloud); 101 | 102 | // b. parse IMU measurement: 103 | ParseIMUData(synced_cloud_msg_ptr->imu, synced_cloud_data.imu); 104 | 105 | // c. parse GNSS odometry: 106 | ParsePoseData(synced_cloud_msg_ptr->gnss_odometry, synced_cloud_data.gnss_odometry); 107 | 108 | // add new message to buffer: 109 | new_cloud_data_.push_back(synced_cloud_data); 110 | 111 | buff_mutex_.unlock(); 112 | } 113 | 114 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/subscriber/loop_pose_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 loop pose 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/loop_pose_subscriber.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization{ 10 | LoopPoseSubscriber::LoopPoseSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 11 | :nh_(nh) { 12 | subscriber_ = nh_.subscribe(topic_name, buff_size, &LoopPoseSubscriber::msg_callback, this); 13 | } 14 | 15 | void LoopPoseSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& loop_pose_msg_ptr) { 16 | buff_mutex_.lock(); 17 | LoopPose loop_pose; 18 | loop_pose.time = loop_pose_msg_ptr->header.stamp.toSec(); 19 | loop_pose.index0 = (unsigned int)loop_pose_msg_ptr->pose.covariance[0]; 20 | loop_pose.index1 = (unsigned int)loop_pose_msg_ptr->pose.covariance[1]; 21 | 22 | loop_pose.pose(0,3) = loop_pose_msg_ptr->pose.pose.position.x; 23 | loop_pose.pose(1,3) = loop_pose_msg_ptr->pose.pose.position.y; 24 | loop_pose.pose(2,3) = loop_pose_msg_ptr->pose.pose.position.z; 25 | 26 | Eigen::Quaternionf q; 27 | q.x() = loop_pose_msg_ptr->pose.pose.orientation.x; 28 | q.y() = loop_pose_msg_ptr->pose.pose.orientation.y; 29 | q.z() = loop_pose_msg_ptr->pose.pose.orientation.z; 30 | q.w() = loop_pose_msg_ptr->pose.pose.orientation.w; 31 | loop_pose.pose.block<3,3>(0,0) = q.matrix(); 32 | 33 | new_loop_pose_.push_back(loop_pose); 34 | buff_mutex_.unlock(); 35 | } 36 | 37 | void LoopPoseSubscriber::ParseData(std::deque& loop_pose_buff) { 38 | buff_mutex_.lock(); 39 | if (new_loop_pose_.size() > 0) { 40 | loop_pose_buff.insert(loop_pose_buff.end(), new_loop_pose_.begin(), new_loop_pose_.end()); 41 | new_loop_pose_.clear(); 42 | } 43 | buff_mutex_.unlock(); 44 | } 45 | } -------------------------------------------------------------------------------- /src/subscriber/odometry_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅odometry数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization{ 10 | OdometrySubscriber::OdometrySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 11 | :nh_(nh) { 12 | subscriber_ = nh_.subscribe(topic_name, buff_size, &OdometrySubscriber::msg_callback, this); 13 | } 14 | 15 | void OdometrySubscriber::msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr) { 16 | buff_mutex_.lock(); 17 | PoseData pose_data; 18 | pose_data.time = odom_msg_ptr->header.stamp.toSec(); 19 | 20 | // set the position: 21 | pose_data.pose(0,3) = odom_msg_ptr->pose.pose.position.x; 22 | pose_data.pose(1,3) = odom_msg_ptr->pose.pose.position.y; 23 | pose_data.pose(2,3) = odom_msg_ptr->pose.pose.position.z; 24 | 25 | // set the orientation: 26 | Eigen::Quaternionf q; 27 | q.x() = odom_msg_ptr->pose.pose.orientation.x; 28 | q.y() = odom_msg_ptr->pose.pose.orientation.y; 29 | q.z() = odom_msg_ptr->pose.pose.orientation.z; 30 | q.w() = odom_msg_ptr->pose.pose.orientation.w; 31 | pose_data.pose.block<3,3>(0,0) = q.matrix(); 32 | 33 | // set the linear velocity: 34 | pose_data.vel.v.x() = odom_msg_ptr->twist.twist.linear.x; 35 | pose_data.vel.v.y() = odom_msg_ptr->twist.twist.linear.y; 36 | pose_data.vel.v.z() = odom_msg_ptr->twist.twist.linear.z; 37 | 38 | // set the angular velocity: 39 | pose_data.vel.w.x() = odom_msg_ptr->twist.twist.angular.x; 40 | pose_data.vel.w.y() = odom_msg_ptr->twist.twist.angular.y; 41 | pose_data.vel.w.z() = odom_msg_ptr->twist.twist.angular.z; 42 | 43 | new_pose_data_.push_back(pose_data); 44 | 45 | buff_mutex_.unlock(); 46 | } 47 | 48 | void OdometrySubscriber::ParseData(std::deque& pose_data_buff) { 49 | buff_mutex_.lock(); 50 | if (new_pose_data_.size() > 0) { 51 | pose_data_buff.insert(pose_data_buff.end(), new_pose_data_.begin(), new_pose_data_.end()); 52 | new_pose_data_.clear(); 53 | } 54 | buff_mutex_.unlock(); 55 | } 56 | } -------------------------------------------------------------------------------- /src/subscriber/pos_vel_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: Subscribe to PosVel messages 3 | * @Author: Ge Yao 4 | * @Date: 2020-11-12 15:14:07 5 | */ 6 | 7 | #include "lidar_localization/subscriber/pos_vel_subscriber.hpp" 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization{ 11 | 12 | PosVelSubscriber::PosVelSubscriber( 13 | ros::NodeHandle& nh, 14 | std::string topic_name, 15 | size_t buff_size 16 | ) 17 | :nh_(nh) { 18 | subscriber_ = nh_.subscribe(topic_name, buff_size, &PosVelSubscriber::msg_callback, this); 19 | } 20 | 21 | void PosVelSubscriber::msg_callback(const PosVelConstPtr& pos_vel_msg_ptr) { 22 | buff_mutex_.lock(); 23 | 24 | PosVelData pos_vel_data; 25 | pos_vel_data.time = pos_vel_msg_ptr->header.stamp.toSec(); 26 | 27 | // a. set the position: 28 | pos_vel_data.pos.x() = pos_vel_msg_ptr->position.x; 29 | pos_vel_data.pos.y() = pos_vel_msg_ptr->position.y; 30 | pos_vel_data.pos.z() = pos_vel_msg_ptr->position.z; 31 | 32 | // b. set the body frame velocity: 33 | pos_vel_data.vel.x() = pos_vel_msg_ptr->velocity.x; 34 | pos_vel_data.vel.y() = pos_vel_msg_ptr->velocity.y; 35 | pos_vel_data.vel.z() = pos_vel_msg_ptr->velocity.z; 36 | 37 | new_pos_vel_data_.push_back(pos_vel_data); 38 | 39 | buff_mutex_.unlock(); 40 | } 41 | 42 | void PosVelSubscriber::ParseData(std::deque& pos_vel_data_buff) { 43 | buff_mutex_.lock(); 44 | 45 | if ( new_pos_vel_data_.size() > 0 ) { 46 | pos_vel_data_buff.insert( 47 | pos_vel_data_buff.end(), 48 | new_pos_vel_data_.begin(), new_pos_vel_data_.end() 49 | ); 50 | new_pos_vel_data_.clear(); 51 | } 52 | 53 | buff_mutex_.unlock(); 54 | } 55 | 56 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/subscriber/velocity_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/velocity_subscriber.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization{ 11 | VelocitySubscriber::VelocitySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 12 | :nh_(nh) { 13 | subscriber_ = nh_.subscribe(topic_name, buff_size, &VelocitySubscriber::msg_callback, this); 14 | } 15 | 16 | void VelocitySubscriber::msg_callback(const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr) { 17 | buff_mutex_.lock(); 18 | VelocityData velocity_data; 19 | velocity_data.time = twist_msg_ptr->header.stamp.toSec(); 20 | 21 | velocity_data.linear_velocity.x = twist_msg_ptr->twist.linear.x; 22 | velocity_data.linear_velocity.y = twist_msg_ptr->twist.linear.y; 23 | velocity_data.linear_velocity.z = twist_msg_ptr->twist.linear.z; 24 | 25 | velocity_data.angular_velocity.x = twist_msg_ptr->twist.angular.x; 26 | velocity_data.angular_velocity.y = twist_msg_ptr->twist.angular.y; 27 | velocity_data.angular_velocity.z = twist_msg_ptr->twist.angular.z; 28 | 29 | new_velocity_data_.push_back(velocity_data); 30 | buff_mutex_.unlock(); 31 | } 32 | 33 | void VelocitySubscriber::ParseData(std::deque& velocity_data_buff) { 34 | buff_mutex_.lock(); 35 | if (new_velocity_data_.size() > 0) { 36 | velocity_data_buff.insert(velocity_data_buff.end(), new_velocity_data_.begin(), new_velocity_data_.end()); 37 | new_velocity_data_.clear(); 38 | } 39 | buff_mutex_.unlock(); 40 | } 41 | } -------------------------------------------------------------------------------- /src/tf_listener/tf_lisener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: tf监听模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 16:10:31 5 | */ 6 | #include "lidar_localization/tf_listener/tf_listener.hpp" 7 | 8 | #include 9 | 10 | namespace lidar_localization { 11 | TFListener::TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id) 12 | :nh_(nh), base_frame_id_(base_frame_id), child_frame_id_(child_frame_id) { 13 | } 14 | 15 | bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) { 16 | try { 17 | tf::StampedTransform transform; 18 | listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform); 19 | TransformToMatrix(transform, transform_matrix); 20 | return true; 21 | } catch (tf::TransformException &ex) { 22 | return false; 23 | } 24 | } 25 | 26 | bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) { 27 | Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ()); 28 | 29 | double roll, pitch, yaw; 30 | tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll); 31 | Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX()); 32 | Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY()); 33 | Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ()); 34 | 35 | // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵 36 | transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); 37 | 38 | return true; 39 | } 40 | } -------------------------------------------------------------------------------- /src/tools/file_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 一些文件读写的方法 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-24 20:09:32 5 | */ 6 | #include "lidar_localization/tools/file_manager.hpp" 7 | 8 | #include 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | bool FileManager::CreateFile(std::ofstream& ofs, std::string file_path) { 13 | ofs.close(); 14 | boost::filesystem::remove(file_path.c_str()); 15 | 16 | ofs.open(file_path.c_str(), std::ios::out); 17 | if (!ofs) { 18 | LOG(WARNING) << "无法生成文件: " << std::endl << file_path << std::endl << std::endl; 19 | return false; 20 | } 21 | 22 | return true; 23 | } 24 | 25 | bool FileManager::InitDirectory(std::string directory_path, std::string use_for) { 26 | if (boost::filesystem::is_directory(directory_path)) { 27 | boost::filesystem::remove_all(directory_path); 28 | } 29 | 30 | return CreateDirectory(directory_path, use_for); 31 | } 32 | 33 | bool FileManager::CreateDirectory(std::string directory_path, std::string use_for) { 34 | if (!boost::filesystem::is_directory(directory_path)) { 35 | boost::filesystem::create_directory(directory_path); 36 | } 37 | 38 | if (!boost::filesystem::is_directory(directory_path)) { 39 | LOG(WARNING) << "CANNOT create directory " << std::endl << directory_path << std::endl << std::endl; 40 | return false; 41 | } 42 | 43 | std::cout << use_for << " output path:" << std::endl << directory_path << std::endl << std::endl; 44 | return true; 45 | } 46 | 47 | bool FileManager::CreateDirectory(std::string directory_path) { 48 | if (!boost::filesystem::is_directory(directory_path)) { 49 | boost::filesystem::create_directory(directory_path); 50 | } 51 | 52 | if (!boost::filesystem::is_directory(directory_path)) { 53 | LOG(WARNING) << "CANNOT create directory " << std::endl << directory_path << std::endl << std::endl; 54 | return false; 55 | } 56 | 57 | return true; 58 | } 59 | } -------------------------------------------------------------------------------- /src/tools/print_info.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-03-02 23:28:54 5 | */ 6 | #include "lidar_localization/tools/print_info.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization { 10 | void PrintInfo::PrintPose(std::string head, Eigen::Matrix4f pose) { 11 | Eigen::Affine3f aff_pose; 12 | aff_pose.matrix() = pose; 13 | float x, y, z, roll, pitch, yaw; 14 | pcl::getTranslationAndEulerAngles(aff_pose, x, y, z, roll, pitch, yaw); 15 | std::cout << head 16 | << x << "," << y << "," << z << "," 17 | << roll * 180 / M_PI << "," << pitch * 180 / M_PI << "," << yaw * 180 / M_PI 18 | << std::endl; 19 | } 20 | } -------------------------------------------------------------------------------- /srv/optimizeMap.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed -------------------------------------------------------------------------------- /srv/saveMap.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed -------------------------------------------------------------------------------- /srv/saveOdometry.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed -------------------------------------------------------------------------------- /srv/saveScanContext.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed --------------------------------------------------------------------------------