├── CMakeLists.txt ├── LICENSE ├── Log ├── dbg.txt ├── fast_lio_time_log.csv ├── fast_lio_time_log_analysis.m ├── guide.md ├── imu.txt ├── mat_out.txt ├── mat_pre.txt ├── plot.py └── pos_log.txt ├── README.md ├── config ├── m2dgr │ ├── ring32_M_0.xml │ ├── ring32_M_1.xml │ ├── ring32_M_2.xml │ ├── ring32_M_3.xml │ ├── ring32_M_4.xml │ ├── ring32_M_5.xml │ ├── ring32_M_6.xml │ ├── ring32_M_7.xml │ ├── ring32_M_8.xml │ └── ring32_v.xml ├── ouster64.yaml ├── rs128.yaml ├── velodyne.yaml ├── velodyne_m2dgr.yaml ├── viral.yaml └── viral │ ├── ring16_M_0.xml │ ├── ring16_M_1.xml │ ├── ring16_M_2.xml │ ├── ring16_M_3.xml │ ├── ring16_M_4.xml │ ├── ring16_M_5.xml │ ├── ring16_M_6.xml │ ├── ring16_M_7.xml │ ├── ring16_M_8.xml │ └── ring16_v.xml ├── 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 │ ├── ikd_Tree.cpp │ └── ikd_Tree.h ├── matplotlibcpp.h ├── point_type.h ├── so3_math.h ├── use-ikfom.hpp └── voxel_map_util.hpp ├── launch ├── gdb_debug_example.launch ├── mapping_m2dgr.launch ├── mapping_ouster64.launch ├── mapping_rs.launch ├── mapping_velodyne.launch └── mapping_viral.launch ├── msg └── Pose6D.msg ├── package.xml └── src ├── IMU_Processing.hpp ├── cloud_process.hpp ├── laserMapping.cpp ├── preprocess.cpp ├── preprocess.h ├── ring_fals ├── Image_normals.hpp ├── precomp.h ├── range_image.cpp └── range_image.h └── tic_toc.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(log_lio) 3 | 4 | SET(CMAKE_BUILD_TYPE "Debug") 5 | 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 9 | 10 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 11 | 12 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 13 | set(CMAKE_CXX_STANDARD 14) 14 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 15 | set(CMAKE_CXX_EXTENSIONS OFF) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 17 | 18 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 19 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 20 | include(ProcessorCount) 21 | ProcessorCount(N) 22 | message("Processer number: ${N}") 23 | if(N GREATER 4) 24 | add_definitions(-DMP_EN) 25 | add_definitions(-DMP_PROC_NUM=3) 26 | message("core for MP: 3") 27 | elseif(N GREATER 3) 28 | add_definitions(-DMP_EN) 29 | add_definitions(-DMP_PROC_NUM=2) 30 | message("core for MP: 2") 31 | else() 32 | add_definitions(-DMP_PROC_NUM=1) 33 | endif() 34 | else() 35 | add_definitions(-DMP_PROC_NUM=1) 36 | endif() 37 | 38 | find_package(OpenMP QUIET) 39 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 40 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 41 | 42 | find_package(PythonLibs REQUIRED) 43 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") 44 | 45 | find_package(catkin REQUIRED COMPONENTS 46 | geometry_msgs 47 | nav_msgs 48 | sensor_msgs 49 | roscpp 50 | rospy 51 | std_msgs 52 | pcl_ros 53 | tf 54 | # livox_ros_driver 55 | message_generation 56 | eigen_conversions 57 | ) 58 | 59 | find_package(Eigen3 REQUIRED) 60 | find_package(PCL 1.7 REQUIRED) 61 | 62 | ## set OpenCV DIR 63 | if ($ENV{ROS_DISTRO} STREQUAL "kinetic") 64 | Set(OpenCV_DIR "/home/hk/opencv-3.3.1/build") # important find opencv version 65 | else() 66 | Set(OpenCV_DIR "/home/autolab/opencv-3.2.0/build") # important find opencv version, Neotic 67 | endif () 68 | 69 | find_package(OpenCV 3.2 QUIET) 70 | 71 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 72 | 73 | include_directories( 74 | ${catkin_INCLUDE_DIRS} 75 | ${EIGEN3_INCLUDE_DIR} 76 | ${PCL_INCLUDE_DIRS} 77 | ${PYTHON_INCLUDE_DIRS} 78 | ${OpenCV_INCLUDE_DIRS} 79 | include 80 | ) 81 | 82 | add_message_files( 83 | FILES 84 | Pose6D.msg 85 | ) 86 | 87 | generate_messages( 88 | DEPENDENCIES 89 | geometry_msgs 90 | ) 91 | 92 | catkin_package( 93 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 94 | DEPENDS EIGEN3 PCL 95 | INCLUDE_DIRS 96 | ) 97 | 98 | add_executable(loglio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp 99 | src/ring_fals/range_image.cpp 100 | ) 101 | target_link_libraries(loglio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} 102 | ${OpenCV_LIBRARIES} 103 | ) 104 | target_include_directories(loglio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /Log/dbg.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tiev-tongji/LOG-LIO/a2f49d66b9e38e05d2e7ebdb2a019c77ea5baf0c/Log/dbg.txt -------------------------------------------------------------------------------- /Log/fast_lio_time_log.csv: -------------------------------------------------------------------------------- 1 | time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time 2 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Log/imu.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tiev-tongji/LOG-LIO/a2f49d66b9e38e05d2e7ebdb2a019c77ea5baf0c/Log/imu.txt -------------------------------------------------------------------------------- /Log/mat_out.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tiev-tongji/LOG-LIO/a2f49d66b9e38e05d2e7ebdb2a019c77ea5baf0c/Log/mat_out.txt -------------------------------------------------------------------------------- /Log/mat_pre.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tiev-tongji/LOG-LIO/a2f49d66b9e38e05d2e7ebdb2a019c77ea5baf0c/Log/mat_pre.txt -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Log/pos_log.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tiev-tongji/LOG-LIO/a2f49d66b9e38e05d2e7ebdb2a019c77ea5baf0c/Log/pos_log.txt -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LOG-LIO 2 | 3 | ---------------------------- 4 | 5 | Our recent work [LOG-LIO2](https://github.com/tiev-tongji/LOG-LIO2) is now available! 6 | 7 | 8 | The trajectory file will be saved in **TUM** format in the file named "/Log/target_path.txt". 9 | 10 | And a more detailed readme is coming soon! 11 | 12 | **Related video:** [Ring FALS](https://youtu.be/cxTLywI7X7M). 13 | 14 | 15 | ## 1. Prerequisites 16 | ### 1.1 **Ubuntu** and **ROS** 17 | **Ubuntu >= 16.04** 18 | 19 | For **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally. 20 | 21 | [//]: # () 22 | [//]: # (ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)) 23 | 24 | ### 1.2 Ring FALS Normal Estimator 25 | Compile follow [Ring FALS normal estimator](https://github.com/tiev-tongji/RingFalsNormal). 26 | 27 | 28 | ## 2. Build 29 | Clone the repository and catkin_make: 30 | 31 | ``` 32 | cd ~/$A_ROS_DIR$/src 33 | git clone https://github.com/tiev-tongji/LOG-LIO.git 34 | cd .. 35 | catkin_make 36 | source devel/setup.bash 37 | ``` 38 | 39 | ## 3. run 40 | We conduct experiments on the [M2DGR](https://github.com/SJTU-ViSYS/M2DGR) and [NTU VIRAL](https://github.com/ntu-aris/ntu_viral_dataset) datasets. 41 | The corresponding launch and yaml file is provided. 42 | 43 | Note that the timestamp in the NTU VARIL dataset is the **_end time_**. 44 | 45 | ``` 46 | cd ~/$LOG_LIO_ROS_DIR$ 47 | source devel/setup.bash 48 | # for the M2DGR dataset 49 | roslaunch log_lio mapping_m2dgr.launch 50 | # for the NTU VIRAL dataset 51 | roslaunch log_lio mapping_viral.launch 52 | ``` 53 | 54 | ## 4. Acknowledgments 55 | 56 | Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), 57 | [Fast-LIO2](https://github.com/hku-mars/FAST_LIO), 58 | [ikd-Tree](https://github.com/hku-mars/ikd-Tree) 59 | and [VoxelMap](https://github.com/hku-mars/VoxelMap). 60 | 61 | ## Citation 62 | 63 | ``` 64 | @article{huang2023log, 65 | title={LOG-LIO: A LiDAR-Inertial Odometry with Efficient Local Geometric Information Estimation}, 66 | author={Huang, Kai and Zhao, Junqiao and Zhu, Zhongyang and Ye, Chen and Feng, Tiantian}, 67 | journal={IEEE Robotics and Automation Letters}, 68 | volume={9}, 69 | number={1}, 70 | pages={459--466}, 71 | year={2023}, 72 | publisher={IEEE} 73 | } 74 | ``` 75 | -------------------------------------------------------------------------------- /config/ouster64.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os_cloud_node/points" 3 | imu_topic: "/os_cloud_node/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 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). 6 | # 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 7 | 8 | preprocess: 9 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | scan_line: 64 11 | timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 12 | blind: 4 13 | 14 | mapping: 15 | acc_cov: 0.1 16 | gyr_cov: 0.1 17 | b_acc_cov: 0.0001 18 | b_gyr_cov: 0.0001 19 | fov_degree: 180 20 | det_range: 150.0 21 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic 22 | extrinsic_T: [ 0.0, 0.0, 0.0 ] 23 | extrinsic_R: [1, 0, 0, 24 | 0, 1, 0, 25 | 0, 0, 1] 26 | 27 | publish: 28 | path_en: false 29 | scan_publish_en: true # false: close all the point cloud output 30 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 31 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 32 | 33 | pcd_save: 34 | pcd_save_en: true 35 | interval: -1 # how many LiDAR frames saved in each pcd file; 36 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 37 | -------------------------------------------------------------------------------- /config/rs128.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | #NCLT 3 | # lid_topic: "/points_raw" 4 | # imu_topic: "/imu_raw" 5 | 6 | #KITTI 7 | # lid_topic: "/kitti/velo/pointcloud" 8 | # imu_topic: "/kitti/oxts/imu" 9 | 10 | #RS LiDar 11 | lid_topic: "/rslidar" 12 | imu_topic: "/imu" 13 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 14 | 15 | preprocess: 16 | lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 RS 17 | scan_line: 128 18 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 19 | blind: 2.5 20 | 21 | mapping: 22 | acc_cov: 0.1 23 | gyr_cov: 0.1 24 | b_acc_cov: 0.0001 25 | b_gyr_cov: 0.0001 26 | fov_degree: 180 27 | det_range: 100.0 28 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 29 | extrinsic_T: [ 1.494363791111, 0.000030061324, 0.903719820280] 30 | extrinsic_R: [ -0.508348737805, 0.861029086113, -0.014507709641, 31 | -0.861100925582, -0.508429079519, -0.002251013466, 32 | -0.009314329527, 0.011348302346, 0.999892223842] 33 | time_delay: 0.0 #time delay from IMU to LiDAR 34 | 35 | publish: 36 | path_en: true 37 | scan_publish_en: true # false: close all the point cloud output 38 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 39 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 40 | 41 | pcd_save: 42 | pcd_save_en: true 43 | interval: 1 # how many LiDAR frames saved in each pcd file; 44 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 45 | -------------------------------------------------------------------------------- /config/velodyne.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/velodyne_points" 3 | imu_topic: "/handsfree/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 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). 6 | # 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 7 | 8 | preprocess: 9 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | scan_line: 32 11 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 12 | timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 13 | blind: 2 14 | 15 | mapping: 16 | acc_cov: 0.1 17 | gyr_cov: 0.1 18 | b_acc_cov: 0.0001 19 | b_gyr_cov: 0.0001 20 | fov_degree: 180 21 | det_range: 100.0 22 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 23 | extrinsic_T: [0.27255, -0.00053, 0.17954] 24 | extrinsic_R: [ 1, 0, 0, 25 | 0, 1, 0, 26 | 0, 0, 1] 27 | 28 | publish: 29 | path_en: true 30 | scan_publish_en: true # false: close all the point cloud output 31 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 32 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 33 | 34 | pcd_save: 35 | pcd_save_en: false 36 | interval: -1 # how many LiDAR frames saved in each pcd file; 37 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 38 | -------------------------------------------------------------------------------- /config/velodyne_m2dgr.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/velodyne_points" 3 | imu_topic: "/handsfree/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 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). 6 | # 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 7 | 8 | preprocess: 9 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 10 | scan_line: 32 11 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 12 | timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 13 | Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) 14 | blind: 2 15 | 16 | mapping: 17 | acc_cov: 3.7686306102624571e-02 18 | gyr_cov: 2.3417543020438883e-03 19 | b_acc_cov: 1.1416642385952368e-03 20 | b_gyr_cov: 1.4428407712885209e-05 21 | fov_degree: 180 22 | det_range: 100.0 23 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 24 | extrinsic_T: [0.27255, -0.00053, 0.17954] 25 | extrinsic_R: [ 1, 0, 0, 26 | 0, 1, 0, 27 | 0, 0, 1] 28 | #voxel map 29 | voxel_hash_en: false # for association 30 | voxel_size: 1.0 31 | max_layer: 4 # 4 layer, 0, 1, 2, 3 32 | layer_point_size: [ 5, 5, 5, 5, 5 ] # min points plane update threshold 33 | plannar_threshold: 0.01 34 | max_points_size: 1000 35 | max_cov_points_size: 1000 36 | 37 | publish: 38 | path_en: true 39 | scan_publish_en: true # false: close all the point cloud output 40 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 41 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 42 | 43 | pcd_save: 44 | pcd_save_en: false 45 | interval: -1 # how many LiDAR frames saved in each pcd file; 46 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 47 | save_ikdtree_map: false 48 | save_final_map: false 49 | 50 | normal: 51 | compute_table: false 52 | ring_table_dir: "/config/m2dgr" 53 | compute_normal: true 54 | check_normal: true 55 | 56 | surfel: 57 | surfel_points_min: 25 # points inside the voxel larger than the value, do pca 58 | surfel_points_max: 100 # max points inside a voxel 59 | planarity: 0.6 # larger than the value is plane [0, 1.0] 60 | mid2min: 100.0 # larger than the value is plane 61 | angle_threshold: 30.0 # angle between ring Fals and surfel normal 62 | cloud_surfel: true # point-to-neighbor surfel association 63 | point_surfel: true # point-to-point surfel association 64 | 65 | # prism or RTK in IMU frame 66 | ground_truth: 67 | extrinsic_T: [ 0.16, 0.0, 0.84 ] ## GNSS in IMU frame 68 | # extrinsic_T: [ 0.27255, -0.00053, 0.17954] ## LiDAR in IMU frame 69 | extrinsic_R: [ 1, 0, 0, 70 | 0, 1, 0, 71 | 0, 0, 1 ] -------------------------------------------------------------------------------- /config/viral.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os1_cloud_node1/points" 3 | imu_topic: "/imu/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 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). 6 | # 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 7 | lidar_time_offset: -0.10 # begin time = lidar timestamp + time offset 8 | 9 | preprocess: 10 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 11 | scan_line: 16 12 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 13 | timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 14 | Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) 15 | blind: 2 16 | 17 | mapping: 18 | acc_cov: 6.0e-2 19 | gyr_cov: 5.0e-3 20 | b_acc_cov: 8.0e-5 21 | b_gyr_cov: 3.0e-6 22 | fov_degree: 180 23 | det_range: 150.0 24 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic 25 | extrinsic_T: [ -0.05, 0.0, 0.055 ] # imu <-- lidar 26 | extrinsic_R: [1, 0, 0, 27 | 0, 1, 0, 28 | 0, 0, 1] 29 | voxel_hash_en: true # for association 30 | 31 | publish: 32 | path_en: true 33 | scan_publish_en: true # false: close all the point cloud output 34 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 35 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 36 | 37 | pcd_save: 38 | pcd_save_en: false 39 | interval: -1 # how many LiDAR frames saved in each pcd file; 40 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 41 | save_ikdtree_map: false 42 | save_final_map: false 43 | 44 | normal: 45 | compute_table: false 46 | ring_table_dir: "/config/viral" 47 | compute_normal: true 48 | check_normal: true 49 | 50 | surfel: 51 | surfel_points_min: 5 # points inside the voxel larger than the value, do pca 52 | surfel_points_max: 1000 # max points inside a voxel 53 | planarity: 0.6 # less than the value is plane [0, 1.0] 54 | mid2min: 100.0 # larger than the value is plane 55 | angle_threshold: 25.0 # angle between ring Fals and surfel normal 56 | cloud_surfel: true # point-to-neighbor surfel association 57 | point_surfel: true # point-to-point surfel association 58 | 59 | # prism in IMU frame 60 | ground_truth: 61 | extrinsic_T: [ -0.293656, -0.012288, -0.273095 ] 62 | extrinsic_R: [ 1, 0, 0, 63 | 0, 1, 0, 64 | 0, 0, 1 ] 65 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | // I + (1 - cos(norm)) / norm^2 * v^ + (1 - sin(norm) / norm) / norm^2 * v^ *v^ 245 | res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); 246 | } 247 | return res; 248 | } 249 | 250 | template 251 | scalar exp(vectview result, vectview vec, const scalar& scale = 1) { 252 | scalar norm2 = vec.squaredNorm(); 253 | std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); 254 | scalar mult = cos_sinc.second * scale; 255 | result = mult * vec; 256 | return cos_sinc.first; 257 | } 258 | 259 | 260 | /** 261 | * Inverse function to @c exp. 262 | * 263 | * @param result @c vectview to the result 264 | * @param w scalar part of input 265 | * @param vec vector part of input 266 | * @param scale scale result by this value 267 | * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result 268 | */ 269 | template 270 | void log(vectview result, 271 | const scalar &w, const vectview vec, 272 | const scalar &scale, bool plus_minus_periodicity) 273 | { 274 | // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division 275 | scalar nv = vec.norm(); 276 | if(nv < tolerance()) { 277 | if(!plus_minus_periodicity && w < 0) { 278 | // find the maximal entry: 279 | int i; 280 | nv = vec.cwiseAbs().maxCoeff(&i); 281 | result = scale * std::atan2(nv, w) * vect::Unit(i); 282 | return; 283 | } 284 | nv = tolerance(); 285 | } 286 | scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); 287 | 288 | result = s * vec; 289 | } 290 | 291 | 292 | } // namespace MTK 293 | 294 | 295 | #endif /* MTKMATH_H_ */ 296 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | #include 12 | #include 13 | 14 | #include "point_type.h" 15 | using namespace std; 16 | using namespace Eigen; 17 | 18 | #define USE_IKFOM 19 | 20 | #define PI_M (3.14159265358) 21 | #define G_m_s2 (9.81) // Gravaty const in GuangDong/China 22 | #define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) 23 | #define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) 24 | #define CUBE_LEN (6.0) 25 | #define LIDAR_SP_LEN (2) 26 | #define INIT_COV (1) 27 | #define NUM_MATCH_POINTS (5) 28 | #define MAX_MEAS_DIM (10000) 29 | 30 | #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] 31 | #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] 32 | #define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) 35 | #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) 36 | 37 | typedef log_lio::Pose6D Pose6D; 38 | typedef pcl::PointXYZINormal PointType; 39 | //typedef ikdTree_PointType PointType; 40 | typedef pcl::PointCloud PointCloudXYZI; 41 | typedef vector> PointVector; 42 | typedef Vector3d V3D; 43 | typedef Matrix3d M3D; 44 | typedef Vector3f V3F; 45 | typedef Matrix3f M3F; 46 | 47 | #define MD(a,b) Matrix 48 | #define VD(a) Matrix 49 | #define MF(a,b) Matrix 50 | #define VF(a) Matrix 51 | 52 | M3D Eye3d(M3D::Identity()); 53 | M3F Eye3f(M3F::Identity()); 54 | V3D Zero3d(0, 0, 0); 55 | V3F Zero3f(0, 0, 0); 56 | 57 | struct MeasureGroup // Lidar data and imu dates for the curent process 58 | { 59 | MeasureGroup() 60 | { 61 | lidar_beg_time = 0.0; 62 | this->lidar.reset(new PointCloudXYZI()); 63 | }; 64 | double lidar_beg_time; 65 | double lidar_end_time; 66 | PointCloudXYZI::Ptr lidar; 67 | deque imu; 68 | }; 69 | 70 | struct StatesGroup 71 | { 72 | StatesGroup() { 73 | this->rot_end = M3D::Identity(); 74 | this->pos_end = Zero3d; 75 | this->vel_end = Zero3d; 76 | this->bias_g = Zero3d; 77 | this->bias_a = Zero3d; 78 | this->gravity = Zero3d; 79 | this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; 80 | this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001; 81 | }; 82 | 83 | StatesGroup(const StatesGroup& b) { 84 | this->rot_end = b.rot_end; 85 | this->pos_end = b.pos_end; 86 | this->vel_end = b.vel_end; 87 | this->bias_g = b.bias_g; 88 | this->bias_a = b.bias_a; 89 | this->gravity = b.gravity; 90 | this->cov = b.cov; 91 | }; 92 | 93 | StatesGroup& operator=(const StatesGroup& b) 94 | { 95 | this->rot_end = b.rot_end; 96 | this->pos_end = b.pos_end; 97 | this->vel_end = b.vel_end; 98 | this->bias_g = b.bias_g; 99 | this->bias_a = b.bias_a; 100 | this->gravity = b.gravity; 101 | this->cov = b.cov; 102 | return *this; 103 | }; 104 | 105 | StatesGroup operator+(const Matrix &state_add) 106 | { 107 | StatesGroup a; 108 | a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 109 | a.pos_end = this->pos_end + state_add.block<3,1>(3,0); 110 | a.vel_end = this->vel_end + state_add.block<3,1>(6,0); 111 | a.bias_g = this->bias_g + state_add.block<3,1>(9,0); 112 | a.bias_a = this->bias_a + state_add.block<3,1>(12,0); 113 | a.gravity = this->gravity + state_add.block<3,1>(15,0); 114 | a.cov = this->cov; 115 | return a; 116 | }; 117 | 118 | StatesGroup& operator+=(const Matrix &state_add) 119 | { 120 | this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 121 | this->pos_end += state_add.block<3,1>(3,0); 122 | this->vel_end += state_add.block<3,1>(6,0); 123 | this->bias_g += state_add.block<3,1>(9,0); 124 | this->bias_a += state_add.block<3,1>(12,0); 125 | this->gravity += state_add.block<3,1>(15,0); 126 | return *this; 127 | }; 128 | 129 | Matrix operator-(const StatesGroup& b) 130 | { 131 | Matrix a; 132 | M3D rotd(b.rot_end.transpose() * this->rot_end); 133 | a.block<3,1>(0,0) = Log(rotd); 134 | a.block<3,1>(3,0) = this->pos_end - b.pos_end; 135 | a.block<3,1>(6,0) = this->vel_end - b.vel_end; 136 | a.block<3,1>(9,0) = this->bias_g - b.bias_g; 137 | a.block<3,1>(12,0) = this->bias_a - b.bias_a; 138 | a.block<3,1>(15,0) = this->gravity - b.gravity; 139 | return a; 140 | }; 141 | 142 | void resetpose() 143 | { 144 | this->rot_end = M3D::Identity(); 145 | this->pos_end = Zero3d; 146 | this->vel_end = Zero3d; 147 | } 148 | 149 | M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point 150 | V3D pos_end; // the estimated position at the end lidar point (world frame) 151 | V3D vel_end; // the estimated velocity at the end lidar point (world frame) 152 | V3D bias_g; // gyroscope bias 153 | V3D bias_a; // accelerator bias 154 | V3D gravity; // the estimated gravity acceleration 155 | Matrix cov; // states covariance 156 | }; 157 | 158 | template 159 | T rad2deg(T radians) 160 | { 161 | return radians * 180.0 / PI_M; 162 | } 163 | 164 | template 165 | T deg2rad(T degrees) 166 | { 167 | return degrees * PI_M / 180.0; 168 | } 169 | 170 | template 171 | auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ 172 | const Matrix &v, const Matrix &p, const Matrix &R) 173 | { 174 | Pose6D rot_kp; 175 | rot_kp.offset_time = t; 176 | for (int i = 0; i < 3; i++) 177 | { 178 | rot_kp.acc[i] = a(i); 179 | rot_kp.gyr[i] = g(i); 180 | rot_kp.vel[i] = v(i); 181 | rot_kp.pos[i] = p(i); 182 | for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); 183 | } 184 | return move(rot_kp); 185 | } 186 | 187 | /* comment 188 | plane equation: Ax + By + Cz + D = 0 189 | convert to: A/D*x + B/D*y + C/D*z = -1 190 | solve: A0*x0 = b0 191 | where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T 192 | normvec: normalized x0 193 | */ 194 | template 195 | bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) 196 | { 197 | MatrixXf A(point_num, 3); 198 | MatrixXf b(point_num, 1); 199 | b.setOnes(); 200 | b *= -1.0f; 201 | 202 | for (int j = 0; j < point_num; j++) 203 | { 204 | A(j,0) = point[j].x; 205 | A(j,1) = point[j].y; 206 | A(j,2) = point[j].z; 207 | } 208 | normvec = A.colPivHouseholderQr().solve(b); 209 | 210 | for (int j = 0; j < point_num; j++) 211 | { 212 | if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) 213 | { 214 | return false; 215 | } 216 | } 217 | 218 | normvec.normalize(); 219 | return true; 220 | } 221 | 222 | float calc_dist(PointType p1, PointType p2){ 223 | 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); 224 | return d; 225 | } 226 | 227 | template 228 | bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) 229 | { 230 | Matrix A; 231 | Matrix b; 232 | A.setZero(); 233 | b.setOnes(); 234 | b *= -1.0f; 235 | 236 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 237 | { 238 | A(j,0) = point[j].x; 239 | A(j,1) = point[j].y; 240 | A(j,2) = point[j].z; 241 | } 242 | 243 | Matrix normvec = A.colPivHouseholderQr().solve(b); 244 | 245 | // n * p + b = 0 246 | T n = normvec.norm(); 247 | pca_result(0) = normvec(0) / n; 248 | pca_result(1) = normvec(1) / n; 249 | pca_result(2) = normvec(2) / n; 250 | pca_result(3) = 1.0 / n; 251 | 252 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 253 | { 254 | if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) 255 | { 256 | return false; 257 | } 258 | } 259 | return true; 260 | } 261 | 262 | #endif -------------------------------------------------------------------------------- /include/point_type.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 12/8/22. 3 | // 4 | 5 | #pragma once 6 | #define PCL_NO_PRECOMPILE 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #ifndef LIO_POINT_TYPE_H 13 | #define LIO_POINT_TYPE_H 14 | 15 | 16 | struct ikdTree_PointType 17 | { 18 | PCL_ADD_POINT4D 19 | PCL_ADD_NORMAL4D; 20 | union { 21 | struct { 22 | float intensity; 23 | float curvature; 24 | float s_xx; 25 | float s_xy; 26 | }; 27 | float data_c[4]; 28 | }; 29 | // Eigen::Matrix3f S; // summation p * p^t 30 | // Eigen::Vector3f mean; // 31 | 32 | union { 33 | struct { 34 | float s_xz; 35 | float s_yy; 36 | float s_yz; 37 | float s_zz; 38 | }; 39 | float data_s[4]; 40 | }; 41 | 42 | union { 43 | struct { 44 | int num_hit; 45 | float mean_x; 46 | float mean_y; 47 | float mean_z; 48 | }; 49 | float data_num[4]; 50 | }; 51 | 52 | union { 53 | struct { 54 | float cov_xx; 55 | float cov_yy; 56 | float cov_zz; 57 | float cov_xy; 58 | }; 59 | float data_cov_1[4]; 60 | }; 61 | 62 | union { 63 | struct { 64 | float cov_xz; 65 | float cov_yz; 66 | float n_cov_xx; 67 | float n_cov_xy; 68 | }; 69 | float data_cov_2[4]; 70 | }; 71 | 72 | union { 73 | struct { 74 | float n_cov_xz; 75 | float n_cov_yz; 76 | float n_cov_zz; 77 | float n_cov_yy; 78 | }; 79 | float data_cov_3[4]; 80 | }; 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 82 | 83 | Eigen::Matrix3d getPointCovMatrix() const 84 | { 85 | Eigen::Matrix3d m; 86 | m(0, 0) = cov_xx; m(0, 1) = cov_xy; m(0, 2) = cov_xz; 87 | m(1, 0) = cov_xy; m(1, 1) = cov_yy; m(1, 2) = cov_yz; 88 | m(2, 0) = cov_xz; m(2, 1) = cov_yz; m(2, 2) = cov_zz; 89 | return m; 90 | } 91 | void getPointCovMatrix(Eigen::Matrix3d & m) const 92 | { 93 | m(0, 0) = cov_xx; m(0, 1) = cov_xy; m(0, 2) = cov_xz; 94 | m(1, 0) = cov_xy; m(1, 1) = cov_yy; m(1, 2) = cov_yz; 95 | m(2, 0) = cov_xz; m(2, 1) = cov_yz; m(2, 2) = cov_zz; 96 | } 97 | void getNormalCovMatrix(Eigen::Matrix3d & m) const 98 | { 99 | m(0, 0) = n_cov_xx; m(0, 1) = n_cov_xy; m(0, 2) = n_cov_xz; 100 | m(1, 0) = n_cov_xy; m(1, 1) = n_cov_yy; m(1, 2) = n_cov_yz; 101 | m(2, 0) = n_cov_xz; m(2, 1) = n_cov_yz; m(2, 2) = n_cov_zz; 102 | } 103 | 104 | void recordPointCovFromMatrix(const Eigen::Matrix3f & m) 105 | { 106 | cov_xx = m(0, 0); cov_xy = m(0, 1); cov_xz = m(0, 2); 107 | cov_yy = m(1, 1); cov_yz = m(1, 2); cov_zz = m(2, 2); 108 | } 109 | void recordNormalCovFromMatrix(const Eigen::Matrix3f & m) 110 | { 111 | n_cov_xx = m(0, 0); n_cov_xy = m(0, 1); n_cov_xz = m(0, 2); 112 | n_cov_yy = m(1, 1); n_cov_yz = m(1, 2); n_cov_zz = m(2, 2); 113 | } 114 | 115 | void updateCov(const ikdTree_PointType& p) 116 | { 117 | // inject small-scale to large-scale 118 | 119 | } 120 | 121 | ///code from VoxelMap 122 | void calcBodyCovBearingRange(const float range_cov, const float degree_cov) { 123 | // if z=0, error will occur in calcBodyCov. To be solved 124 | float z_tmp = z; 125 | if (z == 0) { 126 | z_tmp = 0.001; 127 | } 128 | float range = sqrt(x * x + y * y + z_tmp * z_tmp); 129 | float range_var = range_cov * range_cov; 130 | Eigen::Matrix2f direction_var; 131 | // (angle_cov^2, 0, 132 | // 0, angle_cov^2) 133 | direction_var << pow(sin(DEG2RAD(degree_cov)), 2), 0, 0, pow(sin(DEG2RAD(degree_cov)), 2); 134 | Eigen::Vector3f direction = this->getVector3fMap(); 135 | direction.normalize(); 136 | Eigen::Matrix3f direction_hat; // w^ 137 | direction_hat << 0, -direction(2), direction(1), direction(2), 0, -direction(0), -direction(1), direction(0), 0; 138 | //direction dot base_vector1 = 0 139 | Eigen::Vector3f base_vector1(1, 1, -(direction(0) + direction(1)) / direction(2)); //(1, 1, -(x+y)/z), not unique 140 | base_vector1.normalize(); 141 | Eigen::Vector3f base_vector2 = base_vector1.cross(direction); 142 | base_vector2.normalize(); 143 | Eigen::Matrix N; //N = [base_vector1, base_vector2] 144 | N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), 145 | base_vector1(2), base_vector2(2); 146 | Eigen::Matrix A = range * direction_hat * N; // (d * w^ * N )in the paper 147 | //cov = w * var_d * w^T + A * var_w * A^T 148 | Eigen::Matrix3f cov = direction * range_var * direction.transpose() + 149 | A * direction_var * A.transpose(); 150 | 151 | recordPointCovFromMatrix(cov); 152 | }; 153 | } EIGEN_ALIGN16; 154 | 155 | POINT_CLOUD_REGISTER_POINT_STRUCT (ikdTree_PointType, 156 | (float, x, x) (float, y, y) (float, z, z) 157 | (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) 158 | (float, intensity, intensity) (float, curvature, curvature) (float, s_xx, s_xx) (float, s_xy, s_xy) 159 | (float, s_xz, s_xz) (float, s_yy, s_yy) (float, s_yz, s_yz) (float, s_zz, s_zz) 160 | (int, num_hit, num_hit) (float, mean_x, mean_x) (float, mean_y, mean_y) (float, mean_z, mean_z) 161 | ) 162 | 163 | // Velodyne 164 | struct PointXYZIRT 165 | { 166 | PCL_ADD_POINT4D 167 | PCL_ADD_INTENSITY; 168 | uint16_t ring; 169 | float time; 170 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 171 | } EIGEN_ALIGN16; 172 | 173 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT, 174 | (float, x, x) (float, y, y) (float, z, z) 175 | (float, intensity, intensity) 176 | (uint16_t, ring, ring) 177 | (float, time, time) 178 | ) 179 | 180 | // Ouster 181 | struct ousterPointXYZIRT { 182 | PCL_ADD_POINT4D; 183 | PCL_ADD_INTENSITY; 184 | uint32_t t; 185 | uint16_t reflectivity; 186 | uint8_t ring; 187 | uint16_t noise; 188 | uint32_t range; 189 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 190 | }EIGEN_ALIGN16; 191 | 192 | POINT_CLOUD_REGISTER_POINT_STRUCT(ousterPointXYZIRT, 193 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) 194 | (uint32_t, t, t) (uint16_t, reflectivity, reflectivity) 195 | (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range) 196 | ) 197 | 198 | 199 | #endif //LIO_POINT_TYPE_H 200 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 -------------------------------------------------------------------------------- /include/voxel_map_util.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_MAP_UTIL_HPP 2 | #define VOXEL_MAP_UTIL_HPP 3 | //#include "common_lib.h" 4 | #include "omp.h" 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 | 20 | #include "ikd-Tree/ikd_Tree.h" 21 | 22 | using std::hash; 23 | using std::vector; 24 | using std::unordered_map; 25 | using std::cout; 26 | using std::printf; 27 | using std::mutex; 28 | #define HASH_P 116101 29 | #define MAX_N 10000000000 30 | 31 | static int plane_id = 0; 32 | 33 | // a point to plane matching structure 34 | typedef struct ptpl { 35 | Eigen::Vector3d point; 36 | Eigen::Vector3d point_world; 37 | Eigen::Vector3d normal; 38 | Eigen::Vector3d center; 39 | Eigen::Matrix plane_cov; 40 | double d; 41 | int layer; 42 | Eigen::Matrix3d cov_lidar; 43 | } ptpl; 44 | 45 | // 3D point with covariance 46 | typedef struct pointWithCov { 47 | Eigen::Vector3d point; 48 | Eigen::Vector3d point_world; 49 | Eigen::Matrix3d cov; 50 | Eigen::Matrix3d cov_lidar; 51 | } pointWithCov; 52 | 53 | typedef struct Plane { 54 | Eigen::Vector3f center; 55 | Eigen::Vector3f normal; 56 | Eigen::Vector3f y_normal; //evalsMid 57 | Eigen::Vector3f x_normal; //evalsMax 58 | Eigen::Matrix3f covariance; 59 | Eigen::Matrix plane_cov; //cov (n, q), q = mean_p 60 | float radius = 0; // sqrt(evalsReal(evalsMax)) 61 | float min_eigen_value = 1; 62 | float mid_eigen_value = 1; 63 | float max_eigen_value = 1; 64 | float d = 0; // plane: n * p + d = 0 65 | int points_size = 0; 66 | 67 | bool is_plane = false; 68 | bool is_init = false; 69 | int id; 70 | // is_update and last_update_points_size are only for publish plane 71 | bool is_update = false; 72 | int last_update_points_size = 0; 73 | bool update_enable = true; 74 | } Plane; 75 | 76 | class VoxelInfo; 77 | 78 | class VOXEL_LOC { 79 | public: 80 | int64_t x, y, z; 81 | 82 | VOXEL_LOC(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) 83 | : x(vx), y(vy), z(vz) {} 84 | 85 | VoxelInfo* info_ptr; 86 | bool operator==(const VOXEL_LOC &other) const { 87 | return (x == other.x && y == other.y && z == other.z); 88 | } 89 | }; 90 | 91 | 92 | // Hash value 93 | namespace std { 94 | template<> 95 | struct hash { 96 | int64_t operator()(const VOXEL_LOC &s) const { 97 | using std::hash; 98 | using std::size_t; 99 | return ((((s.z) * HASH_P) % MAX_N + (s.y)) * HASH_P) % MAX_N + (s.x); 100 | } 101 | }; 102 | } // namespace std 103 | 104 | class OctoTree { 105 | public: 106 | vector temp_points_; // all points in an octo tree 107 | vector new_points_; // new points in an octo tree 108 | Plane *plane_ptr_; 109 | int max_layer_; 110 | bool indoor_mode_; 111 | int layer_; 112 | int octo_state_; // 0 is end of tree, 1 is not 113 | OctoTree *leaves_[8]; 114 | double voxel_center_[3]; // x, y, z 115 | vector layer_point_size_; 116 | float quater_length_; 117 | float planer_threshold_; 118 | int max_plane_update_threshold_; 119 | int update_size_threshold_; 120 | int all_points_num_; 121 | int new_points_num_; 122 | int max_points_size_; 123 | int max_cov_points_size_; 124 | bool init_octo_; 125 | bool update_cov_enable_; 126 | bool update_enable_; 127 | 128 | OctoTree(int max_layer, int layer, vector layer_point_size, 129 | int max_point_size, int max_cov_points_size, float planer_threshold) 130 | : max_layer_(max_layer), layer_(layer), 131 | layer_point_size_(layer_point_size), max_points_size_(max_point_size), 132 | max_cov_points_size_(max_cov_points_size), 133 | planer_threshold_(planer_threshold) { 134 | temp_points_.clear(); 135 | octo_state_ = 0; 136 | new_points_num_ = 0; 137 | all_points_num_ = 0; 138 | // when new points num > 5, do a update 139 | update_size_threshold_ = 5; 140 | init_octo_ = false; 141 | update_enable_ = true; 142 | update_cov_enable_ = true; 143 | max_plane_update_threshold_ = layer_point_size_[layer_]; 144 | for (int i = 0; i < 8; i++) { 145 | leaves_[i] = nullptr; 146 | } 147 | plane_ptr_ = new Plane; 148 | } 149 | }; 150 | #endif -------------------------------------------------------------------------------- /launch/gdb_debug_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/mapping_m2dgr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/mapping_ouster64.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/mapping_rs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/mapping_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/mapping_viral.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /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 -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | log_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 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | sensor_msgs 26 | tf 27 | pcl_ros 28 | livox_ros_driver 29 | message_generation 30 | 31 | geometry_msgs 32 | nav_msgs 33 | sensor_msgs 34 | roscpp 35 | rospy 36 | std_msgs 37 | tf 38 | pcl_ros 39 | livox_ros_driver 40 | message_runtime 41 | 42 | rostest 43 | rosbag 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /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 23 | #include 24 | #include 25 | #include "use-ikfom.hpp" 26 | 27 | /// *************Preconfiguration 28 | 29 | #define MAX_INI_COUNT (10) 30 | 31 | const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; 32 | 33 | /// *************IMU Process and undistortion 34 | class ImuProcess 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | 39 | ImuProcess(); 40 | ~ImuProcess(); 41 | 42 | void Reset(); 43 | void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); 44 | void set_extrinsic(const V3D &transl, const M3D &rot); 45 | void set_extrinsic(const V3D &transl); 46 | void set_extrinsic(const MD(4,4) &T); 47 | void set_gyr_cov(const V3D &scaler); 48 | void set_acc_cov(const V3D &scaler); 49 | void set_gyr_bias_cov(const V3D &b_g); 50 | void set_acc_bias_cov(const V3D &b_a); 51 | Eigen::Matrix Q; 52 | void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); 53 | 54 | ofstream fout_imu; 55 | V3D cov_acc; //todo 56 | V3D cov_gyr; 57 | V3D cov_acc_scale; 58 | V3D cov_gyr_scale; 59 | V3D cov_bias_gyr; 60 | V3D cov_bias_acc; 61 | double first_lidar_time; 62 | 63 | private: 64 | void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); 65 | void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); 66 | 67 | PointCloudXYZI::Ptr cur_pcl_un_; 68 | sensor_msgs::ImuConstPtr last_imu_; 69 | deque v_imu_; 70 | vector IMUpose; 71 | vector v_rot_pcl_; 72 | M3D Lidar_R_wrt_IMU; 73 | V3D Lidar_T_wrt_IMU; 74 | V3D mean_acc; 75 | V3D mean_gyr; 76 | V3D angvel_last; 77 | V3D acc_s_last; 78 | double start_timestamp_; 79 | double last_lidar_end_time_; 80 | int init_iter_num = 1; 81 | bool b_first_frame_ = true; 82 | bool imu_need_init_ = true; 83 | }; 84 | 85 | ImuProcess::ImuProcess() 86 | : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) 87 | { 88 | init_iter_num = 1; 89 | Q = process_noise_cov(); 90 | cov_acc = V3D(0.1, 0.1, 0.1); 91 | cov_gyr = V3D(0.1, 0.1, 0.1); 92 | cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); 93 | cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); 94 | mean_acc = V3D(0, 0, -1.0); 95 | mean_gyr = V3D(0, 0, 0); 96 | angvel_last = Zero3d; 97 | Lidar_T_wrt_IMU = Zero3d; 98 | Lidar_R_wrt_IMU = Eye3d; 99 | last_imu_.reset(new sensor_msgs::Imu()); 100 | } 101 | 102 | ImuProcess::~ImuProcess() {} 103 | 104 | void ImuProcess::Reset() 105 | { 106 | // ROS_WARN("Reset ImuProcess"); 107 | mean_acc = V3D(0, 0, -1.0); 108 | mean_gyr = V3D(0, 0, 0); 109 | angvel_last = Zero3d; 110 | imu_need_init_ = true; 111 | start_timestamp_ = -1; 112 | init_iter_num = 1; 113 | v_imu_.clear(); 114 | IMUpose.clear(); 115 | last_imu_.reset(new sensor_msgs::Imu()); 116 | cur_pcl_un_.reset(new PointCloudXYZI()); 117 | } 118 | 119 | void ImuProcess::set_extrinsic(const MD(4,4) &T) 120 | { 121 | Lidar_T_wrt_IMU = T.block<3,1>(0,3); 122 | Lidar_R_wrt_IMU = T.block<3,3>(0,0); 123 | } 124 | 125 | void ImuProcess::set_extrinsic(const V3D &transl) 126 | { 127 | Lidar_T_wrt_IMU = transl; 128 | Lidar_R_wrt_IMU.setIdentity(); 129 | } 130 | 131 | void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) 132 | { 133 | Lidar_T_wrt_IMU = transl; 134 | Lidar_R_wrt_IMU = rot; 135 | } 136 | 137 | void ImuProcess::set_gyr_cov(const V3D &scaler) 138 | { 139 | cov_gyr_scale = scaler; 140 | } 141 | 142 | void ImuProcess::set_acc_cov(const V3D &scaler) 143 | { 144 | cov_acc_scale = scaler; 145 | } 146 | 147 | void ImuProcess::set_gyr_bias_cov(const V3D &b_g) 148 | { 149 | cov_bias_gyr = b_g; 150 | } 151 | 152 | void ImuProcess::set_acc_bias_cov(const V3D &b_a) 153 | { 154 | cov_bias_acc = b_a; 155 | } 156 | 157 | void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N) 158 | { 159 | /** 1. initializing the gravity, gyro bias, acc and gyro covariance 160 | ** 2. normalize the acceleration measurenments to unit gravity **/ 161 | 162 | V3D cur_acc, cur_gyr; 163 | 164 | if (b_first_frame_) 165 | { 166 | Reset(); 167 | N = 1; 168 | b_first_frame_ = false; 169 | const auto &imu_acc = meas.imu.front()->linear_acceleration; 170 | const auto &gyr_acc = meas.imu.front()->angular_velocity; 171 | mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; 172 | mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 173 | first_lidar_time = meas.lidar_beg_time; 174 | } 175 | 176 | for (const auto &imu : meas.imu) 177 | { 178 | const auto &imu_acc = imu->linear_acceleration; 179 | const auto &gyr_acc = imu->angular_velocity; 180 | cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; 181 | cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 182 | 183 | mean_acc += (cur_acc - mean_acc) / N; 184 | mean_gyr += (cur_gyr - mean_gyr) / N; 185 | 186 | cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); 187 | cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); 188 | 189 | // cout<<"acc norm: "<::cov init_P = kf_state.get_P(); 203 | init_P.setIdentity(); 204 | init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; 205 | init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; 206 | init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; 207 | init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; 208 | init_P(21,21) = init_P(22,22) = 0.00001; 209 | kf_state.change_P(init_P); 210 | last_imu_ = meas.imu.back(); 211 | 212 | } 213 | 214 | void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) 215 | { 216 | /*** add the imu of the last frame-tail to the of current frame-head ***/ 217 | auto v_imu = meas.imu; 218 | v_imu.push_front(last_imu_); 219 | const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); 220 | const double &imu_end_time = v_imu.back()->header.stamp.toSec(); 221 | const double &pcl_beg_time = meas.lidar_beg_time; 222 | const double &pcl_end_time = meas.lidar_end_time; 223 | 224 | /*** sort point clouds by offset time ***/ 225 | pcl_out = *(meas.lidar); 226 | sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); 227 | // cout<<"[ IMU Process ]: Process lidar from "<header.stamp.toSec() < last_lidar_end_time_) continue; 248 | 249 | angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 250 | 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 251 | 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); 252 | acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 253 | 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 254 | 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); 255 | 256 | // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; 257 | 258 | acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; 259 | 260 | if(head->header.stamp.toSec() < last_lidar_end_time_) 261 | { 262 | dt = tail->header.stamp.toSec() - last_lidar_end_time_; 263 | // dt = tail->header.stamp.toSec() - pcl_beg_time; 264 | } 265 | else 266 | { 267 | dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); 268 | } 269 | 270 | in.acc = acc_avr; 271 | in.gyro = angvel_avr; 272 | Q.block<3, 3>(0, 0).diagonal() = cov_gyr; 273 | Q.block<3, 3>(3, 3).diagonal() = cov_acc; 274 | Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; 275 | Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; 276 | kf_state.predict(dt, Q, in); 277 | 278 | /* save the poses at each IMU measurements */ 279 | imu_state = kf_state.get_x(); 280 | angvel_last = angvel_avr - imu_state.bg; 281 | acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); 282 | for(int i=0; i<3; i++) 283 | { 284 | acc_s_last[i] += imu_state.grav[i]; 285 | } 286 | double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; 287 | IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); 288 | } 289 | 290 | /*** calculated the pos and attitude prediction at the frame-end ***/ 291 | double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; 292 | dt = note * (pcl_end_time - imu_end_time); 293 | kf_state.predict(dt, Q, in); 294 | 295 | imu_state = kf_state.get_x(); 296 | last_imu_ = meas.imu.back(); 297 | last_lidar_end_time_ = pcl_end_time; 298 | 299 | /*** undistort each lidar point (backward propagation) ***/ 300 | if (pcl_out.points.begin() == pcl_out.points.end()) return; 301 | auto it_pcl = pcl_out.points.end() - 1; 302 | for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) 303 | { 304 | auto head = it_kp - 1; 305 | auto tail = it_kp; 306 | R_imu<rot); 307 | // cout<<"head imu acc: "<vel); 309 | pos_imu<pos); 310 | acc_imu<acc); 311 | angvel_avr<gyr); 312 | 313 | for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) 314 | { 315 | dt = it_pcl->curvature / double(1000) - head->offset_time; 316 | 317 | /* Transform to the 'end' frame, using only the rotation 318 | * Note: Compensation direction is INVERSE of Frame's moving direction 319 | * So if we want to compensate a point at timestamp-i to the frame-e 320 | * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ 321 | M3D R_i(R_imu * Exp(angvel_avr, dt)); 322 | 323 | V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); 324 | V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); 325 | 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! 326 | 327 | V3D normal_i(it_pcl->normal_x, it_pcl->normal_y, it_pcl->normal_z); 328 | V3D normal_compensate = imu_state.offset_R_L_I.conjugate() * imu_state.rot.conjugate() * R_i * imu_state.offset_R_L_I * normal_i; 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 | it_pcl->normal_x = normal_compensate(0); 335 | it_pcl->normal_y = normal_compensate(1); 336 | it_pcl->normal_z = normal_compensate(2); 337 | 338 | if (it_pcl == pcl_out.points.begin()) break; 339 | } 340 | } 341 | } 342 | 343 | void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) 344 | { 345 | double t1,t2,t3; 346 | t1 = omp_get_wtime(); 347 | 348 | if(meas.imu.empty()) {return;}; 349 | ROS_ASSERT(meas.lidar != nullptr); 350 | 351 | if (imu_need_init_) 352 | { 353 | /// The very first lidar frame 354 | IMU_init(meas, kf_state, init_iter_num); 355 | 356 | imu_need_init_ = true; 357 | 358 | last_imu_ = meas.imu.back(); 359 | 360 | state_ikfom imu_state = kf_state.get_x(); 361 | if (init_iter_num > MAX_INI_COUNT) 362 | { 363 | cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); 364 | imu_need_init_ = false; 365 | 366 | cov_acc = cov_acc_scale; 367 | cov_gyr = cov_gyr_scale; 368 | ROS_INFO("IMU Initial Done"); 369 | // 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",\ 370 | // 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]); 371 | fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); 372 | } 373 | 374 | return; 375 | } 376 | 377 | UndistortPcl(meas, kf_state, *cur_pcl_un_); 378 | 379 | t2 = omp_get_wtime(); 380 | t3 = omp_get_wtime(); 381 | 382 | // cout<<"[ IMU Process ]: Time: "< 2 | #include 3 | #include 4 | //#include 5 | #include "ring_fals//range_image.h" 6 | #include 7 | 8 | using namespace std; 9 | 10 | #define IS_VALID(a) ((abs(a)>1e8) ? true : false) 11 | 12 | typedef pcl::PointXYZINormal PointType; 13 | //typedef ikdTree_PointType PointType; 14 | typedef pcl::PointCloud PointCloudXYZI; 15 | 16 | enum LID_TYPE{AVIA = 1, VELODYNE, OUSTER}; //{1, 2, 3} 17 | enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; 18 | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; 19 | enum Surround{Prev, Next}; 20 | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; 21 | 22 | struct orgtype 23 | { 24 | double range; 25 | double dista; 26 | double angle[2]; 27 | double intersect; 28 | E_jump edj[2]; 29 | Feature ftype; 30 | orgtype() 31 | { 32 | range = 0; 33 | edj[Prev] = Nr_nor; 34 | edj[Next] = Nr_nor; 35 | ftype = Nor; 36 | intersect = 2; 37 | } 38 | }; 39 | 40 | namespace velodyne_ros { 41 | struct EIGEN_ALIGN16 Point { 42 | PCL_ADD_POINT4D; 43 | float intensity; 44 | float time; 45 | uint16_t ring; 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | }; 48 | } // namespace velodyne_ros 49 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 50 | (float, x, x) 51 | (float, y, y) 52 | (float, z, z) 53 | (float, intensity, intensity) 54 | (float, time, time) 55 | (uint16_t, ring, ring) 56 | ) 57 | 58 | namespace ouster_ros { 59 | struct EIGEN_ALIGN16 Point { 60 | PCL_ADD_POINT4D; 61 | float intensity; 62 | uint32_t t; 63 | uint16_t reflectivity; 64 | uint8_t ring; 65 | uint16_t ambient; 66 | uint32_t range; 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | }; 69 | } // namespace ouster_ros 70 | 71 | // clang-format off 72 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 73 | (float, x, x) 74 | (float, y, y) 75 | (float, z, z) 76 | (float, intensity, intensity) 77 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 78 | (std::uint32_t, t, t) 79 | (std::uint16_t, reflectivity, reflectivity) 80 | (std::uint8_t, ring, ring) 81 | (std::uint16_t, ambient, ambient) 82 | (std::uint32_t, range, range) 83 | ) 84 | 85 | class Preprocess 86 | { 87 | public: 88 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 89 | 90 | Preprocess(); 91 | ~Preprocess(); 92 | 93 | // void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 94 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 95 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 96 | void initNormalEstimator(); 97 | 98 | // sensor_msgs::PointCloud2::ConstPtr pointcloud; 99 | PointCloudXYZI pl_full, pl_corn, pl_surf; 100 | PointCloudXYZI::Ptr cloud_with_normal; 101 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar 102 | vector typess[128]; //maximum 128 line lidar 103 | float time_unit_scale; 104 | int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; 105 | double blind; 106 | bool feature_enabled, given_offset_time; 107 | ros::Publisher pub_full, pub_surf, pub_corn; 108 | 109 | // parameters for normal estimation 110 | RangeImage range_image; 111 | bool compute_table = false; 112 | int Horizon_SCAN, image_cols; 113 | string ring_table_dir; 114 | 115 | bool runtime_log = false; 116 | bool compute_normal = false; 117 | 118 | private: 119 | // void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); 120 | void ouster_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 121 | void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 122 | void give_feature(PointCloudXYZI &pl, vector &types); 123 | void pub_func(PointCloudXYZI &pl, const ros::Time &ct); 124 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); 125 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); 126 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); 127 | 128 | void projectPointCloud(const pcl::PointCloud::Ptr& cloud); 129 | void computeRingNormals(const pcl::PointCloud::Ptr& cloud); 130 | void extractCloudAndNormals(const pcl::PointCloud::Ptr& cloud); 131 | void flipNormalsTowardCenterAndNormalized(const cv::Vec3f& center, const pcl::PointCloud::Ptr& cloud, cv::Mat& image_normals); 132 | void NormalizeNormals(cv::Mat& image_normals); 133 | void saveNormalPCD(const std::string& file_name, const pcl::PointCloud::Ptr& cloud, const cv::Mat& normals_mat); 134 | void estimateNormals(const pcl::PointCloud::Ptr& cloud); 135 | void estimateNormals(const pcl::PointCloud::Ptr& cloud); 136 | bool pointInImage(const PointXYZIRT& point, int& row, int& col); 137 | 138 | void ouster2velodyne(const pcl::PointCloud& cloud_in, const pcl::PointCloud::Ptr& cloud_out); 139 | 140 | int group_size; 141 | double disA, disB, inf_bound; 142 | double limit_maxmid, limit_midmin, limit_maxmin; 143 | double p2l_ratio; 144 | double jump_up_limit, jump_down_limit; 145 | double cos160; 146 | double edgea, edgeb; 147 | double smallp_intersect, smallp_ratio; 148 | double vx, vy, vz; 149 | 150 | // parameters for normal estimation 151 | cv::Mat rangeMat; 152 | cv::Mat normals; //record ring normals 153 | cv::Mat plane_residual; //record ring normals residual in normal estimation 154 | std::vector image2cloud; 155 | std::vector cloud2image; 156 | double aver_normal_time = 0.0, aver_proj_time = 0.0, aver_compu_time = 0.0, aver_smooth_time = 0.0; 157 | double proj_time = 0.0, compu_time = 0.0, smooth_time = 0.0; 158 | int num_scans = 0; 159 | float ang_res_x; 160 | 161 | }; 162 | -------------------------------------------------------------------------------- /src/ring_fals/Image_normals.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2012, Willow Garage, Inc. 5 | * All rights reserved. 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 Willow Garage, Inc. 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 | 36 | #include "precomp.h" 37 | #include 38 | 39 | /** Just compute the norm of a vector 40 | * @param vec a vector of size 3 and any type T 41 | * @return 42 | */ 43 | template 44 | T 45 | inline 46 | norm_vec(const cv::Vec &vec) 47 | { 48 | return std::sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]); 49 | } 50 | 51 | /** Modify normals to make sure they point towards the camera 52 | * @param normals 53 | */ 54 | template 55 | inline 56 | void 57 | signNormal(const cv::Vec & normal_in, cv::Vec & normal_out) 58 | { 59 | cv::Vec res; 60 | if (normal_in[2] > 0) 61 | res = -normal_in / norm_vec(normal_in); 62 | else 63 | res = normal_in / norm_vec(normal_in); 64 | 65 | normal_out[0] = res[0]; 66 | normal_out[1] = res[1]; 67 | normal_out[2] = res[2]; 68 | } 69 | 70 | /** Normalized normals 71 | * @param normals 72 | */ 73 | template 74 | inline 75 | void 76 | normalizedNormal(const cv::Vec & normal_in, cv::Vec & normal_out) 77 | { 78 | normal_out = normal_in / norm_vec(normal_in); 79 | } 80 | 81 | /** Given 3d points, compute their distance to the origin 82 | * @param points 83 | * @return 84 | */ 85 | template 86 | cv::Mat_ 87 | computeRadius(const cv::Mat &points) 88 | { 89 | typedef cv::Vec PointT; 90 | 91 | // Compute the 92 | cv::Size size(points.cols, points.rows); 93 | cv::Mat_ r(size); 94 | if (points.isContinuous()) 95 | size = cv::Size(points.cols * points.rows, 1); 96 | for (int y = 0; y < size.height; ++y) 97 | { 98 | const PointT* point = points.ptr < PointT > (y), *point_end = points.ptr < PointT > (y) + size.width; 99 | T * row = r[y]; 100 | for (; point != point_end; ++point, ++row) 101 | *row = norm_vec(*point); 102 | } 103 | 104 | return r; 105 | } 106 | 107 | // Compute theta and phi according to equation 3 of 108 | // ``Fast and Accurate Computation of Surface Normals from Range Images`` 109 | // by H. Badino, D. Huber, Y. Park and T. Kanade 110 | template 111 | void 112 | computeThetaPhi(int rows, int cols, const cv::Matx& K, cv::Mat &cos_theta, cv::Mat &sin_theta, 113 | cv::Mat &cos_phi, cv::Mat &sin_phi) 114 | { 115 | // Create some bogus coordinates 116 | cv::Mat depth_image = K(0, 0) * cv::Mat_ < T > ::ones(rows, cols); //fx 117 | cv::Mat points3d; 118 | cv::rgbd::depthTo3d(depth_image, cv::Mat(K), points3d); 119 | 120 | typedef cv::Vec Vec3T; 121 | 122 | 123 | cos_theta = cv::Mat_ < T > (rows, cols); 124 | sin_theta = cv::Mat_ < T > (rows, cols); 125 | cos_phi = cv::Mat_ < T > (rows, cols); 126 | sin_phi = cv::Mat_ < T > (rows, cols); 127 | cv::Mat r = computeRadius(points3d); 128 | 129 | for (int y = 0; y < rows; ++y) 130 | { 131 | 132 | T *row_cos_theta = cos_theta.ptr < T > (y), *row_sin_theta = sin_theta.ptr < T > (y); 133 | T *row_cos_phi = cos_phi.ptr < T > (y), *row_sin_phi = sin_phi.ptr < T > (y); 134 | 135 | const Vec3T * row_points = points3d.ptr < Vec3T > (y), *row_points_end = points3d.ptr < Vec3T 136 | > (y) + points3d.cols; 137 | const T * row_r = r.ptr < T > (y); 138 | for (; row_points < row_points_end; 139 | ++row_cos_theta, ++row_sin_theta, ++row_cos_phi, ++row_sin_phi, ++row_points, ++row_r) 140 | { 141 | // In the paper, z goes away from the camera, y goes down, x goes right 142 | // OpenCV has the same conventions 143 | // Theta goes from z to x (and actually goes from -pi/2 to pi/2, phi goes from z to y 144 | float theta = (float)std::atan2(row_points->val[0], row_points->val[2]); //theta，x/z 145 | *row_cos_theta = std::cos(theta); 146 | *row_sin_theta = std::sin(theta); 147 | float phi = (float)std::asin(row_points->val[1] / (*row_r)); //phi， y/r 148 | *row_cos_phi = std::cos(phi); 149 | *row_sin_phi = std::sin(phi); 150 | } 151 | } 152 | } 153 | 154 | //template 155 | class LIDAR_FALS 156 | { 157 | public: 158 | typedef cv::Matx Mat33T; 159 | typedef cv::Vec Vec9T; 160 | typedef cv::Vec Vec3T; 161 | 162 | //(depth是数据类型 163 | LIDAR_FALS(int rows, int cols, int window_size, int depth, const cv::Mat &K) 164 | {} 165 | 166 | LIDAR_FALS(int rows, int cols, int window_size) 167 | : rows_(rows) 168 | , cols_(cols) 169 | , window_size_(window_size) 170 | {} 171 | 172 | LIDAR_FALS() {} 173 | 174 | ~LIDAR_FALS() {} 175 | 176 | ///计算M逆 177 | /** Compute cached data 178 | */ 179 | virtual void 180 | cache() 181 | { 182 | std::cout << "build look up table.\n"; 183 | 184 | // Compute theta and phi according to equation 3 185 | cv::Mat cos_theta, sin_theta, cos_phi, sin_phi; 186 | computeThetaPhi(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi); 187 | 188 | computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 189 | } 190 | 191 | void computeMInverse(cv::Mat &cos_theta, cv::Mat &sin_theta, cv::Mat &cos_phi, cv::Mat &sin_phi) 192 | { 193 | // Compute all the v_i for every points 194 | std::vector channels(3); 195 | channels[0] = cos_phi.mul(cos_theta); 196 | channels[1] = -cos_phi.mul(sin_theta); 197 | channels[2] = sin_phi; 198 | merge(channels, V_); 199 | 200 | // Compute M 201 | cv::Mat_ M(rows_, cols_); 202 | Mat33T VVt; 203 | const Vec3T * vec = V_[0]; 204 | Vec9T * M_ptr = M[0], *M_ptr_end = M_ptr + rows_ * cols_; 205 | for (; M_ptr != M_ptr_end; ++vec, ++M_ptr) 206 | { 207 | VVt = (*vec) * vec->t(); //v * v_t 208 | *M_ptr = Vec9T(VVt.val); 209 | } 210 | 211 | ///todo BorderTypes::BORDER_TRANSPARENT, error 212 | int border_type = cv::BorderTypes::BORDER_TRANSPARENT; 213 | boxFilter(M, M, M.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 214 | 215 | // Compute M's inverse 216 | Mat33T M_inv; 217 | M_inv_.create(rows_, cols_); 218 | Vec9T * M_inv_ptr = M_inv_[0]; 219 | for (M_ptr = &M(0); M_ptr != M_ptr_end; ++M_inv_ptr, ++M_ptr) 220 | { 221 | // We have a semi-definite matrix 222 | invert(Mat33T(M_ptr->val), M_inv, cv::DECOMP_CHOLESKY); 223 | *M_inv_ptr = Vec9T(M_inv.val); 224 | } 225 | } 226 | 227 | void saveMInverse(std::string dir, std::string filename) 228 | { 229 | std::vector mats(9); 230 | for (int i = 0; i < 9; ++i) { 231 | mats[i].create(M_inv_.rows, M_inv_.cols, CV_32F); 232 | } 233 | cv::Mat mat_vv(M_inv_.rows, M_inv_.cols, CV_32FC3); 234 | for (int i = 0; i < M_inv_.rows; ++i) { 235 | for (int j = 0; j < M_inv_.cols; ++j) { 236 | const Vec9T& tmp(M_inv_.at(i, j)); 237 | for (int k = 0; k < 9; ++k) { 238 | mats[k].at(i, j) = tmp(k); 239 | } 240 | mat_vv.at(i, j) = V_.at(i, j); 241 | } 242 | } 243 | 244 | //save M inverse 245 | for (int i = 0; i < 9; ++i) { 246 | std::string file_id(std::to_string(i)); 247 | cv::FileStorage fs(dir + "/" + filename + "_M_" + file_id + ".xml", cv::FileStorage::WRITE); 248 | fs << filename + "_" + file_id << mats[i]; 249 | fs.release(); 250 | } 251 | //save v v_t 252 | cv::FileStorage fs(dir + "/" + filename + "_v" + + ".xml", cv::FileStorage::WRITE); 253 | fs << filename + "_vv" << mat_vv; 254 | fs.release(); 255 | } 256 | 257 | bool loadMInverse(std::string dir, std::string filename) 258 | { 259 | std::vector mats(9); 260 | for (int i = 0; i < 9; ++i) { 261 | mats[i].create(rows_, cols_, CV_32F); 262 | } 263 | 264 | for (int i = 0; i < 9; ++i) { 265 | std::string file_id(std::to_string(i)); 266 | cv::FileStorage fs(dir + "/" + filename + "_M_" + file_id + ".xml", cv::FileStorage::READ); 267 | if(!fs.isOpened()) 268 | { 269 | // std::cerr << "ERROR: Wrong path to ring normal M file" << std::endl; 270 | return false; 271 | } 272 | fs[filename + "_" + file_id] >> mats[i]; 273 | fs.release(); 274 | } 275 | 276 | M_inv_.create(rows_, cols_); 277 | 278 | for (int i = 0; i < rows_; ++i) { 279 | for (int j = 0; j < cols_; ++j) { 280 | Vec9T& tmp(M_inv_.at(i, j)); 281 | for (int k = 0; k < 9; ++k) { 282 | tmp(k) = mats[k].at(i, j); 283 | } 284 | } 285 | } 286 | 287 | V_.create(rows_, cols_); 288 | cv::FileStorage fs(dir + "/" + filename + "_v" + + ".xml", cv::FileStorage::READ); 289 | fs[filename + "_vv"] >> V_; 290 | fs.release(); 291 | return true; 292 | } 293 | 294 | /** Compute the normals 295 | * @param r 296 | * @return 297 | */ 298 | virtual void 299 | compute(const cv::Mat &r, cv::Mat & normals) const 300 | { 301 | // Compute B 302 | cv::Mat_ B(rows_, cols_); 303 | 304 | const float* row_r = r.ptr < float > (0), *row_r_end = row_r + rows_ * cols_; 305 | const Vec3T *row_V = V_[0]; 306 | Vec3T *row_B = B[0]; 307 | for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V) 308 | { 309 | if (*row_r==FLT_MAX) 310 | *row_B = Vec3T(); 311 | else 312 | *row_B = (*row_V) / (*row_r); //v_i / r_i 313 | } 314 | 315 | ///todo BorderTypes::BORDER_TRANSPARENT, error 316 | // Apply a box filter to B 317 | boxFilter(B, B, B.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 318 | 319 | // compute the Minv*B products 320 | row_r = r.ptr < float > (0); 321 | const Vec3T * B_vec = B[0]; 322 | const Mat33T * M_inv = reinterpret_cast(M_inv_.ptr(0)); 323 | Vec3T *normal = normals.ptr(0); 324 | for (; row_r != row_r_end; ++row_r, ++B_vec, ++normal, ++M_inv) 325 | if (*row_r==FLT_MAX) 326 | { 327 | (*normal)[0] = *row_r; 328 | (*normal)[1] = *row_r; 329 | (*normal)[2] = *row_r; 330 | } 331 | else 332 | { 333 | Mat33T Mr = *M_inv; 334 | Vec3T Br = *B_vec; 335 | Vec3T MBr(Mr(0, 0) * Br[0] + Mr(0, 1)*Br[1] + Mr(0, 2)*Br[2], 336 | Mr(1, 0) * Br[0] + Mr(1, 1)*Br[1] + Mr(1, 2)*Br[2], 337 | Mr(2, 0) * Br[0] + Mr(2, 1)*Br[1] + Mr(2, 2)*Br[2]); 338 | //actually, can not flip normal here, because the point position is unknown 339 | signNormal(MBr, *normal); 340 | } 341 | } 342 | 343 | /** Compute the normals 344 | * @param r 345 | * @return 346 | */ 347 | virtual void 348 | compute(const cv::Mat &r, cv::Mat & normals, cv::Mat & residual) const 349 | { 350 | // Compute B 351 | cv::Mat_ B(rows_, cols_); 352 | 353 | const float* row_r = r.ptr < float > (0), *row_r_end = row_r + rows_ * cols_; 354 | const Vec3T *row_V = V_[0]; 355 | Vec3T *row_B = B[0]; 356 | for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V) 357 | { 358 | if (*row_r==FLT_MAX) 359 | *row_B = Vec3T(); 360 | else 361 | *row_B = (*row_V) / (*row_r); //v_i / r_i 362 | } 363 | 364 | ///todo BorderTypes::BORDER_TRANSPARENT, error 365 | // Apply a box filter to B 366 | boxFilter(B, B, B.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 367 | 368 | // compute the Minv*B products 369 | row_r = r.ptr < float > (0); 370 | const Vec3T * B_vec = B[0]; 371 | const Mat33T * M_inv = reinterpret_cast(M_inv_.ptr(0)); 372 | Vec3T *normal = normals.ptr(0); 373 | for (; row_r != row_r_end; ++row_r, ++B_vec, ++normal, ++M_inv) { 374 | if (*row_r==FLT_MAX) { 375 | (*normal)[0] = *row_r; 376 | (*normal)[1] = *row_r; 377 | (*normal)[2] = *row_r; 378 | } else { 379 | Mat33T Mr = *M_inv; 380 | Vec3T Br = *B_vec; 381 | Vec3T MBr(Mr(0, 0) * Br[0] + Mr(0, 1) * Br[1] + Mr(0, 2) * Br[2], 382 | Mr(1, 0) * Br[0] + Mr(1, 1) * Br[1] + Mr(1, 2) * Br[2], 383 | Mr(2, 0) * Br[0] + Mr(2, 1) * Br[1] + Mr(2, 2) * Br[2]); 384 | // signNormal(MBr, *normal); 385 | normalizedNormal(MBr, *normal); 386 | } 387 | } 388 | 389 | // { 390 | // row_V = V_[0]; 391 | // row_r = r.ptr < float > (0); 392 | // Vec3T *normal = normals.ptr(0); 393 | // float *res = residual.ptr(0); 394 | // for (; row_r != row_r_end; ++row_r, ++normal, ++row_V, ++res) { 395 | // float e = row_V->dot(*normal) - (1.0 / *row_r); 396 | // *res = e * e; 397 | // } 398 | // } 399 | } 400 | 401 | 402 | void setRowsCols(const int& rows, const int& cols) { 403 | rows_ = rows; 404 | cols_ = cols; 405 | } 406 | 407 | private: 408 | int rows_, cols_; 409 | int depth_ = CV_32F; 410 | cv::Mat K_, K_ori_; 411 | int window_size_; 412 | cv::Mat_ V_; //sin(theta) * cos(phi), sin(phi), cos(theta) * cos(phi) 413 | cv::Mat_ M_inv_; //M^-1 414 | }; -------------------------------------------------------------------------------- /src/ring_fals/precomp.h: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 | // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_PRECOMP_H__ 44 | #define __OPENCV_PRECOMP_H__ 45 | 46 | #include "opencv2/rgbd.hpp" 47 | #include "opencv2/calib3d.hpp" 48 | #include "opencv2/imgproc.hpp" 49 | #include "opencv2/core/utility.hpp" 50 | //#include "opencv2/core/private.hpp" 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/ring_fals/range_image.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 12/8/22. 3 | // 4 | 5 | #include "range_image.h" 6 | 7 | void RangeImage::createFromRings(const pcl::PointCloud::Ptr& cloud) 8 | { 9 | if (rows_ == 0 || cols_ == 0) { 10 | std::cout << "rows_ or cols_ == 0, can't create range image.\n"; 11 | return; 12 | } 13 | 14 | range_image = cv::Mat(rows_, cols_, CV_32F, range_init); 15 | cloud_id_image.resize(rows_ * cols_, -1); 16 | int cloudSize = (int)cloud->points.size(); 17 | // range image projection 18 | for (int i = 0; i < cloudSize; ++i) 19 | { 20 | // PointType thisPoint; 21 | // thisPoint.x = laserCloudIn->points[i].x; 22 | // thisPoint.y = laserCloudIn->points[i].y; 23 | // thisPoint.z = laserCloudIn->points[i].z; 24 | // thisPoint.intensity = laserCloudIn->points[i].intensity; 25 | const PointXYZIRT & thisPoint = cloud->points[i]; 26 | 27 | int rowIdn = cloud->points[i].ring; // ring 28 | if (rowIdn < 0 || rowIdn >= rows_) 29 | continue; 30 | 31 | if (downsampleRate > 0 && rowIdn % downsampleRate != 0) //采样行间隔 32 | continue; 33 | 34 | // float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; 35 | // 36 | static float ang_res_x = 360.0/float(cols_); // 360 / 1800 = 0.2 37 | 38 | float horizonAngle = atan2(thisPoint.y, -thisPoint.x); 39 | if (horizonAngle < 0) 40 | horizonAngle += 2 * M_PI; 41 | horizonAngle = horizonAngle * 180 / M_PI; 42 | int columnIdn = round(horizonAngle/ ang_res_x); 43 | 44 | if (columnIdn < 0 || columnIdn >= cols_) 45 | continue; 46 | 47 | float range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);// sqrt(x^2 + y^2 + z^2) 48 | 49 | if (range < range_min || range > range_max) 50 | continue; 51 | 52 | if (range_image.at(rowIdn, columnIdn) != FLT_MAX) 53 | continue; 54 | 55 | // for the amsterdam dataset 56 | // if (range < 6.0 && rowIdn <= 7 && (columnIdn >= 1600 || columnIdn <= 200)) 57 | // continue; 58 | // if (thisPoint.z < -2.0) 59 | // continue; 60 | 61 | range_image.at(rowIdn, columnIdn) = range; // 62 | if (input_value_max < range) 63 | input_value_max = range; 64 | 65 | 66 | // thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne 67 | // // thisPoint = deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0); // Ouster 68 | 69 | int index = columnIdn + rowIdn * cols_; // 70 | cloud_id_image[index] = i; 71 | 72 | } 73 | } 74 | 75 | void RangeImage::saveRangeImage(const std::string &file) { 76 | cv::Mat output = cv::Mat(range_image.rows, range_image.cols, CV_8U , 255); 77 | for (int i = 0; i < range_image.rows; ++i) { 78 | for (int j = 0; j < range_image.cols; ++j) { 79 | if (range_image.at(i, j) < 50.0) 80 | output.at(i, j) = range_image.at(i, j) / 50 * 255.0; 81 | else 82 | output.at(i, j) = 255; 83 | } 84 | } 85 | cv::imwrite(file, output); 86 | } 87 | 88 | void RangeImage::computeNormals(cv::OutputArray normals_out) { 89 | 90 | // cv::Mat points3d = points3d_in.getMat(); 91 | 92 | // Get the normals 93 | normals_out.create(rows_, cols_, CV_MAKETYPE(depth_, 3)); 94 | // if (points3d_in.empty()) 95 | // return; 96 | 97 | cv::Mat normals = normals_out.getMat(); 98 | lidar_fals.compute(range_image, normals); 99 | 100 | }; 101 | 102 | //in lidar frame 103 | void RangeImage::createFromCloud(const int& num_bins_v, const int& num_bins_h, 104 | const pcl::PointCloud::Ptr& cloud) 105 | { 106 | float bin_res_v = 180.0 / (float)num_bins_v; // vertical 107 | float bin_res_h = 360.0 / (float)num_bins_h; // horizon 108 | 109 | range_image = cv::Mat(num_bins_v, num_bins_h, CV_32F, cv::Scalar::all(FLT_MAX)); 110 | cloud_id_image.resize(rows_ * cols_, -1); 111 | 112 | //lidar系下点云 113 | for (int i = 0; i < (int)cloud->size(); ++i) 114 | { 115 | const PointType &p = cloud->points[i]; 116 | 117 | // find row id in range image 118 | float row_angle = atan2(p.z, sqrt(p.x * p.x + p.y * p.y)); // degrees, bottom -> up, -90 -> 0 -> 90 119 | row_angle = -row_angle * 180.0 / M_PI + 90.0; //bottom -> up, 180 -> 90 -> 0 120 | int row_id = round(row_angle / bin_res_v); 121 | 122 | // find column id in range image 123 | float col_angle = atan2(p.y, -p.x) * 180.0 / M_PI; // degrees, back -> left -> front -> right, 0 -> 360 124 | if (col_angle < 0) 125 | col_angle += 360; 126 | int col_id = round(col_angle / bin_res_h); 127 | 128 | // id may be out of boundary 129 | if (row_id < 0 || row_id >= num_bins_v || col_id < 0 || col_id >= num_bins_h) 130 | continue; 131 | // only keep points that's closer 132 | float dist = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); 133 | if (dist < range_image.at(row_id, col_id)) 134 | { 135 | range_image.at(row_id, col_id) = dist; 136 | 137 | int index = col_id + row_id * cols_; // 138 | cloud_id_image[index] = i; 139 | } 140 | } 141 | } 142 | 143 | void RangeImage::computeLookupTable() 144 | { 145 | if (range_image.empty()) 146 | return; 147 | } 148 | 149 | void RangeImage::createAndBuildTableFromRings(const int &num_rings, const int &num_horizon_scan, 150 | const pcl::PointCloud::Ptr &cloud) 151 | { 152 | if (rows_ != num_rings || cols_ != num_horizon_scan) { 153 | std::cout << "rows_ != num_rings || cols_ != num_horizon_scan. reset rows_ and cols\n"; 154 | rows_ = num_rings; 155 | cols_ = num_horizon_scan; 156 | lidar_fals.setRowsCols(num_rings, num_horizon_scan); 157 | } 158 | createAndBuildTableFromRings(cloud); 159 | } 160 | 161 | void RangeImage::createAndBuildTableFromRings(const pcl::PointCloud::Ptr &cloud) 162 | { 163 | if (rows_ == 0 || cols_ == 0) { 164 | std::cout << "rows_ == 0 or cols_ == 0, can't create range image.\n"; 165 | return; 166 | } 167 | 168 | int num_valid_bins = 0; 169 | range_image = cv::Mat(rows_, cols_, CV_32F, range_init); 170 | cloud_id_image.resize(rows_ * cols_, -1); 171 | 172 | cv::Mat cos_theta = cv::Mat(rows_, cols_, CV_32F); 173 | cv::Mat sin_theta = cv::Mat(rows_, cols_, CV_32F); 174 | cv::Mat cos_phi = cv::Mat(rows_, cols_, CV_32F); 175 | cv::Mat sin_phi = cv::Mat(rows_, cols_, CV_32F); 176 | int cloudSize = (int)cloud->points.size(); 177 | // range image projection 178 | for (int i = 0; i < cloudSize; ++i) 179 | { 180 | const PointXYZIRT & thisPoint = cloud->points[i]; 181 | 182 | int rowIdn = cloud->points[i].ring; // ring， 183 | if (rowIdn < 0 || rowIdn >= rows_) 184 | continue; 185 | 186 | if (downsampleRate > 0 && rowIdn % downsampleRate != 0) // 187 | continue; 188 | 189 | float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; //theta 190 | 191 | static float ang_res_x = 360.0/float(cols_); // 360 / 1800 = 0.2 192 | int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + cols_/2; // 193 | if (columnIdn >= cols_) 194 | columnIdn -= cols_; 195 | 196 | if (columnIdn < 0 || columnIdn >= cols_) 197 | continue; 198 | 199 | float range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);// sqrt(x^2 + y^2 + z^2) 200 | 201 | if (range < range_min || range > range_max) 202 | continue; 203 | 204 | if (range_image.at(rowIdn, columnIdn) != FLT_MAX) // 205 | continue; 206 | 207 | // for the amsterdam dataset 208 | // if (range < 6.0 && rowIdn <= 7 && (columnIdn >= 1600 || columnIdn <= 200)) 209 | // continue; 210 | // if (thisPoint.z < -2.0) 211 | // continue; 212 | 213 | range_image.at(rowIdn, columnIdn) = range; // 214 | // if (input_value_max < range) 215 | // input_value_max = range; 216 | 217 | //theta, phi used for building lookup table 218 | if (!table_valid) 219 | { 220 | float theta = (float) std::atan2(-thisPoint.y, thisPoint.x); //，-y / x 221 | float phi = (float) std::asin(thisPoint.z / range); //， z/r 222 | cos_theta.at(rowIdn, columnIdn) = std::cos(theta); 223 | sin_theta.at(rowIdn, columnIdn) = std::sin(theta); 224 | cos_phi.at(rowIdn, columnIdn) = std::cos(phi); 225 | sin_phi.at(rowIdn, columnIdn) = std::sin(phi); 226 | } 227 | 228 | // thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne 229 | // // thisPoint = deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0); // Ouster 230 | 231 | int index = columnIdn + rowIdn * cols_; //点在深度图像中的索引 232 | cloud_id_image[index] = i; 233 | // fullCloud->points[index] = thisPoint; 234 | } 235 | 236 | //save mat for debug 237 | { 238 | // cv::Mat output = cv::Mat(rows_, cols_, CV_8U, 255); 239 | // for (int i = 0; i < rows_; ++i) { 240 | // for (int j = 0; j < cols_; ++j) { 241 | // if (range_image.at(i, j) < 100.0) 242 | // output.at(i, j) = range_image.at(i, j) / input_value_max * 255.0; 243 | // } 244 | // } 245 | // cv::imwrite("/tmp/cos_theta.jpg", cos_theta); 246 | } 247 | 248 | //todo compute M matrix and M inverse 249 | if (!table_valid) { 250 | lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 251 | table_valid = true; 252 | } 253 | } 254 | 255 | void RangeImage::buildTableFromRings(const pcl::PointCloud::Ptr &cloud) { 256 | 257 | if (rows_ == 0 || cols_ == 0) { 258 | std::cout << "rows_ == 0 or cols_ == 0, can't create range image.\n"; 259 | return; 260 | } 261 | 262 | // int num_valid_bins = 0; 263 | // range_image = cv::Mat(rows_, cols_, CV_32F, range_init); 264 | cloud_id_image.resize(rows_ * cols_, -1); 265 | 266 | int cloudSize = (int)cloud->points.size(); 267 | static float ang_res_x = 360.0 / float(cols_); // 360 / 1800 = 0.2 268 | // range image projection 269 | for (int i = 0; i < cloudSize; ++i) 270 | { 271 | const PointXYZIRT & thisPoint = cloud->points[i]; 272 | 273 | int rowIdn = cloud->points[i].ring; // ring， 274 | if (rowIdn < 0 || rowIdn >= rows_) 275 | continue; 276 | 277 | float horizonAngle = atan2(thisPoint.y, -thisPoint.x); //theta 278 | if (horizonAngle < 0) 279 | horizonAngle += 2 * M_PI; 280 | horizonAngle = horizonAngle * 180 / M_PI; 281 | int columnIdn = round(horizonAngle/ ang_res_x); 282 | 283 | if (columnIdn < 0 || columnIdn >= cols_) 284 | continue; 285 | 286 | float range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);// sqrt(x^2 + y^2 + z^2) 287 | 288 | if (range < range_min || range > range_max) 289 | continue; 290 | 291 | //theta, phi used for building lookup table 292 | float theta = (float) std::atan2(-thisPoint.y, thisPoint.x); //，-y / x 293 | float phi = (float) std::asin(thisPoint.z / range); //， z/r 294 | int index = columnIdn + rowIdn * cols_; // 295 | float num_p = static_cast(num_per_pixel[index]); 296 | float num_p_new = num_p + 1; 297 | cos_theta.at(rowIdn, columnIdn) = (num_p * cos_theta.at(rowIdn, columnIdn) + std::cos(theta)) / num_p_new; 298 | sin_theta.at(rowIdn, columnIdn) = (num_p * sin_theta.at(rowIdn, columnIdn) + std::sin(theta)) / num_p_new; 299 | cos_phi.at(rowIdn, columnIdn) = (num_p * cos_phi.at(rowIdn, columnIdn) + std::cos(phi)) / num_p_new; 300 | sin_phi.at(rowIdn, columnIdn) = (num_p * sin_phi.at(rowIdn, columnIdn) + std::sin(phi)) / num_p_new; 301 | 302 | ++num_per_pixel[index]; 303 | // cloud_id_image[index] = i; 304 | } 305 | ++num_cloud; 306 | //todo compute M matrix and M inverse 307 | 308 | if (num_cloud < min_table_cloud) 309 | return; 310 | 311 | // lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 312 | } 313 | 314 | void RangeImage::buildTableFromRings(const int &num_rings, const int &num_horizon_scan, 315 | const pcl::PointCloud::Ptr &cloud) { 316 | if (rows_ != num_rings || cols_ != num_horizon_scan) { 317 | std::cout << "rows_ != num_rings || cols_ != num_horizon_scan. reset rows_ and cols\n"; 318 | rows_ = num_rings; 319 | cols_ = num_horizon_scan; 320 | lidar_fals.setRowsCols(num_rings, num_horizon_scan); 321 | } 322 | buildTableFromRings(cloud); 323 | } 324 | 325 | void RangeImage::buildTableFromCloud() { 326 | float bin_res_v = M_PI / (float)rows_; // vertical(radian) 327 | float bin_res_h = 2.0 * M_PI / (float)cols_; // horizon(radian) 328 | 329 | //lidar系下点云 330 | for (int i = 0; i < rows_; ++i) { 331 | float phi = 0.5 * M_PI - ((float)i + 0.5) * bin_res_v; 332 | float c_phi = std::cos(phi); 333 | float s_phi = std::sin(phi); 334 | for (int j = 0; j < cols_; ++j) { 335 | float theta = ((float)j + 0.5) * bin_res_h - M_PI; 336 | cos_theta.at(i, j) = std::cos(theta); 337 | sin_theta.at(i, j) = std::sin(theta); 338 | cos_phi.at(i, j) = c_phi; 339 | sin_phi.at(i, j) = s_phi; 340 | } 341 | } 342 | lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 343 | } 344 | 345 | void RangeImage::createFromCloud(const pcl::PointCloud::Ptr &cloud) { 346 | createFromCloud(rows_, cols_, cloud); 347 | } 348 | 349 | void RangeImage::computeNormals(const cv::Mat &r, const cv::_OutputArray &normals_out) { 350 | // Get the normals 351 | normals_out.create(rows_, cols_, CV_MAKETYPE(depth_, 3)); 352 | 353 | cv::Mat normals = normals_out.getMat(); 354 | lidar_fals.compute(r, normals); 355 | } 356 | 357 | void RangeImage::computeNormals(const cv::Mat &r, const cv::_OutputArray &normals_out, 358 | cv::Mat &residual) { 359 | // Get the normals 360 | normals_out.create(rows_, cols_, CV_MAKETYPE(depth_, 3)); 361 | residual = cv::Mat(rows_, cols_, CV_32F, cv::Scalar::all(FLT_MAX)); 362 | 363 | 364 | cv::Mat normals = normals_out.getMat(); 365 | // cv::Mat res = residual.getMat(); 366 | lidar_fals.compute(r, normals, residual); 367 | } 368 | -------------------------------------------------------------------------------- /src/ring_fals/range_image.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 12/8/22. 3 | // 4 | 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "point_type.h" 15 | 16 | #include "Image_normals.hpp" 17 | 18 | #ifndef LVI_SAM_RANGE_IMAGE_H 19 | #define LVI_SAM_RANGE_IMAGE_H 20 | 21 | //in lidar frame 22 | class RangeImage 23 | { 24 | typedef pcl::PointXYZI PointType; 25 | typedef cv::Matx Mat33T; 26 | typedef cv::Vec Vec9T; 27 | typedef cv::Vec Vec3T; 28 | public: 29 | explicit RangeImage() 30 | : num_bins_x(360) 31 | , num_bins_y(360) 32 | , lidar_fals() 33 | { 34 | // range_image = cv::Mat(num_bins_y, num_bins_x, CV_32F, cv::Scalar::all(FLT_MAX)); 35 | } 36 | 37 | explicit RangeImage(const int& rows, const int& cols ) 38 | : rows_(rows) 39 | , cols_(cols) 40 | , lidar_fals(rows, cols, 3) //(row，col，window_size) 41 | { 42 | // range_image = cv::Mat(num_bins_y, num_bins_x, CV_32F, cv::Scalar::all(FLT_MAX)); 43 | 44 | //储存计算M逆矩阵的中间变量 45 | cos_theta = cv::Mat(rows_, cols_, CV_32F); 46 | sin_theta = cv::Mat(rows_, cols_, CV_32F); 47 | cos_phi = cv::Mat(rows_, cols_, CV_32F); 48 | sin_phi = cv::Mat(rows_, cols_, CV_32F); 49 | num_per_pixel.resize(rows_ * cols_, 0); 50 | } 51 | 52 | void createFromRings(const pcl::PointCloud::Ptr& cloud); 53 | 54 | //create range imageg and build lookup table for normal computation 55 | void createAndBuildTableFromRings(const pcl::PointCloud::Ptr& cloud); 56 | void createAndBuildTableFromRings(const int& num_rings, const int& num_horizon_scan 57 | , const pcl::PointCloud::Ptr& cloud); 58 | 59 | void buildTableFromRings(const pcl::PointCloud::Ptr& cloud); 60 | void buildTableFromRings(const int& num_rings, const int& num_horizon_scan 61 | , const pcl::PointCloud::Ptr& cloud); 62 | 63 | void createFromCloud(const int& num_bins_v, const int& num_bins_h, 64 | const pcl::PointCloud::Ptr& cloud); 65 | void createFromCloud(const pcl::PointCloud::Ptr& cloud); 66 | 67 | void buildTableFromCloud(); 68 | 69 | void saveRangeImage(const std::string& file); 70 | 71 | void computeNormals(); 72 | void computeNormals(cv::OutputArray normals_out); 73 | void computeNormals(const cv::Mat &range_image, cv::OutputArray normals_out); 74 | void computeNormals(const cv::Mat &range_image, cv::OutputArray normals_out, cv::Mat &residual); 75 | 76 | void computeLookupTable(); 77 | 78 | bool loadLookupTable(std::string dir, std::string filename) 79 | {return lidar_fals.loadMInverse(dir, filename);} 80 | 81 | void saveLookupTable(std::string dir, std::string filename) 82 | {lidar_fals.saveMInverse(dir, filename);} 83 | 84 | void computeMInverse() {lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi);} 85 | 86 | enum CoordinateFrame 87 | { 88 | CAMERA_FRAME = 0, 89 | LASER_FRAME = 1 90 | }; 91 | 92 | // The actual range iamge. 93 | cv::Mat range_image; 94 | 95 | //only for visualization 96 | std::vector cloud_id_image; // 97 | 98 | private: 99 | 100 | int num_bins_x, num_bins_y; 101 | 102 | float resolution_x, resolution_y; 103 | 104 | float range_min = 1.0; 105 | float range_max = 200.0; 106 | 107 | float input_value_max = 0; 108 | int downsampleRate = 1; // 109 | 110 | cv::Scalar range_init = cv::Scalar::all(FLT_MAX); 111 | 112 | int num_cloud = 0; 113 | int min_table_cloud = 10; 114 | 115 | int rows_, cols_; 116 | int depth_ = CV_32F; 117 | cv::Mat K_; 118 | int window_size_; 119 | int method_; 120 | // mutable void* rgbd_normals_impl_; 121 | LIDAR_FALS lidar_fals; 122 | bool table_valid = false; 123 | 124 | cv::Mat cos_theta; 125 | cv::Mat sin_theta; 126 | cv::Mat cos_phi; 127 | cv::Mat sin_phi; 128 | std::vector num_per_pixel; // 129 | }; 130 | 131 | 132 | #endif //LVI_SAM_RANGE_IMAGE_H 133 | -------------------------------------------------------------------------------- /src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | --------------------------------------------------------------------------------