├── .catkin_workspace ├── .clang-format ├── .gitignore ├── .gitmodules ├── README.md ├── doc └── coordinates.md ├── imgs ├── demo.gif └── mapping_framework.drawio.png ├── results ├── A-LOAM-test │ ├── ape.zip │ ├── ape_map.png │ ├── ape_raw.png │ ├── ground_truth.txt │ ├── laser_odom.txt │ ├── optimized.txt │ ├── readme.md │ ├── rpe.zip │ ├── rpe_map.png │ ├── rpe_raw.png │ ├── traj_rpy_view.png │ ├── traj_trajectories.png │ └── traj_xyz_view.png ├── Lego-LOAM-test │ ├── ape.zip │ ├── ape_map.png │ ├── ape_raw.png │ ├── ground_truth.txt │ ├── laser_odom.txt │ ├── optimized.txt │ ├── readme.md │ ├── rpe.zip │ ├── rpe_map.png │ ├── rpe_raw.png │ ├── traj_rpy_view.png │ ├── traj_trajectories.png │ └── traj_xyz_view.png ├── loop_closing_analysis │ ├── distance_gnss │ │ ├── ape.png │ │ ├── attitude.png │ │ ├── ground_truth.txt │ │ ├── laser_odom.txt │ │ ├── optimized.txt │ │ ├── rpe.png │ │ ├── rviz_traj.png │ │ ├── traj.png │ │ ├── traj_ape.png │ │ └── translation.png │ ├── distance_odom │ │ ├── ground_truth.txt │ │ ├── laser_odom.txt │ │ ├── optimized.txt │ │ ├── optimized_rpe.png │ │ ├── optimized_traj.png │ │ ├── optmized_ape.png │ │ ├── plot.sh │ │ ├── rviz_traj.png │ │ ├── traj.png │ │ └── translation.png │ ├── map_imgs │ │ ├── Screenshot from 2021-12-01 15-01-10.png │ │ ├── map_case_1.png │ │ ├── map_case_1_odom.png │ │ ├── map_case_2.png │ │ ├── map_case_2_odom.png │ │ ├── map_case_3.png │ │ ├── side_view_1_odom.png │ │ ├── side_view_1_sc.png │ │ ├── side_view_2_odom.png │ │ └── side_view_2_sc.png │ ├── scan_context │ │ ├── ape.png │ │ ├── ape_traj.png │ │ ├── attitude.png │ │ ├── ground_truth.txt │ │ ├── laser_odom.txt │ │ ├── optimized.txt │ │ ├── rpe.png │ │ ├── rviz_traj.png │ │ ├── traj.png │ │ └── translation.png │ └── stats.csv └── registration_analysis │ ├── GN-ICP │ ├── ape.png │ ├── ground_truth.txt │ ├── laser_odom.txt │ ├── rpe.png │ └── traj.png │ ├── PCL-ICP │ ├── ape.png │ ├── ground_truth.txt │ ├── laser_odom.txt │ ├── rpe.png │ └── traj.png │ ├── PCL-NDT │ ├── ape.png │ ├── ground_truth.txt │ ├── laser_odom.txt │ ├── rpe.png │ └── traj.png │ ├── SVD-ICP │ ├── ape.png │ ├── ground_truth.txt │ ├── laser_odom.txt │ ├── rpe.png │ └── traj.png │ └── stats.csv └── src ├── a-loam ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── docker │ ├── Dockerfile │ ├── Makefile │ └── run.sh ├── include │ └── aloam_velodyne │ │ ├── common.h │ │ └── tic_toc.h ├── launch │ ├── aloam_velodyne_HDL_32.launch │ ├── aloam_velodyne_HDL_64.launch │ ├── aloam_velodyne_VLP_16.launch │ └── kitti_helper.launch ├── package.xml ├── picture │ ├── kitti.png │ └── kitti_gif.gif ├── rviz_cfg │ └── aloam_velodyne.rviz └── src │ ├── kittiHelper.cpp │ ├── laserMapping.cpp │ ├── laserOdometry.cpp │ ├── lidarFactor.hpp │ └── scanRegistration.cpp ├── kitti-lego-loam ├── LICENSE ├── LeGO-LOAM │ ├── LICENSE │ ├── LeGO-LOAM │ │ ├── CMakeLists.txt │ │ ├── CMakeLists.txt.user │ │ ├── include │ │ │ └── utility.h │ │ ├── launch │ │ │ ├── block.png │ │ │ ├── dataset-demo.gif │ │ │ ├── demo.gif │ │ │ ├── google-earth.png │ │ │ ├── jackal-label.jpg │ │ │ ├── odometry.jpg │ │ │ ├── run.launch │ │ │ ├── seg-total.jpg │ │ │ └── test.rviz │ │ ├── package.xml │ │ └── src │ │ │ ├── featureAssociation.cpp │ │ │ ├── imageProjection.cpp │ │ │ ├── mapOptmization.cpp │ │ │ └── transformFusion.cpp │ ├── README.md │ ├── Shan_Englot_IROS_2018_Preprint.pdf │ └── cloud_msgs │ │ ├── CMakeLists.txt │ │ ├── msg │ │ └── cloud_info.msg │ │ └── package.xml ├── README.md └── kittibag │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include │ └── aloam_velodyne │ │ ├── common.h │ │ └── tic_toc.h │ ├── launch │ └── kittibag.launch │ ├── package.xml │ ├── pic │ ├── 1.png │ ├── 2.png │ └── 3.png │ └── src │ └── kittibag.cpp └── mapping_localization ├── CMakeLists.txt ├── cmake ├── OpenCV.cmake ├── PCL.cmake ├── YAML.cmake ├── ceres.cmake ├── eigen.cmake ├── g2o.cmake ├── geographic.cmake ├── global_defination.cmake ├── glog.cmake ├── gtsam.cmake └── sophus.cmake ├── config ├── mapping │ ├── back_end.yaml │ ├── data_pretreat.yaml │ ├── front_end.yaml │ ├── global.yaml │ ├── loop_closing.yaml │ └── viewer.yaml └── matching │ └── matching.yaml ├── include └── mapping_localization │ ├── data_pretreat │ ├── data_pretreat_flow.hpp │ └── external_front_end_adapter.hpp │ ├── global_defination │ └── global_defination.h.in │ ├── mapping │ ├── back_end │ │ ├── back_end.hpp │ │ └── back_end_flow.hpp │ ├── front_end │ │ ├── front_end.hpp │ │ └── front_end_flow.hpp │ ├── global_param │ │ └── global_param.hpp │ ├── loop_closing │ │ ├── loop_closing.hpp │ │ └── loop_closing_flow.hpp │ └── viewer │ │ ├── viewer.hpp │ │ └── viewer_flow.hpp │ ├── matching │ ├── matching.hpp │ └── matching_flow.hpp │ ├── models │ ├── cloud_filter │ │ ├── box_filter.hpp │ │ ├── cloud_filter_interface.hpp │ │ ├── no_filter.hpp │ │ └── voxel_filter.hpp │ ├── graph_optimizer │ │ ├── ceres │ │ │ ├── ceres_graph_optimizer.hpp │ │ │ ├── edges │ │ │ │ ├── prior_position_edge.hpp │ │ │ │ └── relative_pose_edge.hpp │ │ │ └── nodes │ │ │ │ └── pose_se3.hpp │ │ ├── g2o │ │ │ ├── edge │ │ │ │ ├── edge_se3_priorquat.hpp │ │ │ │ └── edge_se3_priorxyz.hpp │ │ │ └── g2o_graph_optimizer.hpp │ │ ├── gtsam │ │ │ └── gtsam_graph_optimizer.hpp │ │ └── interface_graph_optimizer.hpp │ ├── loop_closure │ │ ├── ScanContext │ │ │ ├── KDTreeVectorOfVectorsAdaptor.h │ │ │ ├── Scancontext.h │ │ │ ├── nanoflann.hpp │ │ │ └── tictoc.h │ │ ├── distance_detector.hpp │ │ ├── loop_closure_detector_interface.hpp │ │ └── scan_context_detector.hpp │ ├── registration │ │ ├── icp_registration.hpp │ │ ├── ndt_registration.hpp │ │ ├── pcl_icp_registration.hpp │ │ └── registration_interface.hpp │ └── scan_adjust │ │ └── distortion_adjust.hpp │ ├── publisher │ ├── cloud_publisher.hpp │ ├── imu_publisher.hpp │ ├── key_frame_publisher.hpp │ ├── key_frames_publisher.hpp │ ├── loop_pose_publisher.hpp │ ├── odometry_publisher.hpp │ ├── tf_broadcaster.hpp │ └── trajectory_publisher.hpp │ ├── sensor_data │ ├── cloud_data.hpp │ ├── gnss_data.hpp │ ├── imu_data.hpp │ ├── key_frame.hpp │ ├── loop_pose.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 │ ├── loop_pose_subscriber.hpp │ ├── odometry_subscriber.hpp │ ├── trajectory_subscriber.hpp │ └── velocity_subscriber.hpp │ ├── tf_listener │ └── tf_listener.hpp │ └── tools │ ├── file_manager.hpp │ ├── print_info.hpp │ ├── tic_toc.hpp │ └── utils.hpp ├── launch ├── mapping.launch ├── mapping_with_aloam.launch ├── mapping_with_lego_loam.launch ├── matching.launch └── test_frame.launch ├── package.xml ├── rviz ├── mapping.rviz ├── matching.rviz └── test_frame.rviz ├── src ├── apps │ ├── back_end_node.cpp │ ├── data_pretreat_node.cpp │ ├── front_end_node.cpp │ ├── loop_closing_node.cpp │ ├── matching_node.cpp │ ├── test_frame_node.cpp │ └── viewer_node.cpp ├── data_pretreat │ ├── data_pretreat_flow.cpp │ └── external_front_end_adapter.cpp ├── mapping │ ├── back_end │ │ ├── back_end.cpp │ │ └── back_end_flow.cpp │ ├── front_end │ │ ├── front_end.cpp │ │ └── front_end_flow.cpp │ ├── global_param │ │ └── global_param.cpp │ ├── loop_closing │ │ ├── loop_closing.cpp │ │ └── loop_closing_flow.cpp │ └── viewer │ │ ├── viewer.cpp │ │ └── viewer_flow.cpp ├── matching │ ├── matching.cpp │ └── matching_flow.cpp ├── models │ ├── cloud_filter │ │ ├── box_filter.cpp │ │ ├── no_filter.cpp │ │ └── voxel_filter.cpp │ ├── graph_optimizer │ │ ├── ceres │ │ │ └── ceres_graph_optimizer.cpp │ │ ├── g2o │ │ │ └── g2o_graph_optimizer.cpp │ │ ├── gtsam │ │ │ └── gtsam_graph_optimizer.cpp │ │ └── interface_graph_optimizer.cpp │ ├── loop_closure │ │ ├── ScanContext │ │ │ └── Scancontext.cpp │ │ ├── distance_detector.cpp │ │ └── scan_context_detector.cpp │ ├── registration │ │ ├── icp_registration.cpp │ │ ├── ndt_registration.cpp │ │ └── pcl_icp_registration.cpp │ └── scan_adjust │ │ └── distortion_adjust.cpp ├── publisher │ ├── cloud_publisher.cpp │ ├── key_frame_publisher.cpp │ ├── key_frames_publisher.cpp │ ├── loop_pose_publisher.cpp │ ├── odometry_publisher.cpp │ ├── tf_broadcaster.cpp │ └── trajectory_publisher.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 │ ├── loop_pose_subscriber.cpp │ ├── odometry_subscriber.cpp │ ├── trajectory_subscriber.cpp │ └── velocity_subscriber.cpp ├── tf_listener │ └── tf_lisener.cpp └── tools │ ├── file_manager.cpp │ ├── print_info.cpp │ └── utils.cpp └── srv ├── optimizeMap.srv └── saveMap.srv /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | 3 | # 语言: None Cpp Java Objc Protp 4 | Language: Cpp 5 | 6 | #缩进宽度 7 | IndentWidth: 4 8 | 9 | #tab键盘的宽度 10 | TabWidth: 4 11 | UseTab: Never 12 | 13 | # indent for modifier (public, private, protected) 14 | AccessModifierOffset: -4 15 | 16 | ColumnLimit: 120 17 | 18 | BinPackArguments: false 19 | BinPackParameters: false 20 | 21 | ExperimentalAutoDetectBinPacking: false 22 | AllowAllParametersOfDeclarationOnNextLine: false 23 | 24 | # 保留 Include 块之间的关系 25 | IncludeBlocks: Preserve 26 | 27 | AlignConsecutiveDeclarations: false -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # lidar_localization/slam_data 2 | 3 | src/CMakeLists.txt 4 | 5 | **.vscode 6 | 7 | **build** 8 | **devel** 9 | 10 | **slam_data** 11 | 12 | *.INFO 13 | *.INFO.* 14 | *.log.* 15 | *.WARNING 16 | 17 | # Prerequisites 18 | *.d 19 | 20 | # Compiled Object files 21 | *.slo 22 | *.lo 23 | *.o 24 | *.obj 25 | 26 | # Precompiled Headers 27 | *.gch 28 | *.pch 29 | 30 | # Compiled Dynamic libraries 31 | *.so 32 | *.dylib 33 | *.dll 34 | 35 | # Fortran module files 36 | *.mod 37 | *.smod 38 | 39 | # Compiled Static libraries 40 | *.lai 41 | *.la 42 | *.a 43 | *.lib 44 | 45 | # Executables 46 | *.exe 47 | *.out 48 | *.app -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party_libs/g2o"] 2 | path = third_party_libs/g2o 3 | url = https://github.com/RainerKuemmerle/g2o.git 4 | [submodule "lidar_localization_ws/geographiclib-code"] 5 | path = third_party_libs/geographiclib-code 6 | url = https://github.com/geographiclib/geographiclib 7 | [submodule "third_party_libs/ceres-solver"] 8 | path = third_party_libs/ceres-solver 9 | url = https://github.com/ceres-solver/ceres-solver.git 10 | [submodule "third_party_libs/Sophus"] 11 | path = third_party_libs/Sophus 12 | url = https://github.com/strasdat/Sophus.git 13 | [submodule "third_party_libs/external/a-loam-w-comment"] 14 | path = third_party_libs/external/a-loam-w-comment 15 | url = https://github.com/XiaotaoGuo/a-loam-w-comment 16 | [submodule "third_party_libs/external/kitti-lego-loam"] 17 | path = third_party_libs/external/kitti-lego-loam 18 | url = https://github.com/Mitchell-Lee-93/kitti-lego-loam.git 19 | [submodule "third_party_libs/gtsam"] 20 | path = third_party_libs/gtsam 21 | url = https://github.com/borglab/gtsam.git 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Modular Mapping and Localization Framework 2 | 3 | ## 介绍 4 | 5 | 这个项目基本上是基于任乾大佬的建图定位框架框架进行修改，主要是为了对自动驾驶建图和定位过程中使用的各种算法进行实现和比较。仅为**个人学习使用**。关于原 Repo 参见： [localization_in_auto_driving](https://github.com/Little-Potato-1990/localization_in_auto_driving) ，也推荐大佬的 [从零开始做自动驾驶定位](https://zhuanlan.zhihu.com/c_1114864226103037952) 系列文章。 6 | 7 | ![demo](./imgs/demo.gif) 8 | 9 | 节点间整体的工作流程见（只包含建图部分）： 10 | 11 | ![工作流程](./imgs/mapping_framework.drawio.png) 12 | 13 | ## 测试环境 14 | 15 | - Ubuntu 18.04 (ROS Melodic) / 20.04 (ROS Noetic) 16 | - PCL 1.8/1.10 17 | - Eigen 3.3.5/3.3.7 18 | - [Ceres](https://github.com/ceres-solver/ceres-solver/tree/276d24c73a8c80e77ce822ed4ab6e6286fd2870b)、[G2O](https://github.com/RainerKuemmerle/g2o/tree/f3b1cbb0048197d73cf363cb1c26897493e1aa2b)、[Geographiclib](https://github.com/geographiclib/geographiclib/tree/920702bc36ea13c384686556f25fb6369141a8e1)、[Sophus](https://github.com/strasdat/Sophus/tree/49a7e1286910019f74fb4f0bb3e213c909f8e1b7) 使用版本见 third_party_libs 中的 submodules 19 | 20 | ## 测试数据 21 | 22 | - [x] KITTI with synced IMU (10 Hz) 23 | - [ ] KITTI with raw IMU (100 Hz) 24 | - [ ] KAIST 25 | 26 | ## 已测试模块 27 | 28 | - 前端： 29 | - ICP, NDT 点云配准：[ICP, NDT 实现和性能对比](https://xiaotaoguo.com/p/pointcloud-registration/) 30 | - [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) 31 | - [Lego-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM) （感谢 [@Mitchell-Lee-93](https://github.com/Mitchell-Lee-93) 对 lego-loam 在 KITTI 数据集上做的适配） 32 | - 图优化后端： 33 | - G2O：[G2O 学习记录](https://xiaotaoguo.com/p/g2o-usage-2/) 34 | - Ceres：[Ceres 学习记录](https://xiaotaoguo.com/p/ceres-usage-2/) 35 | - GTSam：[GTSAM 学习记录](https://xiaotaoguo.com/p/gtsam-usage-2/) 36 | - 回环检测： 37 | - 基于距离检测 38 | - 基于 Scan Context 检测：[基于 SC 和距离进行回环检测对比](https://xiaotaoguo.com/p/lidar_loop_closure/) 39 | 40 | -------------------------------------------------------------------------------- /doc/coordinates.md: -------------------------------------------------------------------------------- 1 | ## 坐标系说明 2 | 3 | ### 前端 4 | 5 | - 激光雷达里程计全局坐标系：原点在激光雷达初始位置，姿态和激光雷达初始姿态一致 6 | - 激光雷达里程计载体坐标系：由里程计输出的位姿估计 7 | 8 | ### 后端 9 | 10 | - 全局坐标系：根据 GNSS 信息确定方向，以激光雷达初始位置为原点的东北天坐标系 11 | - 载体里程计坐标系：全局坐标系下的里程计估计的载体位姿 12 | - 优化后的载体位姿：全局坐标系下经过图优化后的载体的位姿全局最优估计 13 | 14 | ### 可视化 15 | 16 | - 校正后的里程计坐标系：从全局优化后的最新一帧和里程计的对应帧可以获得一个相对位姿，通过该位姿可以对后续的里程计位姿估计进行校正（即在优化后的轨迹进行里程计估计） 17 | 18 | ## 轨迹说明 19 | 20 | - 基于增量信息的前端里程计轨迹 -- 可以保证帧间运动平滑性 21 | - 后端位姿优化轨迹 -- 精度更高，不实时 22 | - 后端位姿优化 + 里程计轨迹 -- 在优化后的轨迹基础上，利用里程计的帧间位姿估计进行预测 23 | 24 | -------------------------------------------------------------------------------- /imgs/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/imgs/demo.gif -------------------------------------------------------------------------------- /imgs/mapping_framework.drawio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/imgs/mapping_framework.drawio.png -------------------------------------------------------------------------------- /results/A-LOAM-test/ape.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/ape.zip -------------------------------------------------------------------------------- /results/A-LOAM-test/ape_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/ape_map.png -------------------------------------------------------------------------------- /results/A-LOAM-test/ape_raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/ape_raw.png -------------------------------------------------------------------------------- /results/A-LOAM-test/readme.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/readme.md -------------------------------------------------------------------------------- /results/A-LOAM-test/rpe.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/rpe.zip -------------------------------------------------------------------------------- /results/A-LOAM-test/rpe_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/rpe_map.png -------------------------------------------------------------------------------- /results/A-LOAM-test/rpe_raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/rpe_raw.png -------------------------------------------------------------------------------- /results/A-LOAM-test/traj_rpy_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/traj_rpy_view.png -------------------------------------------------------------------------------- /results/A-LOAM-test/traj_trajectories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/traj_trajectories.png -------------------------------------------------------------------------------- /results/A-LOAM-test/traj_xyz_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/A-LOAM-test/traj_xyz_view.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/ape.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/ape.zip -------------------------------------------------------------------------------- /results/Lego-LOAM-test/ape_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/ape_map.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/ape_raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/ape_raw.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/readme.md: -------------------------------------------------------------------------------- 1 | TODO：Lego-LOAM 的效果略低于 LOAM，原因大概是出在点云投影的环节，作者建议对非线性分布的激光雷达线束最好自己实现投影函数，见：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/issues/12 -------------------------------------------------------------------------------- /results/Lego-LOAM-test/rpe.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/rpe.zip -------------------------------------------------------------------------------- /results/Lego-LOAM-test/rpe_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/rpe_map.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/rpe_raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/rpe_raw.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/traj_rpy_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/traj_rpy_view.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/traj_trajectories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/traj_trajectories.png -------------------------------------------------------------------------------- /results/Lego-LOAM-test/traj_xyz_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/Lego-LOAM-test/traj_xyz_view.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/ape.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/attitude.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/attitude.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/rpe.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/rviz_traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/rviz_traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/traj_ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/traj_ape.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_gnss/translation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_gnss/translation.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/optimized_rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_odom/optimized_rpe.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/optimized_traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_odom/optimized_traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/optmized_ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_odom/optmized_ape.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/plot.sh: -------------------------------------------------------------------------------- 1 | evo_traj kitti laser_odom.txt optimized.txt --ref=ground_truth.txt -p --plot_mode=xy 2 | -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/rviz_traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_odom/rviz_traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_odom/traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/distance_odom/translation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/distance_odom/translation.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/Screenshot from 2021-12-01 15-01-10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/Screenshot from 2021-12-01 15-01-10.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/map_case_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/map_case_1.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/map_case_1_odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/map_case_1_odom.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/map_case_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/map_case_2.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/map_case_2_odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/map_case_2_odom.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/map_case_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/map_case_3.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/side_view_1_odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/side_view_1_odom.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/side_view_1_sc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/side_view_1_sc.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/side_view_2_odom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/side_view_2_odom.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/map_imgs/side_view_2_sc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/map_imgs/side_view_2_sc.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/ape.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/ape_traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/ape_traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/attitude.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/attitude.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/rpe.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/rviz_traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/rviz_traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/traj.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/scan_context/translation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/loop_closing_analysis/scan_context/translation.png -------------------------------------------------------------------------------- /results/loop_closing_analysis/stats.csv: -------------------------------------------------------------------------------- 1 | APE,,,,,,,, 2 | Detection Method,rmse, mean, median, std, min, max, sse, average spend time 3 | Distance_GNSS,5.57020486847518,4.72243917714598,4.12164295551166,2.9539381332285,1.26E-06,12.4920579401563,59106.7822372747,3.22E-06 4 | Distance_Odom,11.0530064883216,8.37993351549253,6.01738834717504,7.20733423026884,1.18E-06,28.9276353991179,232487.516475964,2.11E-06 5 | Scan_Context,4.82988658987946,4.13946570946313,3.71232586195304,2.48849916842991,1.18E-06,10.165431850935,44392.8119084985,0.00133124 6 | ,,,,,,,, 7 | RPE,,,,,,,, 8 | Detection Method,rmse, mean, median, std, min, max, sse, 9 | Distance_GNSS,2.79196240806388,2.53117395598876,2.59032509842558,1.17822429637403,0.966020488077356,6.26272439999063,148.106027672795, 10 | Distance_Odom,2.80425439752408,2.49183666943649,2.16593822716594,1.28630973676048,0.758324863737987,6.53755533451183,149.41301179463, 11 | Scan_Context,2.58753122077578,2.34462804570024,2.44778005792745,1.09454883207889,0.711228053773947,4.81482306752295,127.211038551298, 12 | -------------------------------------------------------------------------------- /results/registration_analysis/GN-ICP/ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/GN-ICP/ape.png -------------------------------------------------------------------------------- /results/registration_analysis/GN-ICP/rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/GN-ICP/rpe.png -------------------------------------------------------------------------------- /results/registration_analysis/GN-ICP/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/GN-ICP/traj.png -------------------------------------------------------------------------------- /results/registration_analysis/PCL-ICP/ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/PCL-ICP/ape.png -------------------------------------------------------------------------------- /results/registration_analysis/PCL-ICP/rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/PCL-ICP/rpe.png -------------------------------------------------------------------------------- /results/registration_analysis/PCL-ICP/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/PCL-ICP/traj.png -------------------------------------------------------------------------------- /results/registration_analysis/PCL-NDT/ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/PCL-NDT/ape.png -------------------------------------------------------------------------------- /results/registration_analysis/PCL-NDT/rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/PCL-NDT/rpe.png -------------------------------------------------------------------------------- /results/registration_analysis/PCL-NDT/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/PCL-NDT/traj.png -------------------------------------------------------------------------------- /results/registration_analysis/SVD-ICP/ape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/SVD-ICP/ape.png -------------------------------------------------------------------------------- /results/registration_analysis/SVD-ICP/rpe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/SVD-ICP/rpe.png -------------------------------------------------------------------------------- /results/registration_analysis/SVD-ICP/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/results/registration_analysis/SVD-ICP/traj.png -------------------------------------------------------------------------------- /results/registration_analysis/stats.csv: -------------------------------------------------------------------------------- 1 | APE,,,,,,,, 2 | Matching Method,rmse, mean, median, std, min, max, sse, average spend time 3 | PCL-NDT,33.1149908392101,29.1933017131291,26.9705071832022,15.6318186199552,1.2567754200473E-06,61.0875322602522,2096704.20615321,0.0142579 4 | GN-ICP,40.8868078596634,31.143699311389,22.9584884705045,26.491527893929,1.03159278495493E-06,90.761132991323,3172945.54609686,0.018154 5 | SVD-ICP,41.419024898049,32.0810375231093,24.04956736836,26.1981422040964,1.03159278495493E-06,89.8960294165723,3269810.89840092,0.0179568 6 | PCL-ICP,457.581470671158,326.260556543286,217.291901389053,320.834617124844,1.2567754200473E-06,1214.25189067013,346943989.413718,0.0305145 7 | ,,,,,,,, 8 | RPE,,,,,,,, 9 | Matching Method,rmse, mean, median, std, min, max, sse, 10 | PCL-NDT,3.35283352862622,2.98007476767413,2.78306417637105,1.53643973189713,0.93975859393972,6.4292773419946,213.588360742923, 11 | GN-ICP,4.95562400808577,4.2776711188505,4.01843775406075,2.50194706348244,0.664711246481311,10.0522839050861,442.047767571289, 12 | SVD-ICP,3.50525435618626,3.17573193924801,3.19893721763695,1.48375697188012,0.764856568236319,6.7628703659909,233.449353929693, 13 | PCL-ICP,287.470382730634,203.659461722175,146.123999660569,202.884313337259,2.69162658793244,768.302734308643,1322227.53515676, 14 | -------------------------------------------------------------------------------- /src/a-loam/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ -------------------------------------------------------------------------------- /src/a-loam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aloam_velodyne) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | nav_msgs 11 | sensor_msgs 12 | roscpp 13 | rospy 14 | rosbag 15 | std_msgs 16 | image_transport 17 | cv_bridge 18 | tf 19 | ) 20 | 21 | #find_package(Eigen3 REQUIRED) 22 | find_package(PCL REQUIRED) 23 | find_package(OpenCV REQUIRED) 24 | find_package(Ceres REQUIRED) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ${PCL_INCLUDE_DIRS} 30 | ${CERES_INCLUDE_DIRS} 31 | ${OpenCV_INCLUDE_DIRS}) 32 | 33 | catkin_package( 34 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 35 | DEPENDS EIGEN3 PCL 36 | INCLUDE_DIRS include 37 | ) 38 | 39 | 40 | add_executable(ascanRegistration src/scanRegistration.cpp) 41 | target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 42 | 43 | add_executable(alaserOdometry src/laserOdometry.cpp) 44 | target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 45 | 46 | add_executable(alaserMapping src/laserMapping.cpp) 47 | target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 48 | 49 | add_executable(kittiHelper src/kittiHelper.cpp) 50 | target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /src/a-loam/LICENSE: -------------------------------------------------------------------------------- 1 | This is an advanced implementation of the algorithm described in the following paper: 2 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | Modifier: Tong Qin qintonguav@gmail.com 6 | Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | Copyright 2013, Ji Zhang, Carnegie Mellon University 9 | Further contributions copyright (c) 2016, Southwest Research Institute 10 | All rights reserved. 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this 16 | list of conditions and the following disclaimer. 17 | 2. Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 3. Neither the name of the copyright holder nor the names of its contributors 21 | may be used to endorse or promote products derived from this software without 22 | specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 25 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 26 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR 32 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 33 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/a-loam/docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-perception 2 | 3 | ENV CERES_VERSION="1.12.0" 4 | ENV PCL_VERSION="1.8.0" 5 | ENV CATKIN_WS=/root/catkin_ws 6 | 7 | # setup processors number used to compile library 8 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \ 9 | # Install dependencies 10 | apt-get update && apt-get install -y \ 11 | cmake \ 12 | libatlas-base-dev \ 13 | libeigen3-dev \ 14 | libgoogle-glog-dev \ 15 | libsuitesparse-dev \ 16 | python-catkin-tools \ 17 | ros-${ROS_DISTRO}-cv-bridge \ 18 | ros-${ROS_DISTRO}-image-transport \ 19 | ros-${ROS_DISTRO}-message-filters \ 20 | ros-${ROS_DISTRO}-tf && \ 21 | rm -rf /var/lib/apt/lists/* && \ 22 | # Build and install Ceres 23 | git clone https://ceres-solver.googlesource.com/ceres-solver && \ 24 | cd ceres-solver && \ 25 | git checkout tags/${CERES_VERSION} && \ 26 | mkdir build && cd build && \ 27 | cmake .. && \ 28 | make -j${USE_PROC} install && \ 29 | cd ../.. && \ 30 | rm -rf ./ceres-solver && \ 31 | # Build and install pcl 32 | git clone https://github.com/PointCloudLibrary/pcl.git && \ 33 | cd pcl && \ 34 | git checkout tags/pcl-${PCL_VERSION} && \ 35 | mkdir build && cd build && \ 36 | cmake .. && \ 37 | make -j${USE_PROC} install && \ 38 | cd ../.. && \ 39 | rm -rf ./pcl && \ 40 | # Setup catkin workspace 41 | mkdir -p $CATKIN_WS/src/A-LOAM/ 42 | 43 | # WORKDIR $CATKIN_WS/src 44 | 45 | # Copy A-LOAM 46 | COPY ./ $CATKIN_WS/src/A-LOAM/ 47 | # use the following line if you only have this dockerfile 48 | # RUN git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git 49 | 50 | # Build A-LOAM 51 | WORKDIR $CATKIN_WS 52 | ENV TERM xterm 53 | ENV PYTHONIOENCODING UTF-8 54 | RUN catkin config \ 55 | --extend /opt/ros/$ROS_DISTRO \ 56 | --cmake-args \ 57 | -DCMAKE_BUILD_TYPE=Release && \ 58 | catkin build && \ 59 | sed -i '/exec "$@"/i \ 60 | source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh 61 | -------------------------------------------------------------------------------- /src/a-loam/docker/Makefile: -------------------------------------------------------------------------------- 1 | all: help 2 | 3 | help: 4 | @echo "" 5 | @echo "-- Help Menu" 6 | @echo "" 7 | @echo " 1. make build - build all images" 8 | # @echo " 1. make pull - pull all images" 9 | @echo " 1. make clean - remove all images" 10 | @echo "" 11 | 12 | build: 13 | @docker build --tag ros:aloam -f ./Dockerfile .. 14 | 15 | clean: 16 | @docker rmi -f ros:aloam 17 | -------------------------------------------------------------------------------- /src/a-loam/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | trap : SIGTERM SIGINT 3 | 4 | function abspath() { 5 | # generate absolute path from relative path 6 | # $1 : relative filename 7 | # return : absolute path 8 | if [ -d "$1" ]; then 9 | # dir 10 | (cd "$1"; pwd) 11 | elif [ -f "$1" ]; then 12 | # file 13 | if [[ $1 = /* ]]; then 14 | echo "$1" 15 | elif [[ $1 == */* ]]; then 16 | echo "$(cd "${1%/*}"; pwd)/${1##*/}" 17 | else 18 | echo "$(pwd)/$1" 19 | fi 20 | fi 21 | } 22 | 23 | if [ "$#" -ne 1 ]; then 24 | echo "Usage: $0 LIDAR_SCAN_NUMBER" >&2 25 | exit 1 26 | fi 27 | 28 | roscore & 29 | ROSCORE_PID=$! 30 | sleep 1 31 | 32 | rviz -d ../rviz_cfg/aloam_velodyne.rviz & 33 | RVIZ_PID=$! 34 | 35 | A_LOAM_DIR=$(abspath "..") 36 | 37 | if [ "$1" -eq 16 ]; then 38 | docker run \ 39 | -it \ 40 | --rm \ 41 | --net=host \ 42 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ 43 | ros:aloam \ 44 | /bin/bash -c \ 45 | "cd /root/catkin_ws/; \ 46 | catkin config \ 47 | --cmake-args \ 48 | -DCMAKE_BUILD_TYPE=Release; \ 49 | catkin build; \ 50 | source devel/setup.bash; \ 51 | roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch rviz:=false" 52 | elif [ "$1" -eq "32" ]; then 53 | docker run \ 54 | -it \ 55 | --rm \ 56 | --net=host \ 57 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ 58 | ros:aloam \ 59 | /bin/bash -c \ 60 | "cd /root/catkin_ws/; \ 61 | catkin config \ 62 | --cmake-args \ 63 | -DCMAKE_BUILD_TYPE=Release; \ 64 | catkin build; \ 65 | source devel/setup.bash; \ 66 | roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch rviz:=false" 67 | elif [ "$1" -eq "64" ]; then 68 | docker run \ 69 | -it \ 70 | --rm \ 71 | --net=host \ 72 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \ 73 | ros:aloam \ 74 | /bin/bash -c \ 75 | "cd /root/catkin_ws/; \ 76 | catkin config \ 77 | --cmake-args \ 78 | -DCMAKE_BUILD_TYPE=Release; \ 79 | catkin build; \ 80 | source devel/setup.bash; \ 81 | roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch rviz:=false" 82 | fi 83 | 84 | wait $ROSCORE_PID 85 | wait $RVIZ_PID 86 | 87 | if [[ $? -gt 128 ]] 88 | then 89 | kill $ROSCORE_PID 90 | kill $RVIZ_PID 91 | fi -------------------------------------------------------------------------------- /src/a-loam/include/aloam_velodyne/common.h: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #pragma once 38 | 39 | #include 40 | 41 | #include 42 | 43 | typedef pcl::PointXYZI PointType; 44 | 45 | inline double rad2deg(double radians) 46 | { 47 | return radians * 180.0 / M_PI; 48 | } 49 | 50 | inline double deg2rad(double degrees) 51 | { 52 | return degrees * M_PI / 180.0; 53 | } 54 | -------------------------------------------------------------------------------- /src/a-loam/include/aloam_velodyne/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | -------------------------------------------------------------------------------- /src/a-loam/launch/aloam_velodyne_HDL_32.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/a-loam/launch/aloam_velodyne_HDL_64.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/a-loam/launch/aloam_velodyne_VLP_16.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/a-loam/launch/kitti_helper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/a-loam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aloam_velodyne 4 | 0.0.0 5 | 6 | 7 | This is an advanced implentation of LOAM. 8 | LOAM is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | qintong 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | rosbag 26 | sensor_msgs 27 | tf 28 | image_transport 29 | 30 | geometry_msgs 31 | nav_msgs 32 | sensor_msgs 33 | roscpp 34 | rospy 35 | std_msgs 36 | rosbag 37 | tf 38 | image_transport 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/a-loam/picture/kitti.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/a-loam/picture/kitti.png -------------------------------------------------------------------------------- /src/a-loam/picture/kitti_gif.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/a-loam/picture/kitti_gif.gif -------------------------------------------------------------------------------- /src/kitti-lego-loam/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Robust Field Autonomy Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Robust Field Autonomy Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lego_loam) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | tf 8 | roscpp 9 | rospy 10 | cv_bridge 11 | image_transport 12 | 13 | pcl_ros 14 | pcl_conversions 15 | 16 | std_msgs 17 | sensor_msgs 18 | geometry_msgs 19 | nav_msgs 20 | cloud_msgs 21 | ) 22 | 23 | find_package(GTSAM REQUIRED QUIET) 24 | find_package(PCL REQUIRED QUIET) 25 | find_package(OpenCV REQUIRED QUIET) 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | CATKIN_DEPENDS cloud_msgs 30 | DEPENDS PCL 31 | ) 32 | 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ${PCL_INCLUDE_DIRS} 37 | ${OpenCV_INCLUDE_DIRS} 38 | ${GTSAM_INCLUDE_DIR} 39 | ) 40 | 41 | link_directories( 42 | include 43 | ${OpenCV_LIBRARY_DIRS} 44 | ${PCL_LIBRARY_DIRS} 45 | ${GTSAM_LIBRARY_DIRS} 46 | ) 47 | 48 | add_executable(imageProjection src/imageProjection.cpp) 49 | add_dependencies(imageProjection ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp) 50 | target_link_libraries(imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 51 | 52 | add_executable(featureAssociation src/featureAssociation.cpp) 53 | add_dependencies(featureAssociation ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp) 54 | target_link_libraries(featureAssociation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 55 | 56 | add_executable(mapOptmization src/mapOptmization.cpp) 57 | target_link_libraries(mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) 58 | 59 | add_executable(transformFusion src/transformFusion.cpp) 60 | target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 61 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/block.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/block.png -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/dataset-demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/dataset-demo.gif -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/demo.gif -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/google-earth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/google-earth.png -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/jackal-label.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/jackal-label.jpg -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/odometry.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/odometry.jpg -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/seg-total.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/launch/seg-total.jpg -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/LeGO-LOAM/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lego_loam 4 | 1.0.0 5 | LeGO-LOAM from IROS 2018 6 | 7 | Tixiao Shan 8 | 9 | BSD 10 | 11 | Tixiao Shan 12 | 13 | catkin 14 | 15 | roscpp 16 | roscpp 17 | rospy 18 | rospy 19 | ros_bridge 20 | ros_bridge 21 | 22 | tf 23 | tf 24 | 25 | pcl_ros 26 | pcl_ros 27 | pcl_conversions 28 | pcl_conversions 29 | 30 | cv_bridge 31 | cv_bridge 32 | 33 | std_msgs 34 | std_msgs 35 | cloud_msgs 36 | cloud_msgs 37 | sensors_msgs 38 | sensors_msgs 39 | geometry_msgs 40 | geometry_msgs 41 | nav_msgs 42 | nav_msgs 43 | 44 | image_transport 45 | image_transport 46 | 47 | gtsam 48 | gtsam 49 | 50 | 51 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/README.md: -------------------------------------------------------------------------------- 1 | Easy tutorial to run Lego-LOAM with KITTI-data 2 | 3 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/Shan_Englot_IROS_2018_Preprint.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/LeGO-LOAM/Shan_Englot_IROS_2018_Preprint.pdf -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/cloud_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cloud_msgs) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | geometry_msgs 8 | std_msgs 9 | nav_msgs 10 | ) 11 | 12 | add_message_files( 13 | DIRECTORY msg 14 | FILES 15 | cloud_info.msg 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES 20 | geometry_msgs 21 | std_msgs 22 | nav_msgs 23 | ) 24 | 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS 28 | message_runtime 29 | message_generation 30 | geometry_msgs 31 | std_msgs 32 | nav_msgs 33 | ) 34 | 35 | include_directories( 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/cloud_msgs/msg/cloud_info.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int32[] startRingIndex 4 | int32[] endRingIndex 5 | 6 | float32 startOrientation 7 | float32 endOrientation 8 | float32 orientationDiff 9 | 10 | bool[] segmentedCloudGroundFlag # true - ground point, false - other points 11 | uint32[] segmentedCloudColInd # point column index in range image 12 | float32[] segmentedCloudRange # point range -------------------------------------------------------------------------------- /src/kitti-lego-loam/LeGO-LOAM/cloud_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cloud_msgs 4 | 1.0.0 5 | The cloud_msgs package contains messages. 6 | 7 | Tixiao Shan 8 | 9 | BSD 10 | 11 | catkin 12 | geometry_msgs 13 | std_msgs 14 | message_runtime 15 | message_generation 16 | nav_msgs 17 | 18 | nav_msgs 19 | message_generation 20 | message_runtime 21 | geometry_msgs 22 | std_msgs 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kittibag) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | nav_msgs 11 | sensor_msgs 12 | roscpp 13 | rospy 14 | rosbag 15 | std_msgs 16 | image_transport 17 | cv_bridge 18 | tf 19 | ) 20 | 21 | #find_package(Eigen3 REQUIRED) 22 | find_package(PCL REQUIRED) 23 | find_package(OpenCV REQUIRED) 24 | include_directories( 25 | include 26 | ${catkin_INCLUDE_DIRS} 27 | ${PCL_INCLUDE_DIRS} 28 | ${OpenCV_INCLUDE_DIRS}) 29 | 30 | catkin_package( 31 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 32 | DEPENDS EIGEN3 PCL 33 | INCLUDE_DIRS include 34 | ) 35 | 36 | add_executable(kittibag src/kittibag.cpp) 37 | target_link_libraries(kittibag ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/LICENSE: -------------------------------------------------------------------------------- 1 | This is an advanced implementation of the algorithm described in the following paper: 2 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | Modifier: Tong Qin qintonguav@gmail.com 6 | Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | Copyright 2013, Ji Zhang, Carnegie Mellon University 9 | Further contributions copyright (c) 2016, Southwest Research Institute 10 | All rights reserved. 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions are met: 14 | 15 | 1. Redistributions of source code must retain the above copyright notice, this 16 | list of conditions and the following disclaimer. 17 | 2. Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 3. Neither the name of the copyright holder nor the names of its contributors 21 | may be used to endorse or promote products derived from this software without 22 | specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 25 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 26 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR 32 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 33 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/include/aloam_velodyne/common.h: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #pragma once 38 | 39 | #include 40 | 41 | #include 42 | 43 | typedef pcl::PointXYZI PointType; 44 | 45 | inline double rad2deg(double radians) 46 | { 47 | return radians * 180.0 / M_PI; 48 | } 49 | 50 | inline double deg2rad(double degrees) 51 | { 52 | return degrees * M_PI / 180.0; 53 | } 54 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/include/aloam_velodyne/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/launch/kittibag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kittibag 4 | 0.0.0 5 | 6 | 7 | This is an advanced implentation of LOAM. 8 | LOAM is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | qintong 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | rosbag 26 | sensor_msgs 27 | tf 28 | image_transport 29 | 30 | geometry_msgs 31 | nav_msgs 32 | sensor_msgs 33 | roscpp 34 | rospy 35 | std_msgs 36 | rosbag 37 | tf 38 | image_transport 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/pic/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/kittibag/pic/1.png -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/pic/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/kittibag/pic/2.png -------------------------------------------------------------------------------- /src/kitti-lego-loam/kittibag/pic/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XiaotaoGuo/modular_mapping_and_localization_framework/bbfbdc08830aa3994a398844c383e3d9cc12fca9/src/kitti-lego-loam/kittibag/pic/3.png -------------------------------------------------------------------------------- /src/mapping_localization/cmake/OpenCV.cmake: -------------------------------------------------------------------------------- 1 | find_package (OpenCV REQUIRED) 2 | include_directories(${OpenCV_INCLUDE_DIRS}) 3 | list(APPEND ALL_TARGET_LIBRARIES ${OpenCV_LIBS}) -------------------------------------------------------------------------------- /src/mapping_localization/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}) -------------------------------------------------------------------------------- /src/mapping_localization/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}) -------------------------------------------------------------------------------- /src/mapping_localization/cmake/ceres.cmake: -------------------------------------------------------------------------------- 1 | find_package(Ceres REQUIRED) 2 | include_directories(${CERES_INCLUDE_DIRS}) 3 | list(APPEND ALL_TARGET_LIBRARIES ${CERES_LIBRARIES}) 4 | 5 | -------------------------------------------------------------------------------- /src/mapping_localization/cmake/geographic.cmake: -------------------------------------------------------------------------------- 1 | find_package (GeographicLib REQUIRED) 2 | include_directories(${GeographicLib_INCLUDE_DIRS}) 3 | list(APPEND ALL_TARGET_LIBRARIES ${GeographicLib_LIBRARIES}) -------------------------------------------------------------------------------- /src/mapping_localization/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) -------------------------------------------------------------------------------- /src/mapping_localization/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 | -------------------------------------------------------------------------------- /src/mapping_localization/cmake/gtsam.cmake: -------------------------------------------------------------------------------- 1 | find_package(GTSAM REQUIRED QUIET) 2 | include_directories(${GTSAM_INCLUDE_DIR}) 3 | list(APPEND ALL_TARGET_LIBRARIES gtsam) 4 | 5 | # gtsam has dependency on Boost::Timer 6 | find_package (Boost 1.55.0 REQUIRED COMPONENTS system timer) 7 | list(APPEND ALL_TARGET_LIBRARIES ${Boost_LIBRARIES}) 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/mapping_localization/cmake/sophus.cmake: -------------------------------------------------------------------------------- 1 | find_package (Sophus REQUIRED) 2 | include_directories(${Sophus_INCLUDE_DIRS}) 3 | list(APPEND ALL_TARGET_LIBRARIES Sophus::Sophus) 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/mapping_localization/config/mapping/back_end.yaml: -------------------------------------------------------------------------------- 1 | # 关键帧 2 | key_frame_distance: 2.0 # 关键帧距离 3 | 4 | # 优化 5 | graph_optimizer_type: g2o # 图优化库，目前支持 [g2o, ceres, gtsam] 6 | 7 | use_gnss: true 8 | use_loop_close: true 9 | 10 | optimize_step_with_key_frame: 10000 # 没有其他信息时，每隔 step 发送一次优化的位姿 11 | optimize_step_with_gnss: 950 # 每累计 step 个 gnss 观测时，优化一次 12 | optimize_step_with_loop: 100 # 每累计 step 个闭环约束时优化一次 13 | 14 | noise_model: 15 | odom: 16 | translation: [0.5, 0.5, 0.5] # meter 17 | rotation: [0.001, 0.001, 0.001] # rad 18 | close_loop: 19 | translation: [0.3, 0.3, 0.3] 20 | rotation: [0.001, 0.001, 0.001] 21 | gnss: 22 | translation: [2.0, 2.0, 2.0] 23 | 24 | g2o: 25 | solver_type: lm_var_cholmod 26 | verbose: true 27 | 28 | ceres: 29 | optimization_strategy: L-M # 优化方法，支持 L-M 和 Dog-Leg 30 | linear_solver_type: SPARSE_NORMAL_CHOLESKY # 线性求解器类型，ceres 默认使用普通的 QR，如果有第三方库，可以使用 SPARSE_NORMAL_CHOLESKY 31 | verbose: true 32 | 33 | gtsam: 34 | verbose: SILENT 35 | optimization_strategy: LevenbergMarquardt 36 | linear_solver_type: MULTIFRONTAL_CHOLESKY -------------------------------------------------------------------------------- /src/mapping_localization/config/mapping/data_pretreat.yaml: -------------------------------------------------------------------------------- 1 | # 是否进行点云去畸变 2 | undistort_pointcloud: false 3 | 4 | # 是否接收外部前端里程计 5 | use_external_front_end: false 6 | 7 | # 原始消息 topic 8 | pointcloud_topic: /kitti/velo/pointcloud 9 | imu_topic: /kitti/oxts/imu 10 | velocity_topic: /kitti/oxts/gps/vel 11 | gnss_topic: /kitti/oxts/gps/fix 12 | 13 | # 预处理设置 14 | lidar_buffer_size: 5 # 保持一定数量点云数据在缓存里，等待其他传感器数据 15 | 16 | # 外部前端 topic 17 | # LeGO-LOAM 18 | external_laser_odom_topic: /aft_mapped_to_init 19 | pose_to_pointcloud: [0, 0, 0, -1.570795, -1.570795, 0] # lidar 到里程计基准坐标系（camera_init, lidar_init, imu_init, etc）的转换 20 | 21 | # A-LOAM 22 | # external_laser_odom_topic: /aft_mapped_to_init 23 | # pose_to_pointcloud: [0, 0, 0, 0, 0, 0] # lidar 到里程计基准坐标系（camera_init, lidar_init, imu_init, etc）的转换 -------------------------------------------------------------------------------- /src/mapping_localization/config/mapping/front_end.yaml: -------------------------------------------------------------------------------- 1 | # 匹配 2 | registration_method: ICP # 选择点云匹配方法，目前支持：NDT, ICP 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 | NDT: 18 | res : 1.0 19 | step_size : 0.1 20 | trans_eps : 0.01 21 | max_iter : 30 22 | 23 | ICP: 24 | matching_method: SVD # PCL, SVD, GN 25 | max_correspodence_dist: 1.0 26 | trans_eps: 1e-6 27 | fitness_eps: 1e-6 28 | max_iter: 30 29 | 30 | ## 滤波相关参数 31 | voxel_filter: 32 | local_map: 33 | leaf_size: [0.6, 0.6, 0.6] 34 | frame: 35 | leaf_size: [1.3, 1.3, 1.3] -------------------------------------------------------------------------------- /src/mapping_localization/config/mapping/global.yaml: -------------------------------------------------------------------------------- 1 | data_path: /home/guo/personal/general_mapping_and_localization_framework/results/temp # 数据存放路径 2 | 3 | # 世界坐标系 (world frame) 4 | global_frame_id: world # 参考东北天坐标系 （原点和初始时刻激光雷达中心重合） 5 | lidar_init_frame_id: world_lidar # 以激光雷达初始位姿为原点的全局坐标系（和参考东北天坐标系相差一个旋转角度） 6 | 7 | # 传感器坐标系（用于获取外参） 8 | imu_frame_id: imu_link 9 | lidar_frame_id: velo_link 10 | camera_frame_id: camera_gray_left 11 | 12 | # 载体坐标系 (body frame) 13 | vehicle_ref_frame_id: vehicle_referenced # 参考载体坐标系---------载体在 reference_global_frame 下的的参考位姿 14 | lidar_odom_frame_id: lidar_odom # Lidar 坐标系----------载体在 lidar_global_frame 下的估计位姿 （前端里程计的输出结果） 15 | vehicle_odom_frame_id: vehicle_odom # 估计载体坐标系---------载体在 reference_global_frame 下的估计位姿 (和东北天对齐) 16 | vehicle_optimized_frame_id: vehicle_optimized # 优化后的估计载体坐标系--载体在 reference_global_frame 下经过优化后的位姿 17 | vehicle_corrected_frame_id: vehicle_corrected 18 | 19 | # 同步后的消息 topic 20 | synced_pointcloud_topic: /synced_cloud 21 | synced_gnss_topic: /synced_gnss 22 | 23 | # 里程计消息： 24 | lidar_odometry_topic: /lidar_odom 25 | vehicle_odometry_topic: /vehicle_odom 26 | vehicle_optimized_odometry_topic: /optimized_odom 27 | 28 | # 轨迹消息 29 | gnss_trajectory_topic: /gnss_trajectory 30 | lidar_odometry_trajectory_topic: /laser_odom_tracjectory 31 | 32 | vehicle_odometry_trajectory_topic: /vehicle_odom_trajectory 33 | vehicle_optimized_trajectory_topic: /vehicle_optimized_trajectory 34 | vehicle_corrected_trajectory_topic: /vehicle_corrected_trajectory 35 | 36 | # 地图消息 37 | global_map_topic: /global_map 38 | local_map_topic: /local_map 39 | current_scan_topic: /current_scan 40 | 41 | # 其他 42 | loop_pose_topic: /loop_pose # 回环消息 43 | key_frame_topic: /key_frame_odom # 关键帧对应的里程计估计位姿 44 | key_frame_gnss_topic: /key_frame_gnss # 关键帧对应的 GNSS 观测位姿 45 | key_frame_optimized_topic: /key_frame_optimized # 关键帧对应优化后的位姿 -------------------------------------------------------------------------------- /src/mapping_localization/config/mapping/loop_closing.yaml: -------------------------------------------------------------------------------- 1 | registration_method: ICP # 选择点云匹配方法，目前支持：NDT 2 | 3 | search_criteria: scan_context # 选择回环检测方法，目前支持 distance_gnss, distance_odom, scan_context 4 | 5 | # 匹配时为了精度更高，应该选用scan-to-map的方式 6 | # map是以历史帧为中心，往前后时刻各选取extend_frame_num个关键帧，放在一起拼接成的 7 | extend_frame_num: 5 8 | fitness_score_limit: 0.2 # 匹配误差小于这个值才认为是有效的 9 | 10 | # 之所以要提供no_filter（即不滤波）模式，是因为闭环检测对计算时间要求没那么高，而点云越稠密，精度就越高，所以滤波与否都有道理 11 | map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 12 | scan_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter 13 | 14 | # 各配置选项对应参数 15 | 16 | ## 回环检测相关参数 17 | distance_gnss: 18 | loop_step: 5 # 防止检测过于频繁，每隔loop_step个关键帧检测一次闭环 19 | detect_area: 10.0 # 检测区域，只有两帧距离小于这个值，才做闭环匹配 20 | diff_num: 100 # 过于小的闭环没有意义，所以只有两帧之间的关键帧个数超出这个值再做检测 21 | 22 | distance_odom: 23 | loop_step: 5 24 | detect_area: 30.0 25 | diff_num: 100 26 | 27 | scan_context: 28 | num_exclude_recent : 100 # 不考虑最近的若干帧 29 | search_ratio: 0.1 # 经过初筛后的搜索范围，0.1 表示在该角度附近搜索 360 * 0.1 = 36 30 | sc_dist_thres: 0.18 31 | 32 | ## 匹配相关参数 33 | NDT: 34 | res : 1.0 35 | step_size : 0.1 36 | trans_eps : 0.01 37 | max_iter : 30 38 | 39 | ICP: 40 | matching_method: SVD # PCL, SVD, GN 41 | max_correspodence_dist: 1.0 42 | trans_eps: 1e-6 43 | fitness_eps: 1e-6 44 | max_iter: 30 45 | 46 | ## 滤波相关参数 47 | voxel_filter: 48 | map: 49 | leaf_size: [0.3, 0.3, 0.3] 50 | scan: 51 | leaf_size: [0.3, 0.3, 0.3] -------------------------------------------------------------------------------- /src/mapping_localization/config/mapping/viewer.yaml: -------------------------------------------------------------------------------- 1 | data_path: /home/guo/personal/general_mapping_and_localization_framework/results/temp # 数据存放路径 2 | 3 | debug_info: false 4 | 5 | # 全局地图 6 | global_map_filter: voxel_filter # 选择全局地图点云滤波方法，目前支持：voxel_filter 7 | 8 | # 局部地图 9 | local_frame_num: 20 10 | local_map_filter: voxel_filter # 选择滑窗小地图点云滤波方法，目前支持：voxel_filter 11 | 12 | # 当前帧 13 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter 14 | 15 | # 各配置选项对应参数 16 | ## 滤波相关参数 17 | voxel_filter: 18 | global_map: 19 | leaf_size: [0.5, 0.5, 0.5] 20 | local_map: 21 | leaf_size: [0.5, 0.5, 0.5] 22 | frame: 23 | leaf_size: [0.5, 0.5, 0.5] -------------------------------------------------------------------------------- /src/mapping_localization/config/matching/matching.yaml: -------------------------------------------------------------------------------- 1 | map_path: /home/rq/matching_map/filtered_map.pcd # 地图存放路径 2 | 3 | # 匹配 4 | registration_method: NDT # 选择点云匹配方法，目前支持：NDT 5 | 6 | # 当前帧 7 | # no_filter指不对点云滤波，在匹配中，理论上点云越稠密，精度越高，但是速度也越慢 8 | # 所以提供这种不滤波的模式做为对比，以方便使用者去体会精度和效率随稠密度的变化关系 9 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter、no_filter 10 | 11 | # 局部地图 12 | # 局部地图从全局地图切割得到，此处box_filter_size是切割区间 13 | # 参数顺序是min_x, max_x, min_y, max_y, min_z, max_z 14 | box_filter_size: [-150.0, 150.0, -150.0, 150.0, -150.0, 150.0] 15 | local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 16 | 17 | # 全局地图 18 | global_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter、no_filter 19 | 20 | 21 | # 各配置选项对应参数 22 | ## 匹配相关参数 23 | NDT: 24 | res : 1.0 25 | step_size : 0.1 26 | trans_eps : 0.01 27 | max_iter : 30 28 | ## 滤波相关参数 29 | voxel_filter: 30 | global_map: 31 | leaf_size: [0.9, 0.9, 0.9] 32 | local_map: 33 | leaf_size: [0.5, 0.5, 0.5] 34 | frame: 35 | leaf_size: [1.5, 1.5, 1.5] -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/data_pretreat/external_front_end_adapter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 第三方前端接口 3 | * @Created Date: 2021-12-02 00:04:47 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-03 17:36:43 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_DATA_PRETREAT_EXTERNAL_FRONT_END_ADAPTER_HPP_ 11 | #define MAPPING_LOCALIZATION_DATA_PRETREAT_EXTERNAL_FRONT_END_ADAPTER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "mapping_localization/publisher/odometry_publisher.hpp" 19 | #include "mapping_localization/sensor_data/pose_data.hpp" 20 | #include "mapping_localization/subscriber/odometry_subscriber.hpp" 21 | 22 | namespace mapping_localization { 23 | 24 | ///@brief 用于接收外部里程计的位姿，并和给定点云消息进行同步输出 25 | class ExternalFrontEndAdapter { 26 | private: 27 | public: 28 | ExternalFrontEndAdapter(std::shared_ptr& external_laser_odom_sub_ptr, 29 | std::shared_ptr& synced_external_lasesr_odom_pub_ptr, 30 | const std::vector& pose_to_pointcloud); 31 | ~ExternalFrontEndAdapter(); 32 | 33 | bool HasData() const; 34 | 35 | /// 36 | ///@brief 和指定点云消息时间进行同步，注意：本函数假定外部里程计的位姿估计和点云时间戳一致，因此不会进行插值 37 | ///@param time 点云时间 38 | ///@return true 当前里程计消息缓存包含该时间内的位姿估计 39 | /// 40 | bool SyncData(double time); 41 | 42 | /// 43 | ///@brief 发布最旧一帧里程计 44 | /// 45 | void PublishData(); 46 | 47 | private: 48 | Eigen::Matrix4f pose_to_pointcloud_ = Eigen::Matrix4f::Identity(); 49 | std::deque external_odom_buff_; 50 | std::shared_ptr external_laser_odom_sub_ptr_; 51 | std::shared_ptr synced_external_lasesr_odom_pub_ptr_; 52 | }; 53 | 54 | } // namespace mapping_localization 55 | 56 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_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 MAPPING_LOCALIZATION_GLOBAL_DEFINATION_H_IN_ 7 | #define MAPPING_LOCALIZATION_GLOBAL_DEFINATION_H_IN_ 8 | 9 | #include 10 | 11 | namespace mapping_localization { 12 | const std::string WORK_SPACE_PATH = "@WORK_SPACE_PATH@"; 13 | } 14 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_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 MAPPING_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_HPP_ 7 | #define MAPPING_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include "mapping_localization/models/cloud_filter/cloud_filter_interface.hpp" 15 | #include "mapping_localization/models/registration/registration_interface.hpp" 16 | #include "mapping_localization/sensor_data/cloud_data.hpp" 17 | #include "mapping_localization/tools/tic_toc.hpp" 18 | 19 | namespace mapping_localization { 20 | class FrontEnd { 21 | public: 22 | struct Frame { 23 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 24 | CloudData cloud_data; 25 | }; 26 | 27 | public: 28 | FrontEnd(const YAML::Node& global_node, const YAML::Node& config_node); 29 | 30 | bool Update(const CloudData& cloud_data, Eigen::Matrix4f& cloud_pose); 31 | bool SetInitPose(const Eigen::Matrix4f& init_pose); 32 | 33 | private: 34 | bool InitWithConfig(); 35 | bool InitParam(const YAML::Node& config_node); 36 | bool InitRegistration(std::shared_ptr& registration_ptr, const YAML::Node& config_node); 37 | bool InitFilter(std::string filter_user, 38 | std::shared_ptr& filter_ptr, 39 | const YAML::Node& config_node); 40 | bool UpdateWithNewFrame(const Frame& new_key_frame); 41 | 42 | private: 43 | std::string data_path_ = ""; 44 | 45 | std::shared_ptr frame_filter_ptr_; 46 | std::shared_ptr local_map_filter_ptr_; 47 | std::shared_ptr registration_ptr_; 48 | 49 | std::deque local_map_frames_; 50 | 51 | CloudData::Cloud_Ptr local_map_ptr_; 52 | Frame current_frame_; 53 | 54 | Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity(); 55 | 56 | float key_frame_distance_ = 2.0; 57 | int local_frame_num_ = 20; 58 | 59 | Timer matching_timer_ = Timer("Scan Matching"); 60 | }; 61 | } // namespace mapping_localization 62 | 63 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_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 MAPPING_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_FLOW_HPP_ 7 | #define MAPPING_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_FLOW_HPP_ 8 | 9 | #include 10 | 11 | #include "mapping_localization/mapping/front_end/front_end.hpp" 12 | #include "mapping_localization/publisher/odometry_publisher.hpp" 13 | #include "mapping_localization/publisher/tf_broadcaster.hpp" 14 | #include "mapping_localization/subscriber/cloud_subscriber.hpp" 15 | 16 | namespace mapping_localization { 17 | class FrontEndFlow { 18 | public: 19 | FrontEndFlow(ros::NodeHandle& nh); 20 | 21 | bool Run(); 22 | 23 | private: 24 | bool ReadData(); 25 | bool HasData(); 26 | bool ValidData(); 27 | bool UpdateLaserOdometry(); 28 | bool PublishData(); 29 | 30 | private: 31 | std::shared_ptr cloud_sub_ptr_; 32 | std::shared_ptr laser_odom_pub_ptr_; 33 | std::shared_ptr laser_odom_tf_pub_ptr_; 34 | std::shared_ptr front_end_ptr_; 35 | 36 | std::deque cloud_data_buff_; 37 | 38 | CloudData current_cloud_data_; 39 | 40 | Eigen::Matrix4f laser_odometry_ = Eigen::Matrix4f::Identity(); 41 | }; 42 | } // namespace mapping_localization 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/mapping/loop_closing/loop_closing_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2020-02-29 03:32:14 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_FLOW_HPP_ 11 | #define MAPPING_LOCALIZATION_MAPPING_LOOP_CLOSING_LOOP_CLOSING_FLOW_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | // subscriber 17 | #include "mapping_localization/subscriber/key_frame_subscriber.hpp" 18 | // publisher 19 | #include "mapping_localization/publisher/loop_pose_publisher.hpp" 20 | // loop closing 21 | #include "mapping_localization/mapping/loop_closing/loop_closing.hpp" 22 | 23 | namespace mapping_localization { 24 | class LoopClosingFlow { 25 | public: 26 | LoopClosingFlow(ros::NodeHandle& nh); 27 | 28 | bool Run(); 29 | 30 | private: 31 | bool ReadData(); 32 | bool HasData(); 33 | bool ValidData(); 34 | bool PublishData(); 35 | 36 | private: 37 | // subscriber 38 | std::shared_ptr key_frame_sub_ptr_; 39 | std::shared_ptr key_gnss_sub_ptr_; 40 | // publisher 41 | std::shared_ptr loop_pose_pub_ptr_; 42 | // loop closing 43 | std::shared_ptr loop_closing_ptr_; 44 | 45 | std::deque key_frame_buff_; 46 | std::deque key_gnss_buff_; 47 | 48 | KeyFrame current_key_frame_; 49 | KeyFrame current_key_gnss_; 50 | }; 51 | } // namespace mapping_localization 52 | 53 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/matching/matching_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: matching 模块任务管理， 放在类里使代码更清晰 3 | * @Created Date: 2020-02-10 08:31:22 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MATCHING_MATCHING_FLOW_HPP_ 11 | #define MAPPING_LOCALIZATION_MATCHING_MATCHING_FLOW_HPP_ 12 | 13 | #include 14 | // subscriber 15 | #include "mapping_localization/subscriber/cloud_subscriber.hpp" 16 | #include "mapping_localization/subscriber/odometry_subscriber.hpp" 17 | // publisher 18 | #include "mapping_localization/publisher/cloud_publisher.hpp" 19 | #include "mapping_localization/publisher/odometry_publisher.hpp" 20 | #include "mapping_localization/publisher/tf_broadcaster.hpp" 21 | // matching 22 | #include "mapping_localization/matching/matching.hpp" 23 | 24 | namespace mapping_localization { 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 | // subscriber 40 | std::shared_ptr cloud_sub_ptr_; 41 | std::shared_ptr gnss_sub_ptr_; 42 | // publisher 43 | std::shared_ptr global_map_pub_ptr_; 44 | std::shared_ptr local_map_pub_ptr_; 45 | std::shared_ptr current_scan_pub_ptr_; 46 | std::shared_ptr laser_odom_pub_ptr_; 47 | std::shared_ptr laser_tf_pub_ptr_; 48 | // matching 49 | std::shared_ptr matching_ptr_; 50 | 51 | std::deque cloud_data_buff_; 52 | std::deque gnss_data_buff_; 53 | 54 | CloudData current_cloud_data_; 55 | PoseData current_gnss_data_; 56 | 57 | Eigen::Matrix4f laser_odometry_ = Eigen::Matrix4f::Identity(); 58 | }; 59 | } // namespace mapping_localization 60 | 61 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/cloud_filter/box_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 从点云中截取一个立方体部分 3 | * @Created Date: 2020-03-04 20:09:37 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_BOX_FILTER_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_BOX_FILTER_HPP_ 12 | 13 | #include 14 | 15 | #include "mapping_localization/models/cloud_filter/cloud_filter_interface.hpp" 16 | 17 | namespace mapping_localization { 18 | class BoxFilter : public CloudFilterInterface { 19 | public: 20 | BoxFilter(YAML::Node node); 21 | BoxFilter() = default; 22 | 23 | bool Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, CloudData::Cloud_Ptr& filtered_cloud_ptr) override; 24 | 25 | void SetSize(std::vector size); 26 | void SetOrigin(std::vector origin); 27 | std::vector GetEdge(); 28 | 29 | private: 30 | void CalculateEdge(); 31 | 32 | private: 33 | pcl::CropBox pcl_box_filter_; 34 | 35 | std::vector origin_; 36 | std::vector size_; 37 | std::vector edge_; 38 | }; 39 | } // namespace mapping_localization 40 | 41 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/cloud_filter/cloud_filter_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云滤波模块的接口 3 | * @Created Date: 2020-02-09 19:29:50 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 12 | 13 | #include 14 | 15 | #include "mapping_localization/sensor_data/cloud_data.hpp" 16 | 17 | namespace mapping_localization { 18 | class CloudFilterInterface { 19 | public: 20 | virtual ~CloudFilterInterface() = default; 21 | 22 | virtual bool Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, CloudData::Cloud_Ptr& filtered_cloud_ptr) = 0; 23 | }; 24 | } // namespace mapping_localization 25 | 26 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/cloud_filter/no_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 不滤波 3 | * @Created Date: 2020-02-09 19:37:49 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_NO_FILTER_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_NO_FILTER_HPP_ 12 | 13 | #include "mapping_localization/models/cloud_filter/cloud_filter_interface.hpp" 14 | 15 | namespace mapping_localization { 16 | class NoFilter : public CloudFilterInterface { 17 | public: 18 | NoFilter(); 19 | 20 | bool Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, CloudData::Cloud_Ptr& filtered_cloud_ptr) override; 21 | }; 22 | } // namespace mapping_localization 23 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/cloud_filter/voxel_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel filter 模块 3 | * @Created Date: 2020-02-09 19:37:49 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 12 | 13 | #include 14 | 15 | #include "mapping_localization/models/cloud_filter/cloud_filter_interface.hpp" 16 | 17 | namespace mapping_localization { 18 | class VoxelFilter : public CloudFilterInterface { 19 | public: 20 | VoxelFilter(const YAML::Node& node); 21 | VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z); 22 | 23 | bool Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, CloudData::Cloud_Ptr& filtered_cloud_ptr) override; 24 | 25 | private: 26 | bool SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z); 27 | 28 | private: 29 | pcl::VoxelGrid voxel_filter_; 30 | }; 31 | } // namespace mapping_localization 32 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/graph_optimizer/ceres/edges/prior_position_edge.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-12-11 19:22:35 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-12 20:11:17 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_CERES_EDGES_PRIOR_POSITION_EDGE_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_CERES_EDGES_PRIOR_POSITION_EDGE_HPP_ 12 | 13 | #include "mapping_localization/models/graph_optimizer/ceres/nodes/pose_se3.hpp" 14 | 15 | namespace mapping_localization { 16 | class PriorPositionEdge { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | PriorPositionEdge(const Eigen::Vector3d& measurement, const Eigen::Matrix3d& sqrt_info) 21 | : measurement_(measurement), sqrt_info_(sqrt_info) {} 22 | 23 | template 24 | bool operator()(const T* const v1, T* residual) const { 25 | Eigen::Map> estimate(v1); 26 | 27 | Eigen::Map> residual_eigen(residual); 28 | residual_eigen = estimate - measurement_; 29 | residual_eigen.applyOnTheLeft(sqrt_info_.template cast()); 30 | 31 | return true; 32 | } 33 | 34 | static ceres::CostFunction* create(const Eigen::Vector3d& measurement, const Eigen::Matrix3d& sqrt_info) { 35 | return ( 36 | new ceres::AutoDiffCostFunction(new PriorPositionEdge(measurement, sqrt_info))); 37 | } 38 | 39 | private: 40 | Eigen::Vector3d measurement_; 41 | Eigen::Matrix3d sqrt_info_; 42 | }; 43 | } // namespace mapping_localization 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/graph_optimizer/ceres/nodes/pose_se3.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-12-11 17:03:05 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-12 17:45:43 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_CERES_NODES_POSE_SE3_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_CERES_NODES_POSE_SE3_HPP_ 12 | 13 | #include 14 | 15 | namespace mapping_localization { 16 | 17 | struct PoseSE3 { 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | Eigen::Vector3d pos = Eigen::Vector3d::Zero(); 21 | Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); 22 | }; 23 | 24 | } // namespace mapping_localization 25 | 26 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/graph_optimizer/g2o/edge/edge_se3_priorquat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 姿态的先验边 3 | * @Created Date: 2020-03-01 18:05:35 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORQUAT_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORQUAT_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | namespace g2o { 17 | class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | EdgeSE3PriorQuat() : g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() {} 21 | 22 | void computeError() override { 23 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 24 | 25 | Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear()); 26 | if (estimate.w() < 0) { 27 | estimate.coeffs() = -estimate.coeffs(); 28 | } 29 | _error = estimate.vec() - _measurement.vec(); 30 | } 31 | 32 | void setMeasurement(const Eigen::Quaterniond& m) override { 33 | _measurement = m; 34 | if (m.w() < 0.0) { 35 | _measurement.coeffs() = -m.coeffs(); 36 | } 37 | } 38 | 39 | virtual bool read(std::istream& is) override { 40 | Eigen::Quaterniond q; 41 | is >> q.w() >> q.x() >> q.y() >> q.z(); 42 | setMeasurement(q); 43 | for (int i = 0; i < information().rows(); ++i) 44 | for (int j = i; j < information().cols(); ++j) { 45 | is >> information()(i, j); 46 | if (i != j) information()(j, i) = information()(i, j); 47 | } 48 | return true; 49 | } 50 | 51 | virtual bool write(std::ostream& os) const override { 52 | Eigen::Quaterniond q = _measurement; 53 | os << q.w() << " " << q.x() << " " << q.y() << " " << q.z(); 54 | for (int i = 0; i < information().rows(); ++i) 55 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 56 | return os.good(); 57 | } 58 | }; 59 | } // namespace g2o 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/graph_optimizer/g2o/edge/edge_se3_priorxyz.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3D pose 中位置的先验边 3 | * @Created Date: 2020-03-01 18:05:35 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-19 20:39:31 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORXYZ_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORXYZ_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | namespace g2o { 17 | class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | EdgeSE3PriorXYZ() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() {} 21 | 22 | void computeError() override { 23 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 24 | 25 | // 直接相减求误差 26 | Eigen::Vector3d estimate = v1->estimate().translation(); 27 | _error = estimate - _measurement; 28 | } 29 | 30 | // TODO：Buggy 31 | void linearizeOplus() override { 32 | // only update position 33 | const VertexSE3* v = static_cast(_vertices[0]); 34 | _jacobianOplusXi.block<3, 3>(0, 0) = v->estimate().rotation(); 35 | _jacobianOplusXi.block<3, 3>(0, 3) = Eigen::Matrix3d::Zero(); 36 | } 37 | 38 | void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } 39 | 40 | virtual bool read(std::istream& is) override { 41 | // 读入观测 42 | Eigen::Vector3d v; 43 | is >> v(0) >> v(1) >> v(2); 44 | 45 | setMeasurement(Eigen::Vector3d(v)); 46 | 47 | // 读入信息矩阵(只读上三角部分) 48 | for (int i = 0; i < information().rows(); ++i) { 49 | for (int j = i; j < information().cols(); ++j) { 50 | is >> information()(i, j); 51 | if (i != j) { 52 | information()(j, i) = information()(i, j); 53 | } 54 | } 55 | } 56 | return true; 57 | } 58 | 59 | virtual bool write(std::ostream& os) const override { 60 | // 写入观测 61 | Eigen::Vector3d v = _measurement; 62 | os << v(0) << " " << v(1) << " " << v(2) << " "; 63 | 64 | // 写入信息矩阵 65 | for (int i = 0; i < information().rows(); ++i) { 66 | for (int j = i; j < information().cols(); ++j) { 67 | os << " " << information()(i, j); 68 | } 69 | } 70 | 71 | return os.good(); 72 | } 73 | }; 74 | } // namespace g2o 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/loop_closure/ScanContext/tictoc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class TicToc 13 | { 14 | public: 15 | TicToc() 16 | { 17 | tic(); 18 | } 19 | 20 | TicToc( bool _disp ) 21 | { 22 | disp_ = _disp; 23 | tic(); 24 | } 25 | 26 | void tic() 27 | { 28 | start = std::chrono::system_clock::now(); 29 | } 30 | 31 | void toc( std::string _about_task ) 32 | { 33 | end = std::chrono::system_clock::now(); 34 | std::chrono::duration elapsed_seconds = end - start; 35 | double elapsed_ms = elapsed_seconds.count() * 1000; 36 | 37 | if( disp_ ) 38 | { 39 | std::cout.precision(3); // 10 for sec, 3 for ms 40 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 41 | } 42 | } 43 | 44 | private: 45 | std::chrono::time_point start, end; 46 | bool disp_ = false; 47 | }; 48 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/loop_closure/distance_detector.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 基于位置比较进行回环检测 3 | * @Created Date: 2021-11-30 22:40:53 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-01 12:57:54 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_LOOP_CLOSURE_DISTANCE_DETECTOR_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_LOOP_CLOSURE_DISTANCE_DETECTOR_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include "mapping_localization/sensor_data/cloud_data.hpp" 20 | #include "mapping_localization/sensor_data/key_frame.hpp" 21 | #include "mapping_localization/sensor_data/pose_data.hpp" 22 | 23 | #include "mapping_localization/models/loop_closure/loop_closure_detector_interface.hpp" 24 | 25 | namespace mapping_localization { 26 | 27 | class DistanceDetector : public LoopClousreDetectorInterface { 28 | public: 29 | DistanceDetector(const YAML::Node& node); 30 | 31 | void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) override; 32 | bool DetectNearestKeyFrame(int& key_frame_index) override; 33 | 34 | private: 35 | int loop_step_ = 5; 36 | int diff_num_ = 10.0; 37 | int detect_area_ = 10.0; 38 | std::deque historical_key_frames; 39 | }; 40 | } // namespace mapping_localization 41 | 42 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/loop_closure/loop_closure_detector_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-30 21:58:59 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-11-30 22:25:45 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_LOOP_CLOSURE_LOOP_CLOSURE_DETECTOR_INTERFACE_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_LOOP_CLOSURE_LOOP_CLOSURE_DETECTOR_INTERFACE_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include "mapping_localization/sensor_data/cloud_data.hpp" 17 | #include "mapping_localization/sensor_data/key_frame.hpp" 18 | #include "mapping_localization/sensor_data/pose_data.hpp" 19 | 20 | namespace mapping_localization { 21 | 22 | class LoopClousreDetectorInterface { 23 | public: 24 | virtual ~LoopClousreDetectorInterface() = default; 25 | 26 | virtual void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) = 0; 27 | virtual bool DetectNearestKeyFrame(int& key_frame_index) = 0; 28 | }; 29 | } // namespace mapping_localization 30 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/loop_closure/scan_context_detector.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 基于 ScanContext 进行回环检测 3 | * @Created Date: 2021-11-30 22:13:00 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-11-30 22:45:25 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_LOOP_CLOSURE_SCAN_CONTEXT_DETECTOR_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_LOOP_CLOSURE_SCAN_CONTEXT_DETECTOR_HPP_ 12 | 13 | #include "mapping_localization/models/loop_closure/ScanContext/Scancontext.h" 14 | 15 | #include "mapping_localization/models/loop_closure/loop_closure_detector_interface.hpp" 16 | 17 | namespace mapping_localization { 18 | class ScanContextDetector : public LoopClousreDetectorInterface { 19 | public: 20 | ScanContextDetector(const YAML::Node& node); 21 | void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) override; 22 | bool DetectNearestKeyFrame(int& key_frame_index) override; 23 | 24 | private: 25 | SCManager sc_manager_; 26 | }; 27 | 28 | } // namespace mapping_localization 29 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_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 MAPPING_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 7 | #define MAPPING_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 8 | 9 | #include 10 | 11 | #include "mapping_localization/models/registration/registration_interface.hpp" 12 | 13 | namespace mapping_localization { 14 | class NDTRegistration : public RegistrationInterface { 15 | public: 16 | NDTRegistration(const YAML::Node& node); 17 | NDTRegistration(float res, float step_size, float trans_eps, int max_iter); 18 | 19 | bool SetInputTarget(const CloudData::Cloud_Ptr& input_target) override; 20 | bool ScanMatch(const CloudData::Cloud_Ptr& input_source, 21 | const Eigen::Matrix4f& predict_pose, 22 | CloudData::Cloud_Ptr& result_cloud_ptr, 23 | Eigen::Matrix4f& result_pose) override; 24 | float GetFitnessScore() override; 25 | 26 | private: 27 | bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter); 28 | 29 | private: 30 | pcl::NormalDistributionsTransform::Ptr ndt_ptr_; 31 | }; 32 | } // namespace mapping_localization 33 | 34 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/registration/pcl_icp_registration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-24 23:24:45 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_REGISTRATION_PCL_ICP_REGISTRATION_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_REGISTRATION_PCL_ICP_REGISTRATION_HPP_ 12 | 13 | #include 14 | 15 | #include "mapping_localization/models/registration/registration_interface.hpp" 16 | 17 | namespace mapping_localization { 18 | 19 | class PCLICPRegistration : public RegistrationInterface { 20 | public: 21 | PCLICPRegistration(const YAML::Node& node); 22 | PCLICPRegistration(float max_correspodence_dist, float trans_eps, float fitness_eps, int max_iter); 23 | 24 | bool SetInputTarget(const CloudData::Cloud_Ptr& input_target) override; 25 | bool ScanMatch(const CloudData::Cloud_Ptr& input_source, 26 | const Eigen::Matrix4f& predict_pose, 27 | CloudData::Cloud_Ptr& result_cloud_ptr, 28 | Eigen::Matrix4f& result_pose) override; 29 | float GetFitnessScore() override; 30 | 31 | private: 32 | bool SetRegistraionParam(float max_correspodence_dist, float trans_eps, float fitness_eps, int max_iter); 33 | 34 | private: 35 | pcl::IterativeClosestPoint::Ptr icp_ptr_; 36 | }; 37 | 38 | } // namespace mapping_localization 39 | 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/registration/registration_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云匹配模块的基类 3 | * @Created Date: 2020-02-08 21:25:11 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include "mapping_localization/sensor_data/cloud_data.hpp" 18 | 19 | namespace mapping_localization { 20 | class RegistrationInterface { 21 | public: 22 | virtual ~RegistrationInterface() = default; 23 | 24 | virtual bool SetInputTarget(const CloudData::Cloud_Ptr& input_target) = 0; 25 | virtual bool ScanMatch(const CloudData::Cloud_Ptr& input_source, 26 | const Eigen::Matrix4f& predict_pose, 27 | CloudData::Cloud_Ptr& result_cloud_ptr, 28 | Eigen::Matrix4f& result_pose) = 0; 29 | virtual float GetFitnessScore() = 0; 30 | }; 31 | } // namespace mapping_localization 32 | 33 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/models/scan_adjust/distortion_adjust.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云畸变补偿 3 | * @Created Date: 2020-02-25 14:38:12 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_ 11 | #define MAPPING_LOCALIZATION_MODELS_SCAN_ADJUST_DISTORTION_ADJUST_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include "glog/logging.h" 18 | #include "mapping_localization/models/scan_adjust/distortion_adjust.hpp" 19 | #include "mapping_localization/sensor_data/cloud_data.hpp" 20 | #include "mapping_localization/sensor_data/velocity_data.hpp" 21 | 22 | namespace mapping_localization { 23 | class DistortionAdjust { 24 | public: 25 | void SetMotionInfo(float scan_period, VelocityData velocity_data); 26 | bool AdjustCloud(CloudData::Cloud_Ptr& input_cloud_ptr, CloudData::Cloud_Ptr& output_cloud_ptr); 27 | 28 | private: 29 | inline Eigen::Matrix3f UpdateMatrix(float real_time); 30 | 31 | private: 32 | float scan_period_; 33 | Eigen::Vector3f velocity_; 34 | Eigen::Vector3f angular_rate_; 35 | }; 36 | } // namespace mapping_localization 37 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_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 MAPPING_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 8 | #define MAPPING_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "mapping_localization/sensor_data/cloud_data.hpp" 16 | 17 | namespace mapping_localization { 18 | class CloudPublisher { 19 | public: 20 | CloudPublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, size_t buff_size); 21 | CloudPublisher() = default; 22 | 23 | void Publish(CloudData::Cloud_Ptr& cloud_ptr_input, double time); 24 | void Publish(CloudData::Cloud_Ptr& cloud_ptr_input); 25 | 26 | bool HasSubscribers(); 27 | 28 | private: 29 | void PublishData(CloudData::Cloud_Ptr& cloud_ptr_input, ros::Time time); 30 | 31 | private: 32 | ros::NodeHandle nh_; 33 | ros::Publisher publisher_; 34 | std::string frame_id_; 35 | }; 36 | } // namespace mapping_localization 37 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/imu_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 在ros中发布IMU数据 3 | * @Created Date: 2020-02-05 02:27:30 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_IMU_PUBLISHER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_IMU_PUBLISHER_HPP_ 12 | 13 | #include "mapping_localization/sensor_data/imu_data.hpp" 14 | #include "sensor_msgs/Imu.h" 15 | 16 | namespace mapping_localization { 17 | class IMUPublisher { 18 | public: 19 | IMUPublisher(ros::NodeHandle& nh, std::string topic_name, size_t buff_size, std::string frame_id); 20 | IMUPublisher() = default; 21 | 22 | void Publish(IMUData imu_data); 23 | 24 | bool HasSubscribers(); 25 | 26 | private: 27 | ros::NodeHandle nh_; 28 | ros::Publisher publisher_; 29 | std::string frame_id_; 30 | }; 31 | } // namespace mapping_localization 32 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/key_frame_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 单个 key frame 信息发布 3 | * @Created Date: 2020-02-06 21:05:47 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_KEY_FRAME_PUBLISHER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_KEY_FRAME_PUBLISHER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include "mapping_localization/sensor_data/key_frame.hpp" 19 | 20 | namespace mapping_localization { 21 | class KeyFramePublisher { 22 | public: 23 | KeyFramePublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, int buff_size); 24 | KeyFramePublisher() = default; 25 | 26 | void Publish(KeyFrame& key_frame); 27 | 28 | bool HasSubscribers(); 29 | 30 | private: 31 | ros::NodeHandle nh_; 32 | ros::Publisher publisher_; 33 | std::string frame_id_ = ""; 34 | }; 35 | } // namespace mapping_localization 36 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/key_frames_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: key frames 信息发布 3 | * @Created Date: 2020-02-06 21:05:47 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_KEY_FRAMES_PUBLISHER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_KEY_FRAMES_PUBLISHER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "mapping_localization/sensor_data/key_frame.hpp" 21 | 22 | namespace mapping_localization { 23 | class KeyFramesPublisher { 24 | public: 25 | KeyFramesPublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, int buff_size); 26 | KeyFramesPublisher() = default; 27 | 28 | void Publish(const std::deque& key_frames); 29 | 30 | bool HasSubscribers(); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Publisher publisher_; 35 | std::string frame_id_ = ""; 36 | }; 37 | } // namespace mapping_localization 38 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/loop_pose_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发送闭环检测的相对位姿 3 | * @Created Date: 2020-02-06 21:05:47 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_LOOP_POSE_PUBLISHER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_LOOP_POSE_PUBLISHER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include "mapping_localization/sensor_data/loop_pose.hpp" 19 | 20 | namespace mapping_localization { 21 | class LoopPosePublisher { 22 | public: 23 | LoopPosePublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, int buff_size); 24 | LoopPosePublisher() = default; 25 | 26 | void Publish(LoopPose& loop_pose); 27 | 28 | bool HasSubscribers(); 29 | 30 | private: 31 | ros::NodeHandle nh_; 32 | ros::Publisher publisher_; 33 | std::string frame_id_ = ""; 34 | }; 35 | } // namespace mapping_localization 36 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/odometry_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: odometry 信息发布 3 | * @Created Date: 2020-02-06 21:05:47 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace mapping_localization { 20 | class OdometryPublisher { 21 | public: 22 | OdometryPublisher(ros::NodeHandle& nh, 23 | std::string topic_name, 24 | std::string base_frame_id, 25 | std::string child_frame_id, 26 | int buff_size); 27 | OdometryPublisher() = default; 28 | 29 | void Publish(const Eigen::Matrix4f& transform_matrix, double time); 30 | void Publish(const Eigen::Matrix4f& transform_matrix); 31 | 32 | bool HasSubscribers(); 33 | 34 | private: 35 | void PublishData(const Eigen::Matrix4f& transform_matrix, ros::Time time); 36 | 37 | private: 38 | ros::NodeHandle nh_; 39 | ros::Publisher publisher_; 40 | nav_msgs::Odometry odometry_; 41 | }; 42 | } // namespace mapping_localization 43 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/tf_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发布tf的类 3 | * @Created Date: 2020-03-05 15:23:26 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_TF_BROADCASTER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_TF_BROADCASTER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace mapping_localization { 21 | class TFBroadCaster { 22 | public: 23 | TFBroadCaster(std::string frame_id, std::string child_frame_id); 24 | TFBroadCaster() = default; 25 | void SendTransform(Eigen::Matrix4f pose, double time); 26 | 27 | protected: 28 | tf::StampedTransform transform_; 29 | tf::TransformBroadcaster broadcaster_; 30 | }; 31 | } // namespace mapping_localization 32 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/publisher/trajectory_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-30 11:03:52 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-11-30 23:47:57 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_PUBLISHER_TRAJECTORY_PUBLISHER_HPP_ 11 | #define MAPPING_LOCALIZATION_PUBLISHER_TRAJECTORY_PUBLISHER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "mapping_localization/sensor_data/key_frame.hpp" 22 | 23 | namespace mapping_localization { 24 | 25 | class TrajectoryPublisher { 26 | public: 27 | TrajectoryPublisher(ros::NodeHandle& nh, 28 | const std::string& topic_name, 29 | const std::string& frame_id, 30 | int buff_size, 31 | bool has_edge_info = false); 32 | 33 | ///@brief 在原有轨迹基础上增加新的位姿，并发布 34 | ///@param key_frame 新的关键帧 35 | void Publish(const KeyFrame& key_frame); 36 | 37 | ///@brief 重置所有关键帧为输入关键帧序列并更新发布 38 | ///@param key_frames 新的关键帧序列 39 | void Publish(const std::deque& key_frames); 40 | 41 | /// 42 | ///@brief 在轨迹中两个点连线 43 | ///@param index1 44 | ///@param index2 45 | /// 46 | bool AddEdge(unsigned int index1, unsigned int index2, bool is_true); 47 | 48 | bool HasSubscribers(); 49 | 50 | private: 51 | geometry_msgs::PoseStamped convertKeyFrameToPose(const KeyFrame& key_frame, const std::string& frame_id); 52 | 53 | private: 54 | ros::NodeHandle nh_; 55 | ros::Publisher publisher_; 56 | nav_msgs::Path path_; 57 | 58 | bool has_edge_info_ = false; 59 | 60 | ros::Publisher true_edge_publisher_; 61 | ros::Publisher false_edge_publisher_; 62 | visualization_msgs::Marker true_edge_list; 63 | visualization_msgs::Marker false_edge_list; 64 | }; 65 | 66 | } // namespace mapping_localization 67 | 68 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/cloud_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2019-07-17 18:17:49 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | namespace mapping_localization { 17 | class CloudData { 18 | public: 19 | using Point = pcl::PointXYZ; 20 | using Cloud = pcl::PointCloud; 21 | using Cloud_Ptr = Cloud::Ptr; 22 | 23 | public: 24 | CloudData() : cloud_ptr(new Cloud()) {} 25 | 26 | public: 27 | double time = 0.0; 28 | Cloud_Ptr cloud_ptr; 29 | }; 30 | } // namespace mapping_localization 31 | 32 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/gnss_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2019-07-17 18:25:13 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-29 19:41:26 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | namespace mapping_localization { 17 | class GNSSData { 18 | public: 19 | double time = 0.0; 20 | double longitude = 0.0; 21 | double latitude = 0.0; 22 | double altitude = 0.0; 23 | double local_E = 0.0; 24 | double local_N = 0.0; 25 | double local_U = 0.0; 26 | int status = 0; 27 | int service = 0; 28 | 29 | static double origin_longitude; 30 | static double origin_latitude; 31 | static double origin_altitude; 32 | 33 | private: 34 | static GeographicLib::LocalCartesian geo_converter; 35 | static bool origin_position_inited; 36 | 37 | public: 38 | void InitOriginPosition(); 39 | void UpdateXYZ(); 40 | 41 | /// 42 | ///@brief 清除未同步缓存中早于指定时间的数据，并取两个离指定时间最近的数据进行线性插值 43 | /// 44 | ///@param UnsyncedData - 未经过时间同步的消息队列 45 | ///@param SyncedData - 经过时间同步的消息队列（最后一个数据是指定时间的插值 GNSS 数据） 46 | ///@param sync_time - 指定同步时间 47 | ///@return true - 同步成功，false - 同步失败 48 | /// 49 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 50 | }; 51 | } // namespace mapping_localization 52 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/imu_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2019-07-17 18:27:40 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-29 19:40:55 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace mapping_localization { 18 | class IMUData { 19 | public: 20 | struct LinearAcceleration { 21 | double x = 0.0; 22 | double y = 0.0; 23 | double z = 0.0; 24 | }; 25 | 26 | struct AngularVelocity { 27 | double x = 0.0; 28 | double y = 0.0; 29 | double z = 0.0; 30 | }; 31 | 32 | class Orientation { 33 | public: 34 | double x = 0.0; 35 | double y = 0.0; 36 | double z = 0.0; 37 | double w = 0.0; 38 | 39 | public: 40 | void Normlize() { 41 | double norm = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0) + pow(w, 2.0)); 42 | x /= norm; 43 | y /= norm; 44 | z /= norm; 45 | w /= norm; 46 | } 47 | }; 48 | 49 | double time = 0.0; 50 | LinearAcceleration linear_acceleration; 51 | AngularVelocity angular_velocity; 52 | Orientation orientation; 53 | 54 | public: 55 | // 把四元数转换成旋转矩阵送出去 56 | Eigen::Matrix3f GetOrientationMatrix(); 57 | 58 | /// 59 | ///@brief 清除未同步缓存中早于指定时间的数据，并取两个离指定时间最近的数据进行线性插值 60 | /// 61 | ///@param UnsyncedData - 未经过时间同步的消息队列 62 | ///@param SyncedData - 经过时间同步的消息队列（最后一个数据是指定时间的插值 IMU 数据） 63 | ///@param sync_time - 指定同步时间 64 | ///@return true - 同步成功，false - 同步失败 65 | /// 66 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 67 | }; 68 | } // namespace mapping_localization 69 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/key_frame.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 关键帧，在各个模块之间传递数据 3 | * @Created Date: 2020-02-28 19:13:26 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-30 11:15:38 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_KEY_FRAME_HPP_ 12 | 13 | #include 14 | 15 | namespace mapping_localization { 16 | class KeyFrame { 17 | public: 18 | double time = 0.0; 19 | unsigned int index = 0; 20 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 21 | 22 | public: 23 | Eigen::Quaternionf GetQuaternion() const; 24 | }; 25 | } // namespace mapping_localization 26 | 27 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/loop_pose.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 关键帧之间的相对位姿，用于闭环检测 3 | * @Created Date: 2020-02-28 19:13:26 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-30 20:20:35 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_LOOP_POSE_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_LOOP_POSE_HPP_ 12 | 13 | #include 14 | 15 | namespace mapping_localization { 16 | class LoopPose { 17 | public: 18 | double time = 0.0; 19 | unsigned int index0 = 0; 20 | unsigned int index1 = 0; 21 | bool confidence = false; // false -- 回环检测到，但是地图效果不好， true -- 回环检测到，且匹配效果较好 22 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 23 | 24 | public: 25 | Eigen::Quaternionf GetQuaternion(); 26 | }; 27 | } // namespace mapping_localization 28 | 29 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/pose_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 存放处理后的IMU姿态以及GNSS位置 3 | * @Created Date: 2020-02-27 23:10:56 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_ 12 | 13 | #include 14 | 15 | namespace mapping_localization { 16 | class PoseData { 17 | public: 18 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 19 | double time = 0.0; 20 | 21 | public: 22 | Eigen::Quaternionf GetQuaternion(); 23 | }; 24 | } // namespace mapping_localization 25 | 26 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/sensor_data/velocity_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: velocity 数据 3 | * @Created Date: 2019-07-17 18:27:40 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-29 19:41:54 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_ 11 | #define MAPPING_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | namespace mapping_localization { 17 | class VelocityData { 18 | public: 19 | struct LinearVelocity { 20 | double x = 0.0; 21 | double y = 0.0; 22 | double z = 0.0; 23 | }; 24 | 25 | struct AngularVelocity { 26 | double x = 0.0; 27 | double y = 0.0; 28 | double z = 0.0; 29 | }; 30 | 31 | double time = 0.0; 32 | LinearVelocity linear_velocity; 33 | AngularVelocity angular_velocity; 34 | 35 | public: 36 | /// 37 | ///@brief 清除未同步缓存中早于指定时间的数据，并取两个离指定时间最近的数据进行线性插值 38 | /// 39 | ///@param UnsyncedData - 未经过时间同步的消息队列 40 | ///@param SyncedData - 经过时间同步的消息队列（最后一个数据是指定时间的插值速度数据） 41 | ///@param sync_time - 指定同步时间 42 | ///@return true - 同步成功，false - 同步失败 43 | /// 44 | static bool SyncData(std::deque& UnsyncedData, 45 | std::deque& SyncedData, 46 | double sync_time); 47 | void TransformCoordinate(Eigen::Matrix4f transform_matrix); 48 | }; 49 | } // namespace mapping_localization 50 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/cloud_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Created Date: 2020-02-05 02:27:30 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "mapping_localization/sensor_data/cloud_data.hpp" 24 | 25 | namespace mapping_localization { 26 | class CloudSubscriber { 27 | public: 28 | CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 29 | CloudSubscriber() = default; 30 | void ParseData(std::deque& deque_cloud_data); 31 | 32 | private: 33 | void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr); 34 | 35 | private: 36 | ros::NodeHandle nh_; 37 | ros::Subscriber subscriber_; 38 | std::deque new_cloud_data_; 39 | 40 | std::mutex buff_mutex_; 41 | }; 42 | } // namespace mapping_localization 43 | 44 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/gnss_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2019-03-31 12:58:10 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "mapping_localization/sensor_data/gnss_data.hpp" 20 | #include "sensor_msgs/NavSatFix.h" 21 | 22 | namespace mapping_localization { 23 | class GNSSSubscriber { 24 | public: 25 | GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | GNSSSubscriber() = default; 27 | void ParseData(std::deque& deque_gnss_data); 28 | 29 | private: 30 | void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_gnss_data_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } // namespace mapping_localization 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/imu_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Created Date: 2019-08-19 19:22:17 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "mapping_localization/sensor_data/imu_data.hpp" 20 | #include "sensor_msgs/Imu.h" 21 | 22 | namespace mapping_localization { 23 | class IMUSubscriber { 24 | public: 25 | IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | IMUSubscriber() = default; 27 | void ParseData(std::deque& deque_imu_data); 28 | 29 | private: 30 | void msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_imu_data_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } // namespace mapping_localization 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/key_frame_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Created Date: 2019-08-19 19:22:17 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_KEY_FRAME_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_KEY_FRAME_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "mapping_localization/sensor_data/key_frame.hpp" 21 | 22 | namespace mapping_localization { 23 | class KeyFrameSubscriber { 24 | public: 25 | KeyFrameSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | KeyFrameSubscriber() = default; 27 | void ParseData(std::deque& key_frame_buff); 28 | 29 | private: 30 | void msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& key_frame_msg_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_key_frame_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } // namespace mapping_localization 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/key_frames_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Created Date: 2019-08-19 19:22:17 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_KEY_FRAMES_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_KEY_FRAMES_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "mapping_localization/sensor_data/key_frame.hpp" 22 | 23 | namespace mapping_localization { 24 | class KeyFramesSubscriber { 25 | public: 26 | KeyFramesSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 27 | KeyFramesSubscriber() = default; 28 | void ParseData(std::deque& deque_key_frames); 29 | 30 | private: 31 | void msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr); 32 | 33 | private: 34 | ros::NodeHandle nh_; 35 | ros::Subscriber subscriber_; 36 | std::deque new_key_frames_; 37 | 38 | std::mutex buff_mutex_; 39 | }; 40 | } // namespace mapping_localization 41 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/loop_pose_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 闭环检测位姿 数据 3 | * @Created Date: 2019-08-19 19:22:17 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_LOOP_POSE_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_LOOP_POSE_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "mapping_localization/sensor_data/loop_pose.hpp" 21 | 22 | namespace mapping_localization { 23 | class LoopPoseSubscriber { 24 | public: 25 | LoopPoseSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | LoopPoseSubscriber() = default; 27 | void ParseData(std::deque& loop_pose_buff); 28 | 29 | private: 30 | void msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& loop_pose_msg_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_loop_pose_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } // namespace mapping_localization 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/odometry_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅odometry数据 3 | * @Created Date: 2019-08-19 19:22:17 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "mapping_localization/sensor_data/pose_data.hpp" 21 | 22 | namespace mapping_localization { 23 | class OdometrySubscriber { 24 | public: 25 | OdometrySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | OdometrySubscriber() = default; 27 | void ParseData(std::deque& deque_pose_data); 28 | 29 | private: 30 | void msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_pose_data_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } // namespace mapping_localization 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/trajectory_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-30 12:09:28 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-11-30 18:46:20 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_TRAJECTORY_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_TRAJECTORY_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "mapping_localization/sensor_data/key_frame.hpp" 22 | 23 | namespace mapping_localization { 24 | class TrajectorySubscriber { 25 | public: 26 | TrajectorySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 27 | TrajectorySubscriber() = default; 28 | void ParseData(std::deque& deque_key_frames); 29 | 30 | private: 31 | void msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr); 32 | 33 | private: 34 | ros::NodeHandle nh_; 35 | ros::Subscriber subscriber_; 36 | std::deque new_key_frames_; 37 | 38 | std::mutex buff_mutex_; 39 | }; 40 | } // namespace mapping_localization 41 | #endif 42 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/subscriber/velocity_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅velocity数据 3 | * @Created Date: 2019-08-19 19:22:17 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_ 11 | #define MAPPING_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "geometry_msgs/TwistStamped.h" 20 | #include "mapping_localization/sensor_data/velocity_data.hpp" 21 | 22 | namespace mapping_localization { 23 | class VelocitySubscriber { 24 | public: 25 | VelocitySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 26 | VelocitySubscriber() = default; 27 | void ParseData(std::deque& deque_velocity_data); 28 | 29 | private: 30 | void msg_callback(const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr); 31 | 32 | private: 33 | ros::NodeHandle nh_; 34 | ros::Subscriber subscriber_; 35 | std::deque new_velocity_data_; 36 | 37 | std::mutex buff_mutex_; 38 | }; 39 | } // namespace mapping_localization 40 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/tf_listener/tf_listener.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: tf监听模块 3 | * @Created Date: 2020-02-06 16:01:21 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_TF_LISTENER_HPP_ 11 | #define MAPPING_LOCALIZATION_TF_LISTENER_HPP_ 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace mapping_localization { 20 | class TFListener { 21 | public: 22 | TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id); 23 | TFListener() = default; 24 | 25 | bool LookupData(Eigen::Matrix4f& transform_matrix); 26 | 27 | private: 28 | bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix); 29 | 30 | private: 31 | ros::NodeHandle nh_; 32 | tf::TransformListener listener_; 33 | std::string base_frame_id_; 34 | std::string child_frame_id_; 35 | }; 36 | } // namespace mapping_localization 37 | 38 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/tools/file_manager.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 读写文件管理 3 | * @Created Date: 2020-02-24 19:22:53 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_TOOLS_FILE_MANAGER_HPP_ 11 | #define MAPPING_LOCALIZATION_TOOLS_FILE_MANAGER_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace mapping_localization { 18 | class FileManager { 19 | public: 20 | static bool CreateFile(std::ofstream& ofs, std::string file_path); 21 | static bool InitDirectory(std::string directory_path, std::string use_for); 22 | static bool CreateDirectory(std::string directory_path, std::string use_for); 23 | static bool CreateDirectory(std::string directory_path); 24 | }; 25 | } // namespace mapping_localization 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/tools/print_info.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 打印信息 3 | * @Created Date: 2020-03-02 23:25:26 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_TOOLS_PRINT_INFO_HPP_ 11 | #define MAPPING_LOCALIZATION_TOOLS_PRINT_INFO_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "pcl/common/eigen.h" 18 | 19 | namespace mapping_localization { 20 | class PrintInfo { 21 | public: 22 | static void PrintPose(std::string head, Eigen::Matrix4f pose); 23 | }; 24 | } // namespace mapping_localization 25 | #endif -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/tools/tic_toc.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 用来测试运行时间 3 | * @Created Date: 2020-03-01 18:12:03 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-28 12:43:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_TOOLS_TIC_TOC_HPP_ 11 | #define MAPPING_LOCALIZATION_TOOLS_TIC_TOC_HPP_ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace mapping_localization { 21 | class TicToc { 22 | public: 23 | TicToc() { tic(); } 24 | 25 | void tic() { start = std::chrono::system_clock::now(); } 26 | 27 | double toc() { 28 | end = std::chrono::system_clock::now(); 29 | std::chrono::duration elapsed_seconds = end - start; 30 | start = std::chrono::system_clock::now(); 31 | return elapsed_seconds.count(); 32 | } 33 | 34 | private: 35 | std::chrono::time_point start, end; 36 | }; 37 | 38 | class RosTicToc { 39 | public: 40 | RosTicToc() { tic(); } 41 | 42 | void tic() { start = ros::Time::now(); } 43 | 44 | double toc() { 45 | end = ros::Time::now(); 46 | ros::Duration duration = (end - start); 47 | return duration.toSec(); 48 | } 49 | 50 | private: 51 | ros::Time start, end; 52 | }; 53 | 54 | class Timer { 55 | public: 56 | Timer(const std::string &name) : name_(name) {} 57 | 58 | void tic() { tictoc_.tic(); } 59 | 60 | void toc() { 61 | double cost = tictoc_.toc(); 62 | min_cost = std::min(cost, min_cost); 63 | max_cost = std::max(cost, max_cost); 64 | average_cost_ = (average_cost_ * count_ + cost) / (count_ + 1); 65 | count_++; 66 | } 67 | 68 | int getCount() const { return count_; } 69 | 70 | friend std::ostream &operator<<(std::ostream &output, const Timer &timer) { 71 | output << "Timer " << timer.name_ << "==> average: " << timer.average_cost_ 72 | << " seconds, min: " << timer.min_cost << " seconds, max: " << timer.max_cost 73 | << " seconds, count: " << timer.count_; 74 | 75 | return output; 76 | } 77 | 78 | private: 79 | RosTicToc tictoc_; 80 | std::string name_; 81 | double average_cost_ = 0.0; 82 | double min_cost = 1000.0; 83 | double max_cost = 0.0; 84 | int count_ = 0; 85 | }; 86 | } // namespace mapping_localization 87 | #endif 88 | -------------------------------------------------------------------------------- /src/mapping_localization/include/mapping_localization/tools/utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-12-10 13:59:59 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-13 17:09:15 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #ifndef MAPPING_LOCALIZATION_TOOLS_UTILS_HPP_ 11 | #define MAPPING_LOCALIZATION_TOOLS_UTILS_HPP_ 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace mapping_localization { 19 | 20 | template 21 | std::ostream& operator<<(std::ostream& os, const std::vector& v) { 22 | os << "["; 23 | for (typename std::vector::const_iterator ii = v.begin(); ii != v.end(); ++ii) { 24 | os << " " << *ii; 25 | } 26 | os << "]"; 27 | return os; 28 | } 29 | 30 | /// 31 | ///@brief template for loading param from yaml node and 32 | /// 33 | ///@tparam T 34 | ///@param node 35 | ///@param name 36 | ///@param value output value, should be with overloaded output stream operator 37 | ///@param default_value de 38 | ///@param options valid options, if provided 39 | ///@return true 40 | ///@return false 41 | /// 42 | template 43 | bool try_load_param(const YAML::Node& node, 44 | std::string name, 45 | T& value, 46 | T default_value, 47 | const std::vector& options = std::vector()) { 48 | bool success = false; 49 | try { 50 | value = node[name].as(); 51 | if (options.size()) { 52 | for (const T& option : options) { 53 | if (option == value) { 54 | success = true; 55 | break; 56 | } 57 | } 58 | } else { 59 | success = true; 60 | } 61 | } catch (...) { 62 | success = false; 63 | } 64 | 65 | if (!success) { 66 | std::cout << "Param " << name << " couldn't be found in yaml file or invalid, check your config!.\n"; 67 | 68 | if (options.size()) { 69 | std::cout << "Valid options: "; 70 | for (const T& option : options) { 71 | std::cout << option << ", "; 72 | } 73 | std::cout << std::endl; 74 | } 75 | 76 | std::cout << " Using default value: " << default_value << "\n"; 77 | value = default_value; 78 | } 79 | 80 | return success; 81 | } 82 | 83 | Eigen::MatrixXd CalculateSe3EdgeInformationMatrix(Eigen::VectorXd noise); 84 | 85 | Eigen::MatrixXd CalculateDiagMatrix(Eigen::VectorXd noise); 86 | Eigen::MatrixXd CalculateSqrtDiagMatrix(Eigen::VectorXd noise); 87 | 88 | } // namespace mapping_localization 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /src/mapping_localization/launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/mapping_localization/launch/mapping_with_aloam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/mapping_localization/launch/mapping_with_lego_loam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/mapping_localization/launch/matching.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/mapping_localization/launch/test_frame.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/mapping_localization/src/apps/back_end_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 前端里程计的node文件 3 | * @Created Date: 2020-02-05 02:56:27 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-11 15:51:22 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "glog/logging.h" 14 | #include "mapping_localization/global_defination/global_defination.h" 15 | #include "mapping_localization/mapping/back_end/back_end_flow.hpp" 16 | 17 | using namespace mapping_localization; 18 | 19 | std::shared_ptr _back_end_flow_ptr; 20 | bool _need_optimize_map = false; 21 | 22 | bool optimize_map_callback(optimizeMap::Request &request, optimizeMap::Response &response) { 23 | _need_optimize_map = true; 24 | response.succeed = true; 25 | return response.succeed; 26 | } 27 | 28 | int main(int argc, char *argv[]) { 29 | google::InitGoogleLogging(argv[0]); 30 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 31 | FLAGS_alsologtostderr = 1; 32 | 33 | ros::init(argc, argv, "back_end_node"); 34 | ros::NodeHandle nh; 35 | 36 | ros::ServiceServer service = nh.advertiseService("optimize_map", optimize_map_callback); 37 | _back_end_flow_ptr = std::make_shared(nh); 38 | 39 | ros::Rate rate(100); 40 | while (ros::ok()) { 41 | ros::spinOnce(); 42 | 43 | _back_end_flow_ptr->Run(); 44 | 45 | if (_need_optimize_map) { 46 | _back_end_flow_ptr->ForceOptimize(); 47 | _need_optimize_map = false; 48 | } 49 | 50 | rate.sleep(); 51 | } 52 | 53 | return 0; 54 | } -------------------------------------------------------------------------------- /src/mapping_localization/src/apps/data_pretreat_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 数据预处理的node文件 3 | * @Created Date: 2020-02-05 02:56:27 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:32:37 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include 11 | 12 | #include "glog/logging.h" 13 | #include "mapping_localization/data_pretreat/data_pretreat_flow.hpp" 14 | #include "mapping_localization/global_defination/global_defination.h" 15 | 16 | using namespace mapping_localization; 17 | 18 | int main(int argc, char *argv[]) { 19 | google::InitGoogleLogging(argv[0]); 20 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 21 | FLAGS_alsologtostderr = 1; 22 | 23 | ros::init(argc, argv, "data_pretreat_node"); 24 | ros::NodeHandle nh; 25 | 26 | std::string cloud_topic; 27 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 28 | 29 | std::shared_ptr data_pretreat_flow_ptr = 30 | std::make_shared(nh, cloud_topic); 31 | 32 | ros::Rate rate(100); 33 | while (ros::ok()) { 34 | ros::spinOnce(); 35 | 36 | data_pretreat_flow_ptr->Run(); 37 | 38 | rate.sleep(); 39 | } 40 | 41 | return 0; 42 | } -------------------------------------------------------------------------------- /src/mapping_localization/src/apps/front_end_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 前端里程计的node文件 3 | * @Created Date: 2020-02-05 02:56:27 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-29 23:04:51 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include 11 | 12 | #include "glog/logging.h" 13 | #include "mapping_localization/global_defination/global_defination.h" 14 | #include "mapping_localization/mapping/front_end/front_end_flow.hpp" 15 | 16 | using namespace mapping_localization; 17 | 18 | int main(int argc, char *argv[]) { 19 | google::InitGoogleLogging(argv[0]); 20 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 21 | FLAGS_alsologtostderr = 1; 22 | 23 | ros::init(argc, argv, "front_end_node"); 24 | ros::NodeHandle nh; 25 | 26 | std::string cloud_topic, odom_topic; 27 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 28 | nh.param("odom_topic", odom_topic, "/laser_odom"); 29 | 30 | std::shared_ptr front_end_flow_ptr = std::make_shared(nh); 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/mapping_localization/src/apps/loop_closing_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 闭环检测的node文件 3 | * @Created Date: 2020-02-05 02:56:27 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:33:15 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | /* 11 | * @Description: 闭环检测的node文件 12 | * @Author: Ren Qian 13 | * @Date: 2020-02-05 02:56:27 14 | */ 15 | #include 16 | 17 | #include "glog/logging.h" 18 | #include "mapping_localization/global_defination/global_defination.h" 19 | #include "mapping_localization/mapping/loop_closing/loop_closing_flow.hpp" 20 | 21 | using namespace mapping_localization; 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 | std::shared_ptr loop_closing_flow_ptr = 32 | std::make_shared(nh); 33 | 34 | ros::Rate rate(100); 35 | while (ros::ok()) { 36 | ros::spinOnce(); 37 | 38 | loop_closing_flow_ptr->Run(); 39 | 40 | rate.sleep(); 41 | } 42 | 43 | return 0; 44 | } -------------------------------------------------------------------------------- /src/mapping_localization/src/apps/matching_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 地图匹配定位的node文件 3 | * @Created Date: 2020-02-05 02:56:27 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:33:33 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include 11 | 12 | #include "glog/logging.h" 13 | #include "mapping_localization/global_defination/global_defination.h" 14 | #include "mapping_localization/matching/matching_flow.hpp" 15 | 16 | using namespace mapping_localization; 17 | 18 | int main(int argc, char *argv[]) { 19 | google::InitGoogleLogging(argv[0]); 20 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 21 | FLAGS_alsologtostderr = 1; 22 | 23 | ros::init(argc, argv, "matching_node"); 24 | ros::NodeHandle nh; 25 | 26 | std::shared_ptr matching_flow_ptr = std::make_shared(nh); 27 | 28 | ros::Rate rate(100); 29 | while (ros::ok()) { 30 | ros::spinOnce(); 31 | 32 | matching_flow_ptr->Run(); 33 | 34 | rate.sleep(); 35 | } 36 | 37 | return 0; 38 | } -------------------------------------------------------------------------------- /src/mapping_localization/src/apps/viewer_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: viewer 的 node 文件 3 | * @Created Date: 2020-02-05 02:56:27 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:33:59 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "glog/logging.h" 14 | #include "mapping_localization/global_defination/global_defination.h" 15 | #include "mapping_localization/mapping/viewer/viewer_flow.hpp" 16 | 17 | using namespace mapping_localization; 18 | 19 | std::shared_ptr _viewer_flow_ptr; 20 | bool _need_save_map = false; 21 | 22 | bool save_map_callback(saveMap::Request &request, saveMap::Response &response) { 23 | _need_save_map = true; 24 | response.succeed = true; 25 | return response.succeed; 26 | } 27 | 28 | int main(int argc, char *argv[]) { 29 | google::InitGoogleLogging(argv[0]); 30 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 31 | FLAGS_alsologtostderr = 1; 32 | 33 | ros::init(argc, argv, "viewer_node"); 34 | ros::NodeHandle nh; 35 | 36 | std::string cloud_topic; 37 | nh.param("cloud_topic", cloud_topic, "/synced_cloud"); 38 | std::shared_ptr _viewer_flow_ptr = 39 | std::make_shared(nh, cloud_topic); 40 | 41 | ros::ServiceServer service = nh.advertiseService("save_map", save_map_callback); 42 | 43 | ros::Rate rate(100); 44 | while (ros::ok()) { 45 | ros::spinOnce(); 46 | 47 | _viewer_flow_ptr->Run(); 48 | if (_need_save_map) { 49 | _need_save_map = false; 50 | _viewer_flow_ptr->SaveMap(); 51 | } 52 | 53 | rate.sleep(); 54 | } 55 | 56 | return 0; 57 | } -------------------------------------------------------------------------------- /src/mapping_localization/src/data_pretreat/external_front_end_adapter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-12-02 00:46:57 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-03 17:36:23 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/data_pretreat/external_front_end_adapter.hpp" 11 | 12 | namespace mapping_localization { 13 | 14 | ExternalFrontEndAdapter::ExternalFrontEndAdapter( 15 | std::shared_ptr& external_laser_odom_sub_ptr, 16 | std::shared_ptr& synced_external_lasesr_odom_pub_ptr, 17 | const std::vector& pose_to_pointcloud) 18 | : external_laser_odom_sub_ptr_(external_laser_odom_sub_ptr), 19 | synced_external_lasesr_odom_pub_ptr_(synced_external_lasesr_odom_pub_ptr) { 20 | double tx = pose_to_pointcloud[0]; 21 | double ty = pose_to_pointcloud[1]; 22 | double tz = pose_to_pointcloud[2]; 23 | double yaw = pose_to_pointcloud[3]; 24 | double pitch = pose_to_pointcloud[4]; 25 | double roll = pose_to_pointcloud[5]; 26 | 27 | Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX()); 28 | Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY()); 29 | Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ()); 30 | 31 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 32 | 33 | Eigen::Matrix3f rotationMatrix = q.matrix(); 34 | 35 | pose_to_pointcloud_.block<3, 1>(0, 3) = Eigen::Vector3f(tx, ty, tz); 36 | pose_to_pointcloud_.block<3, 3>(0, 0) = rotationMatrix; 37 | } 38 | 39 | bool ExternalFrontEndAdapter::SyncData(double time) { 40 | external_laser_odom_sub_ptr_->ParseData(external_odom_buff_); 41 | 42 | while (!external_odom_buff_.empty() && external_odom_buff_.front().time < time) { 43 | external_odom_buff_.pop_front(); 44 | } 45 | 46 | if (external_odom_buff_.empty() || external_odom_buff_.front().time != time) { 47 | return false; 48 | } 49 | 50 | return true; 51 | } 52 | 53 | bool ExternalFrontEndAdapter::HasData() const { return external_odom_buff_.size(); } 54 | 55 | void ExternalFrontEndAdapter::PublishData() { 56 | PoseData pose = external_odom_buff_.front(); 57 | external_odom_buff_.pop_front(); 58 | // transform external odometry pose estitimation to lidar pose under lidar_init frame 59 | pose.pose = pose_to_pointcloud_.inverse() * pose.pose * pose_to_pointcloud_; 60 | synced_external_lasesr_odom_pub_ptr_->Publish(pose.pose, pose.time); 61 | } 62 | 63 | ExternalFrontEndAdapter::~ExternalFrontEndAdapter() {} 64 | } // namespace mapping_localization 65 | -------------------------------------------------------------------------------- /src/mapping_localization/src/models/cloud_filter/box_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 从点云中截取一个立方体部分 3 | * @Created Date: 2019-03-12 23:38:31 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-25 18:53:23 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/cloud_filter/box_filter.hpp" 11 | 12 | #include 13 | #include 14 | 15 | #include "glog/logging.h" 16 | 17 | namespace mapping_localization { 18 | BoxFilter::BoxFilter(YAML::Node node) { 19 | size_.resize(6); 20 | edge_.resize(6); 21 | origin_.resize(3); 22 | 23 | for (size_t i = 0; i < size_.size(); i++) { 24 | size_.at(i) = node["box_filter_size"][i].as(); 25 | } 26 | SetSize(size_); 27 | } 28 | 29 | bool BoxFilter::Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, 30 | CloudData::Cloud_Ptr& output_cloud_ptr) { 31 | output_cloud_ptr->clear(); 32 | pcl_box_filter_.setMin( 33 | Eigen::Vector4f(edge_.at(0), edge_.at(2), edge_.at(4), 1.0e-6)); 34 | pcl_box_filter_.setMax( 35 | Eigen::Vector4f(edge_.at(1), edge_.at(3), edge_.at(5), 1.0e6)); 36 | pcl_box_filter_.setInputCloud(input_cloud_ptr); 37 | pcl_box_filter_.filter(*output_cloud_ptr); 38 | 39 | return true; 40 | } 41 | 42 | void BoxFilter::SetSize(std::vector size) { 43 | size_ = size; 44 | std::cout << "Box Filter 的尺寸为：" << std::endl 45 | << "min_x: " << size.at(0) << ", " 46 | << "max_x: " << size.at(1) << ", " 47 | << "min_y: " << size.at(2) << ", " 48 | << "max_y: " << size.at(3) << ", " 49 | << "min_z: " << size.at(4) << ", " 50 | << "max_z: " << size.at(5) << std::endl 51 | << std::endl; 52 | 53 | CalculateEdge(); 54 | } 55 | 56 | void BoxFilter::SetOrigin(std::vector origin) { 57 | origin_ = origin; 58 | CalculateEdge(); 59 | } 60 | 61 | void BoxFilter::CalculateEdge() { 62 | for (size_t i = 0; i < origin_.size(); ++i) { 63 | edge_.at(2 * i) = size_.at(2 * i) + origin_.at(i); 64 | edge_.at(2 * i + 1) = size_.at(2 * i + 1) + origin_.at(i); 65 | } 66 | } 67 | 68 | std::vector BoxFilter::GetEdge() { return edge_; } 69 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/models/cloud_filter/no_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 不滤波 3 | * @Created Date: 2020-02-09 19:53:20 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-25 18:53:50 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/cloud_filter/no_filter.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | NoFilter::NoFilter() {} 16 | 17 | bool NoFilter::Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, 18 | CloudData::Cloud_Ptr& filtered_cloud_ptr) { 19 | filtered_cloud_ptr.reset(new CloudData::Cloud(*input_cloud_ptr)); 20 | return true; 21 | } 22 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/models/cloud_filter/voxel_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel filter 模块实现 3 | * @Created Date: 2020-02-09 19:53:20 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-25 18:53:20 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/cloud_filter/voxel_filter.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | 16 | VoxelFilter::VoxelFilter(const YAML::Node& node) { 17 | float leaf_size_x = node["leaf_size"][0].as(); 18 | float leaf_size_y = node["leaf_size"][1].as(); 19 | float leaf_size_z = node["leaf_size"][2].as(); 20 | 21 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 22 | } 23 | 24 | VoxelFilter::VoxelFilter(float leaf_size_x, 25 | float leaf_size_y, 26 | float leaf_size_z) { 27 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 28 | } 29 | 30 | bool VoxelFilter::SetFilterParam(float leaf_size_x, 31 | float leaf_size_y, 32 | float leaf_size_z) { 33 | voxel_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z); 34 | 35 | std::cout << "Voxel Filter 的参数为：" << std::endl 36 | << leaf_size_x << ", " << leaf_size_y << ", " << leaf_size_z 37 | << std::endl 38 | << std::endl; 39 | 40 | return true; 41 | } 42 | 43 | bool VoxelFilter::Filter(const CloudData::Cloud_Ptr& input_cloud_ptr, 44 | CloudData::Cloud_Ptr& filtered_cloud_ptr) { 45 | voxel_filter_.setInputCloud(input_cloud_ptr); 46 | voxel_filter_.filter(*filtered_cloud_ptr); 47 | 48 | return true; 49 | } 50 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/models/graph_optimizer/interface_graph_optimizer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2020-03-01 18:35:19 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:36:49 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/graph_optimizer/interface_graph_optimizer.hpp" 11 | 12 | namespace mapping_localization { 13 | void InterfaceGraphOptimizer::SetMaxIterationsNum(int max_iterations_num) { 14 | max_iterations_num_ = max_iterations_num; 15 | } 16 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/models/loop_closure/distance_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-30 22:45:02 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-01 12:57:51 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/loop_closure/distance_detector.hpp" 11 | 12 | namespace mapping_localization { 13 | DistanceDetector::DistanceDetector(const YAML::Node& node) { 14 | loop_step_ = node["loop_step"].as(); 15 | diff_num_ = node["diff_num"].as(); 16 | detect_area_ = node["detect_area"].as(); 17 | } 18 | 19 | void DistanceDetector::addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) { 20 | historical_key_frames.push_back(key_frame); 21 | } 22 | bool DistanceDetector::DetectNearestKeyFrame(int& key_frame_index) { 23 | static int skip_cnt = 0; 24 | static int skip_num = loop_step_; 25 | if (++skip_cnt < skip_num) return false; 26 | 27 | if ((int)historical_key_frames.size() < diff_num_ + 1) return false; 28 | 29 | int key_num = (int)historical_key_frames.size(); 30 | float min_distance = 1000000.0; 31 | float distance = 0.0; 32 | 33 | KeyFrame history_key_frame; 34 | KeyFrame current_key_frame = historical_key_frames.back(); 35 | 36 | key_frame_index = -1; 37 | for (int i = 0; i < key_num - 1; ++i) { 38 | if (key_num - i < diff_num_) break; 39 | 40 | history_key_frame = historical_key_frames.at(i); 41 | distance = fabs(current_key_frame.pose(0, 3) - history_key_frame.pose(0, 3)) + 42 | fabs(current_key_frame.pose(1, 3) - history_key_frame.pose(1, 3)) + 43 | fabs(current_key_frame.pose(2, 3) - history_key_frame.pose(2, 3)); 44 | if (distance < min_distance) { 45 | min_distance = distance; 46 | key_frame_index = i; 47 | } 48 | } 49 | 50 | skip_cnt = 0; 51 | skip_num = (int)min_distance; 52 | if (min_distance > detect_area_) { 53 | // 在接下来当前最小距离的范围内可以预估不会出现回环帧，节省搜索空间 54 | skip_num = std::max((int)(min_distance / 2.0), loop_step_); 55 | return false; 56 | } else { 57 | skip_num = loop_step_; 58 | return true; 59 | } 60 | } 61 | 62 | } // namespace mapping_localization 63 | -------------------------------------------------------------------------------- /src/mapping_localization/src/models/loop_closure/scan_context_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-30 22:18:08 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-01 14:13:44 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/loop_closure/scan_context_detector.hpp" 11 | 12 | namespace mapping_localization { 13 | 14 | ScanContextDetector::ScanContextDetector(const YAML::Node& node) { 15 | unsigned int num_exclude_recent = node["num_exclude_recent"].as(); 16 | double search_ratio = node["search_ratio"].as(); 17 | double sc_dist_thres = node["sc_dist_thres"].as(); 18 | 19 | std::cout << "Scan context 使用的参数为：" 20 | << "num_exclude_recent: " << num_exclude_recent << ", search_ratio: " << search_ratio 21 | << ", sc_dist_thres: " << sc_dist_thres << std::endl; 22 | sc_manager_.setParameters(num_exclude_recent, search_ratio, sc_dist_thres); 23 | } 24 | 25 | void ScanContextDetector::addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) { 26 | sc_manager_.makeAndSaveScancontextAndKeys(*cloud_data); 27 | } 28 | 29 | bool ScanContextDetector::DetectNearestKeyFrame(int& key_frame_index) { 30 | auto detect_result = sc_manager_.detectLoopClosureID(); // first: nn index, second: yaw diff 31 | if (detect_result.first == -1) return false; 32 | key_frame_index = detect_result.first; 33 | return true; 34 | } 35 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/models/registration/ndt_registration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: NDT 匹配模块 3 | * @Created Date: 2020-02-08 21:46:45 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-27 19:58:39 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/models/registration/ndt_registration.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | 16 | NDTRegistration::NDTRegistration(const YAML::Node& node) 17 | : ndt_ptr_(new pcl::NormalDistributionsTransform()) { 18 | float res = node["res"].as(); 19 | float step_size = node["step_size"].as(); 20 | float trans_eps = node["trans_eps"].as(); 21 | int max_iter = node["max_iter"].as(); 22 | 23 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 24 | } 25 | 26 | NDTRegistration::NDTRegistration(float res, float step_size, float trans_eps, int max_iter) 27 | : ndt_ptr_(new pcl::NormalDistributionsTransform()) { 28 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 29 | } 30 | 31 | bool NDTRegistration::SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter) { 32 | ndt_ptr_->setResolution(res); 33 | ndt_ptr_->setStepSize(step_size); 34 | ndt_ptr_->setTransformationEpsilon(trans_eps); 35 | ndt_ptr_->setMaximumIterations(max_iter); 36 | 37 | std::cout << "NDT 的匹配参数为：" << std::endl 38 | << "res: " << res << ", " 39 | << "step_size: " << step_size << ", " 40 | << "trans_eps: " << trans_eps << ", " 41 | << "max_iter: " << max_iter << std::endl 42 | << std::endl; 43 | 44 | return true; 45 | } 46 | 47 | bool NDTRegistration::SetInputTarget(const CloudData::Cloud_Ptr& input_target) { 48 | ndt_ptr_->setInputTarget(input_target); 49 | 50 | return true; 51 | } 52 | 53 | bool NDTRegistration::ScanMatch(const CloudData::Cloud_Ptr& input_source, 54 | const Eigen::Matrix4f& predict_pose, 55 | CloudData::Cloud_Ptr& result_cloud_ptr, 56 | Eigen::Matrix4f& result_pose) { 57 | ndt_ptr_->setInputSource(input_source); 58 | ndt_ptr_->align(*result_cloud_ptr, predict_pose); 59 | result_pose = ndt_ptr_->getFinalTransformation(); 60 | 61 | return true; 62 | } 63 | 64 | float NDTRegistration::GetFitnessScore() { return ndt_ptr_->getFitnessScore(); } 65 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/publisher/cloud_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 通过ros发布点云 3 | * @Created Date: 2020-02-05 02:27:30 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-12 16:06:46 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/publisher/cloud_publisher.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | CloudPublisher::CloudPublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, size_t buff_size) 16 | : nh_(nh), frame_id_(frame_id) { 17 | publisher_ = nh_.advertise(topic_name, buff_size); 18 | } 19 | 20 | void CloudPublisher::Publish(CloudData::Cloud_Ptr& cloud_ptr_input, double time) { 21 | ros::Time ros_time(time); 22 | PublishData(cloud_ptr_input, ros_time); 23 | } 24 | 25 | void CloudPublisher::Publish(CloudData::Cloud_Ptr& cloud_ptr_input) { 26 | ros::Time time = ros::Time::now(); 27 | PublishData(cloud_ptr_input, time); 28 | } 29 | 30 | void CloudPublisher::PublishData(CloudData::Cloud_Ptr& cloud_ptr_input, ros::Time time) { 31 | sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2()); 32 | pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output); 33 | 34 | cloud_ptr_output->header.stamp = time; 35 | cloud_ptr_output->header.frame_id = frame_id_; 36 | publisher_.publish(*cloud_ptr_output); 37 | } 38 | 39 | bool CloudPublisher::HasSubscribers() { return publisher_.getNumSubscribers() != 0; } 40 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/publisher/key_frame_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 单个 key frame 信息发布 3 | * @Created Date: 2020-02-06 21:11:44 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-12 16:06:53 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/publisher/key_frame_publisher.hpp" 11 | 12 | #include 13 | 14 | namespace mapping_localization { 15 | KeyFramePublisher::KeyFramePublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, int buff_size) 16 | : nh_(nh), frame_id_(frame_id) { 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() { return publisher_.getNumSubscribers() != 0; } 43 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/publisher/key_frames_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: key frames 信息发布 3 | * @Created Date: 2020-02-06 21:11:44 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-12 16:07:01 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/publisher/key_frames_publisher.hpp" 11 | 12 | #include 13 | 14 | namespace mapping_localization { 15 | KeyFramesPublisher::KeyFramesPublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, int buff_size) 16 | : nh_(nh), frame_id_(frame_id) { 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() { return publisher_.getNumSubscribers() != 0; } 52 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/publisher/loop_pose_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发送闭环检测的相对位姿 3 | * @Created Date: 2020-02-06 21:11:44 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-12 16:07:07 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/publisher/loop_pose_publisher.hpp" 11 | 12 | #include 13 | 14 | #include "glog/logging.h" 15 | 16 | namespace mapping_localization { 17 | LoopPosePublisher::LoopPosePublisher(ros::NodeHandle& nh, std::string topic_name, std::string frame_id, int buff_size) 18 | : nh_(nh), frame_id_(frame_id) { 19 | publisher_ = nh_.advertise(topic_name, buff_size); 20 | } 21 | 22 | void LoopPosePublisher::Publish(LoopPose& loop_pose) { 23 | geometry_msgs::PoseWithCovarianceStamped pose_stamped; 24 | 25 | ros::Time ros_time(loop_pose.time); 26 | pose_stamped.header.stamp = ros_time; 27 | pose_stamped.header.frame_id = frame_id_; 28 | 29 | pose_stamped.pose.pose.position.x = loop_pose.pose(0, 3); 30 | pose_stamped.pose.pose.position.y = loop_pose.pose(1, 3); 31 | pose_stamped.pose.pose.position.z = loop_pose.pose(2, 3); 32 | 33 | Eigen::Quaternionf q = loop_pose.GetQuaternion(); 34 | pose_stamped.pose.pose.orientation.x = q.x(); 35 | pose_stamped.pose.pose.orientation.y = q.y(); 36 | pose_stamped.pose.pose.orientation.z = q.z(); 37 | pose_stamped.pose.pose.orientation.w = q.w(); 38 | 39 | pose_stamped.pose.covariance[0] = (double)loop_pose.index0; 40 | pose_stamped.pose.covariance[1] = (double)loop_pose.index1; 41 | pose_stamped.pose.covariance[2] = (double)loop_pose.confidence; 42 | 43 | publisher_.publish(pose_stamped); 44 | } 45 | 46 | bool LoopPosePublisher::HasSubscribers() { return publisher_.getNumSubscribers() != 0; } 47 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/publisher/odometry_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 里程计信息发布 3 | * @Created Date: 2020-02-06 21:11:44 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-12 16:07:12 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/publisher/odometry_publisher.hpp" 11 | 12 | namespace mapping_localization { 13 | OdometryPublisher::OdometryPublisher(ros::NodeHandle& nh, 14 | std::string topic_name, 15 | std::string base_frame_id, 16 | std::string child_frame_id, 17 | int buff_size) 18 | : nh_(nh) { 19 | publisher_ = nh_.advertise(topic_name, buff_size); 20 | odometry_.header.frame_id = base_frame_id; 21 | odometry_.child_frame_id = child_frame_id; 22 | } 23 | 24 | void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix, double time) { 25 | ros::Time ros_time(time); 26 | PublishData(transform_matrix, ros_time); 27 | } 28 | 29 | void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix) { 30 | PublishData(transform_matrix, ros::Time::now()); 31 | } 32 | 33 | void OdometryPublisher::PublishData(const Eigen::Matrix4f& transform_matrix, ros::Time time) { 34 | odometry_.header.stamp = time; 35 | 36 | // set the position 37 | odometry_.pose.pose.position.x = transform_matrix(0, 3); 38 | odometry_.pose.pose.position.y = transform_matrix(1, 3); 39 | odometry_.pose.pose.position.z = transform_matrix(2, 3); 40 | 41 | Eigen::Quaternionf q; 42 | q = transform_matrix.block<3, 3>(0, 0); 43 | q.normalize(); 44 | odometry_.pose.pose.orientation.x = q.x(); 45 | odometry_.pose.pose.orientation.y = q.y(); 46 | odometry_.pose.pose.orientation.z = q.z(); 47 | odometry_.pose.pose.orientation.w = q.w(); 48 | 49 | publisher_.publish(odometry_); 50 | } 51 | 52 | bool OdometryPublisher::HasSubscribers() { return publisher_.getNumSubscribers() != 0; } 53 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/publisher/tf_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 发布tf的类 3 | * @Created Date: 2020-03-05 15:23:26 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-12 16:07:18 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/publisher/tf_broadcaster.hpp" 11 | 12 | namespace mapping_localization { 13 | TFBroadCaster::TFBroadCaster(std::string frame_id, std::string child_frame_id) { 14 | transform_.frame_id_ = frame_id; 15 | transform_.child_frame_id_ = child_frame_id; 16 | } 17 | 18 | void TFBroadCaster::SendTransform(Eigen::Matrix4f pose, double time) { 19 | Eigen::Quaternionf q(pose.block<3, 3>(0, 0)); 20 | ros::Time ros_time(time); 21 | transform_.stamp_ = ros_time; 22 | transform_.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w())); 23 | transform_.setOrigin(tf::Vector3(pose(0, 3), pose(1, 3), pose(2, 3))); 24 | broadcaster_.sendTransform(transform_); 25 | } 26 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/sensor_data/key_frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2020-02-28 19:17:00 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-30 11:15:42 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/sensor_data/key_frame.hpp" 11 | 12 | namespace mapping_localization { 13 | Eigen::Quaternionf KeyFrame::GetQuaternion() const { 14 | Eigen::Quaternionf q; 15 | q = pose.block<3, 3>(0, 0); 16 | 17 | return q; 18 | } 19 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/sensor_data/loop_pose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2020-02-28 19:17:00 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:40:12 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/sensor_data/loop_pose.hpp" 11 | 12 | namespace mapping_localization { 13 | Eigen::Quaternionf LoopPose::GetQuaternion() { 14 | Eigen::Quaternionf q; 15 | q = pose.block<3, 3>(0, 0); 16 | 17 | return q; 18 | } 19 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/sensor_data/pose_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2020-02-28 18:50:16 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:40:26 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/sensor_data/pose_data.hpp" 11 | 12 | namespace mapping_localization { 13 | Eigen::Quaternionf PoseData::GetQuaternion() { 14 | Eigen::Quaternionf q; 15 | q = pose.block<3, 3>(0, 0); 16 | 17 | return q; 18 | } 19 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/cloud_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Created Date: 2020-02-05 02:27:30 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-29 19:34:27 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/cloud_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) : nh_(nh) { 16 | subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this); 17 | } 18 | 19 | void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) { 20 | buff_mutex_.lock(); 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 | 25 | new_cloud_data_.push_back(cloud_data); 26 | buff_mutex_.unlock(); 27 | } 28 | 29 | void CloudSubscriber::ParseData(std::deque& cloud_data_buff) { 30 | buff_mutex_.lock(); 31 | if (new_cloud_data_.size() > 0) { 32 | cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end()); 33 | new_cloud_data_.clear(); 34 | } 35 | buff_mutex_.unlock(); 36 | } 37 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/gnss_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2019-03-31 13:10:51 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:41:29 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/gnss_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, 16 | std::string topic_name, 17 | size_t buff_size) 18 | : nh_(nh) { 19 | subscriber_ = 20 | nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this); 21 | } 22 | 23 | void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) { 24 | buff_mutex_.lock(); 25 | GNSSData gnss_data; 26 | gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec(); 27 | gnss_data.latitude = nav_sat_fix_ptr->latitude; 28 | gnss_data.longitude = nav_sat_fix_ptr->longitude; 29 | gnss_data.altitude = nav_sat_fix_ptr->altitude; 30 | gnss_data.status = nav_sat_fix_ptr->status.status; 31 | gnss_data.service = nav_sat_fix_ptr->status.service; 32 | 33 | new_gnss_data_.push_back(gnss_data); 34 | buff_mutex_.unlock(); 35 | } 36 | 37 | void GNSSSubscriber::ParseData(std::deque& gnss_data_buff) { 38 | buff_mutex_.lock(); 39 | if (new_gnss_data_.size() > 0) { 40 | gnss_data_buff.insert( 41 | gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end()); 42 | new_gnss_data_.clear(); 43 | } 44 | buff_mutex_.unlock(); 45 | } 46 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/imu_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Created Date: 2019-06-14 16:44:18 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:41:51 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/imu_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | IMUSubscriber::IMUSubscriber(ros::NodeHandle& nh, 16 | std::string topic_name, 17 | size_t buff_size) 18 | : nh_(nh) { 19 | subscriber_ = 20 | nh_.subscribe(topic_name, buff_size, &IMUSubscriber::msg_callback, this); 21 | } 22 | 23 | void IMUSubscriber::msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr) { 24 | buff_mutex_.lock(); 25 | IMUData imu_data; 26 | imu_data.time = imu_msg_ptr->header.stamp.toSec(); 27 | 28 | imu_data.linear_acceleration.x = imu_msg_ptr->linear_acceleration.x; 29 | imu_data.linear_acceleration.y = imu_msg_ptr->linear_acceleration.y; 30 | imu_data.linear_acceleration.z = imu_msg_ptr->linear_acceleration.z; 31 | 32 | imu_data.angular_velocity.x = imu_msg_ptr->angular_velocity.x; 33 | imu_data.angular_velocity.y = imu_msg_ptr->angular_velocity.y; 34 | imu_data.angular_velocity.z = imu_msg_ptr->angular_velocity.z; 35 | 36 | imu_data.orientation.x = imu_msg_ptr->orientation.x; 37 | imu_data.orientation.y = imu_msg_ptr->orientation.y; 38 | imu_data.orientation.z = imu_msg_ptr->orientation.z; 39 | imu_data.orientation.w = imu_msg_ptr->orientation.w; 40 | 41 | new_imu_data_.push_back(imu_data); 42 | buff_mutex_.unlock(); 43 | } 44 | 45 | void IMUSubscriber::ParseData(std::deque& imu_data_buff) { 46 | buff_mutex_.lock(); 47 | if (new_imu_data_.size() > 0) { 48 | imu_data_buff.insert( 49 | imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end()); 50 | new_imu_data_.clear(); 51 | } 52 | buff_mutex_.unlock(); 53 | } 54 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/key_frame_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Created Date: 2019-06-14 16:44:18 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:42:07 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/key_frame_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | KeyFrameSubscriber::KeyFrameSubscriber(ros::NodeHandle& nh, 16 | std::string topic_name, 17 | size_t buff_size) 18 | : nh_(nh) { 19 | subscriber_ = 20 | nh_.subscribe(topic_name, buff_size, &KeyFrameSubscriber::msg_callback, this); 21 | } 22 | 23 | void KeyFrameSubscriber::msg_callback( 24 | const geometry_msgs::PoseWithCovarianceStampedConstPtr& key_frame_msg_ptr) { 25 | buff_mutex_.lock(); 26 | KeyFrame key_frame; 27 | key_frame.time = key_frame_msg_ptr->header.stamp.toSec(); 28 | key_frame.index = (unsigned int)key_frame_msg_ptr->pose.covariance[0]; 29 | 30 | key_frame.pose(0, 3) = key_frame_msg_ptr->pose.pose.position.x; 31 | key_frame.pose(1, 3) = key_frame_msg_ptr->pose.pose.position.y; 32 | key_frame.pose(2, 3) = key_frame_msg_ptr->pose.pose.position.z; 33 | 34 | Eigen::Quaternionf q; 35 | q.x() = key_frame_msg_ptr->pose.pose.orientation.x; 36 | q.y() = key_frame_msg_ptr->pose.pose.orientation.y; 37 | q.z() = key_frame_msg_ptr->pose.pose.orientation.z; 38 | q.w() = key_frame_msg_ptr->pose.pose.orientation.w; 39 | key_frame.pose.block<3, 3>(0, 0) = q.matrix(); 40 | 41 | new_key_frame_.push_back(key_frame); 42 | buff_mutex_.unlock(); 43 | } 44 | 45 | void KeyFrameSubscriber::ParseData(std::deque& key_frame_buff) { 46 | buff_mutex_.lock(); 47 | if (new_key_frame_.size() > 0) { 48 | key_frame_buff.insert( 49 | key_frame_buff.end(), new_key_frame_.begin(), new_key_frame_.end()); 50 | new_key_frame_.clear(); 51 | } 52 | buff_mutex_.unlock(); 53 | } 54 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/key_frames_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 key frame 数据 3 | * @Created Date: 2019-06-14 16:44:18 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:42:24 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/key_frames_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | KeyFramesSubscriber::KeyFramesSubscriber(ros::NodeHandle& nh, 16 | std::string topic_name, 17 | size_t buff_size) 18 | : nh_(nh) { 19 | subscriber_ = 20 | nh_.subscribe(topic_name, buff_size, &KeyFramesSubscriber::msg_callback, this); 21 | } 22 | 23 | void KeyFramesSubscriber::msg_callback( 24 | const nav_msgs::Path::ConstPtr& key_frames_msg_ptr) { 25 | buff_mutex_.lock(); 26 | new_key_frames_.clear(); 27 | 28 | for (size_t i = 0; i < key_frames_msg_ptr->poses.size(); i++) { 29 | KeyFrame key_frame; 30 | key_frame.time = key_frames_msg_ptr->poses.at(i).header.stamp.toSec(); 31 | key_frame.index = (unsigned int)i; 32 | 33 | key_frame.pose(0, 3) = key_frames_msg_ptr->poses.at(i).pose.position.x; 34 | key_frame.pose(1, 3) = key_frames_msg_ptr->poses.at(i).pose.position.y; 35 | key_frame.pose(2, 3) = key_frames_msg_ptr->poses.at(i).pose.position.z; 36 | 37 | Eigen::Quaternionf q; 38 | q.x() = key_frames_msg_ptr->poses.at(i).pose.orientation.x; 39 | q.y() = key_frames_msg_ptr->poses.at(i).pose.orientation.y; 40 | q.z() = key_frames_msg_ptr->poses.at(i).pose.orientation.z; 41 | q.w() = key_frames_msg_ptr->poses.at(i).pose.orientation.w; 42 | key_frame.pose.block<3, 3>(0, 0) = q.matrix(); 43 | 44 | new_key_frames_.push_back(key_frame); 45 | } 46 | buff_mutex_.unlock(); 47 | } 48 | 49 | void KeyFramesSubscriber::ParseData(std::deque& key_frames_buff) { 50 | buff_mutex_.lock(); 51 | if (new_key_frames_.size() > 0) { 52 | key_frames_buff = new_key_frames_; 53 | new_key_frames_.clear(); 54 | } 55 | buff_mutex_.unlock(); 56 | } 57 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/loop_pose_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅 loop pose 数据 3 | * @Created Date: 2019-06-14 16:44:18 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-30 20:26:48 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/loop_pose_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | LoopPoseSubscriber::LoopPoseSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) : nh_(nh) { 16 | subscriber_ = nh_.subscribe(topic_name, buff_size, &LoopPoseSubscriber::msg_callback, this); 17 | } 18 | 19 | void LoopPoseSubscriber::msg_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& loop_pose_msg_ptr) { 20 | buff_mutex_.lock(); 21 | LoopPose loop_pose; 22 | loop_pose.time = loop_pose_msg_ptr->header.stamp.toSec(); 23 | loop_pose.index0 = (unsigned int)loop_pose_msg_ptr->pose.covariance[0]; 24 | loop_pose.index1 = (unsigned int)loop_pose_msg_ptr->pose.covariance[1]; 25 | loop_pose.confidence = (bool)loop_pose_msg_ptr->pose.covariance[2]; 26 | 27 | loop_pose.pose(0, 3) = loop_pose_msg_ptr->pose.pose.position.x; 28 | loop_pose.pose(1, 3) = loop_pose_msg_ptr->pose.pose.position.y; 29 | loop_pose.pose(2, 3) = loop_pose_msg_ptr->pose.pose.position.z; 30 | 31 | Eigen::Quaternionf q; 32 | q.x() = loop_pose_msg_ptr->pose.pose.orientation.x; 33 | q.y() = loop_pose_msg_ptr->pose.pose.orientation.y; 34 | q.z() = loop_pose_msg_ptr->pose.pose.orientation.z; 35 | q.w() = loop_pose_msg_ptr->pose.pose.orientation.w; 36 | loop_pose.pose.block<3, 3>(0, 0) = q.matrix(); 37 | 38 | new_loop_pose_.push_back(loop_pose); 39 | buff_mutex_.unlock(); 40 | } 41 | 42 | void LoopPoseSubscriber::ParseData(std::deque& loop_pose_buff) { 43 | buff_mutex_.lock(); 44 | if (new_loop_pose_.size() > 0) { 45 | loop_pose_buff.insert(loop_pose_buff.end(), new_loop_pose_.begin(), new_loop_pose_.end()); 46 | new_loop_pose_.clear(); 47 | } 48 | buff_mutex_.unlock(); 49 | } 50 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/odometry_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅odometry数据 3 | * @Created Date: 2019-06-14 16:44:18 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:43:00 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/odometry_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | OdometrySubscriber::OdometrySubscriber(ros::NodeHandle& nh, 16 | std::string topic_name, 17 | size_t buff_size) 18 | : nh_(nh) { 19 | subscriber_ = 20 | nh_.subscribe(topic_name, buff_size, &OdometrySubscriber::msg_callback, this); 21 | } 22 | 23 | void OdometrySubscriber::msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr) { 24 | buff_mutex_.lock(); 25 | PoseData pose_data; 26 | pose_data.time = odom_msg_ptr->header.stamp.toSec(); 27 | 28 | // set the position 29 | pose_data.pose(0, 3) = odom_msg_ptr->pose.pose.position.x; 30 | pose_data.pose(1, 3) = odom_msg_ptr->pose.pose.position.y; 31 | pose_data.pose(2, 3) = odom_msg_ptr->pose.pose.position.z; 32 | 33 | Eigen::Quaternionf q; 34 | q.x() = odom_msg_ptr->pose.pose.orientation.x; 35 | q.y() = odom_msg_ptr->pose.pose.orientation.y; 36 | q.z() = odom_msg_ptr->pose.pose.orientation.z; 37 | q.w() = odom_msg_ptr->pose.pose.orientation.w; 38 | pose_data.pose.block<3, 3>(0, 0) = q.matrix(); 39 | 40 | new_pose_data_.push_back(pose_data); 41 | buff_mutex_.unlock(); 42 | } 43 | 44 | void OdometrySubscriber::ParseData(std::deque& pose_data_buff) { 45 | buff_mutex_.lock(); 46 | if (new_pose_data_.size() > 0) { 47 | pose_data_buff.insert( 48 | pose_data_buff.end(), new_pose_data_.begin(), new_pose_data_.end()); 49 | new_pose_data_.clear(); 50 | } 51 | buff_mutex_.unlock(); 52 | } 53 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/trajectory_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-11-30 12:11:37 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-11-30 15:30:19 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/trajectory_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | TrajectorySubscriber::TrajectorySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) : nh_(nh) { 16 | subscriber_ = nh_.subscribe(topic_name, buff_size, &TrajectorySubscriber::msg_callback, this); 17 | } 18 | 19 | void TrajectorySubscriber::msg_callback(const nav_msgs::Path::ConstPtr& key_frames_msg_ptr) { 20 | buff_mutex_.lock(); 21 | new_key_frames_.clear(); 22 | 23 | for (size_t i = 0; i < key_frames_msg_ptr->poses.size(); i++) { 24 | KeyFrame key_frame; 25 | key_frame.time = key_frames_msg_ptr->poses.at(i).header.stamp.toSec(); 26 | key_frame.index = (unsigned int)i; 27 | 28 | key_frame.pose(0, 3) = key_frames_msg_ptr->poses.at(i).pose.position.x; 29 | key_frame.pose(1, 3) = key_frames_msg_ptr->poses.at(i).pose.position.y; 30 | key_frame.pose(2, 3) = key_frames_msg_ptr->poses.at(i).pose.position.z; 31 | 32 | Eigen::Quaternionf q; 33 | q.x() = key_frames_msg_ptr->poses.at(i).pose.orientation.x; 34 | q.y() = key_frames_msg_ptr->poses.at(i).pose.orientation.y; 35 | q.z() = key_frames_msg_ptr->poses.at(i).pose.orientation.z; 36 | q.w() = key_frames_msg_ptr->poses.at(i).pose.orientation.w; 37 | key_frame.pose.block<3, 3>(0, 0) = q.matrix(); 38 | 39 | new_key_frames_.push_back(key_frame); 40 | } 41 | buff_mutex_.unlock(); 42 | } 43 | 44 | void TrajectorySubscriber::ParseData(std::deque& key_frames_buff) { 45 | buff_mutex_.lock(); 46 | if (new_key_frames_.size() > 0) { 47 | key_frames_buff = new_key_frames_; 48 | new_key_frames_.clear(); 49 | } 50 | buff_mutex_.unlock(); 51 | } 52 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/subscriber/velocity_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Created Date: 2019-06-14 16:44:18 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:43:20 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/subscriber/velocity_subscriber.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | VelocitySubscriber::VelocitySubscriber(ros::NodeHandle& nh, 16 | std::string topic_name, 17 | size_t buff_size) 18 | : nh_(nh) { 19 | subscriber_ = 20 | nh_.subscribe(topic_name, buff_size, &VelocitySubscriber::msg_callback, this); 21 | } 22 | 23 | void VelocitySubscriber::msg_callback( 24 | const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr) { 25 | buff_mutex_.lock(); 26 | VelocityData velocity_data; 27 | velocity_data.time = twist_msg_ptr->header.stamp.toSec(); 28 | 29 | velocity_data.linear_velocity.x = twist_msg_ptr->twist.linear.x; 30 | velocity_data.linear_velocity.y = twist_msg_ptr->twist.linear.y; 31 | velocity_data.linear_velocity.z = twist_msg_ptr->twist.linear.z; 32 | 33 | velocity_data.angular_velocity.x = twist_msg_ptr->twist.angular.x; 34 | velocity_data.angular_velocity.y = twist_msg_ptr->twist.angular.y; 35 | velocity_data.angular_velocity.z = twist_msg_ptr->twist.angular.z; 36 | 37 | new_velocity_data_.push_back(velocity_data); 38 | buff_mutex_.unlock(); 39 | } 40 | 41 | void VelocitySubscriber::ParseData(std::deque& velocity_data_buff) { 42 | buff_mutex_.lock(); 43 | if (new_velocity_data_.size() > 0) { 44 | velocity_data_buff.insert(velocity_data_buff.end(), 45 | new_velocity_data_.begin(), 46 | new_velocity_data_.end()); 47 | new_velocity_data_.clear(); 48 | } 49 | buff_mutex_.unlock(); 50 | } 51 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/tf_listener/tf_lisener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: tf监听模块 3 | * @Created Date: 2020-02-06 16:10:31 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-12-03 14:59:50 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include 11 | 12 | #include "mapping_localization/tf_listener/tf_listener.hpp" 13 | 14 | namespace mapping_localization { 15 | TFListener::TFListener(ros::NodeHandle& nh, 16 | std::string base_frame_id, 17 | std::string child_frame_id) 18 | : nh_(nh), base_frame_id_(base_frame_id), child_frame_id_(child_frame_id) {} 19 | 20 | bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) { 21 | try { 22 | tf::StampedTransform transform; 23 | listener_.lookupTransform( 24 | base_frame_id_, child_frame_id_, ros::Time(0), transform); 25 | TransformToMatrix(transform, transform_matrix); 26 | return true; 27 | } catch (tf::TransformException& ex) { 28 | return false; 29 | } 30 | } 31 | 32 | bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, 33 | Eigen::Matrix4f& transform_matrix) { 34 | Eigen::Translation3f tl_btol(transform.getOrigin().getX(), 35 | transform.getOrigin().getY(), 36 | transform.getOrigin().getZ()); 37 | 38 | double roll, pitch, yaw; 39 | tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll); 40 | Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX()); 41 | Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY()); 42 | Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ()); 43 | 44 | // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵 45 | transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); 46 | 47 | return true; 48 | } 49 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/tools/file_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 一些文件读写的方法 3 | * @Created Date: 2020-02-24 20:09:32 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:43:50 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/tools/file_manager.hpp" 11 | 12 | #include 13 | 14 | #include "glog/logging.h" 15 | 16 | namespace mapping_localization { 17 | bool FileManager::CreateFile(std::ofstream& ofs, std::string file_path) { 18 | ofs.close(); 19 | boost::filesystem::remove(file_path.c_str()); 20 | 21 | ofs.open(file_path.c_str(), std::ios::out); 22 | if (!ofs) { 23 | LOG(WARNING) << "无法生成文件: " << std::endl 24 | << file_path << std::endl 25 | << std::endl; 26 | return false; 27 | } 28 | 29 | return true; 30 | } 31 | 32 | bool FileManager::InitDirectory(std::string directory_path, std::string use_for) { 33 | if (boost::filesystem::is_directory(directory_path)) { 34 | boost::filesystem::remove_all(directory_path); 35 | } 36 | 37 | return CreateDirectory(directory_path, use_for); 38 | } 39 | 40 | bool FileManager::CreateDirectory(std::string directory_path, std::string use_for) { 41 | if (!boost::filesystem::is_directory(directory_path)) { 42 | boost::filesystem::create_directory(directory_path); 43 | } 44 | 45 | if (!boost::filesystem::is_directory(directory_path)) { 46 | LOG(WARNING) << "无法创建文件夹: " << std::endl 47 | << directory_path << std::endl 48 | << std::endl; 49 | return false; 50 | } 51 | 52 | std::cout << use_for << "存放地址：" << std::endl 53 | << directory_path << std::endl 54 | << std::endl; 55 | return true; 56 | } 57 | 58 | bool FileManager::CreateDirectory(std::string directory_path) { 59 | if (!boost::filesystem::is_directory(directory_path)) { 60 | boost::filesystem::create_directory(directory_path); 61 | } 62 | 63 | if (!boost::filesystem::is_directory(directory_path)) { 64 | LOG(WARNING) << "无法创建文件夹: " << std::endl 65 | << directory_path << std::endl 66 | << std::endl; 67 | return false; 68 | } 69 | 70 | return true; 71 | } 72 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/tools/print_info.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2020-03-02 23:28:54 4 | * @Author: Ren Qian 5 | * ----- 6 | * @Last Modified: 2021-11-24 00:44:03 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/tools/print_info.hpp" 11 | 12 | #include "glog/logging.h" 13 | 14 | namespace mapping_localization { 15 | void PrintInfo::PrintPose(std::string head, Eigen::Matrix4f pose) { 16 | Eigen::Affine3f aff_pose; 17 | aff_pose.matrix() = pose; 18 | float x, y, z, roll, pitch, yaw; 19 | pcl::getTranslationAndEulerAngles(aff_pose, x, y, z, roll, pitch, yaw); 20 | std::cout << head << x << "," << y << "," << z << "," << roll * 180 / M_PI << "," 21 | << pitch * 180 / M_PI << "," << yaw * 180 / M_PI << std::endl; 22 | } 23 | } // namespace mapping_localization -------------------------------------------------------------------------------- /src/mapping_localization/src/tools/utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Created Date: 2021-12-10 13:59:42 4 | * @Author: Xiaotao Guo 5 | * ----- 6 | * @Last Modified: 2021-12-13 17:51:37 7 | * @Modified By: Xiaotao Guo 8 | */ 9 | 10 | #include "mapping_localization/tools/utils.hpp" 11 | 12 | namespace mapping_localization { 13 | 14 | Eigen::MatrixXd CalculateDiagMatrix(Eigen::VectorXd noise) { 15 | Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(noise.rows(), noise.rows()); 16 | for (int i = 0; i < noise.rows(); i++) { 17 | information_matrix(i, i) /= noise(i); 18 | } 19 | return information_matrix; 20 | } 21 | 22 | Eigen::MatrixXd CalculateSqrtDiagMatrix(Eigen::VectorXd noise) { 23 | Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(noise.rows(), noise.rows()); 24 | for (int i = 0; i < noise.rows(); i++) { 25 | information_matrix(i, i) /= std::sqrt(noise(i)); 26 | } 27 | return information_matrix; 28 | } 29 | 30 | Eigen::MatrixXd CalculateSe3EdgeInformationMatrix(Eigen::VectorXd noise) { 31 | Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(6, 6); 32 | information_matrix = CalculateDiagMatrix(noise); 33 | return information_matrix; 34 | } 35 | } // namespace mapping_localization 36 | -------------------------------------------------------------------------------- /src/mapping_localization/srv/optimizeMap.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed -------------------------------------------------------------------------------- /src/mapping_localization/srv/saveMap.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed --------------------------------------------------------------------------------