├── FAST-LIO ├── CMakeLists.txt ├── LICENSE ├── Log │ ├── H_OUT.txt │ ├── imu.txt │ ├── plot.py │ └── pos_log.txt ├── README.md ├── config │ ├── avia.yaml │ ├── horizon.yaml │ ├── ouster64.yaml │ ├── ouster64_mulran.yaml │ ├── robosense.yaml │ └── velodyne.yaml ├── doc │ ├── Fast_LIO_2.pdf │ ├── overview_fastlio2.svg │ ├── real_experiment2.gif │ ├── uav01.jpg │ ├── uav_ground.pdf │ └── ulhkwh_fastlio.gif ├── include │ ├── Exp_mat.h │ ├── IKFoM_toolkit │ │ ├── esekfom │ │ │ ├── esekfom.hpp │ │ │ └── util.hpp │ │ └── mtk │ │ │ ├── build_manifold.hpp │ │ │ ├── src │ │ │ ├── SubManifold.hpp │ │ │ ├── mtkmath.hpp │ │ │ └── vectview.hpp │ │ │ ├── startIdx.hpp │ │ │ └── types │ │ │ ├── S2.hpp │ │ │ ├── SOn.hpp │ │ │ ├── vect.hpp │ │ │ └── wrapped_cv_mat.hpp │ ├── common_lib.h │ ├── ikd-Tree │ │ ├── README.md │ │ ├── ikd_Tree.cpp │ │ └── ikd_Tree.h │ ├── matplotlibcpp.h │ ├── so3_math.h │ └── use-ikfom.hpp ├── launch │ ├── fast_lio_slam_rviz.launch │ ├── gdb_debug_example.launch │ ├── mapping_avia.launch │ ├── mapping_horizon.launch │ ├── mapping_ouster64.launch │ ├── mapping_ouster64_mulran.launch │ ├── mapping_robosense.launch │ └── mapping_velodyne.launch ├── msg │ └── Pose6D.msg ├── package.xml ├── rviz_cfg │ ├── .gitignore │ └── loam_livox.rviz └── src │ ├── IMU_Processing.hpp │ ├── laserMapping.cpp │ ├── preprocess.cpp │ └── preprocess.h ├── IMAGE ├── Overview.png └── motivations.png ├── README.md └── SC-PGO ├── CMakeLists.txt ├── README.md ├── Thirdparty ├── Sophus │ ├── .clang-format │ ├── CMakeLists.txt │ ├── LICENSE.txt │ ├── README.rst │ ├── Sophus.code-workspace │ ├── SophusConfig.cmake.in │ ├── appveyor.yml │ ├── cmake_modules │ │ └── FindEigen3.cmake │ ├── doxyfile │ ├── doxyrest-config.lua │ ├── examples │ │ ├── CMakeLists.txt │ │ └── HelloSO3.cpp │ ├── generate_stubs.py │ ├── make_docs.sh │ ├── package.xml │ ├── rst-dir │ │ ├── conf.py │ │ ├── page_index.rst │ │ └── pysophus.rst │ ├── run_format.sh │ ├── scripts │ │ ├── install_docs_deps.sh │ │ ├── install_linux_deps.sh │ │ ├── install_linux_fmt_deps.sh │ │ ├── install_osx_deps.sh │ │ └── run_cpp_tests.sh │ ├── setup.py │ ├── sophus │ │ ├── average.hpp │ │ ├── cartesian.hpp │ │ ├── ceres_local_parameterization.hpp │ │ ├── ceres_manifold.hpp │ │ ├── ceres_typetraits.hpp │ │ ├── common.hpp │ │ ├── example_ensure_handler.cpp │ │ ├── geometry.hpp │ │ ├── interpolate.hpp │ │ ├── interpolate_details.hpp │ │ ├── num_diff.hpp │ │ ├── rotation_matrix.hpp │ │ ├── rxso2.hpp │ │ ├── rxso3.hpp │ │ ├── se2.hpp │ │ ├── se3.hpp │ │ ├── sim2.hpp │ │ ├── sim3.hpp │ │ ├── sim_details.hpp │ │ ├── so2.hpp │ │ ├── so3.hpp │ │ ├── spline.hpp │ │ ├── test_macros.hpp │ │ ├── types.hpp │ │ └── velocities.hpp │ ├── sophus_pybind-stubs │ │ ├── py.typed │ │ └── sophus_pybind.pyi │ ├── sophus_pybind │ │ ├── README │ │ ├── SE3PyBind.h │ │ ├── SO3PyBind.h │ │ ├── SophusPyBind.h │ │ ├── bindings.cpp │ │ ├── examples │ │ │ └── sophus_quickstart_tutorial.ipynb │ │ └── tests │ │ │ └── sophusPybindTests.py │ ├── sympy │ │ ├── cpp_gencode │ │ │ ├── Se2_Dx_exp_x.cpp │ │ │ ├── Se2_Dx_log_this.cpp │ │ │ ├── Se2_Dx_this_mul_exp_x_at_0.cpp │ │ │ ├── Se3_Dx_exp_x.cpp │ │ │ ├── Se3_Dx_log_this.cpp │ │ │ ├── Se3_Dx_this_mul_exp_x_at_0.cpp │ │ │ ├── So2_Dx_exp_x.cpp │ │ │ ├── So2_Dx_log_exp_x_times_this_at_0.cpp │ │ │ ├── So2_Dx_log_this.cpp │ │ │ ├── So2_Dx_this_mul_exp_x_at_0.cpp │ │ │ ├── So3_Dx_exp_x.cpp │ │ │ ├── So3_Dx_log_exp_x_times_this_at_0.cpp │ │ │ ├── So3_Dx_log_this.cpp │ │ │ └── So3_Dx_this_mul_exp_x_at_0.cpp │ │ ├── run_tests.sh │ │ └── sophus │ │ │ ├── __init__.py │ │ │ ├── complex.py │ │ │ ├── cse_codegen.py │ │ │ ├── dual_quaternion.py │ │ │ ├── matrix.py │ │ │ ├── quaternion.py │ │ │ ├── se2.py │ │ │ ├── se3.py │ │ │ ├── so2.py │ │ │ └── so3.py │ └── test │ │ ├── CMakeLists.txt │ │ ├── ceres │ │ ├── CMakeLists.txt │ │ ├── test_ceres_rxso2.cpp │ │ ├── test_ceres_rxso3.cpp │ │ ├── test_ceres_se2.cpp │ │ ├── test_ceres_se3.cpp │ │ ├── test_ceres_sim2.cpp │ │ ├── test_ceres_sim3.cpp │ │ ├── test_ceres_so2.cpp │ │ ├── test_ceres_so3.cpp │ │ └── tests.hpp │ │ └── core │ │ ├── CMakeLists.txt │ │ ├── test_cartesian2.cpp │ │ ├── test_cartesian3.cpp │ │ ├── test_common.cpp │ │ ├── test_geometry.cpp │ │ ├── test_rxso2.cpp │ │ ├── test_rxso3.cpp │ │ ├── test_se2.cpp │ │ ├── test_se3.cpp │ │ ├── test_sim2.cpp │ │ ├── test_sim3.cpp │ │ ├── test_so2.cpp │ │ ├── test_so3.cpp │ │ ├── test_velocities.cpp │ │ └── tests.hpp └── tessil-src │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── appveyor.yml │ ├── cmake │ └── tsl-robin-mapConfig.cmake.in │ ├── doxygen.conf │ ├── include │ └── tsl │ │ ├── robin_growth_policy.h │ │ ├── robin_hash.h │ │ ├── robin_map.h │ │ └── robin_set.h │ ├── tests │ ├── CMakeLists.txt │ ├── custom_allocator_tests.cpp │ ├── main.cpp │ ├── policy_tests.cpp │ ├── robin_map_tests.cpp │ ├── robin_set_tests.cpp │ └── utils.h │ └── tsl-robin-map.natvis ├── include ├── aloam_velodyne │ ├── common.h │ └── tic_toc.h ├── mapmanager.hpp └── scancontext │ ├── KDTreeVectorOfVectorsAdaptor.h │ ├── Scancontext.cpp │ ├── Scancontext.h │ └── nanoflann.hpp ├── launch ├── aloam_mulran.launch ├── aloam_velodyne_HDL_32.launch ├── aloam_velodyne_HDL_64.launch ├── aloam_velodyne_VLP_16.launch ├── fast_lio_slam_rviz.launch ├── fastlio_ouster64.launch └── kitti_helper.launch ├── package.xml ├── picture └── kitti_gif.gif ├── rviz_cfg └── aloam_velodyne.rviz ├── src ├── kittiHelper.cpp ├── laserMapping.cpp ├── laserOdometry.cpp ├── laserPosegraphOptimization.cpp ├── lidarFactor.hpp ├── scanRegistration.cpp └── trajectory_saver.py └── utils ├── python ├── __pycache__ │ └── pypcdMyUtils.cpython-36.pyc ├── bone_table.npy ├── jet_table.npy ├── makeMergedMap.py └── pypcdMyUtils.py └── sample_data └── KAIST03 ├── optimized_poses.txt └── times.txt /FAST-LIO/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fast_lio) 3 | 4 | # SET(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_BUILD_TYPE "Release") 6 | 7 | ADD_COMPILE_OPTIONS(-std=c++14 ) 8 | ADD_COMPILE_OPTIONS(-std=c++14 ) 9 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 10 | 11 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 12 | 13 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 14 | set(CMAKE_CXX_STANDARD 14) 15 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 16 | set(CMAKE_CXX_EXTENSIONS OFF) 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 18 | 19 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 20 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 21 | include(ProcessorCount) 22 | ProcessorCount(N) 23 | message("Processer number: ${N}") 24 | if(N GREATER 4) 25 | add_definitions(-DMP_EN) 26 | add_definitions(-DMP_PROC_NUM=3) 27 | message("core for MP: 3") 28 | elseif(N GREATER 3) 29 | add_definitions(-DMP_EN) 30 | add_definitions(-DMP_PROC_NUM=2) 31 | message("core for MP: 2") 32 | else() 33 | add_definitions(-DMP_PROC_NUM=1) 34 | endif() 35 | else() 36 | add_definitions(-DMP_PROC_NUM=1) 37 | endif() 38 | 39 | find_package(OpenMP QUIET) 40 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 41 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 42 | find_package( GTSAMCMakeTools ) 43 | find_package(GTSAM REQUIRED QUIET) 44 | find_package(PythonLibs REQUIRED) 45 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") 46 | 47 | find_package(catkin REQUIRED COMPONENTS 48 | geometry_msgs 49 | nav_msgs 50 | sensor_msgs 51 | roscpp 52 | rospy 53 | std_msgs 54 | pcl_ros 55 | tf 56 | livox_ros_driver 57 | message_generation 58 | eigen_conversions 59 | ) 60 | 61 | find_package(Eigen3 REQUIRED) 62 | find_package(PCL 1.8 REQUIRED) 63 | 64 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 65 | 66 | include_directories( 67 | ${catkin_INCLUDE_DIRS} 68 | ${EIGEN3_INCLUDE_DIR} 69 | ${PCL_INCLUDE_DIRS} 70 | ${PYTHON_INCLUDE_DIRS} 71 | ${GTSAM_INCLUDE_DIR} 72 | include) 73 | # link directories 74 | link_directories( 75 | include 76 | ${GTSAM_LIBRARY_DIRS} 77 | ) 78 | add_message_files( 79 | FILES 80 | Pose6D.msg 81 | ) 82 | 83 | generate_messages( 84 | DEPENDENCIES 85 | geometry_msgs 86 | ) 87 | 88 | catkin_package( 89 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 90 | DEPENDS EIGEN3 PCL 91 | INCLUDE_DIRS 92 | ) 93 | 94 | add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) 95 | target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} gtsam) 96 | target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /FAST-LIO/Log/imu.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/Log/imu.txt -------------------------------------------------------------------------------- /FAST-LIO/Log/plot.py: -------------------------------------------------------------------------------- 1 | # import matplotlib 2 | # matplotlib.use('Agg') 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | #######for ikfom 8 | fig, axs = plt.subplots(4,2) 9 | lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] 10 | lab_out = ['', 'out-x', 'out-y', 'out-z'] 11 | plot_ind = range(7,10) 12 | a_pre=np.loadtxt('mat_pre.txt') 13 | a_out=np.loadtxt('mat_out.txt') 14 | time=a_pre[:,0] 15 | axs[0,0].set_title('Attitude') 16 | axs[1,0].set_title('Translation') 17 | axs[2,0].set_title('Extrins-R') 18 | axs[3,0].set_title('Extrins-T') 19 | axs[0,1].set_title('Velocity') 20 | axs[1,1].set_title('bg') 21 | axs[2,1].set_title('ba') 22 | axs[3,1].set_title('Gravity') 23 | for i in range(1,4): 24 | for j in range(8): 25 | axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) 26 | axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) 27 | for j in range(8): 28 | # axs[j].set_xlim(386,389) 29 | axs[j%4, j/4].grid() 30 | axs[j%4, j/4].legend() 31 | plt.grid() 32 | #######for ikfom####### 33 | 34 | 35 | #### Draw IMU data 36 | fig, axs = plt.subplots(2) 37 | imu=np.loadtxt('imu.txt') 38 | time=imu[:,0] 39 | axs[0].set_title('Gyroscope') 40 | axs[1].set_title('Accelerameter') 41 | lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] 42 | lab_2 = ['acc-x', 'acc-y', 'acc-z'] 43 | for i in range(3): 44 | # if i==1: 45 | axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) 46 | axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) 47 | for i in range(2): 48 | # axs[i].set_xlim(386,389) 49 | axs[i].grid() 50 | axs[i].legend() 51 | plt.grid() 52 | 53 | # #### Draw time calculation 54 | # plt.figure(3) 55 | # fig = plt.figure() 56 | # font1 = {'family' : 'Times New Roman', 57 | # 'weight' : 'normal', 58 | # 'size' : 12, 59 | # } 60 | # c="red" 61 | # a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') 62 | # a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') 63 | # a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') 64 | # # n = a_out[:,1].size 65 | # # time_mean = a_out[:,1].mean() 66 | # # time_se = a_out[:,1].std() / np.sqrt(n) 67 | # # time_err = a_out[:,1] - time_mean 68 | # # feat_mean = a_out[:,2].mean() 69 | # # feat_err = a_out[:,2] - feat_mean 70 | # # feat_se = a_out[:,2].std() / np.sqrt(n) 71 | # ax1 = fig.add_subplot(111) 72 | # ax1.set_ylabel('Effective Feature Numbers',font1) 73 | # ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) 74 | # ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) 75 | # ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) 76 | # ax1.set_ylim([0, 3000]) 77 | 78 | # ax2 = ax1.twinx() 79 | # ax2.spines['right'].set_color('red') 80 | # ax2.set_ylabel('Compute Time (ms)',font1) 81 | # ax2.yaxis.label.set_color('red') 82 | # ax2.tick_params(axis='y', colors='red') 83 | # ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 84 | # ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 85 | # ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) 86 | # ax2.set_xlim([0.5, 3.5]) 87 | # ax2.set_ylim([0, 100]) 88 | 89 | # plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) 90 | # # # print(time_se) 91 | # # # print(a_out3[:,2]) 92 | # plt.grid() 93 | # plt.savefig("time.pdf", dpi=1200) 94 | plt.show() 95 | -------------------------------------------------------------------------------- /FAST-LIO/Log/pos_log.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/Log/pos_log.txt -------------------------------------------------------------------------------- /FAST-LIO/README.md: -------------------------------------------------------------------------------- 1 | # Node 1: FAST-LIO 2 | 3 |
Add Over-Degeneracy Detection and Initialization Method
4 | -------------------------------------------------------------------------------- /FAST-LIO/config/avia.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/livox/lidar" 3 | imu_topic: "/livox/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 6 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 90 17 | det_range: 450.0 18 | extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] 19 | extrinsic_R: [ 1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: true # 'false' will close all the point cloud output 25 | dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /FAST-LIO/config/horizon.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/livox/lidar" 3 | imu_topic: "/livox/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 6 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 100 17 | det_range: 260.0 18 | extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] 19 | extrinsic_R: [ 1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: true # 'false' will close all the point cloud output 25 | dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /FAST-LIO/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 | 6 | preprocess: 7 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 128 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 180 17 | det_range: 150.0 18 | extrinsic_T: [ 0.0, 0.0, 0.0 ] 19 | extrinsic_R: [1, 0, 0, 20 | 0, 1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: true # 'false' will close all the point cloud output 25 | dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /FAST-LIO/config/ouster64_mulran.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os1_points" 3 | imu_topic: "/imu/data_raw" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 64 9 | blind: 4 10 | 11 | mapping: 12 | acc_cov: 0.1 13 | gyr_cov: 0.1 14 | b_acc_cov: 0.0001 15 | b_gyr_cov: 0.0001 16 | fov_degree: 180 17 | det_range: 150.0 18 | extrinsic_T: [ 1.77, 0.0, -0.05 ] 19 | extrinsic_R: [-1, 0, 0, 20 | 0, -1, 0, 21 | 0, 0, 1] 22 | 23 | publish: 24 | scan_publish_en: true # 'false' will close all the point cloud output 25 | dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. 26 | scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame 27 | -------------------------------------------------------------------------------- /FAST-LIO/config/robosense.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/rslidar_points" 3 | imu_topic: "/imu_raw" 4 | 5 | preprocess: 6 | lidar_type: 4 #1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for L515 LiDAR 7 | scan_line: 32 8 | blind: 1.0 9 | point_filter_num: 1 10 | calib_laser: false # true for KITTI Odometry dataset 11 | 12 | mapping: 13 | down_sample_size: 0.5 14 | max_iteration: 3 15 | voxel_size: 0.5 16 | max_layer: 4 # 4 layer, 0, 1, 2, 3 17 | layer_point_size: [5, 5, 5, 5, 5] 18 | plannar_threshold: 0.01 19 | max_points_size: 1000 20 | max_cov_points_size: 1000 21 | 22 | noise_model: 23 | ranging_cov: 0.04 24 | angle_cov: 0.1 25 | acc_cov_scale: 1.0 26 | gyr_cov_scale: 0.5 27 | 28 | imu: 29 | imu_en: true 30 | extrinsic_T: [ 0, 0, 0] 31 | extrinsic_R: [ 1, 0, 0, 32 | 0, 1, 0, 33 | 0, 0, 1] 34 | 35 | visualization: 36 | pub_voxel_map: false 37 | publish_max_voxel_layer: 2 # only publish 0,1,2 layer's plane 38 | pub_point_cloud: true 39 | dense_map_enable: false 40 | pub_point_cloud_skip: 5 # publish one points per five points 41 | 42 | Result: 43 | write_kitti_log: false 44 | result_path: "/home/ycj/catkin_github/src/VoxelMap/Log/kitt_log.txt" -------------------------------------------------------------------------------- /FAST-LIO/config/velodyne.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/velodyne_points" 3 | imu_topic: "/imu_raw" 4 | # imu_topic: "/imu/data" 5 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 6 | 7 | preprocess: 8 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 9 | scan_line: 16 10 | blind: 4 11 | 12 | mapping: 13 | acc_cov: 0.1 14 | gyr_cov: 0.1 15 | b_acc_cov: 0.0001 16 | b_gyr_cov: 0.0001 17 | fov_degree: 180 18 | det_range: 100.0 19 | # extrinsic_T: [ -0.017189, 0.015727, 0.106037] 20 | # extrinsic_R: [ -0.999887, -0.011619, -0.009487, 21 | # 0.011486, -0.999838, 0.013885, 22 | # -0.009647, 0.013775, 0.99859] 23 | extrinsic_T: [ 0, 0, 0] 24 | extrinsic_R: [ 1, -0.011619, -0.009487, 25 | 0.011486, 1, 0.013885, 26 | -0.009647, 0.013775, 1] 27 | 28 | publish: 29 | scan_publish_en: true # 'false' will close all the point cloud output 30 | dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan. 31 | scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame -------------------------------------------------------------------------------- /FAST-LIO/doc/Fast_LIO_2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/doc/Fast_LIO_2.pdf -------------------------------------------------------------------------------- /FAST-LIO/doc/real_experiment2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/doc/real_experiment2.gif -------------------------------------------------------------------------------- /FAST-LIO/doc/uav01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/doc/uav01.jpg -------------------------------------------------------------------------------- /FAST-LIO/doc/uav_ground.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/doc/uav_ground.pdf -------------------------------------------------------------------------------- /FAST-LIO/doc/ulhkwh_fastlio.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/doc/ulhkwh_fastlio.gif -------------------------------------------------------------------------------- /FAST-LIO/include/Exp_mat.h: -------------------------------------------------------------------------------- 1 | #ifndef EXP_MAT_H 2 | #define EXP_MAT_H 3 | 4 | #include 5 | #include 6 | #include 7 | // #include 8 | 9 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 10 | 11 | template 12 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 13 | { 14 | T ang_norm = ang.norm(); 15 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 16 | if (ang_norm > 0.0000001) 17 | { 18 | Eigen::Matrix r_axis = ang / ang_norm; 19 | Eigen::Matrix K; 20 | K << SKEW_SYM_MATRX(r_axis); 21 | /// Roderigous Tranformation 22 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 23 | } 24 | else 25 | { 26 | return Eye3; 27 | } 28 | } 29 | 30 | template 31 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 32 | { 33 | T ang_vel_norm = ang_vel.norm(); 34 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 35 | 36 | if (ang_vel_norm > 0.0000001) 37 | { 38 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 39 | Eigen::Matrix K; 40 | 41 | K << SKEW_SYM_MATRX(r_axis); 42 | 43 | T r_ang = ang_vel_norm * dt; 44 | 45 | /// Roderigous Tranformation 46 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 47 | } 48 | else 49 | { 50 | return Eye3; 51 | } 52 | } 53 | 54 | template 55 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 56 | { 57 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 58 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 59 | if (norm > 0.00001) 60 | { 61 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 62 | Eigen::Matrix K; 63 | K << SKEW_SYM_MATRX(r_ang); 64 | 65 | /// Roderigous Tranformation 66 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 67 | } 68 | else 69 | { 70 | return Eye3; 71 | } 72 | } 73 | 74 | /* Logrithm of a Rotation Matrix */ 75 | template 76 | Eigen::Matrix Log(const Eigen::Matrix &R) 77 | { 78 | T &&theta = std::acos(0.5 * (R.trace() - 1)); 79 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 80 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 81 | } 82 | 83 | // template 84 | // cv::Mat Exp(const T &v1, const T &v2, const T &v3) 85 | // { 86 | 87 | // T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 88 | // cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); 89 | // if (norm > 0.0000001) 90 | // { 91 | // T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 92 | // cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); 93 | 94 | // /// Roderigous Tranformation 95 | // return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 96 | // } 97 | // else 98 | // { 99 | // return Eye3; 100 | // } 101 | // } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git 3 | * @Date: 2023-04-21 14:55:12 4 | * @LastEditors: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git 5 | * @LastEditTime: 2023-04-24 22:33:01 6 | * @FilePath: /fast_lio_SLAM_ws/home/myx/fighting/fast_lio_SLAM_ws/src/FAST_LIO_SLAM-main/FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp 7 | * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE 8 | */ 9 | /* 10 | * Copyright (c) 2019--2023, The University of Hong Kong 11 | * All rights reserved. 12 | * 13 | * Author: Dongjiao HE 14 | * 15 | * Redistribution and use in source and binary forms, with or without 16 | * modification, are permitted provided that the following conditions 17 | * are met: 18 | * 19 | * * Redistributions of source code must retain the above copyright 20 | * notice, this list of conditions and the following disclaimer. 21 | * * Redistributions in binary form must reproduce the above 22 | * copyright notice, this list of conditions and the following 23 | * disclaimer in the documentation and/or other materials provided 24 | * with the distribution. 25 | * * Neither the name of the Universitaet Bremen nor the names of its 26 | * contributors may be used to endorse or promote products derived 27 | * from this software without specific prior written permission. 28 | * 29 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 30 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 31 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 32 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 33 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 34 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 35 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 36 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 38 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 39 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 40 | * POSSIBILITY OF SUCH DAMAGE. 41 | */ 42 | 43 | #ifndef __MEKFOM_UTIL_HPP__ 44 | #define __MEKFOM_UTIL_HPP__ 45 | 46 | #include 47 | #include "../mtk/src/mtkmath.hpp" 48 | namespace esekfom { 49 | 50 | template 51 | class is_same { 52 | public: 53 | operator bool() { 54 | return false; 55 | } 56 | }; 57 | template 58 | class is_same { 59 | public: 60 | operator bool() { 61 | return true; 62 | } 63 | }; 64 | 65 | template 66 | class is_double { 67 | public: 68 | operator bool() { 69 | return false; 70 | } 71 | }; 72 | 73 | template<> 74 | class is_double { 75 | public: 76 | operator bool() { 77 | return true; 78 | } 79 | }; 80 | 81 | template 82 | static T 83 | id(const T &x) 84 | { 85 | return x; 86 | } 87 | 88 | } // namespace esekfom 89 | 90 | #endif // __MEKFOM_UTIL_HPP__ 91 | -------------------------------------------------------------------------------- /FAST-LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH 3 | * All rights reserved. 4 | * 5 | * Author: Rene Wagner 6 | * Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the DFKI GmbH 19 | * nor the names of its contributors may be used to endorse or 20 | * promote products derived from this software without specific 21 | * prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef WRAPPED_CV_MAT_HPP_ 38 | #define WRAPPED_CV_MAT_HPP_ 39 | 40 | #include 41 | #include 42 | 43 | namespace MTK { 44 | 45 | template 46 | struct cv_f_type; 47 | 48 | template<> 49 | struct cv_f_type 50 | { 51 | enum {value = CV_64F}; 52 | }; 53 | 54 | template<> 55 | struct cv_f_type 56 | { 57 | enum {value = CV_32F}; 58 | }; 59 | 60 | /** 61 | * cv_mat wraps a CvMat around an Eigen Matrix 62 | */ 63 | template 64 | class cv_mat : public matrix 65 | { 66 | typedef matrix base_type; 67 | enum {type_ = cv_f_type::value}; 68 | CvMat cv_mat_; 69 | 70 | public: 71 | cv_mat() 72 | { 73 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 74 | } 75 | 76 | cv_mat(const cv_mat& oth) : base_type(oth) 77 | { 78 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 79 | } 80 | 81 | template 82 | cv_mat(const Eigen::MatrixBase &value) : base_type(value) 83 | { 84 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 85 | } 86 | 87 | template 88 | cv_mat& operator=(const Eigen::MatrixBase &value) 89 | { 90 | base_type::operator=(value); 91 | return *this; 92 | } 93 | 94 | cv_mat& operator=(const cv_mat& value) 95 | { 96 | base_type::operator=(value); 97 | return *this; 98 | } 99 | 100 | // FIXME: Maybe overloading operator& is not a good idea ... 101 | CvMat* operator&() 102 | { 103 | return &cv_mat_; 104 | } 105 | const CvMat* operator&() const 106 | { 107 | return &cv_mat_; 108 | } 109 | }; 110 | 111 | } // namespace MTK 112 | 113 | #endif /* WRAPPED_CV_MAT_HPP_ */ 114 | -------------------------------------------------------------------------------- /FAST-LIO/include/ikd-Tree/README.md: -------------------------------------------------------------------------------- 1 | # ikd-Tree 2 | ikd-Tree is an incremental k-d tree for robotic applications. 3 | -------------------------------------------------------------------------------- /FAST-LIO/include/so3_math.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_MATH_H 2 | #define SO3_MATH_H 3 | 4 | #include 5 | #include 6 | 7 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 8 | 9 | template 10 | Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) 11 | { 12 | Eigen::Matrix skew_sym_mat; 13 | skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; 14 | return skew_sym_mat; 15 | } 16 | 17 | template 18 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 19 | { 20 | T ang_norm = ang.norm(); 21 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 22 | if (ang_norm > 0.0000001) 23 | { 24 | Eigen::Matrix r_axis = ang / ang_norm; 25 | Eigen::Matrix K; 26 | K << SKEW_SYM_MATRX(r_axis); 27 | /// Roderigous Tranformation 28 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 29 | } 30 | else 31 | { 32 | return Eye3; 33 | } 34 | } 35 | 36 | template 37 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 38 | { 39 | T ang_vel_norm = ang_vel.norm(); 40 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 41 | 42 | if (ang_vel_norm > 0.0000001) 43 | { 44 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 45 | Eigen::Matrix K; 46 | 47 | K << SKEW_SYM_MATRX(r_axis); 48 | 49 | T r_ang = ang_vel_norm * dt; 50 | 51 | /// Roderigous Tranformation 52 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 53 | } 54 | else 55 | { 56 | return Eye3; 57 | } 58 | } 59 | 60 | template 61 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 62 | { 63 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 64 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 65 | if (norm > 0.00001) 66 | { 67 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 68 | Eigen::Matrix K; 69 | K << SKEW_SYM_MATRX(r_ang); 70 | 71 | /// Roderigous Tranformation 72 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 73 | } 74 | else 75 | { 76 | return Eye3; 77 | } 78 | } 79 | 80 | /* Logrithm of a Rotation Matrix 旋转矩阵的对数 */ 81 | template 82 | Eigen::Matrix Log(const Eigen::Matrix &R) 83 | { 84 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); 85 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 86 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 87 | } 88 | 89 | template 90 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) 91 | { 92 | T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); 93 | bool singular = sy < 1e-6; 94 | T x, y, z; 95 | if(!singular) 96 | { 97 | x = atan2(rot(2, 1), rot(2, 2)); 98 | y = atan2(-rot(2, 0), sy); 99 | z = atan2(rot(1, 0), rot(0, 0)); 100 | } 101 | else 102 | { 103 | x = atan2(-rot(1, 2), rot(1, 1)); 104 | y = atan2(-rot(2, 0), sy); 105 | z = 0; 106 | } 107 | Eigen::Matrix ang(x, y, z); 108 | return ang; 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /FAST-LIO/include/use-ikfom.hpp: -------------------------------------------------------------------------------- 1 | #ifndef USE_IKFOM_H 2 | #define USE_IKFOM_H 3 | 4 | #include 5 | 6 | typedef MTK::vect<3, double> vect3; 7 | typedef MTK::SO3 SO3; 8 | typedef MTK::S2 S2; 9 | typedef MTK::vect<1, double> vect1; 10 | typedef MTK::vect<2, double> vect2; 11 | 12 | MTK_BUILD_MANIFOLD(state_ikfom, 13 | ((vect3, pos)) 14 | ((SO3, rot)) 15 | ((SO3, offset_R_L_I)) 16 | ((vect3, offset_T_L_I)) 17 | ((vect3, vel)) 18 | ((vect3, bg)) 19 | ((vect3, ba)) 20 | ((S2, grav)) 21 | ); 22 | 23 | MTK_BUILD_MANIFOLD(input_ikfom, 24 | ((vect3, acc)) 25 | ((vect3, gyro)) 26 | ); 27 | 28 | MTK_BUILD_MANIFOLD(process_noise_ikfom, 29 | ((vect3, ng)) 30 | ((vect3, na)) 31 | ((vect3, nbg)) 32 | ((vect3, nba)) 33 | ); 34 | 35 | MTK::get_cov::type process_noise_cov() 36 | { 37 | MTK::get_cov::type cov = MTK::get_cov::type::Zero(); 38 | MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03 39 | MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 40 | MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 41 | MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01 42 | return cov; 43 | } 44 | 45 | 46 | Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in) 47 | { 48 | Eigen::Matrix res = Eigen::Matrix::Zero(); 49 | vect3 omega; 50 | in.gyro.boxminus(omega, s.bg); 51 | vect3 a_inertial = s.rot * (in.acc-s.ba); 52 | for(int i = 0; i < 3; i++ ){ 53 | res(i) = s.vel[i]; 54 | res(i + 3) = omega[i]; 55 | res(i + 12) = a_inertial[i] + s.grav[i]; 56 | } 57 | return res; 58 | } 59 | Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in) 60 | { 61 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 62 | cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); 63 | vect3 acc_; 64 | in.acc.boxminus(acc_, s.ba); 65 | vect3 omega; 66 | in.gyro.boxminus(omega, s.bg); 67 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); 68 | cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); 69 | Eigen::Matrix vec = Eigen::Matrix::Zero(); 70 | Eigen::Matrix grav_matrix; 71 | s.S2_Mx(grav_matrix, vec, 21); 72 | cov.template block<3, 2>(12, 21) = grav_matrix; 73 | cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 74 | return cov; 75 | } 76 | 77 | 78 | Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) 79 | { 80 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 81 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); 82 | cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); 83 | cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); 84 | cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); 85 | return cov; 86 | } 87 | 88 | vect3 SO3ToEuler(const SO3 &orient) 89 | { 90 | Eigen::Matrix _ang; 91 | Eigen::Vector4d q_data = orient.coeffs().transpose(); 92 | //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; 93 | double sqw = q_data[3]*q_data[3]; 94 | double sqx = q_data[0]*q_data[0]; 95 | double sqy = q_data[1]*q_data[1]; 96 | double sqz = q_data[2]*q_data[2]; 97 | double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor 98 | double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; 99 | 100 | if (test > 0.49999*unit) { // singularity at north pole 101 | 102 | _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; 103 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 104 | vect3 euler_ang(temp, 3); 105 | return euler_ang; 106 | } 107 | if (test < -0.49999*unit) { // singularity at south pole 108 | _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; 109 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 110 | vect3 euler_ang(temp, 3); 111 | return euler_ang; 112 | } 113 | 114 | _ang << 115 | std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), 116 | std::asin (2*test/unit), 117 | std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); 118 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 119 | vect3 euler_ang(temp, 3); 120 | // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw 121 | return euler_ang; 122 | } 123 | 124 | #endif -------------------------------------------------------------------------------- /FAST-LIO/launch/fast_lio_slam_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /FAST-LIO/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 | -------------------------------------------------------------------------------- /FAST-LIO/launch/mapping_avia.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /FAST-LIO/launch/mapping_horizon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /FAST-LIO/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 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /FAST-LIO/launch/mapping_ouster64_mulran.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /FAST-LIO/launch/mapping_robosense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /FAST-LIO/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 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /FAST-LIO/msg/Pose6D.msg: -------------------------------------------------------------------------------- 1 | # the preintegrated Lidar states at the time of IMU measurements in a frame 2 | float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point 3 | float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin 4 | float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin 5 | float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin 6 | float64[3] pos # the preintegrated position (global frame) at the Lidar origin 7 | float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin -------------------------------------------------------------------------------- /FAST-LIO/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_lio 4 | 0.0.0 5 | 6 | 7 | This is a modified version of LOAM which is original algorithm 8 | is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | claydergc 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | 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 | -------------------------------------------------------------------------------- /FAST-LIO/rviz_cfg/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/FAST-LIO/rviz_cfg/.gitignore -------------------------------------------------------------------------------- /FAST-LIO/src/preprocess.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | #define IS_VALID(a) ((abs(a)>1e8) ? true : false) 9 | 10 | typedef pcl::PointXYZINormal PointType; 11 | typedef pcl::PointCloud PointCloudXYZI; 12 | 13 | enum LID_TYPE{AVIA = 1, VELO16, OUST64, Robosense}; 14 | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; 15 | enum Surround{Prev, Next}; 16 | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; 17 | struct orgtype 18 | { 19 | double range; 20 | double dista; 21 | double angle[2]; 22 | double intersect; 23 | E_jump edj[2]; 24 | Feature ftype; 25 | orgtype() 26 | { 27 | range = 0; 28 | edj[Prev] = Nr_nor; 29 | edj[Next] = Nr_nor; 30 | ftype = Nor; 31 | intersect = 2; 32 | } 33 | }; 34 | namespace robosense_ros 35 | { 36 | struct EIGEN_ALIGN16 Point 37 | { 38 | PCL_ADD_POINT4D; 39 | uint8_t intensity; 40 | uint16_t ring = 0; 41 | double timestamp = 0; 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | }; 44 | } 45 | POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, 46 | (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp)) 47 | namespace velodyne_ros { 48 | struct EIGEN_ALIGN16 Point { 49 | PCL_ADD_POINT4D; 50 | float intensity; 51 | float time; 52 | uint16_t ring; 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 54 | }; 55 | } // namespace velodyne_ros 56 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 57 | (float, x, x) 58 | (float, y, y) 59 | (float, z, z) 60 | (float, intensity, intensity) 61 | (float, time, time) 62 | (uint16_t, ring, ring) 63 | ) 64 | 65 | namespace ouster_ros { 66 | struct EIGEN_ALIGN16 Point { 67 | PCL_ADD_POINT4D; 68 | float intensity; 69 | uint32_t t; 70 | uint16_t reflectivity; 71 | uint8_t ring; 72 | uint16_t ambient; 73 | uint32_t range; 74 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 75 | }; 76 | } // namespace ouster_ros 77 | 78 | // clang-format off 79 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 80 | (float, x, x) 81 | (float, y, y) 82 | (float, z, z) 83 | (float, intensity, intensity) 84 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 85 | (std::uint32_t, t, t) 86 | (std::uint16_t, reflectivity, reflectivity) 87 | (std::uint8_t, ring, ring) 88 | (std::uint16_t, ambient, ambient) 89 | (std::uint32_t, range, range) 90 | ) 91 | 92 | class Preprocess 93 | { 94 | public: 95 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 96 | 97 | Preprocess(); 98 | ~Preprocess(); 99 | 100 | void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 101 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 102 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 103 | 104 | // sensor_msgs::PointCloud2::ConstPtr pointcloud; 105 | PointCloudXYZI pl_full, pl_corn, pl_surf; 106 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar 107 | vector typess[128]; //maximum 128 line lidar 108 | int lidar_type, point_filter_num, N_SCANS;; 109 | double blind; 110 | bool feature_enabled, given_offset_time; 111 | ros::Publisher pub_full, pub_surf, pub_corn; 112 | 113 | 114 | private: 115 | void RobosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); 116 | void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); 117 | void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 118 | void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 119 | void give_feature(PointCloudXYZI &pl, vector &types); 120 | void pub_func(PointCloudXYZI &pl, const ros::Time &ct); 121 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); 122 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); 123 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); 124 | 125 | int group_size; 126 | double disA, disB, inf_bound; 127 | double limit_maxmid, limit_midmin, limit_maxmin; 128 | double p2l_ratio; 129 | double jump_up_limit, jump_down_limit; 130 | double cos160; 131 | double edgea, edgeb; 132 | double smallp_intersect, smallp_ratio; 133 | double vx, vy, vz; 134 | }; 135 | -------------------------------------------------------------------------------- /IMAGE/Overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/IMAGE/Overview.png -------------------------------------------------------------------------------- /IMAGE/motivations.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lian-yue0515/MM-LINS/2921516d9e1f03521971120aa125986505a34781/IMAGE/motivations.png -------------------------------------------------------------------------------- /SC-PGO/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aloam_velodyne) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | # SET(CMAKE_BUILD_TYPE "Debug") 6 | 7 | # set(CMAKE_CXX_FLAGS "-std=c++11") 8 | set(CMAKE_CXX_STANDARD 17) 9 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 10 | add_subdirectory(Thirdparty/tessil-src) 11 | find_package(catkin REQUIRED COMPONENTS 12 | geometry_msgs 13 | nav_msgs 14 | sensor_msgs 15 | roscpp 16 | rospy 17 | rosbag 18 | std_msgs 19 | image_transport 20 | cv_bridge 21 | tf 22 | ) 23 | 24 | #find_package(Eigen3 REQUIRED) 25 | find_package(PCL REQUIRED) 26 | find_package(OpenCV REQUIRED) 27 | find_package(Ceres REQUIRED) 28 | 29 | find_package(OpenMP REQUIRED) 30 | find_package(GTSAM REQUIRED QUIET) 31 | 32 | include_directories( 33 | include 34 | ${catkin_INCLUDE_DIRS} 35 | ${PCL_INCLUDE_DIRS} 36 | ${CERES_INCLUDE_DIRS} 37 | ${OpenCV_INCLUDE_DIRS} 38 | ${GTSAM_INCLUDE_DIR} 39 | ) 40 | 41 | catkin_package( 42 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 43 | DEPENDS EIGEN3 PCL 44 | INCLUDE_DIRS include 45 | ) 46 | 47 | 48 | add_executable(ascanRegistration src/scanRegistration.cpp) 49 | target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 50 | 51 | add_executable(alaserOdometry src/laserOdometry.cpp) 52 | target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 53 | 54 | add_executable(alaserMapping src/laserMapping.cpp) 55 | target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 56 | 57 | add_executable(alaserPGO 58 | src/laserPosegraphOptimization.cpp 59 | include/scancontext/Scancontext.cpp 60 | ) 61 | target_compile_options(alaserPGO 62 | PRIVATE ${OpenMP_CXX_FLAGS} 63 | ) 64 | target_link_libraries(alaserPGO 65 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} 66 | ${OpenMP_CXX_FLAGS} 67 | gtsam 68 | tsl::robin_map 69 | ) 70 | 71 | add_executable(kittiHelper src/kittiHelper.cpp) 72 | target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /SC-PGO/README.md: -------------------------------------------------------------------------------- 1 | # Node 2: SC-PGO 2 | 3 |
Add Map Management Module and Constraint-Enhanced Pose Graph Optimization
-------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | # BasedOnStyle: Google 3 | AccessModifierOffset: -1 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignEscapedNewlinesLeft: true 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: true 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: true 21 | BinPackParameters: true 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | ColumnLimit: 80 39 | CommentPragmas: '^ IWYU pragma:' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 41 | ConstructorInitializerIndentWidth: 4 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: true 45 | DisableFormat: false 46 | ExperimentalAutoDetectBinPacking: false 47 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 48 | IncludeCategories: 49 | - Regex: '^<.*\.h>' 50 | Priority: 1 51 | - Regex: '^<.*' 52 | Priority: 2 53 | - Regex: '.*' 54 | Priority: 3 55 | IndentCaseLabels: true 56 | IndentWidth: 2 57 | IndentWrappedFunctionNames: false 58 | KeepEmptyLinesAtTheStartOfBlocks: false 59 | MacroBlockBegin: '' 60 | MacroBlockEnd: '' 61 | MaxEmptyLinesToKeep: 1 62 | NamespaceIndentation: None 63 | ObjCBlockIndentWidth: 2 64 | ObjCSpaceAfterProperty: false 65 | ObjCSpaceBeforeProtocolList: false 66 | PenaltyBreakBeforeFirstCallParameter: 1 67 | PenaltyBreakComment: 300 68 | PenaltyBreakFirstLessLess: 120 69 | PenaltyBreakString: 1000 70 | PenaltyExcessCharacter: 1000000 71 | PenaltyReturnTypeOnItsOwnLine: 200 72 | PointerAlignment: Left 73 | ReflowComments: true 74 | SortIncludes: true 75 | SpaceAfterCStyleCast: false 76 | SpaceBeforeAssignmentOperators: true 77 | SpaceBeforeParens: ControlStatements 78 | SpaceInEmptyParentheses: false 79 | SpacesBeforeTrailingComments: 2 80 | SpacesInAngles: false 81 | SpacesInContainerLiterals: true 82 | SpacesInCStyleCastParentheses: false 83 | SpacesInParentheses: false 84 | SpacesInSquareBrackets: false 85 | Standard: Auto 86 | TabWidth: 8 87 | UseTab: Never 88 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright 2011-2017 Hauke Strasdat 2 | 2012-2017 Steven Lovegrove 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to 6 | deal in the Software without restriction, including without limitation the 7 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 8 | sell copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 20 | IN THE SOFTWARE. -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/README.rst: -------------------------------------------------------------------------------- 1 | |GithubCICpp|_ windows: |AppVeyor|_ |GithubCISympy|_ |ci_cov|_ 2 | 3 | 4 | Sophus 5 | ====== 6 | 7 | Overview 8 | -------- 9 | 10 | This is a c++ implementation of Lie groups commonly used for 2d and 3d 11 | geometric problems (i.e. for Computer Vision or Robotics applications). 12 | Among others, this package includes the special orthogonal groups SO(2) and 13 | SO(3) to present rotations in 2d and 3d as well as the special Euclidean group 14 | SE(2) and SE(3) to represent rigid body transformations (i.e. rotations and 15 | translations) in 2d and 3d. 16 | 17 | API documentation: https://strasdat.github.io/Sophus/ 18 | 19 | Cross platform support 20 | ---------------------- 21 | 22 | Sophus compiles with clang and gcc on Linux and OS X as well as msvc on Windows. 23 | The specific compiler and operating system versions which are supported are 24 | the ones which are used in the Continuous Integration (CI): See GitHubCI_ and 25 | AppVeyor_ for details. 26 | 27 | However, it should work (with no to minor modification) on many other 28 | modern configurations as long they support c++14, CMake, Eigen 3.3.X and 29 | (optionally) fmt. The fmt dependency can be eliminated by passing 30 | "-DUSE_BASIC_LOGGING=ON" to cmake when configuring Sophus. 31 | 32 | .. _GitHubCI: https://github.com/strasdat/Sophus/actions 33 | 34 | .. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true 35 | .. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master 36 | 37 | .. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master 38 | .. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master 39 | 40 | .. |GithubCICpp| image:: https://github.com/strasdat/Sophus/actions/workflows/main.yml/badge.svg?branch=master 41 | .. _GithubCICpp: https://github.com/strasdat/Sophus/actions/workflows/main.yml?query=branch%3Amaster 42 | 43 | .. |GithubCISympy| image:: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml/badge.svg?branch=master 44 | .. _GithubCISympy: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml?query=branch%3Amaster 45 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/Sophus.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | } 6 | ], 7 | "settings": {} 8 | } -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include (CMakeFindDependencyMacro) 4 | 5 | @Eigen3_DEPENDENCY@ 6 | @fmt_DEPENDENCY@ 7 | 8 | include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake") 9 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/appveyor.yml: -------------------------------------------------------------------------------- 1 | branches: 2 | only: 3 | - master 4 | 5 | os: Visual Studio 2015 6 | 7 | clone_folder: c:\projects\sophus 8 | 9 | platform: x64 10 | configuration: Debug 11 | 12 | build: 13 | project: c:\projects\sophus\build\Sophus.sln 14 | 15 | install: 16 | - ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip 17 | - cmd: 7z x eigen3.zip -o"C:\projects" -y > nul 18 | - git clone https://github.com/fmtlib/fmt.git 19 | - cd fmt 20 | - git checkout 5.3.0 21 | - mkdir build 22 | - cd build 23 | - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug .. 24 | - cmake --build . 25 | - cmake --build . --target install 26 | - cd ../.. 27 | 28 | before_build: 29 | - cd c:\projects\sophus 30 | - mkdir build 31 | - cd build 32 | - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3.4 .. 33 | 34 | after_build: 35 | - ctest --output-on-failure 36 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | # 13 | # and the following imported target: 14 | # 15 | # Eigen3::Eigen - The header-only Eigen library 16 | # 17 | # This module reads hints about search locations from 18 | # the following environment variables: 19 | # 20 | # EIGEN3_ROOT 21 | # EIGEN3_ROOT_DIR 22 | 23 | # Copyright (c) 2006, 2007 Montel Laurent, 24 | # Copyright (c) 2008, 2009 Gael Guennebaud, 25 | # Copyright (c) 2009 Benoit Jacob 26 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 27 | 28 | if(NOT Eigen3_FIND_VERSION) 29 | if(NOT Eigen3_FIND_VERSION_MAJOR) 30 | set(Eigen3_FIND_VERSION_MAJOR 2) 31 | endif() 32 | if(NOT Eigen3_FIND_VERSION_MINOR) 33 | set(Eigen3_FIND_VERSION_MINOR 91) 34 | endif() 35 | if(NOT Eigen3_FIND_VERSION_PATCH) 36 | set(Eigen3_FIND_VERSION_PATCH 0) 37 | endif() 38 | 39 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 40 | endif() 41 | 42 | macro(_eigen3_check_version) 43 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 44 | 45 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 46 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 47 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 48 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 49 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 50 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 51 | 52 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 53 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 54 | set(EIGEN3_VERSION_OK FALSE) 55 | else() 56 | set(EIGEN3_VERSION_OK TRUE) 57 | endif() 58 | 59 | if(NOT EIGEN3_VERSION_OK) 60 | 61 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 62 | "but at least version ${Eigen3_FIND_VERSION} is required") 63 | endif() 64 | endmacro() 65 | 66 | if (EIGEN3_INCLUDE_DIR) 67 | 68 | # in cache already 69 | _eigen3_check_version() 70 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 71 | set(Eigen3_FOUND ${EIGEN3_VERSION_OK}) 72 | 73 | else () 74 | 75 | # search first if an Eigen3Config.cmake is available in the system, 76 | # if successful this would set EIGEN3_INCLUDE_DIR and the rest of 77 | # the script will work as usual 78 | find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) 79 | 80 | if(NOT EIGEN3_INCLUDE_DIR) 81 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 82 | HINTS 83 | ENV EIGEN3_ROOT 84 | ENV EIGEN3_ROOT_DIR 85 | PATHS 86 | ${CMAKE_INSTALL_PREFIX}/include 87 | ${KDE4_INCLUDE_DIR} 88 | PATH_SUFFIXES eigen3 eigen 89 | ) 90 | endif() 91 | 92 | if(EIGEN3_INCLUDE_DIR) 93 | _eigen3_check_version() 94 | endif() 95 | 96 | include(FindPackageHandleStandardArgs) 97 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 98 | 99 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 100 | 101 | endif() 102 | 103 | if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen) 104 | add_library(Eigen3::Eigen INTERFACE IMPORTED) 105 | set_target_properties(Eigen3::Eigen PROPERTIES 106 | INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}") 107 | endif() 108 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/doxyfile: -------------------------------------------------------------------------------- 1 | DOXYFILE_ENCODING = UTF-8 2 | PROJECT_NAME = "Sophus" 3 | INPUT = sophus 4 | EXTRACT_ALL = YES 5 | ENABLE_PREPROCESSING = YES 6 | MACRO_EXPANSION = YES 7 | WARN_AS_ERROR = YES 8 | EXPAND_ONLY_PREDEF = NO 9 | SKIP_FUNCTION_MACROS = NO 10 | AUTOLINK_SUPPORT = YES 11 | MULTILINE_CPP_IS_BRIEF = YES 12 | MARKDOWN_SUPPORT = YES 13 | INLINE_INHERITED_MEMB = NO 14 | EXCLUDE_SYMBOLS = Eigen::internal Sophus::details Sophus::interp_details Sophus::experimental 15 | GENERATE_LATEX = NO 16 | STRIP_CODE_COMMENTS = NO 17 | 18 | GENERATE_XML = YES 19 | GENERATE_HTML = NO 20 | XML_OUTPUT = xml-dir 21 | XML_PROGRAMLISTING = NO 22 | CASE_SENSE_NAMES = NO 23 | HIDE_UNDOC_RELATIONS = YES 24 | EXTRACT_ALL = YES -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/doxyrest-config.lua: -------------------------------------------------------------------------------- 1 | FRAME_DIR_LIST = { "doxyrest_b/doxyrest/frame/cfamily", "doxyrest_b/doxyrest/frame/common" } 2 | FRAME_FILE = "index.rst.in" 3 | INPUT_FILE = "xml-dir/index.xml" 4 | OUTPUT_FILE = "rst-dir/index.rst" 5 | INTRO_FILE = "page_index.rst" 6 | SORT_GROUPS_BY = "title" 7 | GLOBAL_AUX_COMPOUND_ID = "group_global" 8 | LANGUAGE = cpp 9 | VERBATIM_TO_CODE_BLOCK = "cpp" 10 | ESCAPE_ASTERISKS = true 11 | ESCAPE_PIPES = true 12 | ESCAPE_TRAILING_UNDERSCORES = true 13 | PROTECTION_FILTER = "protected" 14 | EXCLUDE_EMPTY_DEFINES = true 15 | EXCLUDE_DEFAULT_CONSTRUCTORS = false 16 | EXCLUDE_DESTRUCTORS = false 17 | EXCLUDE_PRIMITIVE_TYPEDEFS = true 18 | SHOW_DIRECT_DESCENDANTS = true 19 | TYPEDEF_TO_USING = true 20 | ML_PARAM_LIST_LENGTH_THRESHOLD = 80 -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Tests to run 2 | SET( EXAMPLE_SOURCES HelloSO3) 3 | 4 | FOREACH(example_src ${EXAMPLE_SOURCES}) 5 | ADD_EXECUTABLE( ${example_src} ${example_src}.cpp) 6 | TARGET_LINK_LIBRARIES( ${example_src} sophus) 7 | ENDFOREACH(example_src) 8 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/examples/HelloSO3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "sophus/geometry.hpp" 3 | 4 | int main() { 5 | // The following demonstrates the group multiplication of rotation matrices 6 | 7 | // Create rotation matrices from rotations around the x and y and z axes: 8 | const double kPi = Sophus::Constants::pi(); 9 | Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4); 10 | Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6); 11 | Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3); 12 | 13 | std::cout << "The rotation matrices are" << std::endl; 14 | std::cout << "R1:\n" << R1.matrix() << std::endl; 15 | std::cout << "R2:\n" << R2.matrix() << std::endl; 16 | std::cout << "R3:\n" << R3.matrix() << std::endl; 17 | std::cout << "Their product R1*R2*R3:\n" 18 | << (R1 * R2 * R3).matrix() << std::endl; 19 | std::cout << std::endl; 20 | 21 | // Rotation matrices can act on vectors 22 | Eigen::Vector3d x; 23 | x << 0.0, 0.0, 1.0; 24 | std::cout << "Rotation matrices can act on vectors" << std::endl; 25 | std::cout << "x\n" << x << std::endl; 26 | std::cout << "R2*x\n" << R2 * x << std::endl; 27 | std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl; 28 | std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl; 29 | std::cout << std::endl; 30 | 31 | // SO(3) are internally represented as unit quaternions. 32 | std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl; 33 | std::cout << "R1 in unit quaternion form:\n" 34 | << R1.unit_quaternion().coeffs() << std::endl; 35 | // Note that the order of coefficients of Eigen's quaternion class is 36 | // (imag0, imag1, imag2, real) 37 | std::cout << std::endl; 38 | } -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/generate_stubs.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | 3 | subprocess.run( 4 | "pybind11-stubgen sophus_pybind -o sophus_pybind-stubs/", 5 | shell=True, 6 | check=True, 7 | ) 8 | 9 | subprocess.run("touch sophus_pybind-stubs/py.typed", shell=True, check=True) 10 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/make_docs.sh: -------------------------------------------------------------------------------- 1 | doxygen doxyfile 2 | doxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua 3 | sphinx-build -b html rst-dir html-dir -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sophus 4 | 1.22.10 5 | 6 | C++ implementation of Lie Groups using Eigen. 7 | 8 | https://github.com/strasdat/sophus 9 | https://github.com/strasdat/sophus/issues 10 | Daniel Stonier 11 | Hauke Strasdat 12 | MIT 13 | 14 | cmake 15 | 16 | eigen 17 | eigen 18 | 19 | 20 | cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/rst-dir/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # http://www.sphinx-doc.org/en/master/config 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | import os 14 | import sys 15 | sys.path.insert(0, os.path.abspath('../sympy')) 16 | 17 | 18 | sys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx')) 19 | extensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc'] 20 | 21 | # -- Project information ----------------------------------------------------- 22 | 23 | project = 'Sophus' 24 | copyright = '2019, Hauke Strasdat' 25 | author = 'Hauke Strasdat' 26 | 27 | 28 | # Tell sphinx what the primary language being documented is. 29 | primary_domain = 'cpp' 30 | 31 | # Tell sphinx what the pygments highlight language should be. 32 | highlight_language = 'cpp' 33 | 34 | 35 | # Add any paths that contain templates here, relative to this directory. 36 | templates_path = ['_templates'] 37 | 38 | # List of patterns, relative to source directory, that match files and 39 | # directories to ignore when looking for source files. 40 | # This pattern also affects html_static_path and html_extra_path. 41 | exclude_patterns = [] 42 | 43 | 44 | # -- Options for HTML output ------------------------------------------------- 45 | 46 | # The theme to use for HTML and HTML Help pages. See the documentation for 47 | # a list of builtin themes. 48 | # 49 | html_theme = "sphinx_rtd_theme" 50 | 51 | # Add any paths that contain custom static files (such as style sheets) here, 52 | # relative to this directory. They are copied after the builtin static files, 53 | # so a file named "default.css" will overwrite the builtin "default.css". 54 | html_static_path = ['_static'] 55 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/rst-dir/page_index.rst: -------------------------------------------------------------------------------- 1 | Sophus - Lie groups for 2d/3d Geometry 2 | ======================================= 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | :caption: Contents: 7 | 8 | GitHub Page 9 | pysophus -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/rst-dir/pysophus.rst: -------------------------------------------------------------------------------- 1 | Python API 2 | ========== 3 | 4 | .. automodule:: sophus.matrix 5 | :members: 6 | 7 | .. automodule:: sophus.complex 8 | :members: 9 | 10 | .. automodule:: sophus.quaternion 11 | :members: 12 | 13 | .. automodule:: sophus.so2 14 | :members: 15 | 16 | .. automodule:: sophus.so3 17 | :members: 18 | 19 | .. automodule:: sophus.se2 20 | :members: 21 | 22 | .. automodule:: sophus.se3 23 | :members: -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/run_format.sh: -------------------------------------------------------------------------------- 1 | find . -type d \( -path ./sympy -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o \( -iname "*.hpp" -o -iname "*.cpp" \) -print | xargs clang-format -i 2 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/scripts/install_docs_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | sudo apt-get -qq update 7 | sudo apt-get install doxygen liblua5.3-dev ragel 8 | pip3 install 'sphinx==2.0.1' 9 | pip3 install sphinx_rtd_theme 10 | pip3 install sympy 11 | 12 | git clone https://github.com/vovkos/doxyrest_b 13 | cd doxyrest_b 14 | git reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958 15 | git submodule update --init 16 | mkdir build 17 | cd build 18 | cmake .. 19 | cmake --build . 20 | 21 | cd ../.. 22 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/scripts/install_linux_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | cmake --version 7 | 8 | sudo apt-get -qq update 9 | sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libceres-dev ccache 10 | wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2 11 | tar xvf eigen-3.3.4.tar.bz2 12 | mkdir build-eigen 13 | cd build-eigen 14 | cmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT 15 | sudo make install 16 | cd .. 17 | 18 | git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver 19 | cd ceres-solver 20 | git reset --hard b0aef211db734379319c19c030e734d6e23436b0 21 | mkdir build 22 | cd build 23 | ccache -s 24 | cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. 25 | make -j8 26 | sudo make install 27 | cd ../.. 28 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/scripts/install_linux_fmt_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | git clone https://github.com/fmtlib/fmt.git 7 | cd fmt 8 | git checkout 5.3.0 9 | mkdir build 10 | cd build 11 | cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache .. 12 | make -j8 13 | sudo make install 14 | cd ../.. 15 | -------------------------------------------------------------------------------- /SC-PGO/Thirdparty/Sophus/scripts/install_osx_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | brew update 7 | brew install fmt 8 | brew install ccache 9 | 10 | # Build a specific version of ceres-solver instead of one shipped over brew 11 | curl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb 12 | patch < 4 | 5 | #include 6 | 7 | namespace Sophus { 8 | 9 | /// Templated local parameterization for LieGroup [with implemented 10 | /// LieGroup::Dx_this_mul_exp_x_at_0() ] 11 | template