├── FAST_LIO ├── .github │ └── stale.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── Log │ ├── fast_lio_time_log_analysis.m │ ├── guide.md │ └── plot.py ├── PCD │ └── 1 ├── README.md ├── config │ ├── fast_lio_relocalization_param.yaml │ └── mid360.yaml ├── doc │ ├── Fast_LIO_2.pdf │ ├── avia_scan.png │ ├── avia_scan2.png │ ├── overview_fastlio2.svg │ ├── real_exp_2.png │ ├── real_experiment2.gif │ ├── results │ │ ├── HKU_HW.png │ │ ├── HKU_LG_Indoor.png │ │ ├── HKU_MB_001.png │ │ ├── HKU_MB_002.png │ │ ├── HKU_MB_map.png │ │ ├── Screenshot from 2020-10-15 20-33-35.png │ │ ├── Screenshot from 2020-10-15 20-35-46.png │ │ ├── Screenshot from 2020-10-15 20-37-48.png │ │ ├── indoor_loam.png │ │ ├── indoor_loam_imu.png │ │ ├── indoor_our.png │ │ ├── long_corrid.png │ │ ├── uav2_path.png │ │ ├── uav2_test_map.png │ │ ├── uav_lg.png │ │ ├── uav_lg_0.png │ │ └── uav_lg_2.png │ ├── uav01.jpg │ ├── uav_ground.pdf │ ├── uav_system.png │ └── ulhkwh_fastlio.gif ├── include │ ├── Exp_mat.h │ ├── IKFoM_toolkit │ │ ├── esekfom │ │ │ ├── esekfom.hpp │ │ │ └── util.hpp │ │ └── mtk │ │ │ ├── build_manifold.hpp │ │ │ ├── src │ │ │ ├── SubManifold.hpp │ │ │ ├── mtkmath.hpp │ │ │ └── vectview.hpp │ │ │ ├── startIdx.hpp │ │ │ └── types │ │ │ ├── S2.hpp │ │ │ ├── SOn.hpp │ │ │ ├── vect.hpp │ │ │ └── wrapped_cv_mat.hpp │ ├── common_lib.h │ ├── ikd-Tree │ │ ├── README.md │ │ ├── ikd_Tree.cpp │ │ └── ikd_Tree.h │ ├── matplotlibcpp.h │ ├── so3_math.h │ └── use-ikfom.hpp ├── launch │ ├── mapping.launch.py │ └── relocalization.launch.py ├── msg │ └── Pose6D.msg ├── package.xml ├── rviz │ └── loam_livox.rviz └── src │ ├── IMU_Processing.hpp │ ├── laserMapping.cpp │ ├── preprocess.cpp │ └── preprocess.h ├── README.md ├── example.launch.py ├── icp_relocalization ├── CMakeLists.txt ├── README.md ├── launch │ ├── __pycache__ │ │ └── dll.launch.cpython-310.pyc │ ├── icp.launch.py │ └── sac_ia_gicp.launch.py ├── package.xml ├── result.png ├── rviz │ └── loam_livox.rviz └── src │ ├── icp_node.cpp │ ├── sac_ia_gicp.cpp │ └── transform_publisher.cpp └── reloc.gif /FAST_LIO/.github/stale.yml: -------------------------------------------------------------------------------- 1 | # Number of days of inactivity before an issue becomes stale 2 | daysUntilStale: 21 3 | # Number of days of inactivity before a stale issue is closed 4 | daysUntilClose: 1 5 | # Issues with these labels will never be considered stale 6 | exemptLabels: 7 | - pinned 8 | - security 9 | # Label to use when marking an issue as stale 10 | staleLabel: stale 11 | # Comment to post when marking an issue as stale. Set to `false` to disable 12 | markComment: > 13 | This issue has been automatically marked as stale because it has not had 14 | recent activity. It will be closed if no further activity occurs. Thank you 15 | for your contributions. 16 | # Comment to post when closing a stale issue. Set to `false` to disable 17 | closeComment: false 18 | -------------------------------------------------------------------------------- /FAST_LIO/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | Log/*.png 3 | Log/*.txt 4 | Log/*.csv 5 | Log/*.pdf 6 | .vscode/c_cpp_properties.json 7 | .vscode/settings.json 8 | PCD/*.pcd 9 | -------------------------------------------------------------------------------- /FAST_LIO/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(fast_lio) 3 | 4 | if(NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif() 7 | 8 | ADD_COMPILE_OPTIONS(-std=c++14) 9 | ADD_COMPILE_OPTIONS(-std=c++14) 10 | set(CMAKE_CXX_FLAGS "-std=c++14 -O3") 11 | 12 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 13 | 14 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions") 15 | set(CMAKE_CXX_STANDARD 14) 16 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 17 | set(CMAKE_CXX_EXTENSIONS OFF) 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 19 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 20 | 21 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 22 | 23 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)") 24 | include(ProcessorCount) 25 | ProcessorCount(N) 26 | message("Processer number: ${N}") 27 | 28 | if(N GREATER 4) 29 | add_definitions(-DMP_EN) 30 | add_definitions(-DMP_PROC_NUM=3) 31 | message("core for MP: 3") 32 | elseif(N GREATER 3) 33 | add_definitions(-DMP_EN) 34 | add_definitions(-DMP_PROC_NUM=2) 35 | message("core for MP: 2") 36 | else() 37 | add_definitions(-DMP_PROC_NUM=1) 38 | endif() 39 | else() 40 | add_definitions(-DMP_PROC_NUM=1) 41 | endif() 42 | 43 | find_package(OpenMP QUIET) 44 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 45 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 46 | 47 | find_package(PythonLibs REQUIRED) 48 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") 49 | 50 | # ROS dependencies 51 | find_package(ament_cmake REQUIRED) 52 | find_package(rclcpp REQUIRED) 53 | find_package(rclcpp_components REQUIRED) 54 | find_package(geometry_msgs REQUIRED) 55 | find_package(nav_msgs REQUIRED) 56 | find_package(sensor_msgs REQUIRED) 57 | find_package(std_msgs REQUIRED) 58 | find_package(std_srvs REQUIRED) 59 | find_package(visualization_msgs REQUIRED) 60 | find_package(pcl_ros REQUIRED) 61 | find_package(pcl_conversions REQUIRED) 62 | find_package(livox_ros_driver2 REQUIRED) 63 | find_package(rosidl_default_generators REQUIRED) 64 | 65 | set(dependencies 66 | rclcpp 67 | rclcpp_components 68 | geometry_msgs 69 | nav_msgs 70 | sensor_msgs 71 | std_msgs 72 | std_srvs 73 | visualization_msgs 74 | pcl_ros 75 | pcl_conversions 76 | livox_ros_driver2 77 | ) 78 | 79 | # Thirdparty libraries 80 | find_package(Eigen3 REQUIRED) 81 | find_package(PCL REQUIRED COMPONENTS common io) 82 | 83 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 84 | message(STATUS "PCL: ${PCL_INCLUDE_DIRS}") 85 | 86 | set(msg_files 87 | "msg/Pose6D.msg" 88 | ) 89 | 90 | rosidl_generate_interfaces(${PROJECT_NAME} 91 | ${msg_files} 92 | ) 93 | ament_export_dependencies(rosidl_default_runtime) 94 | 95 | add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) 96 | target_include_directories(fastlio_mapping PUBLIC 97 | $ 98 | $ 99 | ${PCL_INCLUDE_DIRS} 100 | ) 101 | target_link_libraries(fastlio_mapping ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} Eigen3::Eigen) 102 | target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) 103 | 104 | list(APPEND EOL_LIST "foxy" "galactic" "eloquent" "dashing" "crystal") 105 | 106 | if($ENV{ROS_DISTRO} IN_LIST EOL_LIST) 107 | # Custommsg to support foxy & galactic 108 | rosidl_target_interfaces(fastlio_mapping 109 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 110 | else() 111 | rosidl_get_typesupport_target(cpp_typesupport_target 112 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 113 | target_link_libraries(fastlio_mapping ${cpp_typesupport_target}) 114 | endif() 115 | 116 | ament_target_dependencies(fastlio_mapping ${dependencies}) 117 | 118 | # ---------------- Install --------------- # 119 | install(TARGETS fastlio_mapping 120 | DESTINATION lib/${PROJECT_NAME} 121 | ) 122 | 123 | install( 124 | DIRECTORY config launch rviz 125 | DESTINATION share/${PROJECT_NAME} 126 | ) 127 | 128 | ament_package() -------------------------------------------------------------------------------- /FAST_LIO/Log/fast_lio_time_log_analysis.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | 4 | Color_red = [0.6350 0.0780 0.1840]; 5 | Color_blue = [0 0.4470 0.7410]; 6 | Color_orange = [0.8500 0.3250 0.0980]; 7 | Color_green = [0.4660 0.6740 0.1880]; 8 | Color_lightblue = [0.3010 0.7450 0.9330]; 9 | Color_purple = [0.4940 0.1840 0.5560]; 10 | Color_yellow = [0.9290 0.6940 0.1250]; 11 | 12 | fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0); 13 | timestamp_ikd = fast_lio_ikdtree(:,1); 14 | timestamp_ikd = timestamp_ikd - min(timestamp_ikd); 15 | total_time_ikd = fast_lio_ikdtree(:,2)*1e3; 16 | scan_num = fast_lio_ikdtree(:,3); 17 | incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3; 18 | search_time_ikd = fast_lio_ikdtree(:,5)*1e3; 19 | delete_size_ikd = fast_lio_ikdtree(:,6); 20 | delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3; 21 | tree_size_ikd_st = fast_lio_ikdtree(:,8); 22 | tree_size_ikd = fast_lio_ikdtree(:,9); 23 | add_points = fast_lio_ikdtree(:,10); 24 | 25 | fast_lio_forest = csvread("fast_lio_time_log.csv",1,0); 26 | fov_check_time_forest = fast_lio_forest(:,5)*1e3; 27 | average_time_forest = fast_lio_forest(:,2)*1e3; 28 | total_time_forest = fast_lio_forest(:,6)*1e3; 29 | incremental_time_forest = fast_lio_forest(:,3)*1e3; 30 | search_time_forest = fast_lio_forest(:,4)*1e3; 31 | timestamp_forest = fast_lio_forest(:,1); 32 | 33 | % Use slide window to calculate average 34 | L = 1; % Length of slide window 35 | for i = 1:length(timestamp_ikd) 36 | if (i 0); 78 | search_time_ikd = search_time_ikd(index_ikd); 79 | index_forest = find(search_time_forest > 0); 80 | search_time_forest = search_time_forest(index_forest); 81 | 82 | t = nexttile; 83 | hold on; 84 | boxplot_data_ikd = [incremental_time_ikd,total_time_ikd]; 85 | boxplot_data_forest = [incremental_time_forest,total_time_forest]; 86 | Colors_ikd = [Color_blue;Color_blue;Color_blue]; 87 | Colors_forest = [Color_orange;Color_orange;Color_orange]; 88 | % xticks([3,8,13]) 89 | h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3); 90 | h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3); 91 | h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3); 92 | h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3); 93 | ax2 = gca; 94 | ax2.YAxis.Scale = 'log'; 95 | xlim([0.5,6.0]) 96 | ylim([0.0008,100]) 97 | xticks([1.25 3.25 5.25]) 98 | xticklabels({'Nearest Search',' Incremental Updates','Total Time'}); 99 | yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2]) 100 | ax2.YAxis.FontSize = 12; 101 | ax2.XAxis.FontSize = 14.5; 102 | % ax.XAxis.FontWeight = 'bold'; 103 | ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman') 104 | box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')]; 105 | for j=1:length(box_vars) 106 | if (j<=3) 107 | Color = Color_blue; 108 | else 109 | Color = Color_orange; 110 | end 111 | patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color); 112 | end 113 | Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman'); 114 | grid on 115 | set(gca,'YMinorGrid','off') 116 | nexttile; 117 | hold on; 118 | grid on; 119 | box on; 120 | set(gca,'FontSize',12,'FontName','Times New Roman') 121 | plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2); 122 | plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2); 123 | plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2); 124 | lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman') 125 | title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman') 126 | xlabel("time/s",'FontSize',16,'FontName','Times New Roman') 127 | yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1]) 128 | xlim([32,390]); 129 | ylim([0,0.85]); 130 | ax3 = gca; 131 | ax3.YAxis.FontSize = 12; 132 | ax3.XAxis.FontSize = 12; 133 | % print('./Figures/fastlio_exp_combine','-depsc','-r1200') 134 | % exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector') 135 | 136 | -------------------------------------------------------------------------------- /FAST_LIO/Log/guide.md: -------------------------------------------------------------------------------- 1 | Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. 2 | -------------------------------------------------------------------------------- /FAST_LIO/Log/plot.py: -------------------------------------------------------------------------------- 1 | # import matplotlib 2 | # matplotlib.use('Agg') 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | #######for ikfom 8 | fig, axs = plt.subplots(4,2) 9 | lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] 10 | lab_out = ['', 'out-x', 'out-y', 'out-z'] 11 | plot_ind = range(7,10) 12 | a_pre=np.loadtxt('mat_pre.txt') 13 | a_out=np.loadtxt('mat_out.txt') 14 | time=a_pre[:,0] 15 | axs[0,0].set_title('Attitude') 16 | axs[1,0].set_title('Translation') 17 | axs[2,0].set_title('Extrins-R') 18 | axs[3,0].set_title('Extrins-T') 19 | axs[0,1].set_title('Velocity') 20 | axs[1,1].set_title('bg') 21 | axs[2,1].set_title('ba') 22 | axs[3,1].set_title('Gravity') 23 | for i in range(1,4): 24 | for j in range(8): 25 | axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) 26 | axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) 27 | for j in range(8): 28 | # axs[j].set_xlim(386,389) 29 | axs[j%4, j/4].grid() 30 | axs[j%4, j/4].legend() 31 | plt.grid() 32 | #######for ikfom####### 33 | 34 | 35 | #### Draw IMU data 36 | # fig, axs = plt.subplots(2) 37 | # imu=np.loadtxt('imu.txt') 38 | # time=imu[:,0] 39 | # axs[0].set_title('Gyroscope') 40 | # axs[1].set_title('Accelerameter') 41 | # lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] 42 | # lab_2 = ['acc-x', 'acc-y', 'acc-z'] 43 | # for i in range(3): 44 | # # if i==1: 45 | # axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) 46 | # axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) 47 | # for i in range(2): 48 | # # axs[i].set_xlim(386,389) 49 | # axs[i].grid() 50 | # axs[i].legend() 51 | # plt.grid() 52 | 53 | # #### Draw time calculation 54 | # plt.figure(3) 55 | # fig = plt.figure() 56 | # font1 = {'family' : 'Times New Roman', 57 | # 'weight' : 'normal', 58 | # 'size' : 12, 59 | # } 60 | # c="red" 61 | # a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') 62 | # a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') 63 | # a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') 64 | # # n = a_out[:,1].size 65 | # # time_mean = a_out[:,1].mean() 66 | # # time_se = a_out[:,1].std() / np.sqrt(n) 67 | # # time_err = a_out[:,1] - time_mean 68 | # # feat_mean = a_out[:,2].mean() 69 | # # feat_err = a_out[:,2] - feat_mean 70 | # # feat_se = a_out[:,2].std() / np.sqrt(n) 71 | # ax1 = fig.add_subplot(111) 72 | # ax1.set_ylabel('Effective Feature Numbers',font1) 73 | # ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) 74 | # ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) 75 | # ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) 76 | # ax1.set_ylim([0, 3000]) 77 | 78 | # ax2 = ax1.twinx() 79 | # ax2.spines['right'].set_color('red') 80 | # ax2.set_ylabel('Compute Time (ms)',font1) 81 | # ax2.yaxis.label.set_color('red') 82 | # ax2.tick_params(axis='y', colors='red') 83 | # ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 84 | # ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 85 | # ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 86 | # ax2.set_xlim([0.5, 3.5]) 87 | # ax2.set_ylim([0, 100]) 88 | 89 | # plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) 90 | # # # print(time_se) 91 | # # # print(a_out3[:,2]) 92 | # plt.grid() 93 | # plt.savefig("time.pdf", dpi=1200) 94 | plt.show() 95 | -------------------------------------------------------------------------------- /FAST_LIO/PCD/1: -------------------------------------------------------------------------------- 1 | 1 2 | -------------------------------------------------------------------------------- /FAST_LIO/README.md: -------------------------------------------------------------------------------- 1 | > ROS2 Fork repo maintainer: [Ericsiii](https://github.com/Ericsii) 2 | 3 | ## Related Works and Extended Application 4 | 5 | **SLAM:** 6 | 7 | 1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search. 8 | 2. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end. 9 | 3. [LI_Init](https://github.com/hku-mars/LiDAR_IMU_Init): A robust, real-time LiDAR-IMU extrinsic initialization and synchronization package.. 10 | 4. [FAST-LIO-LOCALIZATION](https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION): The integration of FAST-LIO with **Re-localization** function module. 11 | 12 | **Control and Plan:** 13 | 14 | 1. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter. 15 | 2. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning. 16 | 3. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds. 17 | 4. [Bubble Planner](https://arxiv.org/abs/2202.12177): Planning High-speed Smooth Quadrotor Trajectories using Receding Corridors. 18 | 19 | 20 | 21 | ## FAST-LIO 22 | **FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues: 23 | 1. Fast iterated Kalman filter for odometry optimization; 24 | 2. Automaticaly initialized at most steady environments; 25 | 3. Parallel KD-Tree Search to decrease the computation; 26 | 27 | ## FAST-LIO 2.0 (2021-07-05 Update) 28 | 29 | 30 |
31 | 32 | 33 |
34 | 35 | **Related video:** [FAST-LIO2](https://youtu.be/2OvjGnxszf8), [FAST-LIO1](https://youtu.be/iYCY6T79oNU) 36 | 37 | **Pipeline:** 38 |
39 | 40 |
41 | 42 | **New Features:** 43 | 1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate. 44 | 2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy. 45 | 3. Since no requirements for feature extraction, FAST-LIO2 can support many types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs. 46 | 4. Support external IMU. 47 | 5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM). 48 | 49 | **Related papers**: 50 | 51 | [FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf) 52 | 53 | [FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196) 54 | 55 | **Contributors** 56 | 57 | [Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan) 58 | 59 | 63 | 64 | ## 1. Prerequisites 65 | ### 1.1 **Ubuntu** and **ROS** 66 | **Ubuntu >= 20.04** 67 | 68 | The **default from apt** PCL and Eigen is enough for FAST-LIO to work normally. 69 | 70 | ROS >= Foxy (Recommend to use ROS-Humble). [ROS Installation](https://docs.ros.org/en/humble/Installation.html) 71 | 72 | ### 1.2. **PCL && Eigen** 73 | PCL >= 1.8, Follow [PCL Installation](https://pointclouds.org/downloads/#linux). 74 | 75 | Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). 76 | 77 | ### 1.3. **livox_ros_driver2** 78 | Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2). 79 | 80 | You can also use the one I modified [livox_ros_driver2](https://github.com/Ericsii/livox_ros_driver2/tree/feature/use-standard-unit) 81 | 82 | *Remarks:* 83 | - Since the FAST-LIO must support Livox serials LiDAR firstly, so the **livox_ros_driver** must be installed and **sourced** before run any FAST-LIO launch file. 84 | - How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document). 85 | 86 | 87 | ## 2. Build 88 | Clone the repository and colcon build: 89 | 90 | ```bash 91 | cd /src # cd into a ros2 workspace folder 92 | git clone https://github.com/Ericsii/FAST_LIO.git --recursive 93 | cd .. 94 | rosdep install --from-paths src --ignore-src -y 95 | colcon build --symlink-install 96 | . ./install/setup.bash # use setup.zsh if use zsh 97 | ``` 98 | - **Remember to source the livox_ros_driver before build (follow [1.3 livox_ros_driver](#1.3))** 99 | - If you want to use a custom build of PCL, add the following line to ~/.bashrc 100 | ```export PCL_ROOT={CUSTOM_PCL_PATH}``` 101 | ## 3. Directly run 102 | Noted: 103 | 104 | A. Please make sure the IMU and LiDAR are **Synchronized**, that's important. 105 | 106 | B. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important for the forward propagation and backwark propagation. 107 | 108 | C. We recommend to set the **extrinsic_est_en** to false if the extrinsic is give. As for the extrinsic initiallization, please refer to our recent work: [**Robust Real-time LiDAR-inertial Initialization**](https://github.com/hku-mars/LiDAR_IMU_Init). 109 | 110 | ### 3.1 Run use ros launch 111 | Connect to your PC to Livox LiDAR by following [Livox-ros-driver2 installation](https://github.com/Livox-SDK/livox_ros_driver2), then 112 | ```bash 113 | cd 114 | . install/setup.bash # use setup.zsh if use zsh 115 | ros2 launch fast_lio mapping.launch.py config_file:=avia.yaml 116 | ``` 117 | 118 | Change `config_file` parameter to other yaml file under config directory as you need. 119 | 120 | Launch livox ros driver. Use MID360 as an example. 121 | 122 | ```bash 123 | ros2 launch livox_ros_driver2 msg_MID360_launch.py 124 | ``` 125 | 126 | - For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver2/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. ``` livox_lidar.launch ``` can not produce it right now. 127 | - If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver2/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver2) before make the livox_ros_driver pakage. 128 | 129 | ### 3.2 For Livox serials with external IMU 130 | 131 | mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run: 132 | 133 | Edit ``` config/avia.yaml ``` to set the below parameters: 134 | 135 | 1. LiDAR point cloud topic name: ``` lid_topic ``` 136 | 2. IMU topic name: ``` imu_topic ``` 137 | 3. Translational extrinsic: ``` extrinsic_T ``` 138 | 4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) 139 | - The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual. 140 | - FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter ```time_sync_en``` to ture to turn on. But turn on **ONLY IF external time synchronization is really not possible**, since the software time sync cannot make sure accuracy. 141 | 142 | ### 3.4 PCD file save 143 | 144 | Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds. 145 | 146 | *Tips for pcl_viewer:* 147 | - change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. 148 | ``` 149 | 1 is all random 150 | 2 is X values 151 | 3 is Y values 152 | 4 is Z values 153 | 5 is intensity 154 | ``` 155 | 156 | ## 4. Rosbag Example 157 | ### 4.1 Livox Avia Rosbag 158 |
159 | 160 | 161 | 162 | Files: Can be downloaded from [google drive](https://drive.google.com/drive/folders/1CGYEJ9-wWjr8INyan6q1BZz_5VtGB-fP?usp=sharing)**!!!This ros1 bag should be convert to ros2!!!** 163 | 164 | Run: 165 | ```bash 166 | ros2 launch fast_lio mapping.launch.py config_path:= 167 | ros2 bag play 168 | 169 | ``` 170 | 171 | ### 4.2 Velodyne HDL-32E Rosbag 172 | 173 | **NCLT Dataset**: Original bin file can be found [here](http://robots.engin.umich.edu/nclt/). 174 | 175 | We produce [Rosbag Files](https://drive.google.com/drive/folders/1VBK5idI1oyW0GC_I_Hxh63aqam3nocNK?usp=sharing) and [a python script](https://drive.google.com/file/d/1leh7DxbHx29DyS1NJkvEfeNJoccxH7XM/view) to generate Rosbag files: ```python3 sensordata_to_rosbag_fastlio.py bin_file_dir bag_name.bag```**!!!This ros1 bag should be convert to ros2!!!** To convert ros1 bag to ros2 bag, please follow the documentation [Convert rosbag versions](https://ternaris.gitlab.io/rosbags/topics/convert.html) 176 | 177 | Run: 178 | ``` 179 | roslaunch fast_lio mapping_velodyne.launch 180 | rosbag play YOUR_DOWNLOADED.bag 181 | ``` 182 | 183 | ## 5.Implementation on UAV 184 | In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below. 185 | 186 | The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future. 187 | 188 |
189 | 190 | 191 |
192 | 193 | ## 6.Acknowledgments 194 | 195 | Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox). 196 | -------------------------------------------------------------------------------- /FAST_LIO/config/fast_lio_relocalization_param.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | locate_in_prior_map: true 4 | prior_map_path: "/home/sentry_ws/src/FAST_LIO_SAM/PCD/GlobalMap.pcd" 5 | feature_extract_enable: false 6 | point_filter_num: 3 7 | max_iteration: 3 8 | filter_size_surf: 0.5 9 | filter_size_map: 0.5 10 | cube_side_length: 1000.0 11 | runtime_pos_log_enable: false 12 | map_file_path: "./test.pcd" 13 | 14 | common: 15 | odom_frame_id: "odom" 16 | sensor_frame_id: "imu_link" 17 | base_frame_id: "base_link" 18 | send_odom_base_tf: true 19 | lid_topic: "/livox/lidar" 20 | imu_topic: "/imu/data" 21 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 22 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 23 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 24 | 25 | preprocess: 26 | lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for any other pointcloud input 27 | scan_line: 4 28 | blind: 0.5 29 | timestamp_unit: 3 30 | scan_rate: 10 31 | 32 | mapping: 33 | acc_cov: 0.1 34 | gyr_cov: 0.1 35 | b_acc_cov: 0.0001 36 | b_gyr_cov: 0.0001 37 | fov_degree: 360.0 38 | det_range: 100.0 39 | extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic 40 | extrinsic_T: [ -0.011, -0.02329, 0.04412 ] 41 | extrinsic_R: [ 1., 0., 0., 42 | 0., -1., 0., 43 | 0., 0., -1.] 44 | 45 | publish: 46 | path_en: false # true: publish Path 47 | effect_map_en: false # true: publish Effects 48 | map_en: false # true: publish Map cloud 49 | scan_publish_en: true # false: close all the point cloud output 50 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 51 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 52 | 53 | pcd_save: 54 | pcd_save_en: false 55 | interval: -1 # how many LiDAR frames saved in each pcd file; 56 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. -------------------------------------------------------------------------------- /FAST_LIO/config/mid360.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | locate_in_prior_map: false 4 | prior_map_path: "/home/sentry_ws/src/FAST_LIO/PCD/CC#3.pcd" 5 | feature_extract_enable: false 6 | point_filter_num: 3 7 | max_iteration: 3 8 | filter_size_surf: 0.5 9 | filter_size_map: 0.5 10 | cube_side_length: 1000.0 11 | runtime_pos_log_enable: false 12 | map_file_path: "./test.pcd" 13 | 14 | common: 15 | odom_frame_id: "odom" 16 | sensor_frame_id: "imu_link" 17 | base_frame_id: "base_link" 18 | send_odom_base_tf: true 19 | lid_topic: "/livox/lidar" 20 | imu_topic: "/livox/imu" 21 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 22 | time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). 23 | # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 24 | 25 | preprocess: 26 | lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for any other pointcloud input 27 | scan_line: 4 28 | blind: 0.5 29 | timestamp_unit: 3 30 | scan_rate: 10 31 | 32 | mapping: 33 | acc_cov: 0.1 34 | gyr_cov: 0.1 35 | b_acc_cov: 0.0001 36 | b_gyr_cov: 0.0001 37 | fov_degree: 360.0 38 | det_range: 100.0 39 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic 40 | extrinsic_T: [ -0.011, -0.02329, 0.04412 ] 41 | extrinsic_R: [ 1., 0., 0., 42 | 0., -1., 0., 43 | 0., 0., -1.] 44 | 45 | publish: 46 | path_en: true # true: publish Path 47 | effect_map_en: true # true: publish Effects 48 | map_en: true # true: publish Map cloud 49 | scan_publish_en: true # false: close all the point cloud output 50 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 51 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 52 | 53 | pcd_save: 54 | pcd_save_en: true 55 | interval: -1 # how many LiDAR frames saved in each pcd file; 56 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 57 | -------------------------------------------------------------------------------- /FAST_LIO/doc/Fast_LIO_2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/Fast_LIO_2.pdf -------------------------------------------------------------------------------- /FAST_LIO/doc/avia_scan.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/avia_scan.png -------------------------------------------------------------------------------- /FAST_LIO/doc/avia_scan2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/avia_scan2.png -------------------------------------------------------------------------------- /FAST_LIO/doc/real_exp_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/real_exp_2.png -------------------------------------------------------------------------------- /FAST_LIO/doc/real_experiment2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/real_experiment2.gif -------------------------------------------------------------------------------- /FAST_LIO/doc/results/HKU_HW.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/HKU_HW.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/HKU_LG_Indoor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/HKU_LG_Indoor.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/HKU_MB_001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/HKU_MB_001.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/HKU_MB_002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/HKU_MB_002.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/HKU_MB_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/HKU_MB_map.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/Screenshot from 2020-10-15 20-33-35.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/Screenshot from 2020-10-15 20-33-35.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/Screenshot from 2020-10-15 20-35-46.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/Screenshot from 2020-10-15 20-35-46.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/Screenshot from 2020-10-15 20-37-48.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/Screenshot from 2020-10-15 20-37-48.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/indoor_loam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/indoor_loam.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/indoor_loam_imu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/indoor_loam_imu.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/indoor_our.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/indoor_our.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/long_corrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/long_corrid.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/uav2_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/uav2_path.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/uav2_test_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/uav2_test_map.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/uav_lg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/uav_lg.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/uav_lg_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/uav_lg_0.png -------------------------------------------------------------------------------- /FAST_LIO/doc/results/uav_lg_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/results/uav_lg_2.png -------------------------------------------------------------------------------- /FAST_LIO/doc/uav01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/uav01.jpg -------------------------------------------------------------------------------- /FAST_LIO/doc/uav_ground.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/uav_ground.pdf -------------------------------------------------------------------------------- /FAST_LIO/doc/uav_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/uav_system.png -------------------------------------------------------------------------------- /FAST_LIO/doc/ulhkwh_fastlio.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/FAST_LIO/doc/ulhkwh_fastlio.gif -------------------------------------------------------------------------------- /FAST_LIO/include/Exp_mat.h: -------------------------------------------------------------------------------- 1 | #ifndef EXP_MAT_H 2 | #define EXP_MAT_H 3 | 4 | #include 5 | #include 6 | #include 7 | // #include 8 | 9 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 10 | 11 | template 12 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 13 | { 14 | T ang_norm = ang.norm(); 15 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 16 | if (ang_norm > 0.0000001) 17 | { 18 | Eigen::Matrix r_axis = ang / ang_norm; 19 | Eigen::Matrix K; 20 | K << SKEW_SYM_MATRX(r_axis); 21 | /// Roderigous Tranformation 22 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 23 | } 24 | else 25 | { 26 | return Eye3; 27 | } 28 | } 29 | 30 | template 31 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 32 | { 33 | T ang_vel_norm = ang_vel.norm(); 34 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 35 | 36 | if (ang_vel_norm > 0.0000001) 37 | { 38 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 39 | Eigen::Matrix K; 40 | 41 | K << SKEW_SYM_MATRX(r_axis); 42 | 43 | T r_ang = ang_vel_norm * dt; 44 | 45 | /// Roderigous Tranformation 46 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 47 | } 48 | else 49 | { 50 | return Eye3; 51 | } 52 | } 53 | 54 | template 55 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 56 | { 57 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 58 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 59 | if (norm > 0.00001) 60 | { 61 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 62 | Eigen::Matrix K; 63 | K << SKEW_SYM_MATRX(r_ang); 64 | 65 | /// Roderigous Tranformation 66 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 67 | } 68 | else 69 | { 70 | return Eye3; 71 | } 72 | } 73 | 74 | /* Logrithm of a Rotation Matrix */ 75 | template 76 | Eigen::Matrix Log(const Eigen::Matrix &R) 77 | { 78 | T &&theta = std::acos(0.5 * (R.trace() - 1)); 79 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 80 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 81 | } 82 | 83 | // template 84 | // cv::Mat Exp(const T &v1, const T &v2, const T &v3) 85 | // { 86 | 87 | // T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 88 | // cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); 89 | // if (norm > 0.0000001) 90 | // { 91 | // T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 92 | // cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); 93 | 94 | // /// Roderigous Tranformation 95 | // return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 96 | // } 97 | // else 98 | // { 99 | // return Eye3; 100 | // } 101 | // } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019--2023, The University of Hong Kong 3 | * All rights reserved. 4 | * 5 | * Author: Dongjiao HE 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Universitaet Bremen nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef __MEKFOM_UTIL_HPP__ 36 | #define __MEKFOM_UTIL_HPP__ 37 | 38 | #include 39 | #include "../mtk/src/mtkmath.hpp" 40 | namespace esekfom { 41 | 42 | template 43 | class is_same { 44 | public: 45 | operator bool() { 46 | return false; 47 | } 48 | }; 49 | template 50 | class is_same { 51 | public: 52 | operator bool() { 53 | return true; 54 | } 55 | }; 56 | 57 | template 58 | class is_double { 59 | public: 60 | operator bool() { 61 | return false; 62 | } 63 | }; 64 | 65 | template<> 66 | class is_double { 67 | public: 68 | operator bool() { 69 | return true; 70 | } 71 | }; 72 | 73 | template 74 | static T 75 | id(const T &x) 76 | { 77 | return x; 78 | } 79 | 80 | } // namespace esekfom 81 | 82 | #endif // __MEKFOM_UTIL_HPP__ 83 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/build_manifold.hpp 75 | * @brief Macro to automatically construct compound manifolds. 76 | * 77 | */ 78 | #ifndef MTK_AUTOCONSTRUCT_HPP_ 79 | #define MTK_AUTOCONSTRUCT_HPP_ 80 | 81 | #include 82 | 83 | #include 84 | #include 85 | #include 86 | 87 | #include "src/SubManifold.hpp" 88 | #include "startIdx.hpp" 89 | 90 | #ifndef PARSED_BY_DOXYGEN 91 | //////// internals ////// 92 | 93 | #define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple 94 | 95 | #define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) 96 | 97 | #define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) 98 | 99 | #define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() 100 | #define MTK_CONSTRUCTOR_COPY( type, id) id(id) 101 | #define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); 102 | #define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); 103 | #define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); 104 | #define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} 105 | #define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} 106 | #define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} 107 | #define MTK_OSTREAM( type, id) << __var.id << " " 108 | #define MTK_ISTREAM( type, id) >> __var.id 109 | #define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} 110 | #define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} 111 | #define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} 112 | 113 | #define MTK_SUBVARLIST(seq, S2state, SO3state) \ 114 | BOOST_PP_FOR_1( \ 115 | ( \ 116 | BOOST_PP_SEQ_SIZE(seq), \ 117 | BOOST_PP_SEQ_HEAD(seq), \ 118 | BOOST_PP_SEQ_TAIL(seq) (~), \ 119 | 0,\ 120 | 0,\ 121 | S2state,\ 122 | SO3state ),\ 123 | MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) 124 | 125 | #define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ 126 | MTK::SubManifold id; 127 | #define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \ 128 | MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ 129 | enum {DOF = type::DOF + dof}; \ 130 | enum {DIM = type::DIM+dim}; \ 131 | typedef type::scalar scalar; 132 | 133 | #define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state 134 | #define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \ 135 | MTK_APPLY_MACRO_ON_TUPLE(~, \ 136 | BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ 137 | ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) 138 | 139 | #define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state 140 | 141 | //! this used to be BOOST_PP_TUPLE_ELEM_4_0: 142 | #define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a 143 | 144 | #define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state 145 | #define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \ 146 | BOOST_PP_DEC(len), \ 147 | BOOST_PP_SEQ_HEAD(seq), \ 148 | BOOST_PP_SEQ_TAIL(seq), \ 149 | dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ 150 | dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ 151 | S2state,\ 152 | SO3state) 153 | 154 | #endif /* not PARSED_BY_DOXYGEN */ 155 | 156 | 157 | /** 158 | * Construct a manifold. 159 | * @param name is the class-name of the manifold, 160 | * @param entries is the list of sub manifolds 161 | * 162 | * Entries must be given in a list like this: 163 | * @code 164 | * typedef MTK::trafo > Pose; 165 | * typedef MTK::vect Vec3; 166 | * MTK_BUILD_MANIFOLD(imu_state, 167 | * ((Pose, pose)) 168 | * ((Vec3, vel)) 169 | * ((Vec3, acc_bias)) 170 | * ) 171 | * @endcode 172 | * Whitespace is optional, but the double parentheses are necessary. 173 | * Construction is done entirely in preprocessor. 174 | * After construction @a name is also a manifold. Its members can be 175 | * accessed by names given in @a entries. 176 | * 177 | * @note Variable types are not allowed to have commas, thus types like 178 | * @c vect need to be typedef'ed ahead. 179 | */ 180 | #define MTK_BUILD_MANIFOLD(name, entries) \ 181 | struct name { \ 182 | typedef name self; \ 183 | std::vector > S2_state;\ 184 | std::vector > SO3_state;\ 185 | std::vector, int> > vect_state;\ 186 | MTK_SUBVARLIST(entries, S2_state, SO3_state) \ 187 | name ( \ 188 | MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ 189 | ) : \ 190 | MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ 191 | int getDOF() const { return DOF; } \ 192 | void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ 193 | MTK_TRANSFORM(MTK_BOXPLUS, entries)\ 194 | } \ 195 | void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ 196 | MTK_TRANSFORM(MTK_OPLUS, entries)\ 197 | } \ 198 | void boxminus(MTK::vectview __res, const name& __oth) const { \ 199 | MTK_TRANSFORM(MTK_BOXMINUS, entries)\ 200 | } \ 201 | friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ 202 | return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ 203 | } \ 204 | void build_S2_state(){\ 205 | MTK_TRANSFORM(MTK_S2_state, entries)\ 206 | }\ 207 | void build_vect_state(){\ 208 | MTK_TRANSFORM(MTK_vect_state, entries)\ 209 | }\ 210 | void build_SO3_state(){\ 211 | MTK_TRANSFORM(MTK_SO3_state, entries)\ 212 | }\ 213 | void S2_hat(Eigen::Matrix &res, int idx) {\ 214 | MTK_TRANSFORM(MTK_S2_hat, entries)\ 215 | }\ 216 | void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ 217 | MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ 218 | }\ 219 | void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ 220 | MTK_TRANSFORM(MTK_S2_Mx, entries)\ 221 | }\ 222 | friend std::istream& operator>>(std::istream& __is, name& __var){ \ 223 | return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ 224 | } \ 225 | }; 226 | 227 | 228 | 229 | #endif /*MTK_AUTOCONSTRUCT_HPP_*/ 230 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/src/SubManifold.hpp 75 | * @brief Defines the SubManifold class 76 | */ 77 | 78 | 79 | #ifndef SUBMANIFOLD_HPP_ 80 | #define SUBMANIFOLD_HPP_ 81 | 82 | 83 | #include "vectview.hpp" 84 | 85 | 86 | namespace MTK { 87 | 88 | /** 89 | * @ingroup SubManifolds 90 | * Helper class for compound manifolds. 91 | * This class wraps a manifold T and provides an enum IDX refering to the 92 | * index of the SubManifold within the compound manifold. 93 | * 94 | * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". 95 | * 96 | * @tparam T The manifold type of the sub-type 97 | * @tparam idx The index of the sub-type within the compound manifold 98 | */ 99 | template 100 | struct SubManifold : public T 101 | { 102 | enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; 103 | //! manifold type 104 | typedef T type; 105 | 106 | //! Construct from derived type 107 | template 108 | explicit 109 | SubManifold(const X& t) : T(t) {}; 110 | 111 | //! Construct from internal type 112 | //explicit 113 | SubManifold(const T& t) : T(t) {}; 114 | 115 | //! inherit assignment operator 116 | using T::operator=; 117 | 118 | }; 119 | 120 | } // namespace MTK 121 | 122 | 123 | #endif /* SUBMANIFOLD_HPP_ */ 124 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/src/mtkmath.hpp 75 | * @brief several math utility functions. 76 | */ 77 | 78 | #ifndef MTKMATH_H_ 79 | #define MTKMATH_H_ 80 | 81 | #include 82 | 83 | #include 84 | 85 | #include "../types/vect.hpp" 86 | 87 | #ifndef M_PI 88 | #define M_PI 3.1415926535897932384626433832795 89 | #endif 90 | 91 | 92 | namespace MTK { 93 | 94 | namespace internal { 95 | 96 | template 97 | struct traits { 98 | typedef typename Manifold::scalar scalar; 99 | enum {DOF = Manifold::DOF}; 100 | typedef vect vectorized_type; 101 | typedef Eigen::Matrix matrix_type; 102 | }; 103 | 104 | template<> 105 | struct traits : traits > {}; 106 | template<> 107 | struct traits : traits > {}; 108 | 109 | } // namespace internal 110 | 111 | /** 112 | * \defgroup MTKMath Mathematical helper functions 113 | */ 114 | //@{ 115 | 116 | //! constant @f$ \pi @f$ 117 | const double pi = M_PI; 118 | 119 | template inline scalar tolerance(); 120 | 121 | template<> inline float tolerance() { return 1e-5f; } 122 | template<> inline double tolerance() { return 1e-11; } 123 | 124 | 125 | /** 126 | * normalize @a x to @f$[-bound, bound] @f$. 127 | * 128 | * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. 129 | */ 130 | template 131 | inline scalar normalize(scalar x, scalar bound){ //not used 132 | if(std::fabs(x) <= bound) return x; 133 | int r = (int)(x *(scalar(1.0)/ bound)); 134 | return x - ((r + (r>>31) + 1) & ~1)*bound; 135 | } 136 | 137 | /** 138 | * Calculate cosine and sinc of sqrt(x2). 139 | * @param x2 the squared angle must be non-negative 140 | * @return a pair containing cos and sinc of sqrt(x2) 141 | */ 142 | template 143 | std::pair cos_sinc_sqrt(const scalar &x2){ 144 | using std::sqrt; 145 | using std::cos; 146 | using std::sin; 147 | static scalar const taylor_0_bound = boost::math::tools::epsilon(); 148 | static scalar const taylor_2_bound = sqrt(taylor_0_bound); 149 | static scalar const taylor_n_bound = sqrt(taylor_2_bound); 150 | 151 | assert(x2>=0 && "argument must be non-negative"); 152 | 153 | // FIXME check if bigger bounds are possible 154 | if(x2>=taylor_n_bound) { 155 | // slow fall-back solution 156 | scalar x = sqrt(x2); 157 | return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. 158 | } 159 | 160 | // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) 161 | // TODO Find optimal coefficients using Remez algorithm 162 | static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; 163 | scalar cosi = 1., sinc=1; 164 | scalar term = -1/2. * x2; 165 | for(int i=0; i<3; ++i) { 166 | cosi += term; 167 | term *= inv[2*i]; 168 | sinc += term; 169 | term *= -inv[2*i+1] * x2; 170 | } 171 | 172 | return std::make_pair(cosi, sinc); 173 | 174 | } 175 | 176 | template 177 | Eigen::Matrix hat(const Base& v) { 178 | Eigen::Matrix res; 179 | res << 0, -v[2], v[1], 180 | v[2], 0, -v[0], 181 | -v[1], v[0], 0; 182 | return res; 183 | } 184 | 185 | template 186 | Eigen::Matrix A_inv_trans(const Base& v){ 187 | Eigen::Matrix res; 188 | if(v.norm() > MTK::tolerance()) 189 | { 190 | res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); 191 | 192 | } 193 | else 194 | { 195 | res = Eigen::Matrix::Identity(); 196 | } 197 | 198 | return res; 199 | } 200 | 201 | template 202 | Eigen::Matrix A_inv(const Base& v){ 203 | Eigen::Matrix res; 204 | if(v.norm() > MTK::tolerance()) 205 | { 206 | res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); 207 | 208 | } 209 | else 210 | { 211 | res = Eigen::Matrix::Identity(); 212 | } 213 | 214 | return res; 215 | } 216 | 217 | template 218 | Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) 219 | { 220 | Eigen::Matrix res; 221 | scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); 222 | if(norm < MTK::tolerance()){ 223 | res = Eigen::Matrix::Zero(); 224 | res(0, 1) = 1; 225 | res(1, 2) = 1; 226 | res /= length; 227 | } 228 | else{ 229 | res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, 230 | -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); 231 | res /= length; 232 | } 233 | } 234 | 235 | template 236 | Eigen::Matrix A_matrix(const Base & v){ 237 | Eigen::Matrix res; 238 | double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; 239 | double norm = std::sqrt(squaredNorm); 240 | if(norm < MTK::tolerance()){ 241 | res = Eigen::Matrix::Identity(); 242 | } 243 | else{ 244 | res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); 245 | } 246 | return res; 247 | } 248 | 249 | template 250 | scalar exp(vectview result, vectview vec, const scalar& scale = 1) { 251 | scalar norm2 = vec.squaredNorm(); 252 | std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); 253 | scalar mult = cos_sinc.second * scale; 254 | result = mult * vec; 255 | return cos_sinc.first; 256 | } 257 | 258 | 259 | /** 260 | * Inverse function to @c exp. 261 | * 262 | * @param result @c vectview to the result 263 | * @param w scalar part of input 264 | * @param vec vector part of input 265 | * @param scale scale result by this value 266 | * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result 267 | */ 268 | template 269 | void log(vectview result, 270 | const scalar &w, const vectview vec, 271 | const scalar &scale, bool plus_minus_periodicity) 272 | { 273 | // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division 274 | scalar nv = vec.norm(); 275 | if(nv < tolerance()) { 276 | if(!plus_minus_periodicity && w < 0) { 277 | // find the maximal entry: 278 | int i; 279 | nv = vec.cwiseAbs().maxCoeff(&i); 280 | result = scale * std::atan2(nv, w) * vect::Unit(i); 281 | return; 282 | } 283 | nv = tolerance(); 284 | } 285 | scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); 286 | 287 | result = s * vec; 288 | } 289 | 290 | 291 | } // namespace MTK 292 | 293 | 294 | #endif /* MTKMATH_H_ */ 295 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * Copyright (c) 2008--2011, Universitaet Bremen 4 | * All rights reserved. 5 | * 6 | * Author: Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | /** 36 | * @file mtk/src/vectview.hpp 37 | * @brief Wrapper class around a pointer used as interface for plain vectors. 38 | */ 39 | 40 | #ifndef VECTVIEW_HPP_ 41 | #define VECTVIEW_HPP_ 42 | 43 | #include 44 | 45 | namespace MTK { 46 | 47 | /** 48 | * A view to a vector. 49 | * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. 50 | * The dimension of the vector is given as template parameter and type-checked when used in expressions. 51 | * Data has to be modifiable. 52 | * 53 | * @tparam scalar Scalar type of the vector. 54 | * @tparam dim Dimension of the vector. 55 | * 56 | * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct 57 | */ 58 | namespace internal { 59 | template 60 | struct CovBlock { 61 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; 62 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; 63 | }; 64 | 65 | template 66 | struct CovBlock_ { 67 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; 68 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; 69 | }; 70 | 71 | template 72 | struct CrossCovBlock { 73 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; 74 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; 75 | }; 76 | 77 | template 78 | struct CrossCovBlock_ { 79 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; 80 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; 81 | }; 82 | 83 | template 84 | struct VectviewBase { 85 | typedef Eigen::Matrix matrix_type; 86 | typedef typename matrix_type::MapType Type; 87 | typedef typename matrix_type::ConstMapType ConstType; 88 | }; 89 | 90 | template 91 | struct UnalignedType { 92 | typedef T type; 93 | }; 94 | } 95 | 96 | template 97 | class vectview : public internal::VectviewBase::Type { 98 | typedef internal::VectviewBase VectviewBase; 99 | public: 100 | //! plain matrix type 101 | typedef typename VectviewBase::matrix_type matrix_type; 102 | //! base type 103 | typedef typename VectviewBase::Type base; 104 | //! construct from pointer 105 | explicit 106 | vectview(scalar* data, int dim_=dim) : base(data, dim_) {} 107 | //! construct from plain matrix 108 | vectview(matrix_type& m) : base(m.data(), m.size()) {} 109 | //! construct from another @c vectview 110 | vectview(const vectview &v) : base(v) {} 111 | //! construct from Eigen::Block: 112 | template 113 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} 114 | template 115 | vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} 116 | 117 | //! inherit assignment operator 118 | using base::operator=; 119 | //! data pointer 120 | scalar* data() {return const_cast(base::data());} 121 | }; 122 | 123 | /** 124 | * @c const version of @c vectview. 125 | * Compared to @c Eigen::Map this implementation is const correct, i.e., 126 | * data will not be modifiable using this view. 127 | * 128 | * @tparam scalar Scalar type of the vector. 129 | * @tparam dim Dimension of the vector. 130 | * 131 | * @sa vectview 132 | */ 133 | template 134 | class vectview : public internal::VectviewBase::ConstType { 135 | typedef internal::VectviewBase VectviewBase; 136 | public: 137 | //! plain matrix type 138 | typedef typename VectviewBase::matrix_type matrix_type; 139 | //! base type 140 | typedef typename VectviewBase::ConstType base; 141 | //! construct from const pointer 142 | explicit 143 | vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} 144 | //! construct from column vector 145 | template 146 | vectview(const Eigen::Matrix& m) : base(m.data()) {} 147 | //! construct from row vector 148 | template 149 | vectview(const Eigen::Matrix& m) : base(m.data()) {} 150 | //! construct from another @c vectview 151 | vectview(vectview x) : base(x.data()) {} 152 | //! construct from base 153 | vectview(const base &x) : base(x) {} 154 | /** 155 | * Construct from Block 156 | * @todo adapt this, when Block gets const-correct 157 | */ 158 | template 159 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} 160 | template 161 | vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} 162 | 163 | }; 164 | 165 | 166 | } // namespace MTK 167 | 168 | #endif /* VECTVIEW_HPP_ */ 169 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/startIdx.hpp 75 | * @brief Tools to access sub-elements of compound manifolds. 76 | */ 77 | #ifndef GET_START_INDEX_H_ 78 | #define GET_START_INDEX_H_ 79 | 80 | #include 81 | 82 | #include "src/SubManifold.hpp" 83 | #include "src/vectview.hpp" 84 | 85 | namespace MTK { 86 | 87 | 88 | /** 89 | * \defgroup SubManifolds Accessing Submanifolds 90 | * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers 91 | * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. 92 | * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers 93 | * @c &pose::orient and @c &pose::trans give all required information and are still 94 | * valid if the base type gets extended or the actual types of @a orient and @a trans 95 | * change (e.g. from 2D to 3D). 96 | * 97 | * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. 98 | */ 99 | //@{ 100 | 101 | /** 102 | * Determine the index of a sub-variable within a compound variable. 103 | */ 104 | template 105 | int getStartIdx( MTK::SubManifold Base::*) 106 | { 107 | return idx; 108 | } 109 | 110 | template 111 | int getStartIdx_( MTK::SubManifold Base::*) 112 | { 113 | return dim; 114 | } 115 | 116 | /** 117 | * Determine the degrees of freedom of a sub-variable within a compound variable. 118 | */ 119 | template 120 | int getDof( MTK::SubManifold Base::*) 121 | { 122 | return T::DOF; 123 | } 124 | template 125 | int getDim( MTK::SubManifold Base::*) 126 | { 127 | return T::DIM; 128 | } 129 | 130 | /** 131 | * set the diagonal elements of a covariance matrix corresponding to a sub-variable 132 | */ 133 | template 134 | void setDiagonal(Eigen::Matrix &cov, 135 | MTK::SubManifold Base::*, const typename Base::scalar &val) 136 | { 137 | cov.diagonal().template segment(idx).setConstant(val); 138 | } 139 | 140 | template 141 | void setDiagonal_(Eigen::Matrix &cov, 142 | MTK::SubManifold Base::*, const typename Base::scalar &val) 143 | { 144 | cov.diagonal().template segment(dim).setConstant(val); 145 | } 146 | 147 | /** 148 | * Get the subblock of corresponding to two members, i.e. 149 | * \code 150 | * Eigen::Matrix m; 151 | * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; 152 | * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); 153 | * \endcode 154 | * lets you modify mixed covariance entries in a bigger covariance matrix. 155 | */ 156 | template 157 | typename MTK::internal::CovBlock::Type 158 | subblock(Eigen::Matrix &cov, 159 | MTK::SubManifold Base::*, MTK::SubManifold Base::*) 160 | { 161 | return cov.template block(idx1, idx2); 162 | } 163 | 164 | template 165 | typename MTK::internal::CovBlock_::Type 166 | subblock_(Eigen::Matrix &cov, 167 | MTK::SubManifold Base::*, MTK::SubManifold Base::*) 168 | { 169 | return cov.template block(dim1, dim2); 170 | } 171 | 172 | template 173 | typename MTK::internal::CrossCovBlock::Type 174 | subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) 175 | { 176 | return cov.template block(idx1, idx2); 177 | } 178 | 179 | template 180 | typename MTK::internal::CrossCovBlock_::Type 181 | subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) 182 | { 183 | return cov.template block(dim1, dim2); 184 | } 185 | /** 186 | * Get the subblock of corresponding to a member, i.e. 187 | * \code 188 | * Eigen::Matrix m; 189 | * MTK::subblock(m, &Pose::orient) = some_expression; 190 | * \endcode 191 | * lets you modify covariance entries in a bigger covariance matrix. 192 | */ 193 | template 194 | typename MTK::internal::CovBlock_::Type 195 | subblock_(Eigen::Matrix &cov, 196 | MTK::SubManifold Base::*) 197 | { 198 | return cov.template block(dim, dim); 199 | } 200 | 201 | template 202 | typename MTK::internal::CovBlock::Type 203 | subblock(Eigen::Matrix &cov, 204 | MTK::SubManifold Base::*) 205 | { 206 | return cov.template block(idx, idx); 207 | } 208 | 209 | template 210 | class get_cov { 211 | public: 212 | typedef Eigen::Matrix type; 213 | typedef const Eigen::Matrix const_type; 214 | }; 215 | 216 | template 217 | class get_cov_ { 218 | public: 219 | typedef Eigen::Matrix type; 220 | typedef const Eigen::Matrix const_type; 221 | }; 222 | 223 | template 224 | class get_cross_cov { 225 | public: 226 | typedef Eigen::Matrix type; 227 | typedef const type const_type; 228 | }; 229 | 230 | template 231 | class get_cross_cov_ { 232 | public: 233 | typedef Eigen::Matrix type; 234 | typedef const type const_type; 235 | }; 236 | 237 | 238 | template 239 | vectview 240 | subvector_impl_(vectview vec, SubManifold Base::*) 241 | { 242 | return vec.template segment(dim); 243 | } 244 | 245 | template 246 | vectview 247 | subvector_impl(vectview vec, SubManifold Base::*) 248 | { 249 | return vec.template segment(idx); 250 | } 251 | 252 | /** 253 | * Get the subvector corresponding to a sub-manifold from a bigger vector. 254 | */ 255 | template 256 | vectview 257 | subvector_(vectview vec, SubManifold Base::* ptr) 258 | { 259 | return subvector_impl_(vec, ptr); 260 | } 261 | 262 | template 263 | vectview 264 | subvector(vectview vec, SubManifold Base::* ptr) 265 | { 266 | return subvector_impl(vec, ptr); 267 | } 268 | 269 | /** 270 | * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) 271 | */ 272 | template 273 | vectview 274 | subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) 275 | { 276 | return subvector_impl(vectview(vec), ptr); 277 | } 278 | 279 | template 280 | vectview 281 | subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) 282 | { 283 | return subvector_impl_(vectview(vec), ptr); 284 | } 285 | 286 | template 287 | vectview 288 | subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) 289 | { 290 | return subvector_impl_(vectview(vec), ptr); 291 | } 292 | 293 | template 294 | vectview 295 | subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) 296 | { 297 | return subvector_impl(vectview(vec), ptr); 298 | } 299 | 300 | 301 | /** 302 | * const version of subvector(vectview vec,SubManifold Base::*) 303 | */ 304 | template 305 | vectview 306 | subvector_impl(const vectview cvec, SubManifold Base::*) 307 | { 308 | return cvec.template segment(idx); 309 | } 310 | 311 | template 312 | vectview 313 | subvector_impl_(const vectview cvec, SubManifold Base::*) 314 | { 315 | return cvec.template segment(dim); 316 | } 317 | 318 | template 319 | vectview 320 | subvector(const vectview cvec, SubManifold Base::* ptr) 321 | { 322 | return subvector_impl(cvec, ptr); 323 | } 324 | 325 | 326 | } // namespace MTK 327 | 328 | #endif // GET_START_INDEX_H_ 329 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp: -------------------------------------------------------------------------------- 1 | // This is a NEW implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/S2.hpp 75 | * @brief Unit vectors on the sphere, or directions in 3D. 76 | */ 77 | #ifndef S2_H_ 78 | #define S2_H_ 79 | 80 | 81 | #include "vect.hpp" 82 | 83 | #include "SOn.hpp" 84 | #include "../src/mtkmath.hpp" 85 | 86 | 87 | 88 | 89 | namespace MTK { 90 | 91 | /** 92 | * Manifold representation of @f$ S^2 @f$. 93 | * Used for unit vectors on the sphere or directions in 3D. 94 | * 95 | * @todo add conversions from/to polar angles? 96 | */ 97 | template 98 | struct S2 { 99 | 100 | typedef _scalar scalar; 101 | typedef vect<3, scalar> vect_type; 102 | typedef SO3 SO3_type; 103 | typedef typename vect_type::base vec3; 104 | scalar length = scalar(den)/scalar(num); 105 | enum {DOF=2, TYP = 1, DIM = 3}; 106 | 107 | //private: 108 | /** 109 | * Unit vector on the sphere, or vector pointing in a direction 110 | */ 111 | vect_type vec; 112 | 113 | public: 114 | S2() { 115 | if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); 116 | if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); 117 | if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); 118 | } 119 | S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { 120 | vec.normalize(); 121 | vec = vec * length; 122 | } 123 | 124 | S2(const vect_type &_vec) : vec(_vec) { 125 | vec.normalize(); 126 | vec = vec * length; 127 | } 128 | 129 | void oplus(MTK::vectview delta, scalar scale = 1) 130 | { 131 | SO3_type res; 132 | res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); 133 | vec = res.toRotationMatrix() * vec; 134 | } 135 | 136 | void boxplus(MTK::vectview delta, scalar scale=1) { 137 | Eigen::Matrix Bx; 138 | S2_Bx(Bx); 139 | vect_type Bu = Bx*delta;SO3_type res; 140 | res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); 141 | vec = res.toRotationMatrix() * vec; 142 | } 143 | 144 | void boxminus(MTK::vectview res, const S2& other) const { 145 | scalar v_sin = (MTK::hat(vec)*other.vec).norm(); 146 | scalar v_cos = vec.transpose() * other.vec; 147 | scalar theta = std::atan2(v_sin, v_cos); 148 | if(v_sin < MTK::tolerance()) 149 | { 150 | if(std::fabs(theta) > MTK::tolerance() ) 151 | { 152 | res[0] = 3.1415926; 153 | res[1] = 0; 154 | } 155 | else{ 156 | res[0] = 0; 157 | res[1] = 0; 158 | } 159 | } 160 | else 161 | { 162 | S2 other_copy = other; 163 | Eigen::MatrixBx; 164 | other_copy.S2_Bx(Bx); 165 | res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; 166 | } 167 | } 168 | 169 | void S2_hat(Eigen::Matrix &res) 170 | { 171 | Eigen::Matrix skew_vec; 172 | skew_vec << scalar(0), -vec[2], vec[1], 173 | vec[2], scalar(0), -vec[0], 174 | -vec[1], vec[0], scalar(0); 175 | res = skew_vec; 176 | } 177 | 178 | 179 | void S2_Bx(Eigen::Matrix &res) 180 | { 181 | if(S2_typ == 3) 182 | { 183 | if(vec[2] + length > tolerance()) 184 | { 185 | 186 | res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), 187 | -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), 188 | -vec[0], -vec[1]; 189 | res /= length; 190 | } 191 | else 192 | { 193 | res = Eigen::Matrix::Zero(); 194 | res(1, 1) = -1; 195 | res(2, 0) = 1; 196 | } 197 | } 198 | else if(S2_typ == 2) 199 | { 200 | if(vec[1] + length > tolerance()) 201 | { 202 | 203 | res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), 204 | -vec[0], -vec[2], 205 | -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); 206 | res /= length; 207 | } 208 | else 209 | { 210 | res = Eigen::Matrix::Zero(); 211 | res(1, 1) = -1; 212 | res(2, 0) = 1; 213 | } 214 | } 215 | else 216 | { 217 | if(vec[0] + length > tolerance()) 218 | { 219 | 220 | res << -vec[1], -vec[2], 221 | length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), 222 | -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); 223 | res /= length; 224 | } 225 | else 226 | { 227 | res = Eigen::Matrix::Zero(); 228 | res(1, 1) = -1; 229 | res(2, 0) = 1; 230 | } 231 | } 232 | } 233 | 234 | void S2_Nx(Eigen::Matrix &res, S2& subtrahend) 235 | { 236 | if((vec+subtrahend.vec).norm() > tolerance()) 237 | { 238 | Eigen::Matrix Bx; 239 | S2_Bx(Bx); 240 | if((vec-subtrahend.vec).norm() > tolerance()) 241 | { 242 | scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); 243 | scalar v_cos = vec.transpose() * subtrahend.vec; 244 | 245 | res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); 246 | } 247 | else 248 | { 249 | res = 1/length/length*Bx.transpose()*MTK::hat(vec); 250 | } 251 | } 252 | else 253 | { 254 | std::cerr << "No N(x, y) for x=-y" << std::endl; 255 | std::exit(100); 256 | } 257 | } 258 | 259 | void S2_Nx_yy(Eigen::Matrix &res) 260 | { 261 | Eigen::Matrix Bx; 262 | S2_Bx(Bx); 263 | res = 1/length/length*Bx.transpose()*MTK::hat(vec); 264 | } 265 | 266 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 267 | { 268 | Eigen::Matrix Bx; 269 | S2_Bx(Bx); 270 | if(delta.norm() < tolerance()) 271 | { 272 | res = -MTK::hat(vec)*Bx; 273 | } 274 | else{ 275 | vect_type Bu = Bx*delta; 276 | SO3_type exp_delta; 277 | exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); 278 | res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; 279 | } 280 | } 281 | 282 | operator const vect_type&() const{ 283 | return vec; 284 | } 285 | 286 | const vect_type& get_vect() const { 287 | return vec; 288 | } 289 | 290 | friend S2 operator*(const SO3& rot, const S2& dir) 291 | { 292 | S2 ret; 293 | ret.vec = rot * dir.vec; 294 | return ret; 295 | } 296 | 297 | scalar operator[](int idx) const {return vec[idx]; } 298 | 299 | friend std::ostream& operator<<(std::ostream &os, const S2& vec){ 300 | return os << vec.vec.transpose() << " "; 301 | } 302 | friend std::istream& operator>>(std::istream &is, S2& vec){ 303 | for(int i=0; i<3; ++i) 304 | is >> vec.vec[i]; 305 | vec.vec.normalize(); 306 | vec.vec = vec.vec * vec.length; 307 | return is; 308 | 309 | } 310 | }; 311 | 312 | 313 | } // namespace MTK 314 | 315 | 316 | #endif /*S2_H_*/ 317 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/SOn.hpp 75 | * @brief Standard Orthogonal Groups i.e.\ rotatation groups. 76 | */ 77 | #ifndef SON_H_ 78 | #define SON_H_ 79 | 80 | #include 81 | 82 | #include "vect.hpp" 83 | #include "../src/mtkmath.hpp" 84 | 85 | 86 | namespace MTK { 87 | 88 | 89 | /** 90 | * Two-dimensional orientations represented as scalar. 91 | * There is no guarantee that the representing scalar is within any interval, 92 | * but the result of boxminus will always have magnitude @f$\le\pi @f$. 93 | */ 94 | template 95 | struct SO2 : public Eigen::Rotation2D<_scalar> { 96 | enum {DOF = 1, DIM = 2, TYP = 3}; 97 | 98 | typedef _scalar scalar; 99 | typedef Eigen::Rotation2D base; 100 | typedef vect vect_type; 101 | 102 | //! Construct from angle 103 | SO2(const scalar& angle = 0) : base(angle) { } 104 | 105 | //! Construct from Eigen::Rotation2D 106 | SO2(const base& src) : base(src) {} 107 | 108 | /** 109 | * Construct from 2D vector. 110 | * Resulting orientation will rotate the first unit vector to point to vec. 111 | */ 112 | SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; 113 | 114 | 115 | //! Calculate @c this->inverse() * @c r 116 | SO2 operator%(const base &r) const { 117 | return base::inverse() * r; 118 | } 119 | 120 | //! Calculate @c this->inverse() * @c r 121 | template 122 | vect_type operator%(const Eigen::MatrixBase &vec) const { 123 | return base::inverse() * vec; 124 | } 125 | 126 | //! Calculate @c *this * @c r.inverse() 127 | SO2 operator/(const SO2 &r) const { 128 | return *this * r.inverse(); 129 | } 130 | 131 | //! Gets the angle as scalar. 132 | operator scalar() const { 133 | return base::angle(); 134 | } 135 | void S2_hat(Eigen::Matrix &res) 136 | { 137 | res = Eigen::Matrix::Zero(); 138 | } 139 | //! @name Manifold requirements 140 | void S2_Nx_yy(Eigen::Matrix &res) 141 | { 142 | std::cerr << "wrong idx for S2" << std::endl; 143 | std::exit(100); 144 | res = Eigen::Matrix::Zero(); 145 | } 146 | 147 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 148 | { 149 | std::cerr << "wrong idx for S2" << std::endl; 150 | std::exit(100); 151 | res = Eigen::Matrix::Zero(); 152 | } 153 | 154 | void oplus(MTK::vectview vec, scalar scale = 1) { 155 | base::angle() += scale * vec[0]; 156 | } 157 | 158 | void boxplus(MTK::vectview vec, scalar scale = 1) { 159 | base::angle() += scale * vec[0]; 160 | } 161 | void boxminus(MTK::vectview res, const SO2& other) const { 162 | res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); 163 | } 164 | 165 | friend std::istream& operator>>(std::istream &is, SO2& ang){ 166 | return is >> ang.angle(); 167 | } 168 | 169 | }; 170 | 171 | 172 | /** 173 | * Three-dimensional orientations represented as Quaternion. 174 | * It is assumed that the internal Quaternion always stays normalized, 175 | * should this not be the case, call inherited member function @c normalize(). 176 | */ 177 | template 178 | struct SO3 : public Eigen::Quaternion<_scalar, Options> { 179 | enum {DOF = 3, DIM = 3, TYP = 2}; 180 | typedef _scalar scalar; 181 | typedef Eigen::Quaternion base; 182 | typedef Eigen::Quaternion Quaternion; 183 | typedef vect vect_type; 184 | 185 | //! Calculate @c this->inverse() * @c r 186 | template EIGEN_STRONG_INLINE 187 | Quaternion operator%(const Eigen::QuaternionBase &r) const { 188 | return base::conjugate() * r; 189 | } 190 | 191 | //! Calculate @c this->inverse() * @c r 192 | template 193 | vect_type operator%(const Eigen::MatrixBase &vec) const { 194 | return base::conjugate() * vec; 195 | } 196 | 197 | //! Calculate @c this * @c r.conjugate() 198 | template EIGEN_STRONG_INLINE 199 | Quaternion operator/(const Eigen::QuaternionBase &r) const { 200 | return *this * r.conjugate(); 201 | } 202 | 203 | /** 204 | * Construct from real part and three imaginary parts. 205 | * Quaternion is normalized after construction. 206 | */ 207 | SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { 208 | base::normalize(); 209 | } 210 | 211 | /** 212 | * Construct from Eigen::Quaternion. 213 | * @note Non-normalized input may result result in spurious behavior. 214 | */ 215 | SO3(const base& src = base::Identity()) : base(src) {} 216 | 217 | /** 218 | * Construct from rotation matrix. 219 | * @note Invalid rotation matrices may lead to spurious behavior. 220 | */ 221 | template 222 | SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} 223 | 224 | /** 225 | * Construct from arbitrary rotation type. 226 | * @note Invalid rotation matrices may lead to spurious behavior. 227 | */ 228 | template 229 | SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} 230 | 231 | //! @name Manifold requirements 232 | 233 | void boxplus(MTK::vectview vec, scalar scale=1) { 234 | SO3 delta = exp(vec, scale); 235 | *this = *this * delta; 236 | } 237 | void boxminus(MTK::vectview res, const SO3& other) const { 238 | res = SO3::log(other.conjugate() * *this); 239 | } 240 | //} 241 | 242 | void oplus(MTK::vectview vec, scalar scale=1) { 243 | SO3 delta = exp(vec, scale); 244 | *this = *this * delta; 245 | } 246 | 247 | void S2_hat(Eigen::Matrix &res) 248 | { 249 | res = Eigen::Matrix::Zero(); 250 | } 251 | void S2_Nx_yy(Eigen::Matrix &res) 252 | { 253 | std::cerr << "wrong idx for S2" << std::endl; 254 | std::exit(100); 255 | res = Eigen::Matrix::Zero(); 256 | } 257 | 258 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 259 | { 260 | std::cerr << "wrong idx for S2" << std::endl; 261 | std::exit(100); 262 | res = Eigen::Matrix::Zero(); 263 | } 264 | 265 | friend std::ostream& operator<<(std::ostream &os, const SO3& q){ 266 | return os << q.coeffs().transpose() << " "; 267 | } 268 | 269 | friend std::istream& operator>>(std::istream &is, SO3& q){ 270 | vect<4,scalar> coeffs; 271 | is >> coeffs; 272 | q.coeffs() = coeffs.normalized(); 273 | return is; 274 | } 275 | 276 | //! @name Helper functions 277 | //{ 278 | /** 279 | * Calculate the exponential map. In matrix terms this would correspond 280 | * to the Rodrigues formula. 281 | */ 282 | // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround 283 | // static SO3 exp(MTK::vectview dvec, scalar scale = 1){ 284 | static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ 285 | SO3 res; 286 | res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); 287 | return res; 288 | } 289 | /** 290 | * Calculate the inverse of @c exp. 291 | * Only guarantees that exp(log(x)) == x 292 | */ 293 | static typename base::Vector3 log(const SO3 &orient){ 294 | typename base::Vector3 res; 295 | MTK::log(res, orient.w(), orient.vec(), scalar(2), true); 296 | return res; 297 | } 298 | }; 299 | 300 | namespace internal { 301 | template 302 | struct UnalignedType >{ 303 | typedef SO2 type; 304 | }; 305 | 306 | template 307 | struct UnalignedType >{ 308 | typedef SO3 type; 309 | }; 310 | 311 | } // namespace internal 312 | 313 | 314 | } // namespace MTK 315 | 316 | #endif /*SON_H_*/ 317 | 318 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/vect.hpp 75 | * @brief Basic vectors interpreted as manifolds. 76 | * 77 | * This file also implements a simple wrapper for matrices, for arbitrary scalars 78 | * and for positive scalars. 79 | */ 80 | #ifndef VECT_H_ 81 | #define VECT_H_ 82 | 83 | #include 84 | #include 85 | #include 86 | 87 | #include "../src/vectview.hpp" 88 | 89 | namespace MTK { 90 | 91 | static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); 92 | 93 | 94 | /** 95 | * A simple vector class. 96 | * Implementation is basically a wrapper around Eigen::Matrix with manifold 97 | * requirements added. 98 | */ 99 | template 100 | struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { 101 | typedef Eigen::Matrix<_scalar, D, 1, _Options> base; 102 | enum {DOF = D, DIM = D, TYP = 0}; 103 | typedef _scalar scalar; 104 | 105 | //using base::operator=; 106 | 107 | /** Standard constructor. Sets all values to zero. */ 108 | vect(const base &src = base::Zero()) : base(src) {} 109 | 110 | /** Constructor copying the value of the expression \a other */ 111 | template 112 | EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} 113 | 114 | /** Construct from memory. */ 115 | vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } 116 | 117 | void boxplus(MTK::vectview vec, scalar scale=1) { 118 | *this += scale * vec; 119 | } 120 | void boxminus(MTK::vectview res, const vect& other) const { 121 | res = *this - other; 122 | } 123 | 124 | void oplus(MTK::vectview vec, scalar scale=1) { 125 | *this += scale * vec; 126 | } 127 | 128 | void S2_hat(Eigen::Matrix &res) 129 | { 130 | res = Eigen::Matrix::Zero(); 131 | } 132 | 133 | void S2_Nx_yy(Eigen::Matrix &res) 134 | { 135 | std::cerr << "wrong idx for S2" << std::endl; 136 | std::exit(100); 137 | res = Eigen::Matrix::Zero(); 138 | } 139 | 140 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 141 | { 142 | std::cerr << "wrong idx for S2" << std::endl; 143 | std::exit(100); 144 | res = Eigen::Matrix::Zero(); 145 | } 146 | 147 | friend std::ostream& operator<<(std::ostream &os, const vect& v){ 148 | // Eigen sometimes messes with the streams flags, so output manually: 149 | for(int i=0; i>(std::istream &is, vect& v){ 154 | char term=0; 155 | is >> std::ws; // skip whitespace 156 | switch(is.peek()) { 157 | case '(': term=')'; is.ignore(1); break; 158 | case '[': term=']'; is.ignore(1); break; 159 | case '{': term='}'; is.ignore(1); break; 160 | default: break; 161 | } 162 | if(D==Eigen::Dynamic) { 163 | assert(term !=0 && "Dynamic vectors must be embraced"); 164 | std::vector temp; 165 | while(is.good() && is.peek() != term) { 166 | scalar x; 167 | is >> x; 168 | temp.push_back(x); 169 | if(is.peek()==',') is.ignore(1); 170 | } 171 | v = vect::Map(temp.data(), temp.size()); 172 | } else 173 | for(int i=0; i> v[i]; 175 | if(is.peek()==',') { // ignore commas between values 176 | is.ignore(1); 177 | } 178 | } 179 | if(term!=0) { 180 | char x; 181 | is >> x; 182 | if(x!=term) { 183 | is.setstate(is.badbit); 184 | // assert(x==term && "start and end bracket do not match!"); 185 | } 186 | } 187 | return is; 188 | } 189 | 190 | template 191 | vectview tail(){ 192 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 193 | return base::template tail(); 194 | } 195 | template 196 | vectview tail() const{ 197 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 198 | return base::template tail(); 199 | } 200 | template 201 | vectview head(){ 202 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 203 | return base::template head(); 204 | } 205 | template 206 | vectview head() const{ 207 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 208 | return base::template head(); 209 | } 210 | }; 211 | 212 | 213 | /** 214 | * A simple matrix class. 215 | * Implementation is basically a wrapper around Eigen::Matrix with manifold 216 | * requirements added, i.e., matrix is viewed as a plain vector for that. 217 | */ 218 | template::Options> 219 | struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { 220 | typedef Eigen::Matrix<_scalar, M, N, _Options> base; 221 | enum {DOF = M * N, TYP = 4, DIM=0}; 222 | typedef _scalar scalar; 223 | 224 | using base::operator=; 225 | 226 | /** Standard constructor. Sets all values to zero. */ 227 | matrix() { 228 | base::setZero(); 229 | } 230 | 231 | /** Constructor copying the value of the expression \a other */ 232 | template 233 | EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} 234 | 235 | /** Construct from memory. */ 236 | matrix(const scalar* src) : base(src) { } 237 | 238 | void boxplus(MTK::vectview vec, scalar scale = 1) { 239 | *this += scale * base::Map(vec.data()); 240 | } 241 | void boxminus(MTK::vectview res, const matrix& other) const { 242 | base::Map(res.data()) = *this - other; 243 | } 244 | 245 | void S2_hat(Eigen::Matrix &res) 246 | { 247 | res = Eigen::Matrix::Zero(); 248 | } 249 | 250 | void oplus(MTK::vectview vec, scalar scale = 1) { 251 | *this += scale * base::Map(vec.data()); 252 | } 253 | 254 | void S2_Nx_yy(Eigen::Matrix &res) 255 | { 256 | std::cerr << "wrong idx for S2" << std::endl; 257 | std::exit(100); 258 | res = Eigen::Matrix::Zero(); 259 | } 260 | 261 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 262 | { 263 | std::cerr << "wrong idx for S2" << std::endl; 264 | std::exit(100); 265 | res = Eigen::Matrix::Zero(); 266 | } 267 | 268 | friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ 269 | for(int i=0; i>(std::istream &is, matrix& mat){ 275 | for(int i=0; i> mat.data()[i]; 277 | } 278 | return is; 279 | } 280 | };// @todo What if M / N = Eigen::Dynamic? 281 | 282 | 283 | 284 | /** 285 | * A simple scalar type. 286 | */ 287 | template 288 | struct Scalar { 289 | enum {DOF = 1, TYP = 5, DIM=0}; 290 | typedef _scalar scalar; 291 | 292 | scalar value; 293 | 294 | Scalar(const scalar& value = scalar(0)) : value(value) {} 295 | operator const scalar&() const { return value; } 296 | operator scalar&() { return value; } 297 | Scalar& operator=(const scalar& val) { value = val; return *this; } 298 | 299 | void S2_hat(Eigen::Matrix &res) 300 | { 301 | res = Eigen::Matrix::Zero(); 302 | } 303 | 304 | void S2_Nx_yy(Eigen::Matrix &res) 305 | { 306 | std::cerr << "wrong idx for S2" << std::endl; 307 | std::exit(100); 308 | res = Eigen::Matrix::Zero(); 309 | } 310 | 311 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 312 | { 313 | std::cerr << "wrong idx for S2" << std::endl; 314 | std::exit(100); 315 | res = Eigen::Matrix::Zero(); 316 | } 317 | 318 | void oplus(MTK::vectview vec, scalar scale=1) { 319 | value += scale * vec[0]; 320 | } 321 | 322 | void boxplus(MTK::vectview vec, scalar scale=1) { 323 | value += scale * vec[0]; 324 | } 325 | void boxminus(MTK::vectview res, const Scalar& other) const { 326 | res[0] = *this - other; 327 | } 328 | }; 329 | 330 | /** 331 | * Positive scalars. 332 | * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. 333 | */ 334 | template 335 | struct PositiveScalar { 336 | enum {DOF = 1, TYP = 6, DIM=0}; 337 | typedef _scalar scalar; 338 | 339 | scalar value; 340 | 341 | PositiveScalar(const scalar& value = scalar(1)) : value(value) { 342 | assert(value > scalar(0)); 343 | } 344 | operator const scalar&() const { return value; } 345 | PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } 346 | 347 | void boxplus(MTK::vectview vec, scalar scale = 1) { 348 | value *= std::exp(scale * vec[0]); 349 | } 350 | void boxminus(MTK::vectview res, const PositiveScalar& other) const { 351 | res[0] = std::log(*this / other); 352 | } 353 | 354 | void oplus(MTK::vectview vec, scalar scale = 1) { 355 | value *= std::exp(scale * vec[0]); 356 | } 357 | 358 | void S2_hat(Eigen::Matrix &res) 359 | { 360 | res = Eigen::Matrix::Zero(); 361 | } 362 | 363 | void S2_Nx_yy(Eigen::Matrix &res) 364 | { 365 | std::cerr << "wrong idx for S2" << std::endl; 366 | std::exit(100); 367 | res = Eigen::Matrix::Zero(); 368 | } 369 | 370 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 371 | { 372 | std::cerr << "wrong idx for S2" << std::endl; 373 | std::exit(100); 374 | res = Eigen::Matrix::Zero(); 375 | } 376 | 377 | 378 | friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ 379 | is >> s.value; 380 | assert(s.value > 0); 381 | return is; 382 | } 383 | }; 384 | 385 | template 386 | struct Complex : public std::complex<_scalar>{ 387 | enum {DOF = 2, TYP = 7, DIM=0}; 388 | typedef _scalar scalar; 389 | 390 | typedef std::complex Base; 391 | 392 | Complex(const Base& value) : Base(value) {} 393 | Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} 394 | Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} 395 | template 396 | Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} 397 | 398 | void boxplus(MTK::vectview vec, scalar scale = 1) { 399 | Base::real() += scale * vec[0]; 400 | Base::imag() += scale * vec[1]; 401 | }; 402 | void boxminus(MTK::vectview res, const Complex& other) const { 403 | Complex diff = *this - other; 404 | res << diff.real(), diff.imag(); 405 | } 406 | 407 | void S2_hat(Eigen::Matrix &res) 408 | { 409 | res = Eigen::Matrix::Zero(); 410 | } 411 | 412 | void oplus(MTK::vectview vec, scalar scale = 1) { 413 | Base::real() += scale * vec[0]; 414 | Base::imag() += scale * vec[1]; 415 | }; 416 | 417 | void S2_Nx_yy(Eigen::Matrix &res) 418 | { 419 | std::cerr << "wrong idx for S2" << std::endl; 420 | std::exit(100); 421 | res = Eigen::Matrix::Zero(); 422 | } 423 | 424 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 425 | { 426 | std::cerr << "wrong idx for S2" << std::endl; 427 | std::exit(100); 428 | res = Eigen::Matrix::Zero(); 429 | } 430 | 431 | scalar squaredNorm() const { 432 | return std::pow(Base::real(),2) + std::pow(Base::imag(),2); 433 | } 434 | 435 | const scalar& operator()(int i) const { 436 | assert(0<=i && i<2 && "Index out of range"); 437 | return i==0 ? Base::real() : Base::imag(); 438 | } 439 | scalar& operator()(int i){ 440 | assert(0<=i && i<2 && "Index out of range"); 441 | return i==0 ? Base::real() : Base::imag(); 442 | } 443 | }; 444 | 445 | 446 | namespace internal { 447 | 448 | template 449 | struct UnalignedType >{ 450 | typedef vect type; 451 | }; 452 | 453 | } // namespace internal 454 | 455 | 456 | } // namespace MTK 457 | 458 | 459 | 460 | 461 | #endif /*VECT_H_*/ 462 | -------------------------------------------------------------------------------- /FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH 3 | * All rights reserved. 4 | * 5 | * Author: Rene Wagner 6 | * Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the DFKI GmbH 19 | * nor the names of its contributors may be used to endorse or 20 | * promote products derived from this software without specific 21 | * prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef WRAPPED_CV_MAT_HPP_ 38 | #define WRAPPED_CV_MAT_HPP_ 39 | 40 | #include 41 | #include 42 | 43 | namespace MTK { 44 | 45 | template 46 | struct cv_f_type; 47 | 48 | template<> 49 | struct cv_f_type 50 | { 51 | enum {value = CV_64F}; 52 | }; 53 | 54 | template<> 55 | struct cv_f_type 56 | { 57 | enum {value = CV_32F}; 58 | }; 59 | 60 | /** 61 | * cv_mat wraps a CvMat around an Eigen Matrix 62 | */ 63 | template 64 | class cv_mat : public matrix 65 | { 66 | typedef matrix base_type; 67 | enum {type_ = cv_f_type::value}; 68 | CvMat cv_mat_; 69 | 70 | public: 71 | cv_mat() 72 | { 73 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 74 | } 75 | 76 | cv_mat(const cv_mat& oth) : base_type(oth) 77 | { 78 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 79 | } 80 | 81 | template 82 | cv_mat(const Eigen::MatrixBase &value) : base_type(value) 83 | { 84 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 85 | } 86 | 87 | template 88 | cv_mat& operator=(const Eigen::MatrixBase &value) 89 | { 90 | base_type::operator=(value); 91 | return *this; 92 | } 93 | 94 | cv_mat& operator=(const cv_mat& value) 95 | { 96 | base_type::operator=(value); 97 | return *this; 98 | } 99 | 100 | // FIXME: Maybe overloading operator& is not a good idea ... 101 | CvMat* operator&() 102 | { 103 | return &cv_mat_; 104 | } 105 | const CvMat* operator&() const 106 | { 107 | return &cv_mat_; 108 | } 109 | }; 110 | 111 | } // namespace MTK 112 | 113 | #endif /* WRAPPED_CV_MAT_HPP_ */ 114 | -------------------------------------------------------------------------------- /FAST_LIO/include/common_lib.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_LIB_H 2 | #define COMMON_LIB_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | #define USE_IKFOM 16 | 17 | #define PI_M (3.14159265358) 18 | #define G_m_s2 (9.81) // Gravaty const in GuangDong/China 19 | #define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) 20 | #define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) 21 | #define CUBE_LEN (6.0) 22 | #define LIDAR_SP_LEN (2) 23 | #define INIT_COV (1) 24 | #define NUM_MATCH_POINTS (5) 25 | #define MAX_MEAS_DIM (10000) 26 | 27 | #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] 28 | #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] 29 | #define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) 32 | #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) 33 | 34 | typedef fast_lio::msg::Pose6D Pose6D; 35 | typedef pcl::PointXYZINormal PointType; 36 | typedef pcl::PointCloud PointCloudXYZI; 37 | typedef vector> PointVector; 38 | typedef Vector3d V3D; 39 | typedef Matrix3d M3D; 40 | typedef Vector3f V3F; 41 | typedef Matrix3f M3F; 42 | 43 | #define MD(a,b) Matrix 44 | #define VD(a) Matrix 45 | #define MF(a,b) Matrix 46 | #define VF(a) Matrix 47 | 48 | M3D Eye3d(M3D::Identity()); 49 | M3F Eye3f(M3F::Identity()); 50 | V3D Zero3d(0, 0, 0); 51 | V3F Zero3f(0, 0, 0); 52 | 53 | struct MeasureGroup // Lidar data and imu dates for the curent process 54 | { 55 | MeasureGroup() 56 | { 57 | lidar_beg_time = 0.0; 58 | this->lidar.reset(new PointCloudXYZI()); 59 | }; 60 | double lidar_beg_time; 61 | double lidar_end_time; 62 | PointCloudXYZI::Ptr lidar; 63 | deque imu; 64 | }; 65 | 66 | struct StatesGroup 67 | { 68 | StatesGroup() { 69 | this->rot_end = M3D::Identity(); 70 | this->pos_end = Zero3d; 71 | this->vel_end = Zero3d; 72 | this->bias_g = Zero3d; 73 | this->bias_a = Zero3d; 74 | this->gravity = Zero3d; 75 | this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; 76 | this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001; 77 | }; 78 | 79 | StatesGroup(const StatesGroup& b) { 80 | this->rot_end = b.rot_end; 81 | this->pos_end = b.pos_end; 82 | this->vel_end = b.vel_end; 83 | this->bias_g = b.bias_g; 84 | this->bias_a = b.bias_a; 85 | this->gravity = b.gravity; 86 | this->cov = b.cov; 87 | }; 88 | 89 | StatesGroup& operator=(const StatesGroup& b) 90 | { 91 | this->rot_end = b.rot_end; 92 | this->pos_end = b.pos_end; 93 | this->vel_end = b.vel_end; 94 | this->bias_g = b.bias_g; 95 | this->bias_a = b.bias_a; 96 | this->gravity = b.gravity; 97 | this->cov = b.cov; 98 | return *this; 99 | }; 100 | 101 | StatesGroup operator+(const Matrix &state_add) 102 | { 103 | StatesGroup a; 104 | a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 105 | a.pos_end = this->pos_end + state_add.block<3,1>(3,0); 106 | a.vel_end = this->vel_end + state_add.block<3,1>(6,0); 107 | a.bias_g = this->bias_g + state_add.block<3,1>(9,0); 108 | a.bias_a = this->bias_a + state_add.block<3,1>(12,0); 109 | a.gravity = this->gravity + state_add.block<3,1>(15,0); 110 | a.cov = this->cov; 111 | return a; 112 | }; 113 | 114 | StatesGroup& operator+=(const Matrix &state_add) 115 | { 116 | this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 117 | this->pos_end += state_add.block<3,1>(3,0); 118 | this->vel_end += state_add.block<3,1>(6,0); 119 | this->bias_g += state_add.block<3,1>(9,0); 120 | this->bias_a += state_add.block<3,1>(12,0); 121 | this->gravity += state_add.block<3,1>(15,0); 122 | return *this; 123 | }; 124 | 125 | Matrix operator-(const StatesGroup& b) 126 | { 127 | Matrix a; 128 | M3D rotd(b.rot_end.transpose() * this->rot_end); 129 | a.block<3,1>(0,0) = Log(rotd); 130 | a.block<3,1>(3,0) = this->pos_end - b.pos_end; 131 | a.block<3,1>(6,0) = this->vel_end - b.vel_end; 132 | a.block<3,1>(9,0) = this->bias_g - b.bias_g; 133 | a.block<3,1>(12,0) = this->bias_a - b.bias_a; 134 | a.block<3,1>(15,0) = this->gravity - b.gravity; 135 | return a; 136 | }; 137 | 138 | void resetpose() 139 | { 140 | this->rot_end = M3D::Identity(); 141 | this->pos_end = Zero3d; 142 | this->vel_end = Zero3d; 143 | } 144 | 145 | M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point 146 | V3D pos_end; // the estimated position at the end lidar point (world frame) 147 | V3D vel_end; // the estimated velocity at the end lidar point (world frame) 148 | V3D bias_g; // gyroscope bias 149 | V3D bias_a; // accelerator bias 150 | V3D gravity; // the estimated gravity acceleration 151 | Matrix cov; // states covariance 152 | }; 153 | 154 | template 155 | T rad2deg(T radians) 156 | { 157 | return radians * 180.0 / PI_M; 158 | } 159 | 160 | template 161 | T deg2rad(T degrees) 162 | { 163 | return degrees * PI_M / 180.0; 164 | } 165 | 166 | template 167 | auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ 168 | const Matrix &v, const Matrix &p, const Matrix &R) 169 | { 170 | Pose6D rot_kp; 171 | rot_kp.offset_time = t; 172 | for (int i = 0; i < 3; i++) 173 | { 174 | rot_kp.acc[i] = a(i); 175 | rot_kp.gyr[i] = g(i); 176 | rot_kp.vel[i] = v(i); 177 | rot_kp.pos[i] = p(i); 178 | for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); 179 | } 180 | return move(rot_kp); 181 | } 182 | 183 | /* comment 184 | plane equation: Ax + By + Cz + D = 0 185 | convert to: A/D*x + B/D*y + C/D*z = -1 186 | solve: A0*x0 = b0 187 | where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T 188 | normvec: normalized x0 189 | */ 190 | template 191 | bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) 192 | { 193 | MatrixXf A(point_num, 3); 194 | MatrixXf b(point_num, 1); 195 | b.setOnes(); 196 | b *= -1.0f; 197 | 198 | for (int j = 0; j < point_num; j++) 199 | { 200 | A(j,0) = point[j].x; 201 | A(j,1) = point[j].y; 202 | A(j,2) = point[j].z; 203 | } 204 | normvec = A.colPivHouseholderQr().solve(b); 205 | 206 | for (int j = 0; j < point_num; j++) 207 | { 208 | if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) 209 | { 210 | return false; 211 | } 212 | } 213 | 214 | normvec.normalize(); 215 | return true; 216 | } 217 | 218 | float calc_dist(PointType p1, PointType p2){ 219 | float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); 220 | return d; 221 | } 222 | 223 | template 224 | bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) 225 | { 226 | Matrix A; 227 | Matrix b; 228 | A.setZero(); 229 | b.setOnes(); 230 | b *= -1.0f; 231 | 232 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 233 | { 234 | A(j,0) = point[j].x; 235 | A(j,1) = point[j].y; 236 | A(j,2) = point[j].z; 237 | } 238 | 239 | Matrix normvec = A.colPivHouseholderQr().solve(b); 240 | 241 | T n = normvec.norm(); 242 | pca_result(0) = normvec(0) / n; 243 | pca_result(1) = normvec(1) / n; 244 | pca_result(2) = normvec(2) / n; 245 | pca_result(3) = 1.0 / n; 246 | 247 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 248 | { 249 | if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) 250 | { 251 | return false; 252 | } 253 | } 254 | return true; 255 | } 256 | 257 | double get_time_sec(const builtin_interfaces::msg::Time &time) 258 | { 259 | return rclcpp::Time(time).seconds(); 260 | } 261 | 262 | rclcpp::Time get_ros_time(double timestamp) 263 | { 264 | int32_t sec = std::floor(timestamp); 265 | auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9; 266 | uint32_t nanosec = nanosec_d; 267 | return rclcpp::Time(sec, nanosec); 268 | } 269 | 270 | #endif -------------------------------------------------------------------------------- /FAST_LIO/include/ikd-Tree/README.md: -------------------------------------------------------------------------------- 1 | # ikd-Tree 2 | ikd-Tree is an incremental k-d tree for robotic applications. 3 | -------------------------------------------------------------------------------- /FAST_LIO/include/ikd-Tree/ikd_Tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define EPSS 1e-6 14 | #define Minimal_Unbalanced_Tree_Size 10 15 | #define Multi_Thread_Rebuild_Point_Num 1500 16 | #define DOWNSAMPLE_SWITCH true 17 | #define ForceRebuildPercentage 0.2 18 | #define Q_LEN 1000000 19 | 20 | using namespace std; 21 | 22 | // typedef pcl::PointXYZINormal PointType; 23 | // typedef vector> PointVector; 24 | 25 | struct BoxPointType 26 | { 27 | float vertex_min[3]; 28 | float vertex_max[3]; 29 | }; 30 | 31 | enum operation_set 32 | { 33 | ADD_POINT, 34 | DELETE_POINT, 35 | DELETE_BOX, 36 | ADD_BOX, 37 | DOWNSAMPLE_DELETE, 38 | PUSH_DOWN 39 | }; 40 | 41 | enum delete_point_storage_set 42 | { 43 | NOT_RECORD, 44 | DELETE_POINTS_REC, 45 | MULTI_THREAD_REC 46 | }; 47 | 48 | template 49 | class KD_TREE 50 | { 51 | // using MANUAL_Q_ = MANUAL_Q; 52 | // using PointVector = std::vector; 53 | 54 | // using MANUAL_Q_ = MANUAL_Q; 55 | public: 56 | using PointVector = std::vector>; 57 | using Ptr = std::shared_ptr>; 58 | 59 | struct KD_TREE_NODE 60 | { 61 | PointType point; 62 | int division_axis; 63 | int TreeSize = 1; 64 | int invalid_point_num = 0; 65 | int down_del_num = 0; 66 | bool point_deleted = false; 67 | bool tree_deleted = false; 68 | bool point_downsample_deleted = false; 69 | bool tree_downsample_deleted = false; 70 | bool need_push_down_to_left = false; 71 | bool need_push_down_to_right = false; 72 | bool working_flag = false; 73 | pthread_mutex_t push_down_mutex_lock; 74 | float node_range_x[2], node_range_y[2], node_range_z[2]; 75 | float radius_sq; 76 | KD_TREE_NODE *left_son_ptr = nullptr; 77 | KD_TREE_NODE *right_son_ptr = nullptr; 78 | KD_TREE_NODE *father_ptr = nullptr; 79 | // For paper data record 80 | float alpha_del; 81 | float alpha_bal; 82 | }; 83 | 84 | struct Operation_Logger_Type 85 | { 86 | PointType point; 87 | BoxPointType boxpoint; 88 | bool tree_deleted, tree_downsample_deleted; 89 | operation_set op; 90 | }; 91 | // static const PointType zeroP; 92 | 93 | struct PointType_CMP 94 | { 95 | PointType point; 96 | float dist = 0.0; 97 | PointType_CMP(PointType p = PointType(), float d = INFINITY) 98 | { 99 | this->point = p; 100 | this->dist = d; 101 | }; 102 | bool operator<(const PointType_CMP &a) const 103 | { 104 | if (fabs(dist - a.dist) < 1e-10) 105 | return point.x < a.point.x; 106 | else 107 | return dist < a.dist; 108 | } 109 | }; 110 | 111 | class MANUAL_HEAP 112 | { 113 | 114 | public: 115 | MANUAL_HEAP(int max_capacity = 100) 116 | 117 | { 118 | cap = max_capacity; 119 | heap = new PointType_CMP[max_capacity]; 120 | heap_size = 0; 121 | } 122 | 123 | ~MANUAL_HEAP() 124 | { 125 | delete[] heap; 126 | } 127 | void pop() 128 | { 129 | if (heap_size == 0) 130 | return; 131 | heap[0] = heap[heap_size - 1]; 132 | heap_size--; 133 | MoveDown(0); 134 | return; 135 | } 136 | PointType_CMP top() 137 | { 138 | return heap[0]; 139 | } 140 | void push(PointType_CMP point) 141 | { 142 | if (heap_size >= cap) 143 | return; 144 | heap[heap_size] = point; 145 | FloatUp(heap_size); 146 | heap_size++; 147 | return; 148 | } 149 | int size() 150 | { 151 | return heap_size; 152 | } 153 | void clear() 154 | { 155 | heap_size = 0; 156 | return; 157 | } 158 | 159 | private: 160 | PointType_CMP *heap; 161 | void MoveDown(int heap_index) 162 | { 163 | int l = heap_index * 2 + 1; 164 | PointType_CMP tmp = heap[heap_index]; 165 | while (l < heap_size) 166 | { 167 | if (l + 1 < heap_size && heap[l] < heap[l + 1]) 168 | l++; 169 | if (tmp < heap[l]) 170 | { 171 | heap[heap_index] = heap[l]; 172 | heap_index = l; 173 | l = heap_index * 2 + 1; 174 | } 175 | else 176 | break; 177 | } 178 | heap[heap_index] = tmp; 179 | return; 180 | } 181 | void FloatUp(int heap_index) 182 | { 183 | int ancestor = (heap_index - 1) / 2; 184 | PointType_CMP tmp = heap[heap_index]; 185 | while (heap_index > 0) 186 | { 187 | if (heap[ancestor] < tmp) 188 | { 189 | heap[heap_index] = heap[ancestor]; 190 | heap_index = ancestor; 191 | ancestor = (heap_index - 1) / 2; 192 | } 193 | else 194 | break; 195 | } 196 | heap[heap_index] = tmp; 197 | return; 198 | } 199 | int heap_size = 0; 200 | int cap = 0; 201 | }; 202 | 203 | class MANUAL_Q 204 | { 205 | private: 206 | int head = 0, tail = 0, counter = 0; 207 | Operation_Logger_Type q[Q_LEN]; 208 | bool is_empty; 209 | 210 | public: 211 | void pop() 212 | { 213 | if (counter == 0) 214 | return; 215 | head++; 216 | head %= Q_LEN; 217 | counter--; 218 | if (counter == 0) 219 | is_empty = true; 220 | return; 221 | } 222 | Operation_Logger_Type front() 223 | { 224 | return q[head]; 225 | } 226 | Operation_Logger_Type back() 227 | { 228 | return q[tail]; 229 | } 230 | void clear() 231 | { 232 | head = 0; 233 | tail = 0; 234 | counter = 0; 235 | is_empty = true; 236 | return; 237 | } 238 | void push(Operation_Logger_Type op) 239 | { 240 | q[tail] = op; 241 | counter++; 242 | if (is_empty) 243 | is_empty = false; 244 | tail++; 245 | tail %= Q_LEN; 246 | } 247 | bool empty() 248 | { 249 | return is_empty; 250 | } 251 | int size() 252 | { 253 | return counter; 254 | } 255 | }; 256 | 257 | private: 258 | // Multi-thread Tree Rebuild 259 | bool termination_flag = false; 260 | bool rebuild_flag = false; 261 | pthread_t rebuild_thread; 262 | pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; 263 | pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; 264 | // queue Rebuild_Logger; 265 | MANUAL_Q Rebuild_Logger; 266 | PointVector Rebuild_PCL_Storage; 267 | KD_TREE_NODE **Rebuild_Ptr = nullptr; 268 | int search_mutex_counter = 0; 269 | static void *multi_thread_ptr(void *arg); 270 | void multi_thread_rebuild(); 271 | void start_thread(); 272 | void stop_thread(); 273 | void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); 274 | // KD Tree Functions and augmented variables 275 | int Treesize_tmp = 0, Validnum_tmp = 0; 276 | float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; 277 | float delete_criterion_param = 0.5f; 278 | float balance_criterion_param = 0.7f; 279 | float downsample_size = 0.2f; 280 | bool Delete_Storage_Disabled = false; 281 | KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; 282 | PointVector Points_deleted; 283 | PointVector Downsample_Storage; 284 | PointVector Multithread_Points_deleted; 285 | void InitTreeNode(KD_TREE_NODE *root); 286 | void Test_Lock_States(KD_TREE_NODE *root); 287 | void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); 288 | void Rebuild(KD_TREE_NODE **root); 289 | int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); 290 | void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); 291 | void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); 292 | void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); 293 | void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue 294 | void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); 295 | void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); 296 | bool Criterion_Check(KD_TREE_NODE *root); 297 | void Push_Down(KD_TREE_NODE *root); 298 | void Update(KD_TREE_NODE *root); 299 | void delete_tree_nodes(KD_TREE_NODE **root); 300 | void downsample(KD_TREE_NODE **root); 301 | bool same_point(PointType a, PointType b); 302 | float calc_dist(PointType a, PointType b); 303 | float calc_box_dist(KD_TREE_NODE *node, PointType point); 304 | static bool point_cmp_x(PointType a, PointType b); 305 | static bool point_cmp_y(PointType a, PointType b); 306 | static bool point_cmp_z(PointType a, PointType b); 307 | 308 | public: 309 | KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); 310 | ~KD_TREE(); 311 | void Set_delete_criterion_param(float delete_param) 312 | { 313 | delete_criterion_param = delete_param; 314 | } 315 | void Set_balance_criterion_param(float balance_param) 316 | { 317 | balance_criterion_param = balance_param; 318 | } 319 | void set_downsample_param(float downsample_param) 320 | { 321 | downsample_size = downsample_param; 322 | } 323 | void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); 324 | int size(); 325 | int validnum(); 326 | void root_alpha(float &alpha_bal, float &alpha_del); 327 | void Build(PointVector point_cloud); 328 | void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); 329 | void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); 330 | void Radius_Search(PointType point, const float radius, PointVector &Storage); 331 | int Add_Points(PointVector &PointToAdd, bool downsample_on); 332 | void Add_Point_Boxes(vector &BoxPoints); 333 | void Delete_Points(PointVector &PointToDel); 334 | int Delete_Point_Boxes(vector &BoxPoints); 335 | void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); 336 | void acquire_removed_points(PointVector &removed_points); 337 | BoxPointType tree_range(); 338 | PointVector PCL_Storage; 339 | KD_TREE_NODE *Root_Node = nullptr; 340 | int max_queue_size = 0; 341 | }; 342 | 343 | // template 344 | // PointType KD_TREE::zeroP = PointType(0,0,0); 345 | -------------------------------------------------------------------------------- /FAST_LIO/include/so3_math.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_MATH_H 2 | #define SO3_MATH_H 3 | 4 | #include 5 | #include 6 | 7 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 8 | 9 | template 10 | Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) 11 | { 12 | Eigen::Matrix skew_sym_mat; 13 | skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; 14 | return skew_sym_mat; 15 | } 16 | 17 | template 18 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 19 | { 20 | T ang_norm = ang.norm(); 21 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 22 | if (ang_norm > 0.0000001) 23 | { 24 | Eigen::Matrix r_axis = ang / ang_norm; 25 | Eigen::Matrix K; 26 | K << SKEW_SYM_MATRX(r_axis); 27 | /// Roderigous Tranformation 28 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 29 | } 30 | else 31 | { 32 | return Eye3; 33 | } 34 | } 35 | 36 | template 37 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 38 | { 39 | T ang_vel_norm = ang_vel.norm(); 40 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 41 | 42 | if (ang_vel_norm > 0.0000001) 43 | { 44 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 45 | Eigen::Matrix K; 46 | 47 | K << SKEW_SYM_MATRX(r_axis); 48 | 49 | T r_ang = ang_vel_norm * dt; 50 | 51 | /// Roderigous Tranformation 52 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 53 | } 54 | else 55 | { 56 | return Eye3; 57 | } 58 | } 59 | 60 | template 61 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 62 | { 63 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 64 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 65 | if (norm > 0.00001) 66 | { 67 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 68 | Eigen::Matrix K; 69 | K << SKEW_SYM_MATRX(r_ang); 70 | 71 | /// Roderigous Tranformation 72 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 73 | } 74 | else 75 | { 76 | return Eye3; 77 | } 78 | } 79 | 80 | /* Logrithm of a Rotation Matrix */ 81 | template 82 | Eigen::Matrix Log(const Eigen::Matrix &R) 83 | { 84 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); 85 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 86 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 87 | } 88 | 89 | template 90 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) 91 | { 92 | T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); 93 | bool singular = sy < 1e-6; 94 | T x, y, z; 95 | if(!singular) 96 | { 97 | x = atan2(rot(2, 1), rot(2, 2)); 98 | y = atan2(-rot(2, 0), sy); 99 | z = atan2(rot(1, 0), rot(0, 0)); 100 | } 101 | else 102 | { 103 | x = atan2(-rot(1, 2), rot(1, 1)); 104 | y = atan2(-rot(2, 0), sy); 105 | z = 0; 106 | } 107 | Eigen::Matrix ang(x, y, z); 108 | return ang; 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /FAST_LIO/include/use-ikfom.hpp: -------------------------------------------------------------------------------- 1 | #ifndef USE_IKFOM_H 2 | #define USE_IKFOM_H 3 | 4 | #include 5 | 6 | typedef MTK::vect<3, double> vect3; 7 | typedef MTK::SO3 SO3; 8 | typedef MTK::S2 S2; 9 | typedef MTK::vect<1, double> vect1; 10 | typedef MTK::vect<2, double> vect2; 11 | 12 | MTK_BUILD_MANIFOLD(state_ikfom, 13 | ((vect3, pos)) 14 | ((SO3, rot)) 15 | ((SO3, offset_R_L_I)) 16 | ((vect3, offset_T_L_I)) 17 | ((vect3, vel)) 18 | ((vect3, bg)) 19 | ((vect3, ba)) 20 | ((S2, grav)) 21 | ); 22 | 23 | MTK_BUILD_MANIFOLD(input_ikfom, 24 | ((vect3, acc)) 25 | ((vect3, gyro)) 26 | ); 27 | 28 | MTK_BUILD_MANIFOLD(process_noise_ikfom, 29 | ((vect3, ng)) 30 | ((vect3, na)) 31 | ((vect3, nbg)) 32 | ((vect3, nba)) 33 | ); 34 | 35 | MTK::get_cov::type process_noise_cov() 36 | { 37 | MTK::get_cov::type cov = MTK::get_cov::type::Zero(); 38 | MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03 39 | MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 40 | MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 41 | MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01 42 | return cov; 43 | } 44 | 45 | //double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia 46 | //vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); 47 | Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in) 48 | { 49 | Eigen::Matrix res = Eigen::Matrix::Zero(); 50 | vect3 omega; 51 | in.gyro.boxminus(omega, s.bg); 52 | vect3 a_inertial = s.rot * (in.acc-s.ba); 53 | for(int i = 0; i < 3; i++ ){ 54 | res(i) = s.vel[i]; 55 | res(i + 3) = omega[i]; 56 | res(i + 12) = a_inertial[i] + s.grav[i]; 57 | } 58 | return res; 59 | } 60 | 61 | Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in) 62 | { 63 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 64 | cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); 65 | vect3 acc_; 66 | in.acc.boxminus(acc_, s.ba); 67 | vect3 omega; 68 | in.gyro.boxminus(omega, s.bg); 69 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); 70 | cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); 71 | Eigen::Matrix vec = Eigen::Matrix::Zero(); 72 | Eigen::Matrix grav_matrix; 73 | s.S2_Mx(grav_matrix, vec, 21); 74 | cov.template block<3, 2>(12, 21) = grav_matrix; 75 | cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 76 | return cov; 77 | } 78 | 79 | 80 | Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) 81 | { 82 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 83 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); 84 | cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); 85 | cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); 86 | cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); 87 | return cov; 88 | } 89 | 90 | vect3 SO3ToEuler(const SO3 &orient) 91 | { 92 | Eigen::Matrix _ang; 93 | Eigen::Vector4d q_data = orient.coeffs().transpose(); 94 | //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; 95 | double sqw = q_data[3]*q_data[3]; 96 | double sqx = q_data[0]*q_data[0]; 97 | double sqy = q_data[1]*q_data[1]; 98 | double sqz = q_data[2]*q_data[2]; 99 | double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor 100 | double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; 101 | 102 | if (test > 0.49999*unit) { // singularity at north pole 103 | 104 | _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; 105 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 106 | vect3 euler_ang(temp, 3); 107 | return euler_ang; 108 | } 109 | if (test < -0.49999*unit) { // singularity at south pole 110 | _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; 111 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 112 | vect3 euler_ang(temp, 3); 113 | return euler_ang; 114 | } 115 | 116 | _ang << 117 | std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), 118 | std::asin (2*test/unit), 119 | std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); 120 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 121 | vect3 euler_ang(temp, 3); 122 | // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw 123 | return euler_ang; 124 | } 125 | 126 | #endif -------------------------------------------------------------------------------- /FAST_LIO/launch/mapping.launch.py: -------------------------------------------------------------------------------- 1 | import os.path 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument 7 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 8 | from launch.conditions import IfCondition 9 | 10 | from launch_ros.actions import Node 11 | 12 | 13 | def generate_launch_description(): 14 | package_path = get_package_share_directory('fast_lio') 15 | default_config_path = os.path.join(package_path, 'config') 16 | default_rviz_config_path = os.path.join( 17 | package_path, 'rviz', 'loam_livox.rviz') 18 | 19 | use_sim_time = LaunchConfiguration('use_sim_time') 20 | config_path = LaunchConfiguration('config_path') 21 | rviz_use = LaunchConfiguration('rviz') 22 | rviz_cfg = LaunchConfiguration('rviz_cfg') 23 | 24 | declare_use_sim_time_cmd = DeclareLaunchArgument( 25 | 'use_sim_time', default_value='false', 26 | description='Use simulation (Gazebo) clock if true' 27 | ) 28 | declare_config_path_cmd = DeclareLaunchArgument( 29 | 'config_path', default_value=default_config_path, 30 | description='Yaml config file path' 31 | ) 32 | declare_rviz_cmd = DeclareLaunchArgument( 33 | 'rviz', default_value='true', 34 | description='Use RViz to monitor results' 35 | ) 36 | declare_rviz_config_path_cmd = DeclareLaunchArgument( 37 | 'rviz_cfg', default_value=default_rviz_config_path, 38 | description='RViz config file path' 39 | ) 40 | 41 | config = os.path.join( 42 | get_package_share_directory('fast_lio'), 'config', 'mid360.yaml') 43 | 44 | fast_lio_node = Node( 45 | package='fast_lio', 46 | executable='fastlio_mapping', 47 | parameters=[ 48 | config 49 | ], 50 | output='screen', 51 | remappings=[('/Odometry','/state_estimation')] 52 | ) 53 | rviz_node = Node( 54 | package='rviz2', 55 | executable='rviz2', 56 | arguments=['-d', rviz_cfg], 57 | condition=IfCondition(rviz_use) 58 | ) 59 | 60 | ld = LaunchDescription() 61 | ld.add_action(declare_use_sim_time_cmd) 62 | ld.add_action(declare_config_path_cmd) 63 | ld.add_action(declare_rviz_cmd) 64 | ld.add_action(declare_rviz_config_path_cmd) 65 | 66 | ld.add_action(fast_lio_node) 67 | # ld.add_action(rviz_node) 68 | 69 | return ld 70 | -------------------------------------------------------------------------------- /FAST_LIO/launch/relocalization.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource, FrontendLaunchDescriptionSource 7 | from launch_ros.actions import Node 8 | from launch.substitutions import LaunchConfiguration 9 | 10 | def generate_launch_description(): 11 | 12 | # icp relocalization 13 | map_odom_trans = Node( 14 | package='icp_relocalization', 15 | executable='transform_publisher', 16 | name='transform_publisher', 17 | output='screen' 18 | ) 19 | 20 | icp_node = Node( 21 | package='icp_relocalization', 22 | executable='icp_node', 23 | name='icp_node', 24 | output='screen', 25 | parameters=[ 26 | # --- Blue --- 27 | # {'initial_x':14.16}, 28 | # {'initial_y':5.35}, 29 | # {'initial_z':0.0}, 30 | # {'initial_a':3.14}, 31 | 32 | # --- Red --- 33 | {'initial_x':0.0}, 34 | {'initial_y':0.0}, 35 | {'initial_z':0.0}, 36 | {'initial_a':0.0}, 37 | 38 | {'map_voxel_leaf_size':0.5}, 39 | {'cloud_voxel_leaf_size':0.3}, 40 | {'map_frame_id':'map'}, 41 | {'solver_max_iter':100}, 42 | {'max_correspondence_distance':0.1}, 43 | {'RANSAC_outlier_rejection_threshold':0.5}, 44 | {'map_path':'/home/sentry_ws/src/FAST_LIO/PCD/scans.pcd'}, 45 | {'fitness_score_thre':0.2}, # 是最近点距离的平均值，越小越严格 46 | {'converged_count_thre':40}, # pcl pub at 20 hz, 2s 47 | {'pcl_type':'livox'}, 48 | ], 49 | ) 50 | 51 | # fast-lio localization 52 | config = os.path.join( 53 | get_package_share_directory('fast_lio'), 'config', 'fast_lio_relocalization_param.yaml') 54 | 55 | fast_lio_node = Node( 56 | package='fast_lio', 57 | executable='fastlio_mapping', 58 | parameters=[ 59 | config 60 | ], 61 | output='screen', 62 | remappings=[('/Odometry','/state_estimation')] 63 | ) 64 | 65 | rviz_config_file = os.path.join( 66 | get_package_share_directory('fast_lio'), 'rviz', 'loam_livox.rviz') 67 | start_rviz = Node( 68 | package='rviz2', 69 | executable='rviz2', 70 | arguments=['-d', rviz_config_file,'--ros-args', '--log-level', 'warn'], 71 | output='screen' 72 | ) 73 | 74 | delayed_start_lio = TimerAction( 75 | period=5.0, 76 | actions=[ 77 | icp_node, 78 | fast_lio_node 79 | ] 80 | ) 81 | 82 | ld = LaunchDescription() 83 | 84 | ld.add_action(map_odom_trans) 85 | ld.add_action(icp_node) 86 | ld.add_action(start_rviz) 87 | ld.add_action(delayed_start_lio) 88 | 89 | return ld 90 | -------------------------------------------------------------------------------- /FAST_LIO/msg/Pose6D.msg: -------------------------------------------------------------------------------- 1 | # the preintegrated Lidar states at the time of IMU measurements in a frame 2 | float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point 3 | float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin 4 | float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin 5 | float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin 6 | float64[3] pos # the preintegrated position (global frame) at the Lidar origin 7 | float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin -------------------------------------------------------------------------------- /FAST_LIO/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_lio 4 | 0.0.0 5 | 6 | 7 | This is a modified version of LOAM which is original algorithm 8 | 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 | claydergc 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | ament_cmake 20 | rosidl_default_generators 21 | geometry_msgs 22 | nav_msgs 23 | rclcpp 24 | std_msgs 25 | sensor_msgs 26 | common_interfaces 27 | tf2 28 | pcl_ros 29 | pcl_conversions 30 | livox_ros_driver2 31 | 32 | rosidl_default_runtime 33 | rosidl_interface_packages 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /FAST_LIO/rviz/loam_livox.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /CloudRegisteredBody1 9 | - /TF1/Frames1 10 | - /PoseWithCovariance1 11 | - /Odometry1 12 | - /Odometry1/Shape1 13 | Splitter Ratio: 0.5147679448127747 14 | Tree Height: 365 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Buffer Length: 1 55 | Class: rviz_default_plugins/Path 56 | Color: 25; 255; 0 57 | Enabled: true 58 | Head Diameter: 0.30000001192092896 59 | Head Length: 0.20000000298023224 60 | Length: 0.30000001192092896 61 | Line Style: Lines 62 | Line Width: 0.029999999329447746 63 | Name: Path 64 | Offset: 65 | X: 0 66 | Y: 0 67 | Z: 0 68 | Pose Color: 255; 85; 255 69 | Pose Style: None 70 | Radius: 0.029999999329447746 71 | Shaft Diameter: 0.10000000149011612 72 | Shaft Length: 0.10000000149011612 73 | Topic: 74 | Depth: 5 75 | Durability Policy: Volatile 76 | Filter size: 10 77 | History Policy: Keep Last 78 | Reliability Policy: Reliable 79 | Value: /path 80 | Value: true 81 | - Alpha: 0.30000001192092896 82 | Autocompute Intensity Bounds: true 83 | Autocompute Value Bounds: 84 | Max Value: 1.7963272333145142 85 | Min Value: -0.5706579089164734 86 | Value: true 87 | Axis: Z 88 | Channel Name: intensity 89 | Class: rviz_default_plugins/PointCloud2 90 | Color: 255; 255; 255 91 | Color Transformer: AxisColor 92 | Decay Time: 30 93 | Enabled: true 94 | Invert Rainbow: false 95 | Max Color: 255; 255; 255 96 | Max Intensity: 186 97 | Min Color: 0; 0; 0 98 | Min Intensity: 0 99 | Name: CloudRegistered 100 | Position Transformer: XYZ 101 | Selectable: true 102 | Size (Pixels): 1 103 | Size (m): 0.05000000074505806 104 | Style: Points 105 | Topic: 106 | Depth: 5 107 | Durability Policy: Volatile 108 | Filter size: 10 109 | History Policy: Keep Last 110 | Reliability Policy: Reliable 111 | Value: /cloud_registered 112 | Use Fixed Frame: true 113 | Use rainbow: true 114 | Value: true 115 | - Alpha: 0.30000001192092896 116 | Autocompute Intensity Bounds: true 117 | Autocompute Value Bounds: 118 | Max Value: 1.3406777381896973 119 | Min Value: -1.7319529056549072 120 | Value: true 121 | Axis: Z 122 | Channel Name: intensity 123 | Class: rviz_default_plugins/PointCloud2 124 | Color: 255; 255; 255 125 | Color Transformer: AxisColor 126 | Decay Time: 0 127 | Enabled: false 128 | Invert Rainbow: false 129 | Max Color: 255; 255; 255 130 | Max Intensity: 186 131 | Min Color: 0; 0; 0 132 | Min Intensity: 0 133 | Name: CloudRegisteredBody 134 | Position Transformer: XYZ 135 | Selectable: true 136 | Size (Pixels): 1 137 | Size (m): 0.05000000074505806 138 | Style: Points 139 | Topic: 140 | Depth: 5 141 | Durability Policy: Volatile 142 | Filter size: 10 143 | History Policy: Keep Last 144 | Reliability Policy: Reliable 145 | Value: /cloud_registered_body 146 | Use Fixed Frame: true 147 | Use rainbow: true 148 | Value: false 149 | - Class: octomap_rviz_plugins/OccupancyGrid 150 | Enabled: true 151 | Max. Height Display: 3.4028234663852886e+38 152 | Max. Octree Depth: 16 153 | Min. Height Display: -3.4028234663852886e+38 154 | Name: OccupancyGrid 155 | Topic: 156 | Depth: 5 157 | Durability Policy: Volatile 158 | Filter size: 10 159 | History Policy: Keep Last 160 | Reliability Policy: Reliable 161 | Value: /octomap_binary 162 | Value: true 163 | Voxel Alpha: 0.30000001192092896 164 | Voxel Coloring: Z-Axis 165 | Voxel Rendering: Occupied Voxels 166 | - Class: rviz_default_plugins/TF 167 | Enabled: true 168 | Frame Timeout: 15 169 | Frames: 170 | All Enabled: false 171 | base_link: 172 | Value: true 173 | camera_link: 174 | Value: true 175 | chassis_link: 176 | Value: true 177 | gimbal_odom: 178 | Value: true 179 | imu_link: 180 | Value: true 181 | lidar_link: 182 | Value: true 183 | pitch_link: 184 | Value: true 185 | shoot_link: 186 | Value: true 187 | usb_camera_a_link: 188 | Value: true 189 | usb_camera_b_link: 190 | Value: true 191 | yaw_link: 192 | Value: true 193 | Marker Scale: 1 194 | Name: TF 195 | Show Arrows: true 196 | Show Axes: true 197 | Show Names: true 198 | Tree: 199 | {} 200 | Update Interval: 0 201 | Value: true 202 | - Alpha: 0.699999988079071 203 | Class: rviz_default_plugins/Map 204 | Color Scheme: map 205 | Draw Behind: false 206 | Enabled: true 207 | Name: Map 208 | Topic: 209 | Depth: 5 210 | Durability Policy: Volatile 211 | Filter size: 10 212 | History Policy: Keep Last 213 | Reliability Policy: Reliable 214 | Value: /projected_map 215 | Update Topic: 216 | Depth: 5 217 | Durability Policy: Volatile 218 | History Policy: Keep Last 219 | Reliability Policy: Reliable 220 | Value: /projected_map_updates 221 | Use Timestamp: false 222 | Value: true 223 | - Class: rviz_common/Group 224 | Displays: 225 | - Alpha: 1 226 | Autocompute Intensity Bounds: true 227 | Autocompute Value Bounds: 228 | Max Value: 10 229 | Min Value: -10 230 | Value: true 231 | Axis: Z 232 | Channel Name: intensity 233 | Class: rviz_default_plugins/PointCloud2 234 | Color: 255; 255; 255 235 | Color Transformer: FlatColor 236 | Decay Time: 0 237 | Enabled: true 238 | Invert Rainbow: false 239 | Max Color: 255; 255; 255 240 | Max Intensity: 4096 241 | Min Color: 0; 0; 0 242 | Min Intensity: 0 243 | Name: Prior Map 244 | Position Transformer: XYZ 245 | Selectable: true 246 | Size (Pixels): 3 247 | Size (m): 0.05000000074505806 248 | Style: Flat Squares 249 | Topic: 250 | Depth: 5 251 | Durability Policy: Volatile 252 | Filter size: 10 253 | History Policy: Keep Last 254 | Reliability Policy: Reliable 255 | Value: /template_cloud 256 | Use Fixed Frame: true 257 | Use rainbow: true 258 | Value: true 259 | - Alpha: 1 260 | Autocompute Intensity Bounds: true 261 | Autocompute Value Bounds: 262 | Max Value: 10 263 | Min Value: -10 264 | Value: true 265 | Axis: Z 266 | Channel Name: intensity 267 | Class: rviz_default_plugins/PointCloud2 268 | Color: 255; 0; 0 269 | Color Transformer: FlatColor 270 | Decay Time: 0 271 | Enabled: true 272 | Invert Rainbow: false 273 | Max Color: 255; 255; 255 274 | Max Intensity: 4096 275 | Min Color: 0; 0; 0 276 | Min Intensity: 0 277 | Name: Transformed Sensor Cloud 278 | Position Transformer: XYZ 279 | Selectable: true 280 | Size (Pixels): 3 281 | Size (m): 0.05000000074505806 282 | Style: Flat Squares 283 | Topic: 284 | Depth: 5 285 | Durability Policy: Volatile 286 | Filter size: 10 287 | History Policy: Keep Last 288 | Reliability Policy: Reliable 289 | Value: /icp_cloud 290 | Use Fixed Frame: true 291 | Use rainbow: true 292 | Value: true 293 | - Alpha: 1 294 | Axes Length: 1 295 | Axes Radius: 0.10000000149011612 296 | Class: rviz_default_plugins/Pose 297 | Color: 255; 25; 0 298 | Enabled: true 299 | Head Length: 0.30000001192092896 300 | Head Radius: 0.10000000149011612 301 | Name: Pose 302 | Shaft Length: 1 303 | Shaft Radius: 0.05000000074505806 304 | Shape: Arrow 305 | Topic: 306 | Depth: 5 307 | Durability Policy: Volatile 308 | Filter size: 10 309 | History Policy: Keep Last 310 | Reliability Policy: Reliable 311 | Value: /initial_pose 312 | Value: true 313 | Enabled: true 314 | Name: SAC-IA ICP 315 | - Class: rviz_common/Group 316 | Displays: 317 | - Alpha: 1 318 | Autocompute Intensity Bounds: true 319 | Autocompute Value Bounds: 320 | Max Value: 10 321 | Min Value: -10 322 | Value: true 323 | Axis: Z 324 | Channel Name: intensity 325 | Class: rviz_default_plugins/PointCloud2 326 | Color: 255; 255; 255 327 | Color Transformer: FlatColor 328 | Decay Time: 0 329 | Enabled: true 330 | Invert Rainbow: false 331 | Max Color: 255; 255; 255 332 | Max Intensity: 4096 333 | Min Color: 0; 0; 0 334 | Min Intensity: 0 335 | Name: Transformed Sensor Cloud 336 | Position Transformer: XYZ 337 | Selectable: true 338 | Size (Pixels): 3 339 | Size (m): 0.10000000149011612 340 | Style: Flat Squares 341 | Topic: 342 | Depth: 5 343 | Durability Policy: Volatile 344 | Filter size: 10 345 | History Policy: Keep Last 346 | Reliability Policy: Reliable 347 | Value: /transformed_cloud 348 | Use Fixed Frame: true 349 | Use rainbow: true 350 | Value: true 351 | - Alpha: 1 352 | Autocompute Intensity Bounds: true 353 | Autocompute Value Bounds: 354 | Max Value: 10 355 | Min Value: -10 356 | Value: true 357 | Axis: Z 358 | Channel Name: intensity 359 | Class: rviz_default_plugins/PointCloud2 360 | Color: 0; 0; 255 361 | Color Transformer: FlatColor 362 | Decay Time: 0 363 | Enabled: true 364 | Invert Rainbow: false 365 | Max Color: 255; 255; 255 366 | Max Intensity: 4096 367 | Min Color: 0; 0; 0 368 | Min Intensity: 0 369 | Name: Prior Map 370 | Position Transformer: XYZ 371 | Selectable: true 372 | Size (Pixels): 3 373 | Size (m): 0.10000000149011612 374 | Style: Flat Squares 375 | Topic: 376 | Depth: 5 377 | Durability Policy: Volatile 378 | Filter size: 10 379 | History Policy: Keep Last 380 | Reliability Policy: Reliable 381 | Value: /prior_map 382 | Use Fixed Frame: true 383 | Use rainbow: true 384 | Value: true 385 | Enabled: true 386 | Name: Init ICP 387 | - Alpha: 1 388 | Axes Length: 1 389 | Axes Radius: 0.10000000149011612 390 | Class: rviz_default_plugins/PoseWithCovariance 391 | Color: 255; 255; 255 392 | Covariance: 393 | Orientation: 394 | Alpha: 0.5 395 | Color: 255; 255; 127 396 | Color Style: Unique 397 | Frame: Local 398 | Offset: 1 399 | Scale: 1 400 | Value: true 401 | Position: 402 | Alpha: 0.30000001192092896 403 | Color: 204; 51; 204 404 | Scale: 1 405 | Value: true 406 | Value: true 407 | Enabled: true 408 | Head Length: 0.30000001192092896 409 | Head Radius: 0.30000001192092896 410 | Name: PoseWithCovariance 411 | Shaft Length: 1 412 | Shaft Radius: 0.10000000149011612 413 | Shape: Arrow 414 | Topic: 415 | Depth: 5 416 | Durability Policy: Volatile 417 | Filter size: 10 418 | History Policy: Keep Last 419 | Reliability Policy: Reliable 420 | Value: /relocalization_result 421 | Value: true 422 | - Angle Tolerance: 0.10000000149011612 423 | Class: rviz_default_plugins/Odometry 424 | Covariance: 425 | Orientation: 426 | Alpha: 0.5 427 | Color: 255; 255; 127 428 | Color Style: Unique 429 | Frame: Local 430 | Offset: 1 431 | Scale: 1 432 | Value: true 433 | Position: 434 | Alpha: 0.30000001192092896 435 | Color: 204; 51; 204 436 | Scale: 1 437 | Value: true 438 | Value: true 439 | Enabled: true 440 | Keep: 100 441 | Name: Odometry 442 | Position Tolerance: 0.10000000149011612 443 | Shape: 444 | Alpha: 0.800000011920929 445 | Axes Length: 0.10000000149011612 446 | Axes Radius: 0.019999999552965164 447 | Color: 255; 25; 0 448 | Head Length: 0.10000000149011612 449 | Head Radius: 0.05000000074505806 450 | Shaft Length: 0.20000000298023224 451 | Shaft Radius: 0.009999999776482582 452 | Value: Arrow 453 | Topic: 454 | Depth: 5 455 | Durability Policy: Volatile 456 | Filter size: 10 457 | History Policy: Keep Last 458 | Reliability Policy: Reliable 459 | Value: /state_estimation 460 | Value: true 461 | Enabled: true 462 | Global Options: 463 | Background Color: 0; 0; 0 464 | Fixed Frame: map 465 | Frame Rate: 30 466 | Name: root 467 | Tools: 468 | - Class: rviz_default_plugins/Interact 469 | Hide Inactive Objects: true 470 | - Class: rviz_default_plugins/MoveCamera 471 | - Class: rviz_default_plugins/FocusCamera 472 | - Class: rviz_default_plugins/SetInitialPose 473 | Covariance x: 0.25 474 | Covariance y: 0.25 475 | Covariance yaw: 0.06853891909122467 476 | Topic: 477 | Depth: 5 478 | Durability Policy: Volatile 479 | History Policy: Keep Last 480 | Reliability Policy: Reliable 481 | Value: /initialpose 482 | - Class: rviz_default_plugins/PublishPoint 483 | Single click: true 484 | Topic: 485 | Depth: 5 486 | Durability Policy: Volatile 487 | History Policy: Keep Last 488 | Reliability Policy: Reliable 489 | Value: /clicked_point 490 | Transformation: 491 | Current: 492 | Class: rviz_default_plugins/TF 493 | Value: true 494 | Views: 495 | Current: 496 | Class: rviz_default_plugins/Orbit 497 | Distance: 16.77908706665039 498 | Enable Stereo Rendering: 499 | Stereo Eye Separation: 0.05999999865889549 500 | Stereo Focal Distance: 1 501 | Swap Stereo Eyes: false 502 | Value: false 503 | Focal Point: 504 | X: 0.2694757878780365 505 | Y: 0.6372836828231812 506 | Z: -0.8852465152740479 507 | Focal Shape Fixed Size: true 508 | Focal Shape Size: 0.05000000074505806 509 | Invert Z Axis: false 510 | Name: Current View 511 | Near Clip Distance: 0.009999999776482582 512 | Pitch: 1.5697963237762451 513 | Target Frame: 514 | Value: Orbit (rviz_default_plugins) 515 | Yaw: 3.142392873764038 516 | Saved: ~ 517 | Window Geometry: 518 | Displays: 519 | collapsed: true 520 | Height: 1016 521 | Hide Left Dock: true 522 | Hide Right Dock: true 523 | QMainWindow State: 000000ff00000000fd0000000400000000000001dc0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000299fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000299000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007420000003efc0100000002fb0000000800540069006d00650100000000000007420000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000007420000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 524 | Selection: 525 | collapsed: false 526 | Time: 527 | collapsed: false 528 | Tool Properties: 529 | collapsed: false 530 | Views: 531 | collapsed: true 532 | Width: 1858 533 | X: 62 534 | Y: 27 535 | -------------------------------------------------------------------------------- /FAST_LIO/src/IMU_Processing.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "use-ikfom.hpp" 23 | 24 | /// *************Preconfiguration 25 | 26 | #define MAX_INI_COUNT (10) 27 | 28 | const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); }; 29 | 30 | /// *************IMU Process and undistortion 31 | class ImuProcess 32 | { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | 36 | ImuProcess(); 37 | ~ImuProcess(); 38 | 39 | void Reset(); 40 | // void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); 41 | void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu); 42 | void set_extrinsic(const V3D &transl, const M3D &rot); 43 | void set_extrinsic(const V3D &transl); 44 | void set_extrinsic(const MD(4, 4) & T); 45 | void set_gyr_cov(const V3D &scaler); 46 | void set_acc_cov(const V3D &scaler); 47 | void set_gyr_bias_cov(const V3D &b_g); 48 | void set_acc_bias_cov(const V3D &b_a); 49 | Eigen::Matrix Q; 50 | void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); 51 | 52 | ofstream fout_imu; 53 | V3D cov_acc; 54 | V3D cov_gyr; 55 | V3D cov_acc_scale; 56 | V3D cov_gyr_scale; 57 | V3D cov_bias_gyr; 58 | V3D cov_bias_acc; 59 | double first_lidar_time; 60 | 61 | private: 62 | void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); 63 | void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); 64 | 65 | PointCloudXYZI::Ptr cur_pcl_un_; 66 | // sensor_msgs::ImuConstPtr last_imu_; 67 | sensor_msgs::msg::Imu::ConstSharedPtr last_imu_; 68 | deque v_imu_; 69 | vector IMUpose; 70 | vector v_rot_pcl_; 71 | M3D Lidar_R_wrt_IMU; 72 | V3D Lidar_T_wrt_IMU; 73 | V3D mean_acc; 74 | V3D mean_gyr; 75 | V3D angvel_last; 76 | V3D acc_s_last; 77 | double start_timestamp_; 78 | double last_lidar_end_time_; 79 | int init_iter_num = 1; 80 | bool b_first_frame_ = true; 81 | bool imu_need_init_ = true; 82 | }; 83 | 84 | ImuProcess::ImuProcess() 85 | : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) 86 | { 87 | init_iter_num = 1; 88 | Q = process_noise_cov(); 89 | cov_acc = V3D(0.1, 0.1, 0.1); 90 | cov_gyr = V3D(0.1, 0.1, 0.1); 91 | cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); 92 | cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); 93 | mean_acc = V3D(0, 0, -1.0); 94 | mean_gyr = V3D(0, 0, 0); 95 | angvel_last = Zero3d; 96 | Lidar_T_wrt_IMU = Zero3d; 97 | Lidar_R_wrt_IMU = Eye3d; 98 | last_imu_.reset(new sensor_msgs::msg::Imu()); 99 | } 100 | 101 | ImuProcess::~ImuProcess() {} 102 | 103 | void ImuProcess::Reset() 104 | { 105 | // ROS_WARN("Reset ImuProcess"); 106 | mean_acc = V3D(0, 0, -1.0); 107 | mean_gyr = V3D(0, 0, 0); 108 | angvel_last = Zero3d; 109 | imu_need_init_ = true; 110 | start_timestamp_ = -1; 111 | init_iter_num = 1; 112 | v_imu_.clear(); 113 | IMUpose.clear(); 114 | last_imu_.reset(new sensor_msgs::msg::Imu()); 115 | cur_pcl_un_.reset(new PointCloudXYZI()); 116 | } 117 | 118 | void ImuProcess::set_extrinsic(const MD(4, 4) & T) 119 | { 120 | Lidar_T_wrt_IMU = T.block<3, 1>(0, 3); 121 | Lidar_R_wrt_IMU = T.block<3, 3>(0, 0); 122 | } 123 | 124 | void ImuProcess::set_extrinsic(const V3D &transl) 125 | { 126 | Lidar_T_wrt_IMU = transl; 127 | Lidar_R_wrt_IMU.setIdentity(); 128 | } 129 | 130 | void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) 131 | { 132 | Lidar_T_wrt_IMU = transl; 133 | Lidar_R_wrt_IMU = rot; 134 | } 135 | 136 | void ImuProcess::set_gyr_cov(const V3D &scaler) 137 | { 138 | cov_gyr_scale = scaler; 139 | } 140 | 141 | void ImuProcess::set_acc_cov(const V3D &scaler) 142 | { 143 | cov_acc_scale = scaler; 144 | } 145 | 146 | void ImuProcess::set_gyr_bias_cov(const V3D &b_g) 147 | { 148 | cov_bias_gyr = b_g; 149 | } 150 | 151 | void ImuProcess::set_acc_bias_cov(const V3D &b_a) 152 | { 153 | cov_bias_acc = b_a; 154 | } 155 | 156 | void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N) 157 | { 158 | /** 1. initializing the gravity, gyro bias, acc and gyro covariance 159 | ** 2. normalize the acceleration measurenments to unit gravity **/ 160 | 161 | V3D cur_acc, cur_gyr; 162 | 163 | if (b_first_frame_) 164 | { 165 | Reset(); 166 | N = 1; 167 | b_first_frame_ = false; 168 | const auto &imu_acc = meas.imu.front()->linear_acceleration; 169 | const auto &gyr_acc = meas.imu.front()->angular_velocity; 170 | mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; 171 | mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 172 | first_lidar_time = meas.lidar_beg_time; 173 | } 174 | 175 | for (const auto &imu : meas.imu) 176 | { 177 | const auto &imu_acc = imu->linear_acceleration; 178 | const auto &gyr_acc = imu->angular_velocity; 179 | cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; 180 | cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 181 | 182 | mean_acc += (cur_acc - mean_acc) / N; 183 | mean_gyr += (cur_gyr - mean_gyr) / N; 184 | 185 | cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); 186 | cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); 187 | 188 | // cout<<"acc norm: "<::cov init_P = kf_state.get_P(); 202 | init_P.setIdentity(); 203 | init_P(6, 6) = init_P(7, 7) = init_P(8, 8) = 0.00001; 204 | init_P(9, 9) = init_P(10, 10) = init_P(11, 11) = 0.00001; 205 | init_P(15, 15) = init_P(16, 16) = init_P(17, 17) = 0.0001; 206 | init_P(18, 18) = init_P(19, 19) = init_P(20, 20) = 0.001; 207 | init_P(21, 21) = init_P(22, 22) = 0.00001; 208 | kf_state.change_P(init_P); 209 | last_imu_ = meas.imu.back(); 210 | } 211 | 212 | void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) 213 | { 214 | /*** add the imu of the last frame-tail to the of current frame-head ***/ 215 | auto v_imu = meas.imu; 216 | v_imu.push_front(last_imu_); 217 | const double &imu_beg_time = rclcpp::Time(v_imu.front()->header.stamp).seconds(); 218 | const double &imu_end_time = rclcpp::Time(v_imu.back()->header.stamp).seconds(); 219 | const double &pcl_beg_time = meas.lidar_beg_time; 220 | const double &pcl_end_time = meas.lidar_end_time; 221 | 222 | /*** sort point clouds by offset time ***/ 223 | pcl_out = *(meas.lidar); 224 | sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); 225 | // cout<<"[ IMU Process ]: Process lidar from "<header.stamp).seconds(); 246 | double head_stamp = rclcpp::Time(head->header.stamp).seconds(); 247 | 248 | if (tail_stamp < last_lidar_end_time_) 249 | continue; 250 | 251 | angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 252 | 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 253 | 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); 254 | acc_avr << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 255 | 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 256 | 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); 257 | 258 | // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; 259 | 260 | acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; 261 | 262 | if (head_stamp < last_lidar_end_time_) 263 | { 264 | dt = tail_stamp - last_lidar_end_time_; 265 | // dt = tail->header.stamp.toSec() - pcl_beg_time; 266 | } 267 | else 268 | { 269 | dt = tail_stamp - head_stamp; 270 | } 271 | 272 | in.acc = acc_avr; 273 | in.gyro = angvel_avr; 274 | Q.block<3, 3>(0, 0).diagonal() = cov_gyr; 275 | Q.block<3, 3>(3, 3).diagonal() = cov_acc; 276 | Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; 277 | Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; 278 | kf_state.predict(dt, Q, in); 279 | 280 | /* save the poses at each IMU measurements */ 281 | imu_state = kf_state.get_x(); 282 | angvel_last = angvel_avr - imu_state.bg; 283 | acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); 284 | for (int i = 0; i < 3; i++) 285 | { 286 | acc_s_last[i] += imu_state.grav[i]; 287 | } 288 | double &&offs_t = tail_stamp - pcl_beg_time; 289 | IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); 290 | } 291 | 292 | /*** calculated the pos and attitude prediction at the frame-end ***/ 293 | double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; 294 | dt = note * (pcl_end_time - imu_end_time); 295 | kf_state.predict(dt, Q, in); 296 | 297 | imu_state = kf_state.get_x(); 298 | last_imu_ = meas.imu.back(); 299 | last_lidar_end_time_ = pcl_end_time; 300 | 301 | /*** undistort each lidar point (backward propagation) ***/ 302 | if (pcl_out.points.begin() == pcl_out.points.end()) 303 | return; 304 | auto it_pcl = pcl_out.points.end() - 1; 305 | for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) 306 | { 307 | auto head = it_kp - 1; 308 | auto tail = it_kp; 309 | R_imu << MAT_FROM_ARRAY(head->rot); 310 | // cout<<"head imu acc: "<vel); 312 | pos_imu << VEC_FROM_ARRAY(head->pos); 313 | acc_imu << VEC_FROM_ARRAY(tail->acc); 314 | angvel_avr << VEC_FROM_ARRAY(tail->gyr); 315 | 316 | for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) 317 | { 318 | dt = it_pcl->curvature / double(1000) - head->offset_time; 319 | 320 | /* Transform to the 'end' frame, using only the rotation 321 | * Note: Compensation direction is INVERSE of Frame's moving direction 322 | * So if we want to compensate a point at timestamp-i to the frame-e 323 | * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ 324 | M3D R_i(R_imu * Exp(angvel_avr, dt)); 325 | 326 | V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); 327 | V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); 328 | V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I); // not accurate! 329 | 330 | // save Undistorted points and their rotation 331 | it_pcl->x = P_compensate(0); 332 | it_pcl->y = P_compensate(1); 333 | it_pcl->z = P_compensate(2); 334 | 335 | if (it_pcl == pcl_out.points.begin()) 336 | break; 337 | } 338 | } 339 | } 340 | 341 | void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) 342 | { 343 | double t1, t2, t3; 344 | t1 = omp_get_wtime(); 345 | 346 | if (meas.imu.empty()) 347 | { 348 | return; 349 | }; 350 | assert(meas.lidar != nullptr); 351 | 352 | if (imu_need_init_) 353 | { 354 | /// The very first lidar frame 355 | IMU_init(meas, kf_state, init_iter_num); 356 | 357 | imu_need_init_ = true; 358 | 359 | last_imu_ = meas.imu.back(); 360 | 361 | state_ikfom imu_state = kf_state.get_x(); 362 | if (init_iter_num > MAX_INI_COUNT) 363 | { 364 | cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); 365 | imu_need_init_ = false; 366 | 367 | cov_acc = cov_acc_scale; 368 | cov_gyr = cov_gyr_scale; 369 | std::cout << "IMU Initial Done" << std::endl; 370 | // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ 371 | // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); 372 | fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out); 373 | } 374 | 375 | return; 376 | } 377 | 378 | UndistortPcl(meas, kf_state, *cur_pcl_un_); 379 | 380 | t2 = omp_get_wtime(); 381 | t3 = omp_get_wtime(); 382 | 383 | // cout<<"[ IMU Process ]: Time: "< 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | #define IS_VALID(a) ((abs(a) > 1e8) ? true : false) 10 | 11 | typedef pcl::PointXYZINormal PointType; 12 | typedef pcl::PointCloud PointCloudXYZI; 13 | 14 | enum LID_TYPE 15 | { 16 | AVIA = 1, 17 | VELO16, 18 | OUST64, 19 | MID360 20 | }; //{1, 2, 3} 21 | enum TIME_UNIT 22 | { 23 | SEC = 0, 24 | MS = 1, 25 | US = 2, 26 | NS = 3 27 | }; 28 | enum Feature 29 | { 30 | Nor, 31 | Poss_Plane, 32 | Real_Plane, 33 | Edge_Jump, 34 | Edge_Plane, 35 | Wire, 36 | ZeroPoint 37 | }; 38 | enum Surround 39 | { 40 | Prev, 41 | Next 42 | }; 43 | enum E_jump 44 | { 45 | Nr_nor, 46 | Nr_zero, 47 | Nr_180, 48 | Nr_inf, 49 | Nr_blind 50 | }; 51 | 52 | struct orgtype 53 | { 54 | double range; 55 | double dista; 56 | double angle[2]; 57 | double intersect; 58 | E_jump edj[2]; 59 | Feature ftype; 60 | orgtype() 61 | { 62 | range = 0; 63 | edj[Prev] = Nr_nor; 64 | edj[Next] = Nr_nor; 65 | ftype = Nor; 66 | intersect = 2; 67 | } 68 | }; 69 | 70 | namespace velodyne_ros 71 | { 72 | struct EIGEN_ALIGN16 Point 73 | { 74 | PCL_ADD_POINT4D; 75 | float intensity; 76 | float time; 77 | uint16_t ring; 78 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 79 | }; 80 | } // namespace velodyne_ros 81 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 82 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, 83 | intensity)(float, time, time)(uint16_t, ring, 84 | ring)) 85 | 86 | namespace ouster_ros 87 | { 88 | struct EIGEN_ALIGN16 Point 89 | { 90 | PCL_ADD_POINT4D; 91 | float intensity; 92 | uint32_t t; 93 | uint16_t reflectivity; 94 | uint8_t ring; 95 | uint16_t ambient; 96 | uint32_t range; 97 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 98 | }; 99 | } // namespace ouster_ros 100 | 101 | // clang-format off 102 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 103 | (float, x, x) 104 | (float, y, y) 105 | (float, z, z) 106 | (float, intensity, intensity) 107 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 108 | (std::uint32_t, t, t) 109 | (std::uint16_t, reflectivity, reflectivity) 110 | (std::uint8_t, ring, ring) 111 | (std::uint16_t, ambient, ambient) 112 | (std::uint32_t, range, range) 113 | ) 114 | 115 | namespace livox_ros 116 | { 117 | typedef struct { 118 | float x; /**< X axis, Unit:m */ 119 | float y; /**< Y axis, Unit:m */ 120 | float z; /**< Z axis, Unit:m */ 121 | float reflectivity; /**< Reflectivity */ 122 | uint8_t tag; /**< Livox point tag */ 123 | uint8_t line; /**< Laser line id */ 124 | } LivoxPointXyzrtl; 125 | } 126 | POINT_CLOUD_REGISTER_POINT_STRUCT(livox_ros::LivoxPointXyzrtl, 127 | (float, x, x) 128 | (float, y, y) 129 | (float, z, z) 130 | (float, reflectivity, reflectivity) 131 | (uint8_t, tag, tag) 132 | (uint8_t, line, line) 133 | ) 134 | 135 | class Preprocess 136 | { 137 | public: 138 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 139 | 140 | Preprocess(); 141 | ~Preprocess(); 142 | 143 | void process(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg, PointCloudXYZI::Ptr &pcl_out); 144 | void process(const sensor_msgs::msg::PointCloud2::UniquePtr &msg, PointCloudXYZI::Ptr &pcl_out); 145 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 146 | 147 | // sensor_msgs::PointCloud2::ConstPtr pointcloud; 148 | PointCloudXYZI pl_full, pl_corn, pl_surf; 149 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar 150 | vector typess[128]; //maximum 128 line lidar 151 | float time_unit_scale; 152 | int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; 153 | double blind; 154 | bool feature_enabled, given_offset_time; 155 | // ros::Publisher pub_full, pub_surf, pub_corn; 156 | 157 | private: 158 | void avia_handler(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg); 159 | void oust64_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg); 160 | void velodyne_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg); 161 | void mid360_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg); 162 | void default_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg); 163 | void give_feature(PointCloudXYZI &pl, vector &types); 164 | void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct); 165 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); 166 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); 167 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); 168 | 169 | int group_size; 170 | double disA, disB, inf_bound; 171 | double limit_maxmid, limit_midmin, limit_maxmin; 172 | double p2l_ratio; 173 | double jump_up_limit, jump_down_limit; 174 | double cos160; 175 | double edgea, edgeb; 176 | double smallp_intersect, smallp_ratio; 177 | double vx, vy, vz; 178 | }; 179 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fast-LIO2-Localization 2 | 3 | Modified Fast-LIO2 for localization in prior map. 4 | 5 | ![](./reloc.gif) 6 | 7 | Blue point is the prior map, white point is current scan. When the white point stop twinkling, icp registration successed, and fast-lio2 starts in relocalization mode. 8 | 9 | ## Usage 10 | 11 | ### Mapping 12 | 13 | Same as Fast-LIO2. 14 | 15 | ```bash 16 | ros2 launch fast_lio mapping.launch.py 17 | ``` 18 | 19 | ### Relocalization 20 | 21 | A example of launch file is provided at `example.launch.py`. It will call the `icp_node` and localization.launch.py to do re-localization. 22 | 23 | **IMPORTANT** 24 | 25 | 1. CHANGE the `map_path` in `example.launch.py` AND `prior_map_path` in `FAST_LIO/config/fast_lio_relocalization_param.yaml` to the path of the map you want to localize in, THEY SHOULD BE THE SAME. 26 | 2. Give an initial guess of the pose of the robot in the map by changing `initial_x`, `initial_y` etc. in `example.launch.py`, or give your initial guess in the rviz using `2D Pose Estimation`. 27 | 28 | ## Notice 29 | 30 | I'd love to hear from you if you have any suggestions or find any bugs. Please feel free to open an issue or make a pull request or contact me in any way you like. 31 | -------------------------------------------------------------------------------- /example.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource, FrontendLaunchDescriptionSource 7 | from launch_ros.actions import Node 8 | from launch.substitutions import LaunchConfiguration 9 | 10 | def generate_launch_description(): 11 | 12 | config_path = os.path.join( 13 | get_package_share_directory('fast_lio'), 'config') 14 | 15 | # icp relocalization 16 | map_odom_trans = Node( 17 | package='icp_relocalization', 18 | executable='transform_publisher', 19 | name='transform_publisher', 20 | output='screen' 21 | ) 22 | 23 | icp_node = Node( 24 | package='icp_relocalization', 25 | executable='icp_node', 26 | name='icp_node', 27 | output='screen', 28 | parameters=[ 29 | {'initial_x':0.0}, 30 | {'initial_y':0.0}, 31 | {'initial_z':0.0}, 32 | {'initial_a':0.0}, 33 | 34 | {'map_voxel_leaf_size':0.5}, 35 | {'cloud_voxel_leaf_size':0.3}, 36 | {'map_frame_id':'map'}, 37 | {'solver_max_iter':100}, 38 | {'max_correspondence_distance':0.1}, 39 | {'RANSAC_outlier_rejection_threshold':0.5}, 40 | # {'map_path':'/home/sentry_ws/src/sentry_bringup/maps/CC#0.pcd'}, 41 | {'map_path':'/home/sentry_ws/src/FAST_LIO_SAM/PCD/GlobalMap.pcd'}, 42 | {'fitness_score_thre':0.2}, # 是最近点距离的平均值，越小越严格 43 | {'converged_count_thre':40}, # pcl pub at 20 hz, 2s 44 | {'pcl_type':'livox'}, 45 | ], 46 | ) 47 | 48 | # fast-lio localization 49 | fast_lio_param = os.path.join( 50 | config_path, 'fast_lio_relocalization_param.yaml') 51 | fast_lio_node = Node( 52 | package='fast_lio', 53 | executable='fastlio_mapping', 54 | parameters=[ 55 | fast_lio_param 56 | ], 57 | output='screen', 58 | remappings=[('/Odometry','/state_estimation')] 59 | ) 60 | 61 | rviz_config_file = os.path.join( 62 | get_package_share_directory('icp_relocalization'), 'rviz', 'loam_livox.rviz') 63 | start_rviz = Node( 64 | package='rviz2', 65 | executable='rviz2', 66 | arguments=['-d', rviz_config_file,'--ros-args', '--log-level', 'warn'], 67 | output='screen' 68 | ) 69 | 70 | delayed_start_lio = TimerAction( 71 | period=5.0, 72 | actions=[ 73 | icp_node, 74 | fast_lio_node 75 | ] 76 | ) 77 | 78 | ld = LaunchDescription() 79 | 80 | ld.add_action(map_odom_trans) 81 | ld.add_action(start_rviz) 82 | ld.add_action(delayed_start_lio) 83 | 84 | return ld 85 | -------------------------------------------------------------------------------- /icp_relocalization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(icp_relocalization) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wextra -Wpedantic) 6 | SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g") 7 | set(CMAKE_BUILD_TYPE Release) 8 | 9 | set(USE_LIVOX 1) 10 | # define use_livox 11 | if(USE_LIVOX) 12 | add_definitions(-DUSE_LIVOX) 13 | endif() 14 | 15 | # find dependencies 16 | find_package(ament_cmake REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(tf2 REQUIRED) 21 | find_package(pcl_ros REQUIRED) 22 | find_package(octomap_ros REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(tf2_eigen REQUIRED) 25 | find_package(tf2_geometry_msgs REQUIRED) 26 | find_package(OpenMP REQUIRED) 27 | find_package(Eigen3) 28 | if(USE_LIVOX) 29 | find_package(livox_ros_driver2 REQUIRED) 30 | endif() 31 | 32 | include_directories(${OpenMP_INCLUDE_DIRS}) 33 | include_directories(${EIGEN3_INCLUDE_DIR}) 34 | 35 | add_executable(icp_node src/icp_node.cpp) 36 | ament_target_dependencies(icp_node rclcpp std_msgs geometry_msgs tf2 tf2_ros tf2_geometry_msgs pcl_ros ) 37 | if(USE_LIVOX) 38 | ament_target_dependencies(icp_node livox_ros_driver2) 39 | endif() 40 | 41 | add_executable(transform_publisher src/transform_publisher.cpp) 42 | ament_target_dependencies(transform_publisher rclcpp std_msgs geometry_msgs tf2 tf2_ros tf2_geometry_msgs pcl_ros) 43 | 44 | 45 | add_executable(sac_ia_gicp 46 | src/sac_ia_gicp.cpp 47 | ) 48 | ament_target_dependencies(sac_ia_gicp rclcpp std_msgs PCL pcl_ros Eigen3) 49 | 50 | # Install launch files 51 | install( 52 | DIRECTORY launch rviz 53 | DESTINATION share/${PROJECT_NAME} 54 | ) 55 | 56 | # Install nodes 57 | install( 58 | TARGETS icp_node transform_publisher sac_ia_gicp 59 | DESTINATION lib/${PROJECT_NAME} 60 | ) 61 | 62 | ament_package() 63 | -------------------------------------------------------------------------------- /icp_relocalization/README.md: -------------------------------------------------------------------------------- 1 | # ICP & SAC-IA relocalization 2 | 3 | 1. `icp_node.cpp`: simple icp relocalization 4 | 2. `san_ia_gicp`: cpp + ros2 implementation of [Point Cloud Registration Method Based on SAC-IA and NDT Fusion](http://www.jgg09.com/EN/abstract/abstract12346.shtml#). 5 | 6 |

7 | 8 |

9 | -------------------------------------------------------------------------------- /icp_relocalization/launch/__pycache__/dll.launch.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/icp_relocalization/launch/__pycache__/dll.launch.cpython-310.pyc -------------------------------------------------------------------------------- /icp_relocalization/launch/icp.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='icp_relocalization', 8 | executable='transform_publisher', 9 | name='transform_publisher', 10 | output='screen' 11 | ), 12 | Node( 13 | package='icp_relocalization', 14 | executable='icp_node', 15 | name='icp_node', 16 | output='screen', 17 | parameters=[ 18 | {'initial_x':0.0}, 19 | {'initial_y':0.0}, 20 | {'initial_z':0.1}, 21 | {'initial_a':-2.2}, 22 | # {'initial_roll':0.0}, 23 | # {'initial_pitch':0.0}, 24 | # {'initial_yaw':0.0}, 25 | {'map_voxel_leaf_size':0.5}, 26 | {'cloud_voxel_leaf_size':0.3}, 27 | {'map_frame_id':'map'}, 28 | {'solver_max_iter':75}, 29 | {'max_correspondence_distance':0.1}, 30 | {'RANSAC_outlier_rejection_threshold':1.0}, 31 | {'map_path':'/home/sentry_ws/src/FAST_LIO_SAM/PCD/GlobalMap.pcd'}, 32 | {'fitness_score_thre':0.1}, # 是最近点距离的平均值，越小越严格 33 | {'converged_count_thre':50}, 34 | {'pcl_type':'livox'}, 35 | ], 36 | ) 37 | ]) 38 | -------------------------------------------------------------------------------- /icp_relocalization/launch/sac_ia_gicp.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import LaunchConfiguration, Command 7 | from launch_ros.actions import Node 8 | 9 | def generate_launch_description(): 10 | # Get package directory 11 | package_dir = get_package_share_directory('sac_ia_gicp') 12 | 13 | # Define node 14 | sac_ia_gicp_node = Node( 15 | package='sac_ia_gicp', 16 | executable='sac_ia_gicp', 17 | name='sac_ia_gicp_node', 18 | parameters=[ 19 | {'target_pcd_file': "/home/sentry_ws/src/FAST_LIO_SAM/PCD/GlobalMap.pcd"}, 20 | {'num_threads': 8}, 21 | {'k_serach_source': 100}, 22 | {'k_serach_target': 100}, 23 | # Please tune these param with a clear understanding of the algorithm 24 | # A good reference would be the official PCL github repo 25 | {'voxel_grid_leaf_size_source': 0.3}, 26 | {'voxel_grid_leaf_size_target': 0.3}, 27 | {'sac_ia_min_sample_distance': 0.1}, # the minimum distance between samples 28 | {'sac_ia_correspondence_randomness': 50}, # the number of neighbors to use when selecting a random feature correspondence. 29 | {'sac_ia_num_samples':5}, # nr_samples the number of samples to use during each iteration 30 | {'icp_max_correspondence_distance': 1.0}, 31 | {'icp_max_iteration': 10000}, 32 | {'icp_transformation_epsilon': 0.01}, # the transformation epsilon (maximum allowable translation squared difference) 33 | {'icp_euclidean_fitness_epsilon': 0.01}, # useless 34 | {'fitness_score_thre':0.1}, 35 | {'mode':0}, # 0 - keep running; 1 - run when fast-lio need recovery 36 | {'max_optimize_times':3} 37 | ], 38 | remappings=[ 39 | ('/source_cloud', '/cloud_registered_body'), 40 | ], 41 | ) 42 | 43 | # Create launch description 44 | ld = LaunchDescription() 45 | 46 | # Add actions to launch description 47 | ld.add_action(sac_ia_gicp_node) 48 | 49 | return ld -------------------------------------------------------------------------------- /icp_relocalization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | icp_relocalization 4 | 1.0.0 5 | ICP Relocalization 6 | PolarisXQ 7 | Apache-2.0 8 | 9 | ament_cmake 10 | 11 | rclcpp 12 | std_msgs 13 | nav_msgs 14 | geometry_msgs 15 | tf2 16 | pcl_ros 17 | std_msgs 18 | nav_msgs 19 | tf2 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /icp_relocalization/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/icp_relocalization/result.png -------------------------------------------------------------------------------- /icp_relocalization/src/transform_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | class TransformPublisherNode : public rclcpp::Node 7 | { 8 | public: 9 | TransformPublisherNode() 10 | : Node("transform_publisher_node") 11 | { 12 | this->declare_parameter("odom_frame_id","odom"); 13 | this->declare_parameter("map_frame_id","map"); 14 | 15 | this->get_parameter_or("odom_frame_id", odom_frame_id, "odom"); 16 | this->get_parameter_or("map_frame_id", map_frame_id, "map"); 17 | subscription_ = this->create_subscription( 18 | "icp_result", 10, std::bind(&TransformPublisherNode::callback, this, std::placeholders::_1)); 19 | broadcaster_ = std::make_shared(this); 20 | } 21 | 22 | private: 23 | void callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) 24 | { 25 | geometry_msgs::msg::TransformStamped transform; 26 | transform.header.stamp = this->now(); 27 | transform.header.frame_id = map_frame_id; 28 | transform.child_frame_id = odom_frame_id; 29 | transform.transform.translation.x = msg->pose.pose.position.x; 30 | transform.transform.translation.y = msg->pose.pose.position.y; 31 | transform.transform.translation.z = msg->pose.pose.position.z; 32 | transform.transform.rotation = msg->pose.pose.orientation; 33 | 34 | broadcaster_->sendTransform(transform); 35 | } 36 | 37 | rclcpp::Subscription::SharedPtr subscription_; 38 | std::shared_ptr broadcaster_; 39 | std::string odom_frame_id, map_frame_id; 40 | }; 41 | 42 | int main(int argc, char * argv[]) 43 | { 44 | rclcpp::init(argc, argv); 45 | rclcpp::spin(std::make_shared()); 46 | rclcpp::shutdown(); 47 | return 0; 48 | } -------------------------------------------------------------------------------- /reloc.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PolarisXQ/Fast-LIO2-Localization/7bef4657657c356638737826fbead67620377fb7/reloc.gif --------------------------------------------------------------------------------